diff --git a/hsm2040.c b/hsm2040.c index 818f891..444049c 100644 --- a/hsm2040.c +++ b/hsm2040.c @@ -26,6 +26,9 @@ // Device descriptors #include "hsm2040.h" +extern void do_flash(); +extern void low_flash_init(); + static uint8_t itf_num; #if MAX_RES_APDU_DATA_SIZE > MAX_CMD_APDU_DATA_SIZE @@ -1541,6 +1544,8 @@ void led_off_all() } #define RANDOM_BYTES_LENGTH 32 +#include "hardware/flash.h" +#include "hardware/sync.h" extern void neug_task(); int main(void) @@ -1566,6 +1571,8 @@ int main(void) random_init(); + low_flash_init(); + while (1) { prev_millis = board_millis(); @@ -1573,6 +1580,7 @@ int main(void) tud_task(); // tinyusb device task led_blinking_task(); neug_task(); + do_flash(); } return 0; diff --git a/low_flash.c b/low_flash.c index db8310c..ea1f2be 100644 --- a/low_flash.c +++ b/low_flash.c @@ -5,32 +5,161 @@ #include "pico/stdlib.h" #include "hardware/flash.h" #include "hardware/sync.h" +#include "pico/mutex.h" +#include "gnuk.h" #include +#define TOTAL_FLASH_PAGES 4 + +typedef struct PageFlash { + uint8_t page[FLASH_PAGE_SIZE]; + uintptr_t address; + bool ready; + bool erase; +} PageFlash_t; + +static PageFlash_t flash_pages[TOTAL_FLASH_PAGES]; + +static mutex_t mtx_flash; + +static uint8_t ready_pages = 0; + +bool flash_available = false; + + +//this function has to be called from the core 0 +void do_flash() +{ + if (mutex_try_enter(&mtx_flash, NULL) == true) + { + if (flash_available == true && ready_pages > 0) + { + //printf(" DO_FLASH AVAILABLE\r\n"); + for (int r = 0; r < TOTAL_FLASH_PAGES; r++) + { + if (flash_pages[r].ready == true) + { + printf("\r\n"); + uint32_t ints = save_and_disable_interrupts(); + flash_range_erase(flash_pages[r].address-XIP_BASE, FLASH_PAGE_SIZE); + flash_range_program(flash_pages[r].address-XIP_BASE, flash_pages[r].page, FLASH_PAGE_SIZE); + restore_interrupts (ints); + //printf("WRITEN %X !\r\n",flash_pages[r].address); + flash_pages[r].ready = false; + ready_pages--; + } + else if (flash_pages[r].erase == true) + { + + printf("\r\n"); + uint32_t ints = save_and_disable_interrupts(); + flash_range_erase(flash_pages[r].address-XIP_BASE, FLASH_PAGE_SIZE); + restore_interrupts (ints); + flash_pages[r].erase = false; + ready_pages--; + } + } + flash_available = false; + if (ready_pages != 0) { + DEBUG_INFO("ERROR: DO FLASH DOES NOT HAVE ZERO PAGES"); + } + } + mutex_exit(&mtx_flash); + } +} + +//this function has to be called from the core 0 +void low_flash_init() +{ + mutex_init(&mtx_flash); + memset(flash_pages, 0, sizeof(PageFlash_t)*TOTAL_FLASH_PAGES); +} + +void low_flash_available() +{ + mutex_enter_blocking(&mtx_flash); + flash_available = true; + mutex_exit(&mtx_flash); +} + int flash_program_halfword (uintptr_t addr, uint16_t data) { - off_t offset; - uint8_t buf[FLASH_PAGE_SIZE]; - memset(buf, 0, sizeof(uint8_t)*FLASH_PAGE_SIZE); + off_t offset; + uintptr_t addr_alg = addr & -FLASH_PAGE_SIZE; + PageFlash_t *p = NULL; + if (ready_pages == TOTAL_FLASH_PAGES) { + DEBUG_INFO("ERROR: ALL FLASH PAGES CACHED\r\n"); + return 1; + } + mutex_enter_blocking(&mtx_flash); - buf[0] = (data & 0xff); - buf[1] = (data >> 8); - uint32_t ints = save_and_disable_interrupts(); - flash_range_program(addr-XIP_BASE, buf, FLASH_PAGE_SIZE); - restore_interrupts (ints); - return 0; + for (int r = 0; r < TOTAL_FLASH_PAGES; r++) + { + if ((!flash_pages[r].ready && !flash_pages[r].erase) || flash_pages[r].address == addr_alg) //first available + { + p = &flash_pages[r]; + if (!flash_pages[r].ready && !flash_pages[r].erase) + { + memcpy(p->page, (uint8_t *)addr_alg, FLASH_PAGE_SIZE); + ready_pages++; + p->address = addr_alg; + p->ready = true; + } + break; + } + } + + if (!p) + { + DEBUG_INFO("ERROR: FLASH CANNOT FIND A PAGE (rare error)\r\n"); + mutex_exit(&mtx_flash); + return 1; + } + + p->page[addr&(FLASH_PAGE_SIZE-1)] = (data & 0xff); + p->page[(addr&(FLASH_PAGE_SIZE-1))+1] = (data >> 8); + printf("Flash: modified page %X with data %x %x at [%x-%x] (top page %X)\r\n",addr_alg,(data & 0xff),data>>8,addr&(FLASH_PAGE_SIZE-1),(addr&(FLASH_PAGE_SIZE-1))+1,addr); + mutex_exit(&mtx_flash); + return 0; } -static const uint8_t erased[] = { [0 ... 1023 ] = 0xff }; - int flash_erase_page (uintptr_t addr) { - uint32_t ints = save_and_disable_interrupts(); - flash_range_erase(addr-XIP_BASE, FLASH_SECTOR_SIZE); - restore_interrupts (ints); - return 0; + /* + uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE; + PageFlash_t *p = NULL; + if (ready_pages == TOTAL_FLASH_PAGES) { + DEBUG_INFO("ERROR: ALL FLASH PAGES CACHED\r\n"); + return 1; + } + mutex_enter_blocking(&mtx_flash); + + for (int r = 0; r < TOTAL_FLASH_PAGES; r++) + { + if ((!flash_pages[r].ready && !flash_pages[r].erase) || flash_pages[r].address == addr_alg) //first available + { + p = &flash_pages[r]; + if (!flash_pages[r].ready && !flash_pages[r].erase) + { + ready_pages++; + p->address = addr_alg; + } + p->erase = true; + break; + } + } + + if (!p) + { + DEBUG_INFO("ERROR: FLASH CANNOT FIND A PAGE (rare error)\r\n"); + mutex_exit(&mtx_flash); + return 1; + } + mutex_exit(&mtx_flash); + */ + return 0; } int @@ -48,7 +177,10 @@ flash_check_blank (const uint8_t *p_start, size_t size) int flash_write (uintptr_t dst_addr, const uint8_t *src, size_t len) { + size_t len_alg = (len + (FLASH_SECTOR_SIZE - 1)) & -FLASH_SECTOR_SIZE; + uintptr_t add_alg = dst_addr & -FLASH_SECTOR_SIZE; + printf("WRITE ATTEMPT %X (%d) %X (%d)\r\n",dst_addr,len,add_alg,len_alg); uint32_t ints = save_and_disable_interrupts(); - flash_range_program(dst_addr-XIP_BASE, src, (len%FLASH_PAGE_SIZE == 0 ? len : ((size_t)(len/FLASH_PAGE_SIZE)+1)*FLASH_PAGE_SIZE)); + flash_range_program(add_alg-XIP_BASE, src, len_alg); restore_interrupts (ints); }