From 30a517908c0a28d030db578704940146a12a90e5 Mon Sep 17 00:00:00 2001 From: Pol Henarejos Date: Sat, 5 Feb 2022 01:31:24 +0100 Subject: [PATCH] Rewrite flash from scratch. We are migrating to a file system approach. Signed-off-by: Pol Henarejos --- CMakeLists.txt | 1 + file.c | 154 ++++++++++ file.h | 68 +++++ flash.c | 753 +++++-------------------------------------------- low_flash.c | 98 +++---- 5 files changed, 340 insertions(+), 734 deletions(-) create mode 100644 file.c create mode 100644 file.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b2db732..6847ba5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ target_sources(hsm2040 PUBLIC ${CMAKE_CURRENT_LIST_DIR}/debug.c ${CMAKE_CURRENT_LIST_DIR}/openpgp-do.c ${CMAKE_CURRENT_LIST_DIR}/ac.c + ${CMAKE_CURRENT_LIST_DIR}/file.c ${CMAKE_CURRENT_LIST_DIR}/flash.c ${CMAKE_CURRENT_LIST_DIR}/low_flash.c ${CMAKE_CURRENT_LIST_DIR}/call-rsa.c diff --git a/file.c b/file.c new file mode 100644 index 0000000..ba84cd9 --- /dev/null +++ b/file.c @@ -0,0 +1,154 @@ +#include "file.h" +#include "gnuk.h" +#include + +//puts FCI in the RAPDU +void process_fci(const file_t *pe) { + uint8_t *p = res_APDU; + uint8_t buf[64]; + res_APDU_size = 0; + res_APDU[res_APDU_size++] = 0x6f; + res_APDU[res_APDU_size++] = 0x00; //computed later + + res_APDU[res_APDU_size++] = 0x81; + res_APDU[res_APDU_size++] = 2; + if (pe->data) + memcpy(res_APDU+res_APDU_size, pe->data, 2); + else + memset(res_APDU+res_APDU_size, 0, 2); + res_APDU_size += 2; + + res_APDU[res_APDU_size++] = 0x82; + res_APDU[res_APDU_size++] = 1; + res_APDU[res_APDU_size] = 0; + if (pe->type == FILE_TYPE_INTERNAL_EF) + res_APDU[res_APDU_size++] |= 0x08; + else if (pe->type == FILE_TYPE_WORKING_EF) + res_APDU[res_APDU_size++] |= pe->ef_structure & 0x7; + else if (pe->type == FILE_TYPE_DF) + res_APDU[res_APDU_size++] |= 0x38; + + res_APDU[res_APDU_size++] = 0x83; + res_APDU[res_APDU_size++] = 2; + put_uint16_t(pe->fid, res_APDU+res_APDU_size); + res_APDU_size += 2; + res_APDU[1] = res_APDU_size-2; +} + +const uint8_t t[] = { + 0x01,0xbb, + 0x7F,0x21,0x82,0x01,0xB6,0x7F,0x4E,0x82,0x01,0x6E,0x5F,0x29,0x01,0x00,0x42,0x0E,0x44,0x45,0x43,0x56,0x43,0x41,0x65,0x49,0x44,0x30,0x30,0x31,0x30,0x32,0x7F,0x49,0x82,0x01,0x1D,0x06,0x0A,0x04,0x00,0x7F,0x00,0x07,0x02,0x02,0x02,0x02,0x03,0x81,0x20,0xA9,0xFB,0x57,0xDB,0xA1,0xEE,0xA9,0xBC,0x3E,0x66,0x0A,0x90,0x9D,0x83,0x8D,0x72,0x6E,0x3B,0xF6,0x23,0xD5,0x26,0x20,0x28,0x20,0x13,0x48,0x1D,0x1F,0x6E,0x53,0x77,0x82,0x20,0x7D,0x5A,0x09,0x75,0xFC,0x2C,0x30,0x57,0xEE,0xF6,0x75,0x30,0x41,0x7A,0xFF,0xE7,0xFB,0x80,0x55,0xC1,0x26,0xDC,0x5C,0x6C,0xE9,0x4A,0x4B,0x44,0xF3,0x30,0xB5,0xD9,0x83,0x20,0x26,0xDC,0x5C,0x6C,0xE9,0x4A,0x4B,0x44,0xF3,0x30,0xB5,0xD9,0xBB,0xD7,0x7C,0xBF,0x95,0x84,0x16,0x29,0x5C,0xF7,0xE1,0xCE,0x6B,0xCC,0xDC,0x18,0xFF,0x8C,0x07,0xB6,0x84,0x41,0x04,0x8B,0xD2,0xAE,0xB9,0xCB,0x7E,0x57,0xCB,0x2C,0x4B,0x48,0x2F,0xFC,0x81,0xB7,0xAF,0xB9,0xDE,0x27,0xE1,0xE3,0xBD,0x23,0xC2,0x3A,0x44,0x53,0xBD,0x9A,0xCE,0x32,0x62,0x54,0x7E,0xF8,0x35,0xC3,0xDA,0xC4,0xFD,0x97,0xF8,0x46,0x1A,0x14,0x61,0x1D,0xC9,0xC2,0x77,0x45,0x13,0x2D,0xED,0x8E,0x54,0x5C,0x1D,0x54,0xC7,0x2F,0x04,0x69,0x97,0x85,0x20,0xA9,0xFB,0x57,0xDB,0xA1,0xEE,0xA9,0xBC,0x3E,0x66,0x0A,0x90,0x9D,0x83,0x8D,0x71,0x8C,0x39,0x7A,0xA3,0xB5,0x61,0xA6,0xF7,0x90,0x1E,0x0E,0x82,0x97,0x48,0x56,0xA7,0x86,0x41,0x04,0x33,0x47,0xEC,0xF9,0x6F,0xFB,0x4B,0xD9,0xB8,0x55,0x4E,0xFB,0xCC,0xFC,0x7D,0x0B,0x24,0x2F,0x10,0x71,0xE2,0x9B,0x4C,0x9C,0x62,0x2C,0x79,0xE3,0x39,0xD8,0x40,0xAF,0x67,0xBE,0xB9,0xB9,0x12,0x69,0x22,0x65,0xD9,0xC1,0x6C,0x62,0x57,0x3F,0x45,0x79,0xFF,0xD4,0xDE,0x2D,0xE9,0x2B,0xAB,0x40,0x9D,0xD5,0xC5,0xD4,0x82,0x44,0xA9,0xF7,0x87,0x01,0x01,0x5F,0x20,0x0E,0x44,0x45,0x43,0x56,0x43,0x41,0x65,0x49,0x44,0x30,0x30,0x31,0x30,0x32,0x7F,0x4C,0x12,0x06,0x09,0x04,0x00,0x7F,0x00,0x07,0x03,0x01,0x02,0x02,0x53,0x05,0xFE,0x0F,0x01,0xFF,0xFF,0x5F,0x25,0x06,0x01,0x00,0x01,0x00,0x01,0x08,0x5F,0x24,0x06,0x01,0x03,0x01,0x00,0x01,0x08,0x5F,0x37,0x40,0x50,0x67,0x14,0x5C,0x68,0xCA,0xE9,0x52,0x0F,0x5B,0xB3,0x48,0x17,0xF1,0xCA,0x9C,0x43,0x59,0x3D,0xB5,0x64,0x06,0xC6,0xA3,0xB0,0x06,0xCB,0xF3,0xF3,0x14,0xE7,0x34,0x9A,0xCF,0x0C,0xC6,0xBF,0xEB,0xCB,0xDE,0xFD,0x10,0xB4,0xDC,0xF0,0xF2,0x31,0xDA,0x56,0x97,0x7D,0x88,0xF9,0xF9,0x01,0x82,0xD1,0x99,0x07,0x6A,0x56,0x50,0x64,0x51 +}; +const uint8_t token_info[] = { + 0x0, 0x1f, + 0x30, 0x1d, 0x2, 0x1, 0x2, 0x4, 0x4, 0x6, 0x0, 0x0, 0x0, 0xc, 0x6, 0x4d, 0x61, 0x6e, 0x75, 0x49, 0x44, 0x80, 0x6, 0x50, 0x61, 0x74, 0x61, 0x74, 0x61, 0x3, 0x2, 0x7, 0x80 +}; + +const file_t file_entries[] = { + { .fid = 0x3f00, .parent = 0xff, .name = NULL, .type = FILE_TYPE_DF, .data = NULL, .ef_structure = 0, .acl = {0} }, // MF + { .fid = 0x2f00, .parent = 0, .name = NULL, .type = FILE_TYPE_WORKING_EF, .data = NULL, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, //EF.DIR + { .fid = 0x2f01, .parent = 0, .name = NULL, .type = FILE_TYPE_WORKING_EF, .data = NULL, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, //EF.ATR + { .fid = 0x2f02, .parent = 0, .name = NULL, .type = FILE_TYPE_WORKING_EF,.data = (uint8_t *)t, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, //EF.GDO + { .fid = 0x2f03, .parent = 0, .name = NULL, .type = FILE_TYPE_WORKING_EF,.data = (uint8_t *)token_info, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, //EF.TokenInfo + { .fid = 0x5015, .parent = 0, .name = NULL, .type = FILE_TYPE_DF, .data = NULL, .ef_structure = 0, .acl = {0} }, //DF.PKCS15 + { .fid = 0x5031, .parent = 0, .name = NULL, .type = FILE_TYPE_WORKING_EF, .data = NULL, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, //EF.ODF + { .fid = 0x5032, .parent = 0, .name = NULL, .type = FILE_TYPE_WORKING_EF, .data = NULL, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, //EF.TokenInfo + { .fid = 0x5033, .parent = 0, .name = NULL, .type = FILE_TYPE_WORKING_EF, .data = NULL, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, //EF.UnusedSpace + { .fid = 0x0000, .parent = 0, .name = openpgpcard_aid, .type = FILE_TYPE_WORKING_EF, .data = NULL, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, + { .fid = 0x0000, .parent = 0, .name = sc_hsm_aid, .type = FILE_TYPE_WORKING_EF, .data = NULL, .ef_structure = FILE_EF_TRANSPARENT, .acl = {0} }, + { .fid = 0x0000, .parent = 0xff, .name = NULL, .type = FILE_TYPE_UNKNOWN, .data = NULL, .ef_structure = 0, .acl = {0} } //end +}; + +const file_t *MF = &file_entries[0]; +const file_t *file_last = &file_entries[sizeof(file_entries)/sizeof(file_t)-1]; +const file_t *file_openpgp = &file_entries[sizeof(file_entries)/sizeof(file_t)-3]; +const file_t *file_sc_hsm = &file_entries[sizeof(file_entries)/sizeof(file_t)-2]; + +bool card_terminated = false; + +bool is_parent(const file_t *child, const file_t *parent) { + if (child == parent) + return true; + if (child == MF) + return false; + return is_parent(&file_entries[child->parent], parent); +} + +const file_t *search_by_name(uint8_t *name, uint16_t namelen) { + for (const file_t *p = file_entries; p != file_last; p++) { + if (p->name && *p->name == apdu.cmd_apdu_data_len && memcmp(p->name+1, name, namelen) == 0) { + return p; + } + } + return NULL; +} + +const file_t *search_by_fid(const uint16_t fid, const file_t *parent, const uint8_t sp) { + + for (const file_t *p = file_entries; p != file_last; p++) { + if (p->fid != 0x0000 && p->fid == fid) { + if (!parent || (parent && is_parent(p, parent))) { + if (!sp || sp == SPECIFY_ANY || (((sp & SPECIFY_EF) && (p->type & FILE_TYPE_INTERNAL_EF)) || ((sp & SPECIFY_DF) && p->type == FILE_TYPE_DF))) + return p; + } + } + } + return NULL; +} + +uint8_t make_path_buf(const file_t *pe, uint8_t *buf, uint8_t buflen, const file_t *top) { + if (!buflen) + return 0; + if (pe == top) //MF or relative DF + return 0; + put_uint16_t(pe->fid, buf); + return make_path_buf(&file_entries[pe->parent], buf+2, buflen-2, top)+2; +} + +uint8_t make_path(const file_t *pe, const file_t *top, uint8_t *path) { + uint8_t buf[MAX_DEPTH*2], *p = path; + put_uint16_t(pe->fid, buf); + uint8_t depth = make_path_buf(&file_entries[pe->parent], buf+2, sizeof(buf)-2, top)+2; + for (int d = depth-2; d >= 0; d -= 2) { + memcpy(p, buf+d, 2); + p += 2; + } + return depth; +} + +const file_t *search_by_path(const uint8_t *pe_path, uint8_t pathlen, const file_t *parent) { + uint8_t path[MAX_DEPTH*2]; + if (pathlen > sizeof(path)) { + return NULL; + } + for (const file_t *p = file_entries; p != file_last; p++) { + uint8_t depth = make_path(p, parent, path); + if (pathlen == depth && memcmp(path, pe_path, depth)) + return p; + } + return NULL; +} + +uint8_t file_selection; +const file_t *currentEF = NULL; +const file_t *currentDF = NULL; +const file_t *selected_applet = NULL; +bool isUserAuthenticated = false; + +bool authenticate_action(const file_t *ef, uint8_t op) { + uint8_t acl = ef->acl[op]; + if (acl == 0x0) + return true; + else if (acl == 0xff) + return false; + else if (acl == 0x90 || acl & 0x9F == 0x10) { + // PIN required. + if(isUserAuthenticated) { + return true; + } + else { + return false; + } + } + return false; +} diff --git a/file.h b/file.h new file mode 100644 index 0000000..1880b31 --- /dev/null +++ b/file.h @@ -0,0 +1,68 @@ +#ifndef _FILE_H_ +#define _FILE_H_ + +#include +#include "pico/stdlib.h" + +#define FILE_TYPE_UNKNOWN 0x00 +#define FILE_TYPE_DF 0x04 +#define FILE_TYPE_INTERNAL_EF 0x03 +#define FILE_TYPE_WORKING_EF 0x01 +#define FILE_TYPE_BSO 0x10 + +/* EF structures */ +#define FILE_EF_UNKNOWN 0x00 +#define FILE_EF_TRANSPARENT 0x01 +#define FILE_EF_LINEAR_FIXED 0x02 +#define FILE_EF_LINEAR_FIXED_TLV 0x03 +#define FILE_EF_LINEAR_VARIABLE 0x04 +#define FILE_EF_LINEAR_VARIABLE_TLV 0x05 +#define FILE_EF_CYCLIC 0x06 +#define FILE_EF_CYCLIC_TLV 0x07 + +#define ACL_OP_DELETE_SELF 0x00 +#define ACL_OP_CREATE_DF 0x01 +#define ACL_OP_CREATE_EF 0x02 +#define ACL_OP_DELETE_CHILD 0x03 +#define ACL_OP_WRITE 0x04 +#define ACL_OP_UPDATE_ERASE 0x05 +#define ACL_OP_READ_SEARCH 0x06 + +#define SPECIFY_EF 0x1 +#define SPECIFY_DF 0x2 +#define SPECIFY_ANY 0x3 + +#define MAX_DEPTH 4 + + +typedef struct file +{ + const uint16_t fid; + const uint8_t parent; //entry number in the whole table!! + const uint8_t *name; + const uint8_t type; + uint8_t *data; //should include 2 bytes len at begining + const uint8_t ef_structure; + const uint8_t acl[7]; +} file_t; + +extern const file_t *currentEF; +extern const file_t *currentDF; +extern const file_t *selected_applet; + +extern const file_t *MF; +extern const file_t *file_last; +extern const file_t *file_openpgp; +extern const file_t *file_sc_hsm; +extern bool card_terminated; + +extern const file_t *search_by_fid(const uint16_t fid, const file_t *parent, const uint8_t sp); +extern const file_t *search_by_name(uint8_t *name, uint16_t namelen); +extern const file_t *search_by_path(const uint8_t *pe_path, uint8_t pathlen, const file_t *parent); +extern bool authenticate_action(const file_t *ef, uint8_t op); +extern void process_fci(const file_t *pe); + +extern const file_t file_entries[]; + +#endif + diff --git a/flash.c b/flash.c index 0c1ab13..9c1bbf8 100644 --- a/flash.c +++ b/flash.c @@ -42,169 +42,24 @@ #include "hardware/flash.h" #include "hsm2040.h" #include "tusb.h" - -extern void low_flash_available(); - -/* - * Flash memory map - * - * _text - * .text - * .ctors - * .dtors - * _etext - * .data - * _bss_start - * .bss - * _end - * - * ch_certificate_startp - * <2048 bytes> - * _keystore_pool - * Three flash pages for keystore - * a page contains a key data of: - * For RSA-2048: 512-byte (p, q and N) - * For RSA-4096: 1024-byte (p, q and N) - * For ECDSA/ECDH and EdDSA, there are padding after public key - * _data_pool - * - */ - -#define FLASH_DATA_POOL_HEADER_SIZE 2 -#define FLASH_DATA_POOL_SIZE (2048*1024) - -static uint16_t flash_page_size; -static const uint8_t *data_pool; -static uint8_t *last_p; - -/* The first halfword is generation for the data page (little endian) */ -const uint8_t flash_data[4] __attribute__ ((section (".gnuk_data"))) = { - 0x00, 0x00, 0xff, 0xff -}; +#include "file.h" #define FLASH_TARGET_OFFSET (PICO_FLASH_SIZE_BYTES >> 1) // DATA starts at the mid of flash +#define FLASH_DATA_HEADER_SIZE (sizeof(uintptr_t)+sizeof(uint32_t)) +//To avoid possible future allocations, data region starts at the begining of flash and goes upwards to the center region -const uint8_t *flash_addr_key_storage_start = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET); -const uint8_t *flash_addr_data_storage_start = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET + 2048 * 1024); // 2 MB -const uint8_t *ch_certificate_start = (const uint8_t *) (XIP_BASE + FLASH_TARGET_OFFSET - FLASH_SECTOR_SIZE); -#define FLASH_ADDR_KEY_STORAGE_START flash_addr_key_storage_start -#define FLASH_ADDR_DATA_STORAGE_START flash_addr_data_storage_start +const uintptr_t start_data_pool = (XIP_BASE + FLASH_TARGET_OFFSET); +const uintptr_t end_data_pool = (XIP_BASE + PICO_FLASH_SIZE_BYTES)-FLASH_DATA_HEADER_SIZE; //This is a fixed value. DO NOT CHANGE +#define FLASH_ADDR_DATA_STORAGE_START start_data_pool extern int flash_erase_page (uintptr_t addr); extern int flash_program_halfword (uintptr_t addr, uint16_t data); +int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len) ; extern int flash_check_blank (const uint8_t *p_start, size_t size); extern int flash_write (uintptr_t dst_addr, const uint8_t *src, size_t len); -static int key_available_at (const uint8_t *k, int key_size) -{ - int i; - - for (i = 0; i < key_size; i++) - if (k[i]) - break; - if (i == key_size) /* It's ZERO. Released key. */ - return 0; - - for (i = 0; i < key_size; i++) - if (k[i] != 0xff) - break; - if (i == key_size) /* It's FULL. Unused key. */ - return 0; - - return 1; -} - -void -flash_do_storage_init (const uint8_t **p_do_start, const uint8_t **p_do_end) -{ - uint16_t gen0, gen1; - uint16_t *gen0_p = (uint16_t *)FLASH_ADDR_DATA_STORAGE_START; - uint16_t *gen1_p; - - flash_page_size = FLASH_SECTOR_SIZE * 8; // 32 KB - - gen1_p = (uint16_t *)(FLASH_ADDR_DATA_STORAGE_START + flash_page_size); - data_pool = FLASH_ADDR_DATA_STORAGE_START; - - /* Check data pool generation and choose the page */ - gen0 = *gen0_p; - gen1 = *gen1_p; - - if (gen0 == 0xffff && gen1 == 0xffff) - { - gen0 = 0x0000; - *gen0_p = gen0; - } - - if (gen0 == 0xffff) - /* Use another page if a page is erased. */ - data_pool = FLASH_ADDR_DATA_STORAGE_START + flash_page_size; - else if (gen1 == 0xffff) - /* Or use different page if another page is erased. */ - data_pool = FLASH_ADDR_DATA_STORAGE_START; - else if ((gen0 == 0xfffe && gen1 == 0) || gen1 > gen0) - /* When both pages have valid header, use newer page. */ - data_pool = FLASH_ADDR_DATA_STORAGE_START + flash_page_size; - - *p_do_start = data_pool + FLASH_DATA_POOL_HEADER_SIZE; - *p_do_end = data_pool + flash_page_size; -} - -static uint8_t *flash_key_getpage (enum kind_of_key kk); - -void -flash_terminate (void) -{ - int i; - - for (i = 0; i < 3; i++) - flash_erase_page ((uintptr_t)flash_key_getpage (i)); - flash_erase_page ((uintptr_t)FLASH_ADDR_DATA_STORAGE_START); - flash_erase_page ((uintptr_t)(FLASH_ADDR_DATA_STORAGE_START + flash_page_size)); - data_pool = FLASH_ADDR_DATA_STORAGE_START; - last_p = (uint8_t *)FLASH_ADDR_DATA_STORAGE_START + FLASH_DATA_POOL_HEADER_SIZE; -#if defined(CERTDO_SUPPORT) - flash_erase_page ((uintptr_t)ch_certificate_start); - if (FLASH_CH_CERTIFICATE_SIZE > flash_page_size) - flash_erase_page ((uintptr_t)(ch_certificate_start + flash_page_size)); -#endif -} - -void -flash_activate (void) -{ - flash_program_halfword ((uintptr_t)FLASH_ADDR_DATA_STORAGE_START, 0); - low_flash_available(); -} - - -void -flash_key_storage_init (void) -{ - const uint8_t *p; - int i; - - /* For each key, find its address. */ - p = FLASH_ADDR_KEY_STORAGE_START; - for (i = 0; i < 3; i++) - { - const uint8_t *k; - int key_size = gpg_get_algo_attr_key_size (i, GPG_KEY_STORAGE); - - kd[i].pubkey = NULL; - for (k = p; k < p + flash_page_size; k += key_size) - if (key_available_at (k, key_size)) - { - int prv_len = gpg_get_algo_attr_key_size (i, GPG_KEY_PRIVATE); - - kd[i].pubkey = k + prv_len; - break; - } - - p += flash_page_size; - } -} +void low_flash_available(); /* * Flash data pool managenent @@ -229,539 +84,79 @@ flash_key_storage_init (void) * PAD: optional byte for 16-bit alignment */ -void -flash_set_data_pool_last (const uint8_t *p) -{ - last_p = (uint8_t *)p; -} - -/* - * We use two pages - */ -static int -flash_copying_gc (void) -{ - uint8_t *src, *dst; - uint16_t generation; - - if (data_pool == FLASH_ADDR_DATA_STORAGE_START) - { - src = (uint8_t *)FLASH_ADDR_DATA_STORAGE_START; - dst = (uint8_t *)FLASH_ADDR_DATA_STORAGE_START + flash_page_size; +uintptr_t allocate_free_addr(uint16_t size) { + if (size > FLASH_SECTOR_SIZE) + return 0x0; //ERROR + for (uintptr_t base = end_data_pool; base >= start_data_pool; base = *(uintptr_t *)base) { + uintptr_t addr_alg = base & -FLASH_SECTOR_SIZE; //start address of sector + uintptr_t potential_addr = base-size-sizeof(uint16_t)-sizeof(uintptr_t); + if (*(uintptr_t *)base == 0x0) { //we are at the end + //now we check if we fit in the current sector + if (addr_alg <= potential_addr) //it fits in the current sector + { + *(uintptr_t *)potential_addr = 0x0; + *(uintptr_t *)base = potential_addr; + return potential_addr; + } + else if (addr_alg-FLASH_SECTOR_SIZE >= start_data_pool) { //check whether it fits in the next sector, so we take addr_aligned as the base + potential_addr = addr_alg-size-sizeof(uint16_t)-sizeof(uintptr_t); + *(uintptr_t *)potential_addr = 0x0; + *(uintptr_t *)base = potential_addr; + return potential_addr; + } + return 0x0; + } + //we check if |base-(next_addr+size_next_addr)| >= |base-potential_addr| + else if (base-(*(uintptr_t *)base+*(uint16_t *)(*(uintptr_t *)base+sizeof(uintptr_t))) >= base-potential_addr && addr_alg <= potential_addr) { + *(uintptr_t *)potential_addr = *(uintptr_t *)base; + *(uintptr_t *)base = potential_addr; + return potential_addr; + } } - else - { - src = (uint8_t *)FLASH_ADDR_DATA_STORAGE_START + flash_page_size; - dst = (uint8_t *)FLASH_ADDR_DATA_STORAGE_START; - } - - generation = *(uint16_t *)src; - data_pool = dst; - gpg_data_copy (data_pool + FLASH_DATA_POOL_HEADER_SIZE); - if (generation == 0xfffe) - generation = 0; - else - generation++; - flash_program_halfword ((uintptr_t)dst, generation); - flash_erase_page ((uintptr_t)src); - low_flash_available(); - return 0; + return 0x0; //probably never reached } -static int -is_data_pool_full (size_t size) -{ - return last_p + size > data_pool + flash_page_size; -} - -static uint8_t * -flash_data_pool_allocate (size_t size) -{ - uint8_t *p; - - size = (size + 1) & ~1; /* allocation unit is 1-halfword (2-byte) */ - - if (is_data_pool_full (size)) { - if (flash_copying_gc () < 0 || /*still*/ is_data_pool_full (size)) { - TU_LOG1 ("!!!! FATAL: %d\r\n",FATAL_FLASH); - return NULL; - } - } - - p = last_p; - last_p += size; - return p; -} - -void -flash_do_write_internal (const uint8_t *p, int nr, const uint8_t *data, int len) -{ - uint16_t hw; - uintptr_t addr; - int i; - - addr = (uintptr_t)p; - hw = nr | (len << 8); - if (flash_program_halfword (addr, hw) != 0) - flash_warning ("DO WRITE ERROR"); - addr += 2; - - for (i = 0; i < len/2; i++) - { - hw = data[i*2] | (data[i*2+1]<<8); - if (flash_program_halfword (addr, hw) != 0) - flash_warning ("DO WRITE ERROR"); - addr += 2; - } - - if ((len & 1)) - { - hw = data[i*2] | 0xff00; - if (flash_program_halfword (addr, hw) != 0) - flash_warning ("DO WRITE ERROR"); - } - - low_flash_available(); -} - -const uint8_t * -flash_do_write (uint8_t nr, const uint8_t *data, int len) -{ - const uint8_t *p; - - DEBUG_INFO ("flash DO\r\n"); - - p = flash_data_pool_allocate (2 + len); - if (p == NULL) - { - DEBUG_INFO ("flash data pool allocation failure.\r\n"); - return NULL; - } - - flash_do_write_internal (p, nr, data, len); - DEBUG_INFO ("flash DO...done\r\n"); - return p + 1; -} - -void -flash_warning (const char *msg) -{ - (void)msg; - DEBUG_INFO ("FLASH: "); - DEBUG_INFO (msg); - DEBUG_INFO ("\r\n"); -} - -void -flash_do_release (const uint8_t *do_data) -{ - uintptr_t addr = (uintptr_t)do_data - 1; - uintptr_t addr_tag = addr; - int i; - int len = do_data[0]; - - /* Don't filling zero for data in code (such as ds_count_initial_value) */ - if (do_data < FLASH_ADDR_DATA_STORAGE_START - || do_data > FLASH_ADDR_DATA_STORAGE_START + FLASH_DATA_POOL_SIZE) - return; - - addr += 2; - - /* Fill zero for content and pad */ - for (i = 0; i < len/2; i ++) - { - if (flash_program_halfword (addr, 0) != 0) - flash_warning ("fill-zero failure"); - addr += 2; - } - - if ((len & 1)) - { - if (flash_program_halfword (addr, 0) != 0) - flash_warning ("fill-zero pad failure"); - } - - /* Fill 0x0000 for "tag_number and length" word */ - if (flash_program_halfword (addr_tag, 0) != 0) - flash_warning ("fill-zero tag_nr failure"); - - //CAUTION: flash_do_release is followed by a flash_write. Thus, we can avoid a single write - //low_flash_available(); -} - - -static uint8_t * -flash_key_getpage (enum kind_of_key kk) -{ - /* There is a page for each KK. */ - return (uint8_t *)FLASH_ADDR_KEY_STORAGE_START + (flash_page_size * kk); -} - -uint8_t * -flash_key_alloc (enum kind_of_key kk) -{ - uint8_t *k, *k0 = flash_key_getpage (kk); - int i; - int key_size = gpg_get_algo_attr_key_size (kk, GPG_KEY_STORAGE); - - /* Seek free space in the page. */ - for (k = k0; k < k0 + flash_page_size; k += key_size) - { - const uint32_t *p = (const uint32_t *)k; - - for (i = 0; i < key_size/4; i++) - if (p[i] != 0xffffffff) - break; - - if (i == key_size/4) /* Yes, it's empty. */ - return k; - } - - /* Should not happen as we have enough free space all time, but just - in case. */ - return NULL; -} - -int -flash_key_write (uint8_t *key_addr, - const uint8_t *key_data, int key_data_len, - const uint8_t *pubkey, int pubkey_len) -{ - uint16_t hw; - uintptr_t addr; - int i; - - addr = (uintptr_t)key_addr; - for (i = 0; i < key_data_len/2; i ++) - { - hw = key_data[i*2] | (key_data[i*2+1]<<8); - if (flash_program_halfword (addr, hw) != 0) - return -1; - addr += 2; - } - - for (i = 0; i < pubkey_len/2; i ++) - { - hw = pubkey[i*2] | (pubkey[i*2+1]<<8); - if (flash_program_halfword (addr, hw) != 0) - return -1; - addr += 2; - } - - low_flash_available(); - return 0; -} - -static int -flash_check_all_other_keys_released (const uint8_t *key_addr, int key_size) -{ - uintptr_t start = (uintptr_t)key_addr & ~(flash_page_size - 1); - const uint32_t *p = (const uint32_t *)start; - - while (p < (const uint32_t *)(start + flash_page_size)) - if (p == (const uint32_t *)key_addr) - p += key_size/4; - else - if (*p) - return 0; - else - p++; - - return 1; -} - -static void -flash_key_fill_zero_as_released (uint8_t *key_addr, int key_size) -{ - int i; - uintptr_t addr = (uintptr_t)key_addr; - - for (i = 0; i < key_size/2; i++) - flash_program_halfword (addr + i*2, 0); - - low_flash_available(); -} - -void -flash_key_release (uint8_t *key_addr, int key_size) -{ - if (flash_check_all_other_keys_released (key_addr, key_size)) - flash_erase_page (((uintptr_t)key_addr & ~(flash_page_size - 1))); - else - flash_key_fill_zero_as_released (key_addr, key_size); -} - -void -flash_key_release_page (enum kind_of_key kk) -{ - flash_erase_page ((uintptr_t)flash_key_getpage (kk)); -} - - -void -flash_clear_halfword (uintptr_t addr) -{ - flash_program_halfword (addr, 0); - low_flash_available(); -} - - -void -flash_put_data_internal (const uint8_t *p, uint16_t hw) -{ - flash_program_halfword ((uintptr_t)p, hw); - low_flash_available(); -} - -void -flash_put_data (uint16_t hw) -{ - uint8_t *p; - - p = flash_data_pool_allocate (2); - if (p == NULL) - { - DEBUG_INFO ("data allocation failure.\r\n"); - return; - } - - flash_program_halfword ((uintptr_t)p, hw); - low_flash_available(); -} - - -void -flash_bool_clear (const uint8_t **addr_p) -{ - const uint8_t *p; - - if ((p = *addr_p) == NULL) - return; - - flash_program_halfword ((uintptr_t)p, 0); - *addr_p = NULL; - low_flash_available(); -} - -void -flash_bool_write_internal (const uint8_t *p, int nr) -{ - flash_program_halfword ((uintptr_t)p, nr); - low_flash_available(); -} - -const uint8_t * -flash_bool_write (uint8_t nr) -{ - uint8_t *p; - uint16_t hw = nr; - - p = flash_data_pool_allocate (2); - if (p == NULL) - { - DEBUG_INFO ("bool allocation failure.\r\n"); - return NULL; - } - - flash_program_halfword ((uintptr_t)p, hw); - low_flash_available(); - return p; -} - - -void -flash_enum_clear (const uint8_t **addr_p) -{ - flash_bool_clear (addr_p); -} - -void -flash_enum_write_internal (const uint8_t *p, int nr, uint8_t v) -{ - uint16_t hw = nr | (v << 8); - - flash_program_halfword ((uintptr_t)p, hw); - low_flash_available(); -} - -const uint8_t * -flash_enum_write (uint8_t nr, uint8_t v) -{ - uint8_t *p; - uint16_t hw = nr | (v << 8); - - p = flash_data_pool_allocate (2); - if (p == NULL) - { - DEBUG_INFO ("enum allocation failure.\r\n"); - return NULL; - } - - flash_program_halfword ((uintptr_t)p, hw); - low_flash_available(); - return p; -} - - -int -flash_cnt123_get_value (const uint8_t *p) -{ - if (p == NULL) +int flash_clear_file(file_t *file) { + uintptr_t prev_addr = (uintptr_t)(file->data+*(uint16_t *)file->data); + uintptr_t base_addr = (uintptr_t)file->data-sizeof(uintptr_t); + uintptr_t next_addr = *(uintptr_t *)base_addr; + *(uintptr_t *)prev_addr = next_addr; + *(uint16_t *)file->data = 0; return 0; - else - { - uint8_t v = *p; - - /* - * After erase, a halfword in flash memory becomes 0xffff. - * The halfword can be programmed to any value. - * Then, the halfword can be programmed to zero. - * - * Thus, we can represent value 1, 2, and 3. - */ - if (v == 0xff) - return 1; - else if (v == 0x00) - return 3; - else - return 2; - } } -void -flash_cnt123_write_internal (const uint8_t *p, int which, int v) +int flash_write_data_to_file(file_t *file, const uint8_t *data, uint16_t len) { + if (len > FLASH_SECTOR_SIZE) + return 1; + if (file->data) { //already in flash + uint16_t size_file_flash = *(uint16_t *)file->data; + if (len <= size_file_flash) { //it fits, no need to move it + flash_program_halfword((uintptr_t)file->data, len); + flash_program_block((uintptr_t)file->data+sizeof(uint16_t), data, len); + low_flash_available(); + return 0; + } + else { //we clear the old file + flash_clear_file(file); + } + } + uintptr_t new_addr = allocate_free_addr(len); + if (new_addr == 0x0) + return 2; + file->data = (uint8_t *)new_addr+sizeof(uintptr_t); + + flash_program_halfword((uintptr_t)file->data, len); + flash_program_block((uintptr_t)file->data+sizeof(uint16_t), data, len); + low_flash_available(); + return 0; +} + +void flash_warning (const char *msg) { - uint16_t hw; - - hw = NR_COUNTER_123 | (which << 8); - flash_program_halfword ((uintptr_t)p, hw); - - if (v == 1) - return; - else if (v == 2) - flash_program_halfword ((uintptr_t)p+2, 0xc3c3); - else /* v == 3 */ - flash_program_halfword ((uintptr_t)p+2, 0); - low_flash_available(); -} - -void -flash_cnt123_increment (uint8_t which, const uint8_t **addr_p) -{ - const uint8_t *p; - uint16_t hw; - - if ((p = *addr_p) == NULL) - { - p = flash_data_pool_allocate (4); - if (p == NULL) - { - DEBUG_INFO ("cnt123 allocation failure.\r\n"); - return; - } - hw = NR_COUNTER_123 | (which << 8); - flash_program_halfword ((uintptr_t)p, hw); - *addr_p = p + 2; - } - else - { - uint8_t v = *p; - - if (v == 0) - return; - - if (v == 0xff) - hw = 0xc3c3; - else - hw = 0; - - flash_program_halfword ((uintptr_t)p, hw); - } - low_flash_available(); -} - -void -flash_cnt123_clear (const uint8_t **addr_p) -{ - const uint8_t *p; - - if ((p = *addr_p) == NULL) - return; - - flash_program_halfword ((uintptr_t)p, 0); - p -= 2; - flash_program_halfword ((uintptr_t)p, 0); - *addr_p = NULL; - low_flash_available(); + (void)msg; + DEBUG_INFO ("FLASH: "); + DEBUG_INFO (msg); + DEBUG_INFO ("\r\n"); } -#if defined(CERTDO_SUPPORT) -int -flash_erase_binary (uint8_t file_id) -{ - if (file_id == FILEID_CH_CERTIFICATE) - { - const uint8_t *p = ch_certificate_start; - if (flash_check_blank (p, FLASH_CH_CERTIFICATE_SIZE) == 0) - { - flash_erase_page ((uintptr_t)p); - if (FLASH_CH_CERTIFICATE_SIZE > flash_page_size) - flash_erase_page ((uintptr_t)p + flash_page_size); - } - low_flash_available(); - - return 0; - } - - return -1; -} -#endif - - -int -flash_write_binary (uint8_t file_id, const uint8_t *data, - uint16_t len, uint16_t offset) -{ - uint16_t maxsize; - const uint8_t *p; - - if (file_id == FILEID_SERIAL_NO) - { - maxsize = 6; - p = &openpgpcard_aid[8]; - } -#if defined(CERTDO_SUPPORT) - else if (file_id == FILEID_CH_CERTIFICATE) - { - maxsize = FLASH_CH_CERTIFICATE_SIZE; - p = ch_certificate_start; - } -#endif - else - return -1; - - if (offset + len > maxsize || (offset&1) || (len&1)) - return -1; - else - { - uint16_t hw; - uintptr_t addr; - int i; - - if (flash_check_blank (p + offset, len) == 0) - return -1; - - addr = (uintptr_t)p + offset; - for (i = 0; i < len/2; i++) - { - hw = data[i*2] | (data[i*2+1]<<8); - if (flash_program_halfword (addr, hw) != 0) - flash_warning ("DO WRITE ERROR"); - addr += 2; - } - - low_flash_available(); - return 0; - } -} diff --git a/low_flash.c b/low_flash.c index fdbb39e..fa26a2a 100644 --- a/low_flash.c +++ b/low_flash.c @@ -12,14 +12,15 @@ #define TOTAL_FLASH_PAGES 4 -typedef struct PageFlash { +typedef struct page_flash { uint8_t page[FLASH_SECTOR_SIZE]; uintptr_t address; bool ready; bool erase; -} PageFlash_t; + size_t page_size; //this param is for easy erase. It allows to erase with a single call. IT DOES NOT APPLY TO WRITE +} page_flash_t; -static PageFlash_t flash_pages[TOTAL_FLASH_PAGES]; +static page_flash_t flash_pages[TOTAL_FLASH_PAGES]; static mutex_t mtx_flash; @@ -57,7 +58,7 @@ void do_flash() { while (multicore_lockout_start_timeout_us(1000) == false); printf("WRITTING\r\n"); - flash_range_erase(flash_pages[r].address-XIP_BASE, FLASH_SECTOR_SIZE); + flash_range_erase(flash_pages[r].address-XIP_BASE, flash_pages[r].page_size ? ((int)(flash_pages[r].page_size/FLASH_SECTOR_SIZE))*FLASH_SECTOR_SIZE : FLASH_SECTOR_SIZE); while (multicore_lockout_end_timeout_us(1000) == false); flash_pages[r].erase = false; ready_pages--; @@ -76,7 +77,7 @@ void do_flash() void low_flash_init() { mutex_init(&mtx_flash); - memset(flash_pages, 0, sizeof(PageFlash_t)*TOTAL_FLASH_PAGES); + memset(flash_pages, 0, sizeof(page_flash_t)*TOTAL_FLASH_PAGES); } void low_flash_available() @@ -86,18 +87,9 @@ void low_flash_available() mutex_exit(&mtx_flash); } -int -flash_program_halfword (uintptr_t addr, uint16_t data) -{ - off_t offset; +page_flash_t *find_free_page(uintptr_t addr) { 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); - + page_flash_t *p = NULL; 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 @@ -110,59 +102,66 @@ flash_program_halfword (uintptr_t addr, uint16_t data) p->address = addr_alg; p->ready = true; } - break; + return p; } } + return NULL; +} - if (!p) +int flash_program_block(uintptr_t addr, const uint8_t *data, size_t len) { + uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE; + page_flash_t *p = NULL; + + mutex_enter_blocking(&mtx_flash); + if (ready_pages == TOTAL_FLASH_PAGES) { + mutex_exit(&mtx_flash); + DEBUG_INFO("ERROR: ALL FLASH PAGES CACHED\r\n"); + return 1; + } + if (!(p = find_free_page(addr))) { DEBUG_INFO("ERROR: FLASH CANNOT FIND A PAGE (rare error)\r\n"); mutex_exit(&mtx_flash); return 1; } - - p->page[addr&(FLASH_SECTOR_SIZE-1)] = (data & 0xff); - p->page[(addr&(FLASH_SECTOR_SIZE-1))+1] = (data >> 8); + memcpy(&p->page[addr&(FLASH_SECTOR_SIZE-1)], data, len); //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_SECTOR_SIZE-1),(addr&(FLASH_SECTOR_SIZE-1))+1,addr); mutex_exit(&mtx_flash); return 0; } -int -flash_erase_page (uintptr_t addr) +int flash_program_halfword (uintptr_t addr, uint16_t data) +{ + return flash_program_block(addr, (const uint8_t *)&data, sizeof(uint16_t)); +} + +int flash_program_word (uintptr_t addr, uint32_t data) +{ + return flash_program_block(addr, (const uint8_t *)&data, sizeof(uint32_t)); +} + +int flash_erase_page (uintptr_t addr, size_t page_size) { - /* uintptr_t addr_alg = addr & -FLASH_SECTOR_SIZE; - PageFlash_t *p = NULL; + page_flash_t *p = NULL; + + mutex_enter_blocking(&mtx_flash); if (ready_pages == TOTAL_FLASH_PAGES) { + mutex_exit(&mtx_flash); 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) + if (!(p = find_free_page(addr))) { DEBUG_INFO("ERROR: FLASH CANNOT FIND A PAGE (rare error)\r\n"); mutex_exit(&mtx_flash); return 1; } + p->erase = true; + p->ready = false; + p->page_size = page_size; mutex_exit(&mtx_flash); - */ + return 0; } @@ -177,14 +176,3 @@ flash_check_blank (const uint8_t *p_start, size_t size) return 1; } - -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(add_alg-XIP_BASE, src, len_alg); - restore_interrupts (ints); -}