better dpctl output for persistent storage

CMakeLists.txt

@@ -1,4 +1,5 @@
 option(USE_USBCDC_FOR_STDIO "Export an extra USB-CDC interface for stdio, instead of echoing it to a UART port (and requiring UART loopback for receiving stdio output on a host computer)." OFF)
+option(PERSISTENT_STORAGE "Use persistent storage (usually on-chip/on-board flash) to save the current mode, and settings of modes" ON)
 set(FAMILY "rp2040" CACHE STRING "Board/MCU family, decides which drivers to use. Set to RP2040 by default.")
 set(BOARD "raspberry_pi_pico" CACHE STRING "Board used, determines the pinout. Defaults to the Raspberry Pi Pico.")
 
@@ -45,10 +46,10 @@ if(FAMILY STREQUAL "rp2040")
   # need uart stdio, usb is busy doing other stuff
   if(USE_USBCDC_FOR_STDIO)
     # we're going to manually implement this case
-	#pico_enable_stdio_uart(${PROJECT} 0)
+    #pico_enable_stdio_uart(${PROJECT} 0)
-    target_compile_definitions(${PROJECT} PUBLIC USE_USBCDC_FOR_STDIO=1 PICO_STDIO_USB=1)
+    target_compile_definitions(${PROJECT} PUBLIC USE_USBCDC_FOR_STDIO=1)
   else()
-	  #pico_enable_stdio_uart(${PROJECT} 1)
+    #pico_enable_stdio_uart(${PROJECT} 1)
   endif()
   pico_enable_stdio_uart(${PROJECT} 1)
   pico_enable_stdio_usb(${PROJECT} 0)
@@ -69,6 +70,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
@@ -93,10 +96,14 @@ target_sources(${PROJECT} PUBLIC
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_sump/sump_hw.c
 )
 if(USE_USBCDC_FOR_STDIO)
+  target_compile_definitions(${PROJECT} PUBLIC USE_USBCDC_FOR_STDIO=1)
   target_sources(${PROJECT} PUBLIC
     ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/cdc_stdio.c
   )
 endif()
+if (PERSISTENT_STORAGE)
+  target_compile_definitions(${PROJECT} PUBLIC PERSISTENT_STORAGE=1)
+endif()
 target_include_directories(${PROJECT} PUBLIC
   ${CMAKE_CURRENT_SOURCE_DIR}/src/
   ${CMAKE_CURRENT_SOURCE_DIR}/libco/
@@ -108,7 +115,7 @@ target_include_directories(${PROJECT} PUBLIC
   ${CMAKE_CURRENT_SOURCE_DIR}/bsp/default/
 )
 
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Werror=implicit-function-declaration -Werror=return-type")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Werror=implicit-function-declaration -Werror=return-type -Werror=maybe-uninitialized")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
 
 add_custom_target(fix_db ALL WORKING_DIRECTORY ${OUTPUT_DIR}

bsp/rp2040/bsp-info.h

@@ -22,4 +22,9 @@
 #endif
 #define CFG_TUD_VENDOR 1
 
+/* don't access storage for RAM-only builds */
+#if !PICO_NO_FLASH
+#define DBOARD_HAS_STORAGE
+#endif
+
 #endif

bsp/rp2040/bsp-storage.h

@@ -0,0 +1,66 @@
+// vim: set et:
+
+#ifndef BSP_STORAGE_H_
+#define BSP_STORAGE_H_
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#if !PICO_NO_FLASH
+
+#include <string.h>
+
+#include <hardware/regs/addressmap.h>
+#include <hardware/regs/xip.h>
+#include <hardware/structs/xip_ctrl.h>
+#include <hardware/flash.h>
+#include <hardware/sync.h>
+#include <pico/bootrom.h>
+
+#ifndef PICO_FLASH_SIZE_BYTES
+#error "PICO_FLASH_SIZE_BYTES not defined"
+#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 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;
+
+    // memcmp pre: if no changes, don't flash
+    if (!memcmp(src, (uint8_t*)(XIP_BASE+offset), size)) return true;
+
+    flash_range_erase(offset, size);
+    flash_range_program(offset, src, size);
+
+    return !memcmp(src, (uint8_t*)(XIP_BASE+offset), size);
+}
+
+#endif
+
+#endif
+

host/dpctl/__init__.py

@@ -52,6 +52,12 @@ def dpctl_do(args: Any) -> int:
             return 1
         return devcmds.sump_overclock_set(conn, oven)
 
+    def sto_info(conn, args):
+        return devcmds.storage_info(conn)
+    def sto_flush(conn, args):
+        return devcmds.storage_flush(conn)
+    def sto_get(conn, args):
+        return devcmds.storage_get(conn, args.mode)
 
     #print(repr(args))
     cmds = {
@@ -59,6 +65,9 @@ def dpctl_do(args: Any) -> int:
         'get-mode-info': get_mode_info,
         'set-mode': set_mode,
         'bootloader': bootloader,
+        'storage-info': sto_info,
+        'storage-flush': sto_flush,
+        'storage-get': sto_get,
 
         'uart-cts-rts': uart_hw_flowctl,
         'tempsensor': tempsensor,
@@ -142,6 +151,15 @@ def main() -> int:
 
     bootloader = subcmds.add_parser("bootloader", help="Set the device in bootloader mode")
 
+    # persistent storage commands
+    storagehdr = subcmds.add_parser("storage-info", help="Get persistent storage info")
+
+    storageflush = subcmds.add_parser("storage-flush", help="Flush persistent storage data to storage medium")
+
+    storageget = subcmds.add_parser("storage-get", help="Get data of a particular mode")
+    storageget.add_argument('mode', default=None, nargs='?',
+                             help="Mode to get data of. Defaults to the current mode, 'all' means all modes.")
+
     # mode 1 commands
     usbhwfctl = subcmds.add_parser("uart-cts-rts", help="Get, enable/disable"+\
                                    " UART hardware flow control")

host/dpctl/commands.py

@@ -85,6 +85,56 @@ def set_mode(dev: DPDevice, mode: int) -> int:
 # ---
 
 
+def storage_info(dev: DPDevice) -> int:
+    try:
+        res = dev.storage_info()
+        print("magic: %s, version=%04x, current mode=%d, #modes=%d, DJB2(table)=%d" \
+              % (('OK' if res.magic == b'\xf0\x9f\x8f\xb3\xef\xb8\x8f\xe2\x80\x8d\xe2\x9a\xa7\xef\xb8\x8f' else 'BAD!'),
+                 res.version, res.curmode, res.nmodes, res.table_djb2))
+        for md in res.mode_data:
+            print("\tmode %d version %04x: 0x%x..+0x%x, DJB2=%d" % \
+                (md.mode, md.version, md.offset, md.datasize, md.data_djb2))
+        if len(res.mode_data) == 0:
+            print("No mode data");
+        return 0
+    except Exception as e:
+        print("Could not get storage info: %s" % str(e))
+        traceback.print_exc()
+        return 1
+
+
+def storage_flush(dev: DPDevice) -> int:
+    try:
+        res = dev.storage_flush()
+        print("storage saved" if res else "no write needed")
+        return 0
+    except Exception as e:
+        print("Could not flush persistent storage: %s" % str(e))
+        return 1
+
+
+def storage_get(dev: DPDevice, mode: str) -> int:
+    try:
+        if mode == 'all':
+            for m in dev.mode_info.keys():
+                res = dev.storage_get(mode)
+                print(repr(res)) # TODO
+            return 0
+        elif mode is None:
+            mode = dev.current_mode
+        else: mode = int(mode,0)
+
+        res = dev.storage_get(mode)
+        print(repr(res)) # TODO
+        return 0
+    except Exception as e:
+        print("Could not get storage data of mode %d: %s" % (mode, str(e)))
+        return 1
+
+
+# ---
+
+
 def uart_hw_flowctl_get(dev: DPDevice) -> int:
     try:
         res = dev.m1_usb_hw_flowctl_get()

host/dpctl/protocol.py

@@ -18,6 +18,56 @@ STAT_BADARG     = 0x04
 STAT_ILLSTATE   = 0x05
 
 
+class StorageInfoMode(NamedTuple):
+    version: int
+    datasize: int
+    offset: int
+    mode: int
+    data_djb2: int
+
+    def from_bytes(b: bytes) -> StorageInfoMode:
+        assert len(b) == 12
+        v, ds, oam, d = struct.unpack('<HHII', b)
+        return StorageInfoMode(v, ds, oam & ((1<<28)-1), (oam >> 28) & 15, d)
+
+    def list_from_bytes(b: bytes) -> List[StorageInfoMode]:
+        nelem = len(b) // 12
+        assert nelem * 12 == len(b)
+
+        r = [None]*nelem
+        for i in range(nelem): r[i] = StorageInfoMode.from_bytes(b[(i*12):((i+1)*12)])
+        return [re for re in r if re.version != 0xffff and re.datasize != 0xffff]
+
+
+class StorageInfo(NamedTuple):
+    magic: bytes
+    version: int
+    curmode: int
+    nmodes: int
+    reserved: bytes
+    table_djb2: int
+    mode_data: List[StorageInfoMode]
+
+    def from_bytes(b: bytes) -> StorageInfo:
+        assert len(b) == 256
+
+        mag = b[:16]
+        ver, cm, nm = struct.unpack('<HBB', b[16:20])
+        res = b[20:28] + b[245-32:]
+        d2tab = struct.unpack('<I', b[28:32])[0]
+
+        mdat = StorageInfoMode.list_from_bytes(b[32:256-32])
+
+        assert len(mdat) == nm
+
+        return StorageInfo(mag, ver, cm, nm, res, d2tab, mdat)
+
+    def __str__(self):
+        return "StorageInfo(magic=%s, version=%d, curmode=%d, nmodes=%d, table_djb2=%d, mode_data=%s)" \
+            % (' '.join(hex(b) for b in self.magic), self.version,
+               self.curmode, self.nmodes, self.table_djb2, repr(self.mode_data))
+
+
 class JtagMatch(NamedTuple):
     tck: int
     tms: int
@@ -144,11 +194,11 @@ class DPDevice:
 
         if (plen & 0x80) != 0:
             plen &= 0x7f
-            plen |= self.read(1) << 7
+            plen |= self.read(1)[0] << 7
 
             if (plen & 0x4000) != 0:
                 plen &= 0x3fff
-                plen |= self.read(1) << 14
+                plen |= self.read(1)[0] << 14
 
         bs = self.read(plen)
         #print("==> got resp %d res %s" % (resp, repr(bs)))
@@ -236,6 +286,32 @@ class DPDevice:
 
         return { i for i in range(0, 8) if (pl[0] & (1<<i)) != 0 }
 
+    # persistent storage
+
+    def storage_info(self) -> StorageInfo:
+        cmd = bytearray(b'\x0c')
+        self.write(b'\x0c')
+        stat, pl = self.read_resp()
+        check_statpl(stat, pl, "get storage info", 256, 256)
+
+        return StorageInfo.from_bytes(pl)
+
+    def storage_flush(self) -> bool:
+        self.write(b'\x0e')
+        stat, pl = self.read_resp()
+        check_statpl(stat, pl, "flush storage", 1, 1)
+        return pl[0]
+
+    def storage_get(self, mode: int) -> bytes:
+        cmd = bytearray(b'\x0d\x00')
+        cmd[1] = mode
+        self.write(cmd)
+        stat, pl = self.read_resp()
+        check_statpl(stat, pl, "get storage data", None, None)
+
+        return pl # TODO: parse
+
+
     # mode 1 commands
 
     def m1_usb_hw_flowctl_get(self) -> bool:

src/.gitignore

@@ -0,0 +1 @@
+_storage_test/

src/m_default/_default.c

@@ -3,6 +3,7 @@
 #include <tusb.h>
 
 #include "mode.h"
+#include "storage.h"
 #include "thread.h"
 #include "usbstdio.h"
 #include "vnd_cfg.h"
@@ -35,6 +36,8 @@ enum m_default_feature {
     mdef_feat_tempsense = 1<<4,
 };
 
+static bool data_dirty = false;
+
 #ifdef DBOARD_HAS_UART
 static cothread_t uartthread;
 static uint8_t    uartstack[THREAD_STACK_SIZE];
@@ -81,6 +84,17 @@ static void enter_cb(void) {
     serprogthread = co_derive(serprogstack, sizeof serprogstack, serprog_thread_fn);
     thread_enter(serprogthread);  // will call cdc_serprog_init() on correct thread
 #endif
+
+    if (!data_dirty) { // only read when not read yet
+        struct mode_info mi = storage_mode_get_info(1);
+        if (mi.size != 0 && mi.version == 0x0010 /* TODO: version migration? */) {
+            uint8_t dst[2];
+            storage_mode_read(1, dst, 0, 2);
+
+            cdc_uart_set_hwflow(dst[0]);
+            tempsense_set_addr(dst[1]);
+        }
+    }
 }
 static void leave_cb(void) {
     // TODO: CMSISDAP?
@@ -144,7 +158,12 @@ static void handle_cmd_cb(uint8_t cmd) {
         break;
     case mdef_cmd_tempsense:
 #ifdef DBOARD_HAS_TEMPSENSOR
-        tempsense_bulk_cmd();
+        {
+            uint8_t addra = tempsense_get_addr();
+            tempsense_bulk_cmd();
+            uint8_t addrb = tempsense_get_addr();
+            data_dirty |= addra != addrb;
+        }
 #else
         vnd_cfg_write_str(cfg_resp_illcmd, "temperature sensor not implemented on this device");
 #endif
@@ -156,9 +175,10 @@ static void handle_cmd_cb(uint8_t cmd) {
             resp = cdc_uart_get_hwflow() ? 1 : 0;
             vnd_cfg_write_resp(cfg_resp_ok, 1, &resp);
         } else {
-            if (cdc_uart_set_hwflow(resp != 0))
+            if (cdc_uart_set_hwflow(resp != 0)) {
                 vnd_cfg_write_resp(cfg_resp_ok, 0, NULL);
-            else
+                data_dirty = true;
+            } else
                 vnd_cfg_write_str(cfg_resp_illcmd, "UART flow control setting not supported on this device");
         }
 #else
@@ -387,6 +407,19 @@ static bool my_vendor_control_xfer_cb(uint8_t rhport, uint8_t ep_addr,
 }
 #endif
 
+static uint16_t my_get_size(void) { return 2; }
+static void my_get_data(void* dst, size_t offset, size_t maxsize) {
+    (void)offset; (void)maxsize;
+
+    uint8_t* d = dst;
+
+    d[0] = cdc_uart_get_hwflow() ? 1 : 0;
+    d[1] = tempsense_get_addr();
+
+    data_dirty = false;
+}
+static bool my_is_dirty(void) { return data_dirty; }
+
 extern struct mode m_01_default;
 // clang-format off
 struct mode m_01_default = {
@@ -402,6 +435,13 @@ struct mode m_01_default = {
     .task  = task_cb,
     .handle_cmd = handle_cmd_cb,
 
+    .storage = {
+        .stclass = mode_storage_32b,
+        .get_size = my_get_size,
+        .get_data = my_get_data,
+        .is_dirty = my_is_dirty
+    },
+
 #if defined(DBOARD_HAS_CMSISDAP) && CFG_TUD_HID > 0
 #if 0
     .tud_hid_get_report_cb = my_hid_get_report_cb,

src/m_sump/_sump.c

@@ -3,6 +3,7 @@
 #include <tusb.h>
 
 #include "mode.h"
+#include "storage.h"
 #include "thread.h"
 #include "usbstdio.h"
 #include "vnd_cfg.h"
@@ -20,6 +21,8 @@ enum m_sump_feature {
     msump_feat_sump      = 1<<0,
 };
 
+static bool data_dirty = false;
+
 #ifdef DBOARD_HAS_SUMP
 static cothread_t sumpthread;
 static uint8_t    sumpstack[THREAD_STACK_SIZE];
@@ -44,6 +47,16 @@ static void enter_cb(void) {
     sumpthread = co_derive(sumpstack, sizeof sumpstack, sump_thread_fn);
     thread_enter(sumpthread);
 #endif
+
+    if (!data_dirty) { // only read when not read yet
+        struct mode_info mi = storage_mode_get_info(4);
+        if (mi.size != 0 && mi.version == 0x0010 /* TODO: version migration? */) {
+            uint8_t dst[1];
+            storage_mode_read(4, dst, 0, 1);
+
+            sump_hw_set_overclock(dst[0]);
+        }
+    }
 }
 static void leave_cb(void) {
 #ifdef DBOARD_HAS_SUMP
@@ -133,7 +146,6 @@ enum {
 #define EPNUM_CDC_STDIO_NOTIF   0x85
 
 // clang-format off
-// TODO: replace magic 64s by actual buffer size macros
 static const uint8_t desc_configuration[] = {
     TUD_CONFIG_DESCRIPTOR(1, ITF_NUM__TOTAL, STRID_CONFIG, CONFIG_TOTAL_LEN,
         TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, 100),
@@ -178,6 +190,17 @@ static void my_cdc_line_coding_cb(uint8_t itf, cdc_line_coding_t const* line_cod
 }
 #endif
 
+static uint16_t my_get_size(void) { return 1; }
+static void my_get_data(void* dst, size_t offset, size_t maxsize) {
+    (void)offset; (void)maxsize;
+
+    uint8_t* d = dst;
+    d[0] = sump_hw_get_overclock();
+
+    data_dirty = false;
+}
+static bool my_is_dirty(void) { return data_dirty; }
+
 extern struct mode m_04_sump;
 // clang-format off
 struct mode m_04_sump = {
@@ -188,6 +211,13 @@ struct mode m_04_sump = {
     .usb_desc = desc_configuration,
     .string_desc = string_desc_arr,
 
+    .storage = {
+        .stclass = mode_storage_32b,
+        .get_size = my_get_size,
+        .get_data = my_get_data,
+        .is_dirty = my_is_dirty
+    },
+
     .enter = enter_cb,
     .leave = leave_cb,
     .task  = task_cb,

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,15 +32,24 @@ int main() {
     vndcfg_thread = co_derive(vndcfg_stack, sizeof vndcfg_stack, vndcfg_thread_fn);
     thread_enter(vndcfg_thread);
 
-    modes_init();
+#if defined(PERSISTENT_STORAGE) && defined(DBOARD_HAS_STORAGE)
+    int startupmode = storage_init();
+#else
+    int startupmode = -1;
+#endif
+
+    // FIXME: put elsewhere?
+#ifdef USE_USBCDC_FOR_STDIO
+    stdio_usb_set_itf_num(0);
+#endif
+
+    modes_init(startupmode);
     if (mode_current) mode_current->enter();
 
     tusb_init();
 
     // FIXME: put elsewhere?
 #ifdef USE_USBCDC_FOR_STDIO
-    stdio_usb_set_itf_num(0);
-
     stdio_usb_init();
 #endif
 

src/mode.h

@@ -9,6 +9,8 @@
 #include "tusb_config.h"
 #include <tusb.h>
 
+#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;

src/modeset.c

@@ -5,6 +5,7 @@
 #include "alloc.h"
 #include "board.h" /* bsp_reset_bootloader() */
 #include "mode.h"
+#include "storage.h"
 
 extern struct mode m_01_default, m_03_jscan, m_04_sump;
 
@@ -37,9 +38,13 @@ enum tusbgot_index {
 
 #define ORDEF(a,b) ((a != NULL) ? a : b)
 
-void modes_init(void) {
+void modes_init(int newid) {
-    // switch to the default mode, but without doing a USB reboot thing
+    if (newid < 0 || newid >= 16 || mode_list[newid] == NULL) {
-    mode_current_id = &mode_default - mode_list;
+        // 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;
@@ -167,3 +172,13 @@ void modes_switch(uint8_t newmode) {
     if (mode_current) mode_current->enter();
 }
 
+#if defined(PERSISTENT_STORAGE) && defined(DBOARD_HAS_STORAGE)
+void tud_umount_cb(void) {
+    storage_flush_data();
+}
+void tud_suspend_cb(bool remote_wakeup_en) {
+    (void)remote_wakeup_en;
+    storage_flush_data();
+}
+#endif
+

src/storage.c

@@ -0,0 +1,190 @@
+// vim: set et:
+
+#include "bsp-info.h"
+#include "bsp-storage.h"
+#include "mode.h"
+#include "storage.h"
+
+#if !defined(PERSISTENT_STORAGE) || !defined(DBOARD_HAS_STORAGE)
+int storage_init(void) { return -1; }
+bool storage_flush_data(void) { return false; }
+struct mode_info storage_mode_get_info(int _) {
+    (void)_; return (struct mode_info){ .size = 0, .version = 0 };
+}
+void storage_mode_read(int _, void* __, size_t ___, size_t ____) {
+    (void)_; (void)__; (void)___; (void)____;
+}
+#else
+
+#include "storage_internal.h"
+
+uint32_t str_hash_djb2_digest(uint32_t hash, const void* data, size_t len) {
+    const uint8_t* d = data;
+
+    for (size_t i = 0; i < len; ++i) hash += hash * 33 + d[i];
+
+    return hash;
+}
+
+bool header_valid = false;
+struct storage_header header_tmp;
+uint8_t data_tmp[256];
+uint16_t mode_bad = 0;
+
+static int in_init = 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;
+}
+
+int storage_init(void) {
+    ++in_init;
+
+    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();
+        if (in_init == 1) storage_flush_data();
+        --in_init;
+        return -1;
+    }
+
+    if (header_tmp.fwversion != STORAGE_VER) {
+        // TODO: migrate... if there were any older versions
+        header_valid = false;
+        --in_init;
+        return -1;
+    }
+
+    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();
+        if (in_init == 1) storage_flush_data();
+        --in_init;
+        return -1;
+    }
+
+    header_valid = true;
+    --in_init;
+    return header_tmp.curmode;
+}
+
+struct mode_info storage_mode_get_info(int mode) {
+    #define DEF_RETVAL ({ \
+        if (mode < 16 && mode > 0 && header_valid && header_tmp.nmodes != 0) mode_bad |= 1<<mode; \
+        (struct mode_info){ .size = 0, .version = 0 }; \
+    }) \
+
+
+    if (mode >= 16 || !header_valid || mode <= 0 || header_tmp.nmodes == 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 == 0xffff || md.version == 0xffff || md.offsetandmode == 0xffffffffu)
+            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, size_t offset, size_t maxlen) {
+    if (mode >= 16 || !header_valid || mode <= 0 || header_tmp.nmodes == 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 == 0xffff || md.version == 0xffff || md.offsetandmode == 0xffffffffu)
+            continue; // empty (wut?)
+
+        // found it!
+
+        if (mdsize == 0) { mode_bad |= 1<<mode; return; /* no data stored */ }
+        if (mdoffset == 0 || mdoffset >= STORAGE_SIZE) {
+            mode_bad |= 1<<mode; return; /* bad offset */
+        }
+        // program code collision cases
+        if (mdoffset < storage_get_program_offset() && mdoffset+mdsize >=
+                storage_get_program_offset()) { mode_bad |= 1<<mode; return; }
+        if (mdoffset < storage_get_program_offset()+storage_get_program_size()
+                && mdoffset+mdsize >= storage_get_program_offset()+storage_get_program_size()) {
+            mode_bad |= 1<<mode; return;
+        }
+        if (mdoffset >= storage_get_program_offset()
+                && mdoffset+mdsize <= storage_get_program_offset()+storage_get_program_size()) {
+            mode_bad |= 1<<mode; return;
+        }
+
+        if (offset >= mdsize) return;
+
+        // skip hash check in this case
+        storage_read(dst, mdoffset + offset, mdsize < maxlen ? mdsize : maxlen);
+        return;
+    }
+}
+
+#endif /* BOARD_HAS_STORAGE */
+

src/storage.h

@@ -0,0 +1,50 @@
+// vim: set et:
+
+#ifndef STORAGE_H_
+#define STORAGE_H_
+
+#include <stddef.h>
+#include <stdint.h>
+
+#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;
+    uint16_t (*get_size)(void);
+    // if stclass < 512b, offset & maxsize can be ignored
+    void     (*get_data)(void* dst, size_t offset, size_t maxsize);
+    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, size_t offset, size_t maxlen);
+
+// global functions
+
+// reads all data, creates table if needed
+int storage_init(void);
+
+// flush edits if anything has been edited
+bool storage_flush_data(void);
+
+bool storage_priv_mode_has(int mode);
+void* storage_priv_get_header_ptr(void);
+
+#endif
+

src/storage_internal.h

@@ -0,0 +1,74 @@
+// 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_digest(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  <reserved (0xff)       >  <tbl djb2>  |  fwver: current version (bcd)
+ * 20 | <mode data table ...>                              |  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 | <reserved (0xff) ...>                              |  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 {
+    // +0
+    uint8_t magic[STORAGE_MAGIC_LEN];
+    // +16
+    uint16_t fwversion;
+    uint8_t curmode;
+    uint8_t nmodes; // *stored* modes, not modes it knows of
+    uint8_t reserved[8];
+    uint32_t table_djb2;
+    // +32
+    struct mode_data mode_data_table[MAX_MDT_ELEMENTS]; // 192 bytes in size
+    // +224
+    uint8_t reserved2[32];
+};
+
+// 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[256];
+extern uint16_t mode_bad;
+
+#endif
+

src/storage_save.c

@@ -0,0 +1,172 @@
+// vim: set et:
+
+#include "bsp-info.h"
+#include "bsp-storage.h"
+#include "mode.h"
+#include "storage.h"
+
+#if defined(PERSISTENT_STORAGE) && defined(DBOARD_HAS_STORAGE)
+
+#include "storage_internal.h"
+
+bool storage_priv_mode_has(int i) {
+    if (mode_list[i]->storage.stclass == mode_storage_none) return false;
+    if (mode_list[i]->storage.get_size == NULL) return false;
+    if (mode_list[i]->storage.get_data == NULL) return false;
+    if (mode_list[i]->storage.is_dirty == NULL) return false;
+
+    return true;
+}
+void* storage_priv_get_header_ptr(void) {
+    return &header_tmp;
+}
+
+static struct mode_storage msto[16];
+
+static size_t storage_allocate_new(void) {
+    static const size_t stclass_sz[] = { 0, 32, 128, 512, 0xffffffff };
+
+    memcpy(header_tmp.magic, STORAGE_MAGIC, STORAGE_MAGIC_LEN);
+    memset(header_tmp.reserved, 0xff, sizeof(header_tmp.reserved));
+    memset(header_tmp.reserved2, 0xff, sizeof(header_tmp.reserved2));
+    memset(header_tmp.mode_data_table, 0xff, sizeof(header_tmp.mode_data_table));
+
+    header_tmp.fwversion = STORAGE_VER;
+    header_tmp.curmode = mode_current_id;
+    header_tmp.nmodes = 0;
+
+    size_t current_page = STORAGE_SIZE - STORAGE_ERASEWRITE_ALIGN,
+           current_page_end = STORAGE_SIZE - sizeof(struct storage_header);
+    size_t current_wrhead = current_page;
+    size_t npages = 1;
+
+    for (enum mode_storage_class stcls = mode_storage_32b; stcls <= mode_storage_big; ++stcls) {
+        for (int mode = 1; mode < 16; ++mode) {
+            if (mode_list[mode] == NULL || !storage_priv_mode_has(mode)) continue;
+            if (mode_list[mode]->storage.stclass != stcls) continue;
+
+            // too big for the class? don't write the data, then
+            uint16_t dsize = mode_list[mode]->storage.get_size();
+            if (dsize > stclass_sz[stcls]) continue;
+
+            if (current_wrhead + dsize > current_page_end) { // FIXME: data that is >1 page size (do we want to support this?)
+                current_page_end = current_page;
+                current_page -= STORAGE_ERASEWRITE_ALIGN;
+                current_wrhead = current_page;
+                ++npages;
+
+                if (current_page < storage_get_program_offset() + storage_get_program_size())
+                    return 0; // welp, out of space
+            }
+
+            struct mode_data* md = &header_tmp.mode_data_table[header_tmp.nmodes];
+            md->version = mode_list[mode]->version;
+            md->datasize = dsize;
+            md->offsetandmode = current_wrhead | ((uint32_t)mode << 28);
+            msto[header_tmp.nmodes] = mode_list[mode]->storage; // copy to RAM because mode_list is in rodata!
+
+            current_wrhead += stclass_sz[stcls];
+
+            uint32_t hash = str_hash_djb2_init();
+
+            for (size_t i = 0; i < dsize; i += sizeof(data_tmp)) {
+                size_t tohash = sizeof(data_tmp);
+                if (dsize - i < tohash) tohash = dsize - i;
+
+                mode_list[mode]->storage.get_data(data_tmp, i, tohash);
+
+                hash = str_hash_djb2_digest(hash, data_tmp, tohash);
+            }
+
+            md->data_djb2 = hash;
+            ++header_tmp.nmodes;
+        }
+    }
+
+    header_tmp.table_djb2 = str_hash_djb2(header_tmp.mode_data_table,
+            sizeof(header_tmp.mode_data_table));
+
+    return npages;
+}
+
+static void st_safe_memcpy(void* dst, const void* src, size_t size) {
+    const uint64_t* s = src;
+    uint64_t* d = dst;
+    for (size_t i = 0; i < (size>>3); ++i) {
+        d[i] = s[i];
+        asm volatile("":::"memory");
+    }
+
+    if (size & 7) {
+        for (size_t i = (size>>3)<<3; i < size; ++i) {
+            ((uint8_t*)dst)[i] = ((const uint8_t*)src)[i];
+            asm volatile("":::"memory");
+        }
+    }
+}
+
+static void storage_serialize_xip(size_t page,
+        size_t pagestart, size_t pageend, void* dest) {
+    if (page == 0) {
+        st_safe_memcpy((uint8_t*)dest + pageend - pagestart,
+                &header_tmp, sizeof(struct storage_header));
+    }
+
+    for (size_t i = 0; i < header_tmp.nmodes; ++i) {
+        struct mode_data* md = &header_tmp.mode_data_table[i];
+        uint32_t mdoffset = md->offsetandmode & ((1<<28)-1);
+        uint32_t mdsize = md->datasize;
+
+        if (mdoffset < pagestart || mdoffset + mdsize >= pageend)
+            continue; // must be fully within the page
+
+        msto[i].get_data((uint8_t*)dest + mdoffset - pagestart, 0, mdsize);
+    }
+}
+
+static void storage_write_data(void) {
+    size_t npages = storage_allocate_new();
+    if (npages == 0) {
+        storage_init();
+        return; // TODO: error, somehow
+    }
+
+    static uint8_t base[STORAGE_ERASEWRITE_ALIGN]; // TODO FIXME: HUGE RAM HOG!
+
+    size_t current_page = STORAGE_SIZE - STORAGE_ERASEWRITE_ALIGN,
+           current_page_end = STORAGE_SIZE - sizeof(struct storage_header);
+    for (size_t page = 0; page < npages; ++page) {
+        storage_serialize_xip(page, current_page, current_page_end, base);
+
+        if (!storage_erasewrite(current_page, base, STORAGE_ERASEWRITE_ALIGN)) {
+            storage_init();
+            return; // TODO: error, somehow
+        }
+
+        current_page_end = current_page;
+        current_page -= STORAGE_ERASEWRITE_ALIGN;
+    }
+
+    // TODO:
+    // * save on a timer event?
+    // * try to save when unplugging???
+}
+
+bool storage_flush_data(void) {
+    if (mode_bad != 0 || mode_current_id != header_tmp.curmode) {
+        storage_write_data();
+        return true;
+    } else for (int i = 1; i < 16; ++i) {
+        if (mode_list[i] == NULL || !storage_priv_mode_has(i)) continue;
+
+        if (mode_list[i]->storage.is_dirty()) {
+            storage_write_data();
+            return true;
+        }
+    }
+
+    return false;
+}
+
+#endif
+

src/vnd_cfg.c

@@ -18,6 +18,8 @@ static uint32_t rxavail, rxpos, txpos;
 
 static int VND_N_CFG = 0;
 
+extern uint8_t data_tmp[256];
+
 void vnd_cfg_init(void) {
     rxavail = 0;
     rxpos   = 0;
@@ -191,6 +193,47 @@ void vnd_cfg_task(void) {
         case cfg_cmd_get_infostr:
             vnd_cfg_write_str(cfg_resp_ok, INFO_PRODUCT(INFO_BOARDNAME));
             break;
+#if defined(PERSISTENT_STORAGE) && defined(DBOARD_HAS_STORAGE)
+        case cfg_cmd_storage_get_header:
+            vnd_cfg_write_resp(cfg_resp_ok, 256, storage_priv_get_header_ptr());
+            break;
+        case cfg_cmd_storage_get_modedata:
+            verbuf[0] = vnd_cfg_read_byte();
+            if (verbuf[0] == 0 || verbuf[0] >= 16 || mode_list[verbuf[0]] == NULL) {
+                vnd_cfg_write_resp(cfg_resp_nosuchmode, 0, NULL);
+            } else if (!storage_priv_mode_has(verbuf[0])) {
+                vnd_cfg_write_resp(cfg_resp_badarg, 0, NULL);
+            } else {
+                uint32_t len = storage_mode_get_info(verbuf[0]).size;
+                vnd_cfg_write_byte(cfg_resp_ok);
+                if (len < (1<<7)) {
+                    vnd_cfg_write_byte(len);
+                } else if (len < (1<<14)) {
+                    vnd_cfg_write_byte((len & 0x7f) | 0x80);
+                    vnd_cfg_write_byte((len >> 7) & 0x7f);
+                } else {
+                    vnd_cfg_write_byte((len & 0x7f) | 0x80);
+                    vnd_cfg_write_byte(((len >> 7) & 0x7f) | 0x80);
+                    vnd_cfg_write_byte(((len >> 14) & 0x7f));
+                }
+
+                for (size_t i = 0; i < len; i += sizeof data_tmp) {
+                    size_t tosend = sizeof data_tmp;
+                    if (tosend > len - i) tosend = len - i;
+                    storage_mode_read(verbuf[0], data_tmp, i, tosend);
+
+                    for (size_t ii = 0; ii < tosend; ++ii)
+                        vnd_cfg_write_byte(data_tmp[ii]);
+                }
+
+                vnd_cfg_write_flush();
+            }
+            break;
+        case cfg_cmd_storage_flush_data:
+            verbuf[0] = storage_flush_data() ? 1 : 0;
+            vnd_cfg_write_resp(cfg_resp_ok, 1, verbuf);
+            break;
+#endif
         default:
             vnd_cfg_write_resp(cfg_resp_illcmd, 0, NULL);
             break;

src/vnd_cfg.h

@@ -30,6 +30,12 @@ enum cfg_cmd {
     cfg_cmd_get_cur_mode = 0x02,
     cfg_cmd_set_cur_mode = 0x03,
     cfg_cmd_get_infostr  = 0x04,
+
+#if defined(PERSISTENT_STORAGE) && defined(DBOARD_HAS_STORAGE)
+    cfg_cmd_storage_get_header   = 0x0c,
+    cfg_cmd_storage_get_modedata = 0x0d,
+    cfg_cmd_storage_flush_data   = 0x0e,
+#endif
 };
 
 // common commands for every mode