From 3c3795f8cb0a5ccd33d195aa5f7469fa36474aa1 Mon Sep 17 00:00:00 2001 From: sys64738 Date: Tue, 24 Aug 2021 00:16:40 +0200 Subject: [PATCH] initial persistent storage code --- CMakeLists.txt | 2 + bsp/rp2040/bsp-info.h | 5 ++ bsp/rp2040/bsp-storage.h | 59 ++++++++++++++ src/main.c | 5 +- src/mode.h | 7 +- src/modeset.c | 10 ++- src/storage.c | 172 +++++++++++++++++++++++++++++++++++++++ src/storage.h | 46 +++++++++++ src/storage_internal.h | 69 ++++++++++++++++ src/storage_save.c | 55 +++++++++++++ 10 files changed, 425 insertions(+), 5 deletions(-) create mode 100644 bsp/rp2040/bsp-storage.h create mode 100644 src/storage.c create mode 100644 src/storage.h create mode 100644 src/storage_internal.h create mode 100644 src/storage_save.c diff --git a/CMakeLists.txt b/CMakeLists.txt index aa80748..6517772 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,8 @@ target_sources(${PROJECT} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/main.c ${CMAKE_CURRENT_SOURCE_DIR}/src/alloc.c ${CMAKE_CURRENT_SOURCE_DIR}/src/modeset.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/storage.c + ${CMAKE_CURRENT_SOURCE_DIR}/src/storage_save.c ${CMAKE_CURRENT_SOURCE_DIR}/src/thread.c ${CMAKE_CURRENT_SOURCE_DIR}/src/tusb_plt.S ${CMAKE_CURRENT_SOURCE_DIR}/src/usb_descriptors.c diff --git a/bsp/rp2040/bsp-info.h b/bsp/rp2040/bsp-info.h index d2fd1a8..2120d37 100644 --- a/bsp/rp2040/bsp-info.h +++ b/bsp/rp2040/bsp-info.h @@ -22,4 +22,9 @@ #endif #define CFG_TUD_VENDOR 1 +/* don't access storage for RAM-only builds */ +#ifndef PICO_NO_FLASH +#define BOARD_HAS_STORAGE +#endif + #endif diff --git a/bsp/rp2040/bsp-storage.h b/bsp/rp2040/bsp-storage.h new file mode 100644 index 0000000..983544e --- /dev/null +++ b/bsp/rp2040/bsp-storage.h @@ -0,0 +1,59 @@ +// vim: set et: + +#ifndef BSP_STORAGE_H_ +#define BSP_STORAGE_H_ + +#include +#include +#include + +#ifndef PICO_NO_FLASH + +#include +#include + +#ifndef PICO_FLASH_SIZE_BYTES +#warning "PICO_FLASH_SIZE_BYTES not defined, defaulting to 2 megabytes. This WILL break if your flash is smaller!" +#define PICO_FLASH_SIZE_BYTES (2*1024*1024) +#endif + +static inline size_t storage_get_program_size(void) { + extern uint8_t __flash_binary_start, __flash_binary_end; + + return (size_t)&__flash_binary_end - (size_t)&__flash_binary_start; +} + +static inline size_t storage_get_program_offset(void) { + extern uint8_t __flash_binary_start; + + return (size_t)&__flash_binary_start - XIP_BASE; +} + +#define STORAGE_SIZE PICO_FLASH_SIZE_BYTES +#define STORAGE_ERASEWRITE_ALIGN FLASH_SECTOR_SIZE +// reads don't require any alignment + +// --- + +static inline void storage_read(void* dest, size_t offset, size_t size) { + // TODO: XIP/SSI DMA? + // * XIP DMA: used for loading stuff in the background while running code + // * SSI DMA: blocking & fast, code needs to run from RAM. a bit unwieldy + memcpy(dest, (uint8_t*)(XIP_BASE+offset), size); +} +static inline bool storage_erasewrite(size_t offset, const void* src, size_t size) { + // bad alignment => give an error in advance + if (offset & (FLASH_SECTOR_SIZE - 1)) return false; + if (size & (FLASH_SECTOR_SIZE - 1)) return false; + + flash_range_erase(offset, size); + flash_range_program(offset, src, size); + + // maybe not a bad idea either + return !memcmp(src, (uint8_t*)(XIP_BASE+offset), size); +} + +#endif + +#endif + diff --git a/src/main.c b/src/main.c index 54902ad..35a8b2f 100644 --- a/src/main.c +++ b/src/main.c @@ -7,6 +7,7 @@ #include "tusb.h" #include "mode.h" +#include "storage.h" #include "thread.h" #include "usbstdio.h" #include "vnd_cfg.h" @@ -31,7 +32,9 @@ int main() { vndcfg_thread = co_derive(vndcfg_stack, sizeof vndcfg_stack, vndcfg_thread_fn); thread_enter(vndcfg_thread); - modes_init(); + storage_init(); // sets mode_next_id + + modes_init(mode_next_id); if (mode_current) mode_current->enter(); tusb_init(); diff --git a/src/mode.h b/src/mode.h index 6256eba..7d3c8de 100644 --- a/src/mode.h +++ b/src/mode.h @@ -9,6 +9,8 @@ #include "tusb_config.h" #include +#include "storage.h" + // clang-format off struct mode { @@ -19,6 +21,8 @@ struct mode { const uint8_t* usb_desc; const char** string_desc; + struct mode_storage storage; + void (*enter)(void); // claim required hardware. no tusb calls here please void (*leave)(void); // release current in-use hardware. no tusb calls here please void (*task )(void); @@ -46,8 +50,9 @@ struct mode { }; // call this BEFORE tusb_init! -void modes_init(void); +void modes_init(int init_mode); +// IMMEDIATELY switches mode. use "mode_next_id' to do a graceful switch void modes_switch(uint8_t newmode); extern int mode_current_id; diff --git a/src/modeset.c b/src/modeset.c index 53ed69c..c7abdac 100644 --- a/src/modeset.c +++ b/src/modeset.c @@ -37,9 +37,13 @@ enum tusbgot_index { #define ORDEF(a,b) ((a != NULL) ? a : b) -void modes_init(void) { - // switch to the default mode, but without doing a USB reboot thing - mode_current_id = &mode_default - mode_list; +void modes_init(int newid) { + if (newid < 0 || newid >= 16 || mode_list[newid] == NULL) { + // switch to the default mode, but without doing a USB reboot thing + mode_current_id = &mode_default - mode_list; + } else { + mode_current_id = newid; + } mode_next_id = -1; //if (!mode_default) return; diff --git a/src/storage.c b/src/storage.c new file mode 100644 index 0000000..993e85b --- /dev/null +++ b/src/storage.c @@ -0,0 +1,172 @@ +// vim: set et: + +#include "bsp-info.h" +#include "bsp-storage.h" +#include "mode.h" +#include "storage.h" + +#ifndef BOARD_HAS_STORAGE +void storage_init(void) { } +void storage_flush_data(void) { } +struct mode_info storage_mode_get_size(int _) { + (void)_; return (struct mode_info){ .size = 0, .version = 0 }; +} +void storage_mode_read(int _, void* __) { (void)_; (void)__; } +#else + +#include "storage_internal.h" + +bool header_valid = false; +struct storage_header header_tmp; +uint8_t data_tmp[1024 - sizeof(struct storage_header)]; +uint16_t mode_bad = 0; + +static void storage_init_defaults(void) { + memcpy(header_tmp.magic, STORAGE_MAGIC, STORAGE_MAGIC_LEN); + header_tmp.fwversion = STORAGE_VER; + header_tmp.curmode = mode_current_id; + header_tmp.nmodes = 0; + memset(header_tmp.reserved, 0xff, sizeof(header_tmp.reserved)); + memset(header_tmp.mode_data_table, 0xff, sizeof(header_tmp.mode_data_table)); + header_tmp.table_djb2 = str_hash_djb2(header_tmp.mode_data_table, + sizeof(struct mode_data)*MAX_MDT_ELEMENTS); + + header_valid = true; +} + +void storage_init(void) { + mode_next_id = -1; // by default, boot to default mode + + mode_bad = 0; + storage_read(&header_tmp, STORAGE_SIZE - sizeof(struct storage_header), + sizeof(struct storage_header)); + + bool bad = false; + if (memcmp(header_tmp.magic, STORAGE_MAGIC, STORAGE_MAGIC_LEN)) { + storage_init_defaults(); + storage_flush_data(); + return; + } + + if (header_tmp.fwversion != STORAGE_VER) { + // TODO: migrate... if there were any older versions + header_valid = false; + return; + } + + if (header_tmp.nmodes >= 16) bad = true; + else if (str_hash_djb2(header_tmp.mode_data_table, + sizeof(struct mode_data)*MAX_MDT_ELEMENTS) != header_tmp.table_djb2) + bad = true; + else if (header_tmp.curmode >= 16 || header_tmp.curmode == 0 + || mode_list[header_tmp.curmode] == NULL) + bad = true; + + if (bad) { + storage_init_defaults(); + storage_flush_data(); + return; + } + + mode_next_id = header_tmp.curmode; + + header_valid = true; +} + +struct mode_info storage_mode_get_info(int mode) { + #define DEF_RETVAL ({ \ + if (mode < 16 && mode > 0 && header_valid) mode_bad |= 1<= 16 || !header_valid || mode <= 0) return DEF_RETVAL; + + for (size_t i = 0; i < header_tmp.nmodes; ++i) { + struct mode_data md = header_tmp.mode_data_table[i]; + int mdmode = (uint8_t)(md.offsetandmode >> 28); + uint16_t mdsize = md.datasize; + uint32_t mdoffset = md.offsetandmode & ((1<<28)-1); + + if (mdmode != mode) continue; + if (~mdsize == 0 || ~md.version == 0 || ~offsetandmode == 0) + continue; // empty (wut?) + + // found it! + + if (mdsize == 0) return DEF_RETVAL; // no data stored + if (mdoffset == 0 || mdoffset >= STORAGE_SIZE) + return DEF_RETVAL; // bad offset + // program code collision cases + if (mdoffset < storage_get_program_offset() && mdoffset+mdsize >= + storage_get_program_offset()) return DEF_RETVAL; + if (mdoffset < storage_get_program_offset()+storage_get_program_size() + && mdoffset+mdsize >= storage_get_program_offset()+storage_get_program_size()) + return DEF_RETVAL; + if (mdoffset >= storage_get_program_offset() + && mdoffset+mdsize <= storage_get_program_offset()+storage_get_program_size()) + return DEF_RETVAL; + + // now check whether the data hash is corrupted + uint32_t hash = str_hash_djb2_init(); + + for (size_t i = 0; i < mdsize; i += sizeof(data_tmp)) { + size_t toread = sizeof(data_tmp); + if (mdsize - i < toread) toread = mdsize - i; + + storage_read(data_tmp, mdoffset + i, toread); + + hash = str_hash_djb2_digest(hash, data_tmp, toread); + } + + if (hash != md.data_djb2) return DEF_RETVAL; + + return (struct mode_info) { + .size = mdsize, + .version = md.version + }; + } + + return DEF_RETVAL; + + #undef DEF_RETVAL +} +void storage_mode_read(int mode, void* dst) { + if (mode >= 16 || !header_valid || mode <= 0) return; + + for (size_t i = 0; i < header_tmp.nmodes; ++i) { + struct mode_data md = header_tmp.mode_data_table[i]; + int mdmode = (uint8_t)(md.offsetandmode >> 28); + uint16_t mdsize = md.datasize; + uint32_t mdoffset = md.offsetandmode & ((1<<28)-1); + + if (mdmode != mode) continue; + if (~mdsize == 0 || ~md.version == 0 || ~offsetandmode == 0) + continue; // empty (wut?) + + // found it! + + if (mdsize == 0) { mode_bad |= 1<= STORAGE_SIZE) { + mode_bad |= 1<= + storage_get_program_offset()) { mode_bad |= 1<= storage_get_program_offset()+storage_get_program_size()) { + mode_bad |= 1<= storage_get_program_offset() + && mdoffset+mdsize <= storage_get_program_offset()+storage_get_program_size()) { + mode_bad |= 1< +#include + +#define STORAGE_VER 0x0010 + +enum mode_storage_class { + mode_storage_none, // this mode has no storage + mode_storage_32b , // this mode typically won't use more than 32 bytes of data + mode_storage_128b, // this mode typically won't use more than 128 bytes of data + mode_storage_512b, // this mode typically won't use more than 512 bytes of data + mode_storage_big // this mode uses a lot of data +}; + +// mode callbacks used by the storage subsystem +struct mode_storage { + enum mode_storage_class stclass; + uint32_t (*get_size)(void); + void (*get_data)(void* dst); + bool (*is_dirty)(void); // if data was changed since last mode_read/get_data call +}; + +struct mode_info { + uint32_t size; + uint16_t version; +}; + +// functions mode-specific code can use to retrieve the save data + +struct mode_info storage_mode_get_info(int mode); // returns size=0 for none found +void storage_mode_read(int mode, void* dst); + +// global functions + +// reads all data, creates table if needed +void storage_init(void); + +// flush edits if anything has been edited +void storage_flush_data(void); + +#endif + diff --git a/src/storage_internal.h b/src/storage_internal.h new file mode 100644 index 0000000..3b28681 --- /dev/null +++ b/src/storage_internal.h @@ -0,0 +1,69 @@ +// vim: set et: + +#ifndef STORAGE_INTERNAL_H_ +#define STORAGE_INTERNAL_H_ + +inline static uint32_t str_hash_djb2_init(void) { + return 5381; +} +uint32_t str_hash_djb2_digest(uint32_t hash, const void* data, size_t len); +inline static uint32_t str_hash_djb2(const void* data, size_t len) { + return str_hash_djb2_digestt(str_hash_djb2_init(), data, len); +} + +/* + * storage header (in last 256b of flash) (endianness is whatever is native to the device): + * + * 3 7 b f + * --+----------------------------------------------------+ + * 0 | f0 9f 8f b3 ef b8 8f e2 80 8d e2 9a a7 ef b8 8f | magic number + * 10 | fwver mm nm | fwver: current version (bcd) + * 20 | | mm: current mode, nm: number of modes saved (~size of "mode data table") + * 30 | | tbl djb2: djb2-hash of "mode data table" (entire table, not nmodes) + * 40 | | empty "mode data table" entries are 0xff-filled + * + * mode data table: array of: + * struct mode_data { + * uint16_t version; + * uint16_t datasize; + * uint28_t flashoffset; // from beginning of flash + * uint4_t modeid; + * uint32_t data_djb2; + * }; + * + * mode data blobs are typically allocated smallest to largest (according to + * mode_storage_class), from the last page of flash (cf. bsp-storage.h) down + */ + +#define STORAGE_MAGIC "\xf0\x9f\x8f\xb3\xef\xb8\x8f\xe2\x80\x8d\xe2\x9a\xa7\xef\xb8\x8f" +#define STORAGE_MAGIC_LEN 16 + +__attribute__((__packed__)) struct mode_data { + uint16_t version; + uint16_t datasize; + uint32_t offsetandmode; // mode ID stored in MSNybble + uint32_t data_djb2; +}; + +#define MAX_MDT_ELEMENTS ((256 - 64) / sizeof(struct mode_data)) /* should be 16 */ + +__attribute__((__packed__)) struct storage_header { + uint8_t magic[STORAGE_MAGIC_LEN]; + uint16_t fwversion; + uint8_t curmode; + uint8_t nmodes; // *stored* modes, not modes it knows of + uint8_t reserved[8]; + uint32_t table_djb2; + struct mode_data mode_data_table[MAX_MDT_ELEMENTS]; +}; + +// TODO: static assert sizeof(struct storage_header) == 256 +// TODO: static assert MAX_MDT_ELEMENTS >= 16 + +extern bool header_valid; +extern struct storage_header header_tmp; +extern uint8_t data_tmp[1024 - sizeof(struct storage_header)]; +extern uint16_t mode_bad; + +#endif + diff --git a/src/storage_save.c b/src/storage_save.c new file mode 100644 index 0000000..ff94809 --- /dev/null +++ b/src/storage_save.c @@ -0,0 +1,55 @@ +// vim: set et: + +#include "bsp-info.h" +#include "bsp-storage.h" +#include "mode.h" +#include "storage.h" + +#ifdef BOARD_HAS_STORAGE + +#include "storage_internal.h" + +static void storage_write_data(void) { + // BIG TODO + // * try to allocate every mode data thing, smaller stuff (cf. mode storage + // class) at higher addrs, bigger ones at lower ones. can use get_size(), + // but maybe keep data aligned + // * for each flash page, copy data into a big buffer. maybe use the XIP + // cache because the running mode may be already using all data. all + // get_data() functions and the bsp write function need to run from RAM, + // though + // * then write it out + // * maybe we could only write pages that changed? or maybe the bsp code + // can take care of this + + // also TODO: + // * call storage_flush_data on vnd cfg command + // * vnd cfg command to read storage data + // * save on a timer event? + // * try to save when unplugging??? +} + +static bool storage_mode_has(int mode) { + if (mode_list[i]->storage.stclass == mode_storage_none) return false; + if (mode_list[i]->get_size == NULL) return false; + if (mode_list[i]->get_data == NULL) return false; + if (mode_list[i]->is_dirty == NULL) return false; + + return true; +} + +void storage_flush_data(void) { + if (mode_bad != 0 || mode_current_id != header_tmp.curmode) { + storage_write_data(); + } else for (int i = 1; i < 16; ++i) { + if (mode_list[i] == NULL || !storage_mode_has(i)) continue; + + if (mode_list[i]->storage.is_dirty()) { + storage_write_data(i); + return; + } + } +} + +#endif +