better dpctl output for persistent storage

clear remaining 32 reserved bytes
get storage to work now
2021-09-25 13:58:06 +02:00 · 2021-09-25 13:58:06 +02:00 · 2021-09-25 13:58:06 +02:00 · 2021-09-25 13:58:06 +02:00 · 2021-09-25 13:58:06 +02:00 · 2021-09-25 13:58:06 +02:00
43 changed files with 1020 additions and 2241 deletions
--- a/.gitignore
+++ b/.gitignore
@ -1,5 +1,6 @@
 cmake-build/
-build*/
+build/
+build-flash/
 ex/
 compile_commands.json
 _old/
--- a/CMSIS-DAP/Firmware/Source/DAP.c
+++ b/CMSIS-DAP/Firmware/Source/DAP.c
@ -1642,12 +1642,10 @@ uint32_t DAP_ProcessCommand(const uint8_t *request, uint8_t *response) {

  *response++ = *request;

-  //printf("dap cmd=%02hhx\n", *request);
  switch (*request++) {
    case ID_DAP_Info:
      num = DAP_Info(*request, response+1);
      *response = (uint8_t)num;
-      //printf("cmd info\n");
      return ((2U << 16) + 2U + num);

    case ID_DAP_HostStatus:
@ -1754,11 +1752,9 @@ uint32_t DAP_ProcessCommand(const uint8_t *request, uint8_t *response) {

    default:
      *(response-1) = ID_DAP_Invalid;
-      //printf("cmd invalid\n");
      return ((1U << 16) | 1U);
  }

-  //printf("cmd end\n");
  return ((1U << 16) + 1U + num);
 }

--- a/CMakeLists.txt
+++ b/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.")

@ -21,18 +22,21 @@ if(FAMILY STREQUAL "rp2040")
  option(PICO_NO_FLASH "Disable writing the compiled program to flash, and only load it to RAM. Useful for testing, but not much else (OFF by default)." OFF)
  option(PICO_COPY_TO_RAM "Run all code in RAM, while the program is also stored on flash. On bootup, everything will be copied to RAM (OFF by default)." OFF)

-  include(pico_sdk_import.cmake)
-  set(TOP "${PICO_SDK_PATH}/lib/tinyusb")
-  get_filename_component(TOP "${TOP}" REALPATH)
+  if (USE_SYSTEMWIDE_PICOSDK)
+    set(TOP "$ENV{PICO_SDK_PATH}/lib/tinyusb")
+    get_filename_component(TOP "${TOP}" REALPATH)
+    include(cmake/pico_sdk_import.cmake)
+  else()
+    set(PICO_SDK_PATH ${TOP}/hw/mcu/raspberrypi/pico-sdk)
+    include(${PICO_SDK_PATH}/pico_sdk_init.cmake)
+  endif()

  include(${TOP}/hw/bsp/${FAMILY}/family.cmake) # tinyusb stuff
-  include(${TOP}/hw/bsp/family_support.cmake) # tinyusb stuff

  family_get_project_name(PROJECT ${CMAKE_CURRENT_LIST_DIR})
-  #set(PROJECT DragonProbe)
-  project(${PROJECT} C CXX ASM)
-  #family_initialize_project(PROJECT ${CMAKE_CURRENT_LIST_DIR}) # calls pico_sdk_init()
-  pico_sdk_init()
+  project(${PROJECT})
+  family_initialize_project(PROJECT ${CMAKE_CURRENT_LIST_DIR}) # calls pico_sdk_init()
+  #pico_sdk_init()

  #pico_set_program_name(${PROJECT} "${PROJECT}")
  #pico_set_program_version(${PROJECT} "0.1")
@ -42,16 +46,13 @@ 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)
-    target_compile_definitions(${PROJECT} PUBLIC USE_USBCDC_FOR_STDIO=1 PICO_STDIO_USB=1)
+    #pico_enable_stdio_uart(${PROJECT} 0)
+    target_compile_definitions(${PROJECT} PUBLIC USE_USBCDC_FOR_STDIO=1)
  else()
-	  #pico_enable_stdio_uart(${PROJECT} 1)
-	  #set_target_properties(${PROJECT} PROPERTIES PICO_TARGET_STDIO_UART 1)
+    #pico_enable_stdio_uart(${PROJECT} 1)
  endif()
  pico_enable_stdio_uart(${PROJECT} 1)
  pico_enable_stdio_usb(${PROJECT} 0)
-  #set_target_properties(${PROJECT} PROPERTIES PICO_TARGET_STDIO_UART 1)
-  #set_target_properties(${PROJECT} PROPERTIES PICO_TARGET_STDIO_USB 0)
 else()
  message(FATAL_ERROR "Invalid FAMILY '${FAMILY}' specified")
 endif()
@ -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
@ -77,8 +80,6 @@ target_sources(${PROJECT} PUBLIC
  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_default/cdc_serprog.c
  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_default/tempsensor.c
  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_default/vnd_i2ctinyusb.c
-  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_isp/_isp.c
-  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_isp/mehfet.c
  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_jscan/_jscan.c
  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_jscan/jscan.c
  ${CMAKE_CURRENT_SOURCE_DIR}/src/m_sump/_sump.c
@ -91,16 +92,18 @@ target_sources(${PROJECT} PUBLIC
  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/i2c_tinyusb.c
  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/spi_serprog.c
  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/tempsensor.c
-  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_isp/mehfet_hw.c
-  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_isp/sbw_hw.c
  ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_jscan/jscan_hw.c
  ${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/
@ -112,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}
@ -144,7 +147,6 @@ if(FAMILY STREQUAL "rp2040")
  pico_generate_pio_header(${PROJECT} ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/dap_swd.pio)
  pico_generate_pio_header(${PROJECT} ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/swo_uart_rx.pio)
  pico_generate_pio_header(${PROJECT} ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_default/swo_manchester_encoding.pio)
-  pico_generate_pio_header(${PROJECT} ${CMAKE_CURRENT_SOURCE_DIR}/bsp/${FAMILY}/m_isp/sbw.pio)

  pico_add_extra_outputs(${PROJECT})

--- a/bsp/rp2040/bsp-info.h
+++ b/bsp/rp2040/bsp-info.h
@ -9,10 +9,6 @@
 /*#define USB_VID 0x1d50*/ /* OpenMoko */
 #define USB_PID 0x1312

-/* openFPGAloader silliness */
-/*#define USB_VID 0x0d28*/ /* NXP */
-/*#define USB_PID 0x0204*/ /* ARM mbed */
-
 // TODO: other RP2040 boards
 #define INFO_BOARDNAME "RP2040 Pico"

@ -24,6 +20,11 @@
 #else
 #define CFG_TUD_CDC 2
 #endif
-#define CFG_TUD_VENDOR 2
+#define CFG_TUD_VENDOR 1
+
+/* don't access storage for RAM-only builds */
+#if !PICO_NO_FLASH
+#define DBOARD_HAS_STORAGE
+#endif

 #endif
--- a/bsp/rp2040/bsp-storage.h
+++ b/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
+
--- a/bsp/rp2040/m_default/bsp-feature.h
+++ b/bsp/rp2040/m_default/bsp-feature.h
@ -25,9 +25,8 @@ enum {
    CDC_N__NITF
 };
 enum {
-    VND_N_CMSISDAP = 0,
 #if CFG_TUD_VENDOR > 0
-    VND_N_CFG,
+    VND_N_CFG = 0,
 #endif

    VND_N__NITF
--- a/bsp/rp2040/m_default/cdc_uart.c
+++ b/bsp/rp2040/m_default/cdc_uart.c
@ -48,10 +48,8 @@ void cdc_uart_init(void) {

    gpio_set_function(PINOUT_UART_TX, GPIO_FUNC_UART);
    gpio_set_function(PINOUT_UART_RX, GPIO_FUNC_UART);
-    gpio_set_function(PINOUT_UART_CTS, GPIO_FUNC_SIO);
-    gpio_set_function(PINOUT_UART_RTS, GPIO_FUNC_SIO);
    uart_init(PINOUT_UART_INTERFACE, lc_brate/*PINOUT_UART_BAUDRATE*/);
-    //uart_set_hw_flow(PINOUT_UART_INTERFACE, hwflow, hwflow);
+    uart_set_hw_flow(PINOUT_UART_INTERFACE, hwflow, hwflow);
    uart_set_format(PINOUT_UART_INTERFACE, lc_data, lc_stop, lc_parity);

    bi_decl(bi_2pins_with_func(PINOUT_UART_TX, PINOUT_UART_RX, GPIO_FUNC_UART));
@ -60,8 +58,6 @@ void cdc_uart_deinit(void) {
    uart_deinit(PINOUT_UART_INTERFACE);
    gpio_set_function(PINOUT_UART_TX, GPIO_FUNC_NULL);
    gpio_set_function(PINOUT_UART_RX, GPIO_FUNC_NULL);
-    gpio_set_function(PINOUT_UART_CTS, GPIO_FUNC_NULL);
-    gpio_set_function(PINOUT_UART_RTS, GPIO_FUNC_NULL);
 }

 void cdc_uart_task(void) {
@ -94,8 +90,6 @@ bool cdc_uart_get_hwflow(void) {
 bool cdc_uart_set_hwflow(bool enable) {
    hwflow = enable;
    //uart_set_hw_flow(PINOUT_UART_INTERFACE, enable, enable);
-    // TODO: CTS
-    gpio_put(PINOUT_UART_RTS, enable);
    return true;
 }

--- a/bsp/rp2040/m_default/dap_jtag.c
+++ b/bsp/rp2040/m_default/dap_jtag.c
@ -4,8 +4,6 @@
 #include <hardware/gpio.h>
 #include <hardware/pio.h>

-#include "util.h"
-
 #include "DAP_config.h"
 #include <DAP.h>

@ -17,24 +15,20 @@
 int jtagsm = -1, jtagoffset = -1;

 void PORT_OFF(void) {
-    //printf("disable\n");
-    if (jtagsm >= 0) {
+    if (jtagsm) {
        pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, jtagsm, false);
        pio_sm_unclaim(PINOUT_JTAG_PIO_DEV, jtagsm);
    }
-    if (jtagoffset >= 0) {
+    if (jtagoffset)
        pio_remove_program(PINOUT_JTAG_PIO_DEV, &dap_jtag_program, jtagoffset);
-    }
    jtagoffset = jtagsm = -1;

-    if (swdsm >= 0) {
+    if (swdsm) {
        pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, swdsm, false);
        pio_sm_unclaim(PINOUT_JTAG_PIO_DEV, swdsm);
    }
-    if (swdoffset >= 0) {
+    if (swdoffset)
        pio_remove_program(PINOUT_JTAG_PIO_DEV, &dap_swd_program, swdoffset);
-    }
-    swdoffset = swdsm = -1;

    sio_hw->gpio_oe_clr = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK |
                          PINOUT_TDI_MASK  //| PINOUT_TDO_MASK
@ -54,22 +48,17 @@ void PORT_JTAG_SETUP(void) {
    sio_hw->gpio_oe_clr = PINOUT_TDO_MASK;

    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TCK],
-        PADS_BANK0_GPIO0_IE_BITS  // bits to set: input enable
-            | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS         // bits to mask out: input enable, output disable
-            | PADS_BANK0_GPIO0_SLEWFAST_BITS);
-    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TMS],
-        PADS_BANK0_GPIO0_IE_BITS | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS
-            | PADS_BANK0_GPIO0_SLEWFAST_BITS);
-    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TDI],
-        PADS_BANK0_GPIO0_IE_BITS | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS
-            | PADS_BANK0_GPIO0_SLEWFAST_BITS);
+        PADS_BANK0_GPIO0_IE_BITS,  // bits to set: input enable
+        PADS_BANK0_GPIO0_IE_BITS |
+            PADS_BANK0_GPIO0_OD_BITS);  // bits to mask out: input enable, output disable
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TMS], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TDI], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TDO],
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS  // TDO needs to have its output disabled
-            | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS | PADS_BANK0_GPIO0_SLEWFAST_BITS);
+        PADS_BANK0_GPIO0_IE_BITS |
+            PADS_BANK0_GPIO0_OD_BITS,  // TDO needs to have its output disabled
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_nTRST], PADS_BANK0_GPIO0_IE_BITS,
        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_nRESET], PADS_BANK0_GPIO0_IE_BITS,
@ -94,16 +83,16 @@ void PORT_JTAG_SETUP(void) {
                        | PINOUT_nTRST_MASK | PINOUT_nRESET_MASK;
 }*/

-/*void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
+void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
    uint32_t n = info & JTAG_SEQUENCE_TCK;
    if (n == 0) n = 64;

-//    printf("seq hi 0x%lx\n", info);
-//
-//    printf("%s", "tdi: ");
-//    for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
-//        printf("0x%x ", ((const uint8_t*)tdi)[j]);
-//    }
+    printf("seq hi 0x%lx\n", info);
+
+    printf("%s", "tdi: ");
+    for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
+        printf("0x%x ", ((const uint8_t*)tdi)[j]);
+    }

    if (info & JTAG_SEQUENCE_TMS) PIN_SWDIO_TMS_SET();
    else PIN_SWDIO_TMS_CLR();
@ -126,32 +115,17 @@ void PORT_JTAG_SETUP(void) {
    n = info & JTAG_SEQUENCE_TCK;
    if (n == 0) n = 64;

-//    if (info & JTAG_SEQUENCE_TDO) {
-//        printf("%s", "\ntdo: ");
-//        for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
-//            printf("0x%x ", ((const uint8_t*)tdo)[j]);
-//        }
-//        printf("%c", '\n');
-//    } else printf("%s", "\nno tdo\n");
-}*/
-
-void jtag_tms_seq(uint32_t count, const uint8_t* data) {
-    for (size_t i = 0; i < count; ++i) {
-        uint8_t byte = data[i >> 3],
-                bit  = (byte >> (i & 7)) & 1;
-
-        if (bit) PIN_SWDIO_TMS_SET();
-        else PIN_SWDIO_TMS_CLR();
-        PIN_SWCLK_TCK_CLR();
-        PIN_DELAY_SLOW(DAP_Data.clock_delay);
-        PIN_SWCLK_TCK_SET();
-        PIN_DELAY_SLOW(DAP_Data.clock_delay);
-    }
+    if (info & JTAG_SEQUENCE_TDO) {
+        printf("%s", "\ntdo: ");
+        for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
+            printf("0x%x ", ((const uint8_t*)tdo)[j]);
+        }
+        printf("%c", '\n');
+    } else printf("%s", "\nno tdo\n");
 }
 #else

 void PORT_JTAG_SETUP(void) {
-    //printf("jtag setup\n");
    resets_hw->reset &= ~(RESETS_RESET_IO_BANK0_BITS | RESETS_RESET_PADS_BANK0_BITS);

    /* set to default high level */
@ -163,22 +137,17 @@ void PORT_JTAG_SETUP(void) {
    sio_hw->gpio_oe_clr = PINOUT_TDO_MASK;

    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TCK],
-        PADS_BANK0_GPIO0_IE_BITS  // bits to set: input enable
-            | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS         // bits to mask out: input enable, output disable
-            | PADS_BANK0_GPIO0_SLEWFAST_BITS);
-    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TMS],
-        PADS_BANK0_GPIO0_IE_BITS | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS
-            | PADS_BANK0_GPIO0_SLEWFAST_BITS);
-    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TDI],
-        PADS_BANK0_GPIO0_IE_BITS | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS
-            | PADS_BANK0_GPIO0_SLEWFAST_BITS);
+        PADS_BANK0_GPIO0_IE_BITS,  // bits to set: input enable
+        PADS_BANK0_GPIO0_IE_BITS |
+            PADS_BANK0_GPIO0_OD_BITS);  // bits to mask out: input enable, output disable
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TMS], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TDI], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_TDO],
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS  // TDO needs to have its output disabled
-            | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS | PADS_BANK0_GPIO0_SLEWFAST_BITS);
+        PADS_BANK0_GPIO0_IE_BITS |
+            PADS_BANK0_GPIO0_OD_BITS,  // TDO needs to have its output disabled
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_nTRST], PADS_BANK0_GPIO0_IE_BITS,
        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
    hw_write_masked(&padsbank0_hw->io[PINOUT_JTAG_nRESET], PADS_BANK0_GPIO0_IE_BITS,
@ -196,19 +165,23 @@ void PORT_JTAG_SETUP(void) {
    iobank0_hw->io[PINOUT_JTAG_nTRST].ctrl  = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
    iobank0_hw->io[PINOUT_JTAG_nRESET].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;

-    if (jtagsm == -1) jtagsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, false);
+    if (jtagsm == -1) jtagsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, true);
    if (jtagoffset == -1)
        jtagoffset = pio_add_program(PINOUT_JTAG_PIO_DEV, &dap_jtag_program);
    dap_jtag_program_init(PINOUT_JTAG_PIO_DEV, jtagsm, jtagoffset,
             50*1000, PINOUT_JTAG_TCK, PINOUT_JTAG_TDI, PINOUT_JTAG_TDO);
 }

-#define JTAG_SEQUENCE_NO_TMS 0x80000u /* should be large enough */
+static uint8_t bitswap(uint8_t in) {
+    static const uint8_t lut[16] = {
+        0x0, 0x8, 0x4, 0xc, 0x2, 0xa, 0x6, 0xe,
+        0x1, 0x9, 0x5, 0xd, 0x3, 0xb, 0x7, 0xf
+    };
+
+    return (lut[in&0xf] << 4) | lut[in>>4];
+}

 void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
-    //printf("jtag seq\n");
-    //pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, jtagsm, true);
-
    float div = (float)clock_get_hz(clk_sys) / (4 * DAP_Data.clock_freq);
    if (div < 2) div = 2;
    else if (div > 65536) div = 65536;
@ -219,10 +192,8 @@ void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
    uint32_t n = info & JTAG_SEQUENCE_TCK;
    if (n == 0) n = 64;

-    //if (!(n & JTAG_SEQUENCE_NO_TMS)) {
-        if (info & JTAG_SEQUENCE_TMS) PIN_SWDIO_TMS_SET();
-        else PIN_SWDIO_TMS_CLR();
-    //}
+    if (info & JTAG_SEQUENCE_TMS) PIN_SWDIO_TMS_SET();
+    else PIN_SWDIO_TMS_CLR();

    io_wo_8* tx = (io_wo_8*)&PINOUT_JTAG_PIO_DEV->txf[jtagsm];
    io_ro_8* rx = (io_ro_8*)&PINOUT_JTAG_PIO_DEV->rxf[jtagsm];
@ -232,9 +203,9 @@ void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
    //printf("n=%lu bytelen=%lu last_shift=%lu\n", n, bytelen, last_shift);
    uint32_t txremain = bytelen,
             rxremain = last_shift ? bytelen : (bytelen + 1);
-    //printf("txremain=%lu rxremain=%lu\n", txremain, rxremain);
+    /*printf("txremain=%lu rxremain=%lu\n", txremain, rxremain);

-    /*printf("%s", "tdi: ");
+    printf("%s", "tdi: ");
    for (size_t j = 0; j < ((n + 7) >> 3); ++j) {
        printf("0x%x ", ((const uint8_t*)tdi)[j]);
    }
@ -274,54 +245,6 @@ void JTAG_Sequence(uint32_t info, const uint8_t* tdi, uint8_t* tdo) {
        }
        printf("%c", '\n');
    } else printf("%s", "no tdo\n");*/
-
-    //pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, jtagsm, false);
-}
-
-void jtag_tms_seq(uint32_t count, const uint8_t* data) {
-    //printf("jtag tms seq\n");
-    // work around openFPGAloader bug (how did this even get here?)
-    if (DAP_Data.clock_delay == 0) {
-        DAP_Data.clock_delay = 8;
-    }
-
-    /*pio_sm_set_out_pins(PINOUT_JTAG_PIO_DEV, jtagsm, PINOUT_JTAG_TMS, 1);
-    pio_sm_set_set_pins(PINOUT_JTAG_PIO_DEV, jtagsm, PINOUT_JTAG_TMS, 1);
-    pio_sm_set_pins(PINOUT_JTAG_PIO_DEV, jtagsm, gpio_get(PINOUT_JTAG_TMS)?1:0);
-    gpio_set_function(PINOUT_JTAG_TMS, GPIO_FUNC_PIO0 + ((PINOUT_JTAG_PIO_DEV == pio0) ? 0 : 1));
-
-    for (uint32_t i = 0, n; i < count; i += n) {
-        n = count - i;
-        if (n == 0) break;
-        if (n > 64) n = 64;
-        n &= JTAG_SEQUENCE_TCK;
-        JTAG_Sequence(n | JTAG_SEQUENCE_NO_TMS, data, NULL);
-    }
-
-    gpio_put(PINOUT_JTAG_TMS, data[(count >> 3)] & (1 << (count & 7)));
-    gpio_set_function(PINOUT_JTAG_TMS, GPIO_FUNC_SIO);
-    pio_sm_set_out_pins(PINOUT_JTAG_PIO_DEV, jtagsm, PINOUT_JTAG_TDI, 1);
-    pio_sm_set_set_pins(PINOUT_JTAG_PIO_DEV, jtagsm, PINOUT_JTAG_TDI, 1);*/
-
-    // FIXME: above doesn't seem to work somehow -- so fall back to bit-banging
-
-    const uint8_t tdibit = 0xff;
-    PIN_SWCLK_TCK_SET();
-    gpio_set_function(PINOUT_JTAG_TMS, GPIO_FUNC_SIO);
-    gpio_set_function(PINOUT_JTAG_TCK, GPIO_FUNC_SIO);
-    for (size_t i = 0; i < count; ++i) {
-        uint8_t byte = data[i >> 3],
-                bit  = byte & (1 << (i & 7));//(byte >> (i & 7)) & 1;
-
-        //JTAG_Sequence(1 | (bit ? JTAG_SEQUENCE_TMS : 0), &tdibit, NULL);
-        if (bit) PIN_SWDIO_TMS_SET();
-        else PIN_SWDIO_TMS_CLR();
-        PIN_SWCLK_TCK_CLR();
-        PIN_DELAY_SLOW(DAP_Data.clock_delay);
-        PIN_SWCLK_TCK_SET();
-        PIN_DELAY_SLOW(DAP_Data.clock_delay);
-    }
-    gpio_set_function(PINOUT_JTAG_TCK, GPIO_FUNC_PIO0);
 }
 #endif

@ -354,7 +277,6 @@ static void jtag_seq(uint32_t num, int tms, const void* tdi, void* tdo) {
 }

 uint32_t JTAG_ReadIDCode(void) {
-    //printf("jtag readID\n");
    // tdi=NULL: ~~0xff!~~ repeat last-seen bit, ignore otherwise
    // tdo=NULL: ignore
    jtag_seq(1, JTAG_SEQUENCE_TMS, NULL, NULL);
@ -395,7 +317,6 @@ uint32_t JTAG_ReadIDCode(void) {
 }

 void JTAG_IR(uint32_t ir) {
-    //printf("jtag IR\n");
    jtag_seq(2,JTAG_SEQUENCE_TMS, NULL, NULL);
    jtag_seq(2,0, NULL, NULL);
    uint64_t v = ~(uint64_t)0;
@ -497,7 +418,7 @@ static uint8_t xfer_base(uint32_t request, uint32_t* data, bool check_ack) {
 exit:
    jtag_seq(1,JTAG_SEQUENCE_TMS, NULL, NULL);
    jtag_seq(1,0, NULL, NULL);
-    PIN_TDI_OUT(1); // TDI HI (no clk)
+    PIN_TDI_OUT(1); // TODO: TDI HI (no clk)
    if (request & DAP_TRANSFER_TIMESTAMP) DAP_Data.timestamp = TIMESTAMP_GET();
    if (check_ack) jtag_seq(DAP_Data.transfer.idle_cycles, 0, NULL, NULL);
    return (uint8_t)ack;
@ -611,12 +532,10 @@ exit:
 }

 void JTAG_WriteAbort(uint32_t data) {
-    //printf("jtag wrabort\n");
    xfer_base(0 /* write,A2=0,A3=0 */, &data, false);
 }

 uint8_t JTAG_Transfer(uint32_t request, uint32_t* data) {
-    //printf("jtag xfer\n");
    return xfer_base(request, data, true);
 }
 //#endif
--- a/bsp/rp2040/m_default/dap_jtag.pio
+++ b/bsp/rp2040/m_default/dap_jtag.pio
@ -33,7 +33,6 @@ static inline void dap_jtag_program_init(PIO pio, uint sm, uint offset,
        uint16_t freq, uint pin_tck, uint pin_tdi, uint pin_tdo) {
    pio_sm_config c = dap_jtag_program_get_default_config(offset);
    sm_config_set_out_pins(&c, pin_tdi, 1);
-    //sm_config_set_set_pins(&c, pin_tdi, 1);
    sm_config_set_in_pins(&c, pin_tdo);
    sm_config_set_sideset_pins(&c, pin_tck);
    // (shift to left, autopush/pull, threshold=nbits)
--- a/bsp/rp2040/m_default/dap_swd.c
+++ b/bsp/rp2040/m_default/dap_swd.c
@ -8,8 +8,6 @@
 #include "DAP_config.h"
 #include <DAP.h>

-#include "util.h"
-
 #include "dap_swd.pio.h"

 #define SWD_PIO
@ -24,12 +22,10 @@ void PORT_SWD_SETUP(void) {
    sio_hw->gpio_oe_set = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK;
    sio_hw->gpio_set    = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK;

-    hw_write_masked(&padsbank0_hw->io[PINOUT_SWCLK],
-        PADS_BANK0_GPIO0_IE_BITS | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS | PADS_BANK0_GPIO0_SLEWFAST_BITS);
-    hw_write_masked(&padsbank0_hw->io[PINOUT_SWDIO],
-        PADS_BANK0_GPIO0_IE_BITS | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS | PADS_BANK0_GPIO0_SLEWFAST_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_SWCLK], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_SWDIO], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
    iobank0_hw->io[PINOUT_SWCLK].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
    iobank0_hw->io[PINOUT_SWDIO].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
 }
@ -103,23 +99,20 @@ void SWD_Sequence (uint32_t info, const uint8_t *swdo, uint8_t *swdi) {
 #else

 void PORT_SWD_SETUP(void) {
-    //printf("swd setup\n");
    resets_hw->reset &= ~(RESETS_RESET_IO_BANK0_BITS | RESETS_RESET_PADS_BANK0_BITS);

    /* set to default high level */
    sio_hw->gpio_oe_set = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK;
    sio_hw->gpio_set    = PINOUT_SWCLK_MASK | PINOUT_SWDIO_MASK;

-    hw_write_masked(&padsbank0_hw->io[PINOUT_SWCLK],
-        PADS_BANK0_GPIO0_IE_BITS | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS | PADS_BANK0_GPIO0_SLEWFAST_BITS);
-    hw_write_masked(&padsbank0_hw->io[PINOUT_SWDIO],
-        PADS_BANK0_GPIO0_IE_BITS | (GPIO_SLEW_RATE_SLOW << PADS_BANK0_GPIO0_SLEWFAST_LSB),
-        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS | PADS_BANK0_GPIO0_SLEWFAST_BITS);
-    iobank0_hw->io[PINOUT_SWCLK].ctrl = GPIO_FUNC_PIO0 << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
-    iobank0_hw->io[PINOUT_SWDIO].ctrl = GPIO_FUNC_PIO0 << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
+    hw_write_masked(&padsbank0_hw->io[PINOUT_SWCLK], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    hw_write_masked(&padsbank0_hw->io[PINOUT_SWDIO], PADS_BANK0_GPIO0_IE_BITS,
+        PADS_BANK0_GPIO0_IE_BITS | PADS_BANK0_GPIO0_OD_BITS);
+    iobank0_hw->io[PINOUT_SWCLK].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;
+    iobank0_hw->io[PINOUT_SWDIO].ctrl = GPIO_FUNC_SIO << IO_BANK0_GPIO0_CTRL_FUNCSEL_LSB;

-    if (swdsm == -1) swdsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, false);
+    if (swdsm == -1) swdsm = pio_claim_unused_sm(PINOUT_JTAG_PIO_DEV, true);
    if (swdoffset == -1)
        swdoffset = pio_add_program(PINOUT_JTAG_PIO_DEV, &dap_swd_program);
    dap_swd_program_init(PINOUT_JTAG_PIO_DEV, swdsm, swdoffset,
@ -142,8 +135,16 @@ inline static void PIN_SWDIO_SET_PIO(void) {
            (1u << PINOUT_SWDIO), (1u << PINOUT_SWDIO));
 }

+static uint8_t bitswap(uint8_t in) {
+    static const uint8_t lut[16] = {
+        0x0, 0x8, 0x4, 0xc, 0x2, 0xa, 0x6, 0xe,
+        0x1, 0x9, 0x5, 0xd, 0x3, 0xb, 0x7, 0xf
+    };
+
+    return (lut[in&0xf] << 4) | lut[in>>4];
+}
+
 void SWD_Sequence(uint32_t info, const uint8_t* swdo, uint8_t* swdi) {
-    //printf("swd sequence\n");
    pio_sm_set_enabled(PINOUT_JTAG_PIO_DEV, swdsm, true);
    //busy_wait_us_32(0);

@ -215,8 +216,7 @@ void SWD_Sequence(uint32_t info, const uint8_t* swdo, uint8_t* swdi) {
 }
 #endif

-static void swd_seq(uint32_t count, uint32_t flags, const uint8_t* swdo, uint8_t* swdi) {
-    //printf("swd seqbase count=%lu\n", count);
+void swd_seq(uint32_t count, uint32_t flags, const uint8_t* swdo, uint8_t* swdi) {
    static uint64_t last_bit = ~(uint64_t)0;
    uint64_t devnull = 0;

@ -252,38 +252,24 @@ static void swd_seq(uint32_t count, uint32_t flags, const uint8_t* swdo, uint8_t
        uint8_t lastbyte = swdo[((count + 7) >> 3) - 1];
        last_bit = (lastbyte & (1 << (count & 7))) ? ~(uint64_t)0 : (uint64_t)0;
    }
-
-    //printf("swd seqbase end\n");
 }

-void jtag_tms_seq(uint32_t count, const uint8_t* data);
-
 void SWJ_Sequence(uint32_t count, const uint8_t* data) {
-    //printf("swj sequence\n");
+    swd_seq(count, 0, data, NULL);

-    // we can't just do a swd_seq() call here, as the debugger might be in JTAG
-    // instead of SWD mode.
+    /*for (uint32_t i = 0, k = 0; i < count; ++i) {
+        if ((i & 7) == 0) {
+            val = data[k];
+            ++k;
+        }

-    if ((swdsm == -1 || swdoffset == -1) && jtagsm >= 0 && jtagoffset >= 0) {
-        jtag_tms_seq(count, data); // JTAG mode -- handle in JTAG code
-    } else if (swdsm >= 0 && swdoffset >= 0) {
-        swd_seq(count, 0, data, NULL); // SWD mode - we can do just this
-    } else {
-        //printf("E: SWJ_Sequence while not in JTAG or SBW mode\n");
-        // welp - can't really report an error to the upper CMSIS-DAP layers
-
-        jtag_tms_seq(count, data); // uses only SIO for now so ¯\_(ツ)_/¯
-    }
-
-    // hackier but stabler
-    /*jtag_tms_seq(count, data);
-    if (swdsm != -1 && swdoffset != -1 && jtagsm == -1 && jtagoffset == -1) {
-        gpio_set_function(PINOUT_JTAG_TMS, GPIO_FUNC_PIO0);
+        swdio = (val >> (i & 7)) & 1;
+        // SET SWDIO
+        // SWCLK LO; DELAY; SWCLK HI; DELAY
    }*/
 }

 uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {
-    //printf("swd xfer request=%08lx\n", request);
    uint32_t parity;
    uint8_t swdo;

@ -296,13 +282,11 @@ uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {

    uint8_t ack = 0;
    swd_seq(3, SWD_SEQUENCE_DIN, NULL, &ack);
-    //printf("  ack=%hhu\n", ack);

    uint32_t num;
    switch (ack) {
    case DAP_TRANSFER_OK:
        if (request & DAP_TRANSFER_RnW) {
-            //printf("  xfer ok, r\n");
            uint64_t val = 0;
            parity = 0;
            // FIXME: this is little-endian-only!
@ -318,7 +302,6 @@ uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {

            //PIN_SWDIO_OUT_ENABLE();
        } else { // write
-            //printf("  xfer ok, w\n");
            swd_seq(DAP_Data.swd_conf.turnaround, SWD_SEQUENCE_DIN, NULL, NULL);

            //PIN_SWDIO_OUT_ENABLE();
@ -333,45 +316,36 @@ uint8_t SWD_Transfer(uint32_t request, uint32_t* data) {
            swd_seq(33, 0, (const uint8_t*)&out, NULL);
        }

-        //printf("  set ts\n");
        if (request & DAP_TRANSFER_TIMESTAMP) DAP_Data.timestamp = TIMESTAMP_GET();

        num = DAP_Data.transfer.idle_cycles;
-        //printf("  idlecyc=%lu\n", num);
-        for (uint32_t i = 0; i < num; i += 64) {
+        for (uint32_t i = 0; i < num; num += 64) {
            uint64_t swdio = 0;

            uint32_t cyc = num - i;
            if (cyc > 64) cyc = 64;

-            //printf("  sequence of %lu\n", cyc);
            SWD_Sequence((cyc & SWD_SEQUENCE_CLK), (const uint8_t*)&swdio, NULL);
        }
-        //printf("  idlecyc end\n");
        break;
    case DAP_TRANSFER_WAIT: case DAP_TRANSFER_FAULT:
        num = DAP_Data.swd_conf.turnaround;
        if (DAP_Data.swd_conf.data_phase &&  (request & DAP_TRANSFER_RnW)) {
            num += 33; // 32 bits + parity
        }
-        //printf("  wait/fault: %lu\n", num);

        swd_seq(num, SWD_SEQUENCE_DIN, NULL, NULL);

        if (DAP_Data.swd_conf.data_phase && !(request & DAP_TRANSFER_RnW)) {
-            //printf("  w/f dataphase\n");
-
            uint64_t swdio = 0;
            swd_seq(33, 0, (const uint8_t*)&swdio, NULL); // 32 data bits + parity
        }
        break;
    default: // protocol error
-        //printf("  proto error\n");
        swd_seq(DAP_Data.swd_conf.turnaround + 33, SWD_SEQUENCE_DIN, NULL, NULL);
        break;
    }

-    //printf("  finished\n");
    PIN_SWDIO_OUT_ENABLE();
    PIN_SWDIO_SET_PIO();
    return ack;
--- a/bsp/rp2040/m_default/pinout.h
+++ b/bsp/rp2040/m_default/pinout.h
@ -15,11 +15,11 @@
 #define PINOUT_JTAG_TCK    2  // == SWCLK
 #define PINOUT_JTAG_TMS    3  // == SWDIO
 #define PINOUT_JTAG_TDI    4
-#define PINOUT_JTAG_TDO    5  // == SWO
+#define PINOUT_JTAG_TDO    5
 #define PINOUT_JTAG_nTRST  6
 #define PINOUT_JTAG_nRESET 7
 #define PINOUT_JTAG_PIO_DEV pio0
-#define PINOUT_JTAG_SWO_DEV pio0
+#define PINOUT_JTAG_SWO_DEV pio1

 // SPI config
 #define PINOUT_SPI_DEV  spi1
@ -62,17 +62,15 @@
 *   SWO-MC	1
 *
 * PIO:
- *   PIO0: (max. 4 SM, max. 32 insn)
- *     JTAG	1	6
- *     SWD	1	11
- *     SWO	2	6 (manchester) + 9 (uart)
+ *   PIO0: (max. 4)
+ *     JTAG	1
+ *     SWD	1
+ *   PIO1: (max. 4)
+ *     SWO	2
 *
- *     PIO0 IS NOW FULL!
- *   PIO1: (max. 4 SM, max. 32 insn)
+ * UART0: stdio
 *
- * UART: stdio
- *   0: stdio
- *   1: USB-CDC/DAP-UART
+ * UART1: USB-CDC/DAP-UART
 *
 * SPI:
 *   1: SPI access
@ -81,7 +79,7 @@
 *   0: I2C access
 *
 * ADC:
- *   temperature sensor
+ *
 */

 #endif
--- a/bsp/rp2040/m_isp/bsp-feature.h
+++ b/bsp/rp2040/m_isp/bsp-feature.h
@ -1,35 +0,0 @@
-
-#ifndef BSP_FEATURE_M_ISP_H_
-#define BSP_FEATURE_M_ISP_H_
-
-#define DBOARD_HAS_UART
-#define DBOARD_HAS_CMSISDAP
-#define DBOARD_HAS_MEHFET
-
-#include "bsp-info.h"
-
-enum {
-    HID_N_CMSISDAP = 0,
-
-    HID_N__NITF
-};
-enum {
-    CDC_N_UART = 0,
-#ifdef USE_USBCDC_FOR_STDIO
-    CDC_N_STDIO,
-#endif
-
-    CDC_N__NITF
-};
-enum {
-    VND_N_CMSISDAP = 0,
-#if CFG_TUD_VENDOR > 0
-    VND_N_CFG,
-#endif
-    VND_N_MEHFET,
-
-    VND_N__NITF
-};
-
-#endif
-
--- a/bsp/rp2040/m_isp/mehfet_hw.c
+++ b/bsp/rp2040/m_isp/mehfet_hw.c
@ -1,205 +0,0 @@
-// vim: set et:
-
-#include <hardware/gpio.h>
-#include <hardware/timer.h>
-#include <pico/time.h>
-
-#include "util.h"
-#include "m_isp/pinout.h"
-#include "m_isp/sbw_hw.h"
-
-#include "m_isp/mehfet.h"
-
-void mehfet_hw_delay_ms(uint32_t t) { busy_wait_ms   (t); }
-void mehfet_hw_delay_us(uint32_t t) { busy_wait_us_32(t); }
-
-static absolute_time_t target;
-void mehfet_hw_timer_start(bool us, uint32_t to_reach) {
-    target = us ? make_timeout_time_us(to_reach) : make_timeout_time_ms(to_reach);
-}
-bool mehfet_hw_timer_reached(void) { return time_reached(target); }
-
-
-void mehfet_hw_init(void) {
-    // don't init things just yet: PIO SM1 probably needs to be shared between
-    // multiple ISP/ICE/... protocols, so we only init stuff once actually starting
-
-    // TODO: init pin gpio mux stuff (to GPIO and not PIO first!)
-    //    ^: or not: this will probably keep the target into reset, maybe not
-    //       desired...
-}
-void mehfet_hw_deinit(void) {
-    // shrug
-    sbw_deinit(); // can't hurt
-
-    gpio_set_function(PINOUT_SBW_TCK , GPIO_FUNC_NULL);
-    gpio_set_function(PINOUT_SBW_TDIO, GPIO_FUNC_NULL);
-}
-
-__attribute__((__const__))
-enum mehfet_caps mehfet_hw_get_caps(void) {
-    // only support SBW for now. we could add JTAG, but this will probably
-    // become messy very quickly, as it's behind CMSIS-DAP. implement later
-    // if someone needs it.
-    return mehfet_cap_sbw_entryseq |
-        mehfet_cap_has_reset_tap | mehfet_cap_has_irshift | mehfet_cap_has_drshift;
-}
-
-const char* /*error string, NULL if no error*/ mehfet_hw_connect(enum mehfet_conn conn) {
-    sbw_preinit(conn & mehfet_conn_nrstmask);
-
-    if (!sbw_init()) {
-        mehfet_hw_deinit();
-        return "SBW PIO init failed";
-    }
-
-    return NULL;
-}
-void mehfet_hw_disconnect(void) {
-    sbw_deinit();
-}
-
-void mehfet_hw_set_clkspeed(bool fast) {
-    if (fast) {
-        sbw_set_freq(true, 350e3);
-    } else {
-        sbw_set_freq(false, 50e3);
-    }
-}
-uint8_t mehfet_hw_get_old_lines(void) {
-    return (sbw_get_last_tclk() ? 1 : 0)
-         | (sbw_get_last_tms () ? 2 : 0)
-         | (sbw_get_last_tdi () ? 4 : 0);
-}
-
-void mehfet_hw_tdio_seq(uint32_t ncyc, bool tmslvl, const uint8_t* tdi, uint8_t* tdo) {
-    sbw_sequence(ncyc, tmslvl, tdi, tdo);
-}
-void mehfet_hw_tms_seq(uint32_t ncyc, bool tdilvl, const uint8_t* tms) {
-    sbw_tms_sequence(ncyc, tdilvl, tms);
-}
-void mehfet_hw_tclk_edge(bool newtclk) {
-    sbw_clrset_tclk(newtclk);
-}
-void mehfet_hw_tclk_burst(uint32_t ncyc) {
-    sbw_tclk_burst(ncyc);
-}
-
-enum mehfet_resettap_status mehfet_hw_reset_tap(enum mehfet_resettap_flags flags) {
-    enum mehfet_resettap_status rv = 0;
-
-    if (flags & mehfet_rsttap_do_reset) {
-        // TDI always 1
-        // TMS=1,1,1,1,1,1    -- reset TAP state to initial
-        // TMS=0              -- test-logic-reset to run-test/idle
-        // TMS=1,0,1,0,1      -- perform fuse check
-        // TMS=1,0            -- back to run-test/idle (needed for SBW only)
-
-        //const uint16_t tms_seq = 0x1abf;//0x3f | (0<<6) | (0x15 << 7) | (0x1 << 12);
-        const uint8_t tms_seq[2] = {0xbf,0x1a};
-        sbw_tms_sequence(14, true, tms_seq);
-    }
-
-    if (flags & mehfet_rsttap_fuse_do) {
-        // TDI always 1
-        // TMS=01010110 // same sequence as above, but without TAP reset
-        const uint8_t tms_seq = 0x6a;
-        sbw_tms_sequence(8, true, &tms_seq);
-    }
-
-    if (flags & mehfet_rsttap_fuse_read) {
-        for (size_t i = 0; i < 3; ++i) {
-            mehfet_hw_shift_ir(0x14); // CNTRL_SIG_CAPTURE
-            uint16_t dr = mehfet_hw_shift_dr16(0xaaaa);
-            if (dr == 0x5555) {
-                rv |= mehfet_rsttap_fuse_blown;
-                break;
-            }
-        }
-    }
-
-    return rv;
-}
-
-uint8_t mehfet_hw_shift_ir(uint8_t newir) {
-    // 1100: run-test/idle -> select-dr-scan -> select-ir-scan -> capture-ir -> shift-ir
-    const uint8_t tms_seqa = 0x03;
-    const uint8_t tms_seqb = 0x03 >> 1;
-    sbw_tms_sequence(1, sbw_get_last_tclk(), &tms_seqa);
-    sbw_tms_sequence(3, true, &tms_seqb);
-
-    // 7 data bits with TMS=0
-    // 1 data bit with TMS=1 (to exit1-ir)
-    uint8_t res = 0, resb = 0, newir2 = newir >> 7;
-    sbw_sequence(7, false, &newir , &res );
-    sbw_sequence(1, true , &newir2, &resb);
-    res |= resb << 7;
-
-    // TMS=1 (to update-ir)
-    // TMS=0 (to run-test/idle)
-    const uint8_t tms_seq_2a = 0x01;
-    const uint8_t tms_seq_2b = 0x01 >> 1;
-    sbw_tms_sequence(1, true, &tms_seq_2a);
-    sbw_tms_sequence(1, sbw_get_last_tclk(), &tms_seq_2b);
-
-    return bitswap(res); // fsr also needed here
-}
-
-static void bitswap_n(uint32_t nbytes, uint8_t* data) {
-    for (uint32_t i = 0, j = nbytes - 1; i < nbytes; ++i, --j) {
-        if (i == j) data[i] = bitswap(data[i]);
-        else {
-            uint8_t tmp = bitswap(data[i]);
-            data[i] = bitswap(data[j]);
-            data[j] = tmp;
-        }
-    }
-}
-
-void mehfet_hw_shift_dr(uint32_t nbits, uint8_t* drin, uint8_t* drout) {
-    // 100: run-test/idle -> select-dr-scan -> capture-dr -> shift-dr
-    const uint8_t tms_seqa = 0x01;
-    const uint8_t tms_seqb = 0x01 >> 1;
-    sbw_tms_sequence(1, sbw_get_last_tclk(), &tms_seqa);
-    sbw_tms_sequence(2, true, &tms_seqb);
-
-    if (nbits == 16) { // fast path: DR is often 16 bits wide
-        // DR content is MSB-first instead of LSB-first (IR is the latter)
-        uint16_t newdr = bitswap(drin[1]) | ((uint16_t)bitswap(drin[0]) << 8);
-
-        // 15 data bits with TMS=0
-        // 1 data bit with TMS=1 (to exit1-dr)
-        uint16_t res = 0;
-        uint8_t newdr2 = newdr >> 15, resb = 0;
-        // this is little-endian-only, but that's fine on the rp2040
-        sbw_sequence(15, false, (const uint8_t*)&newdr, (uint8_t*)&res);
-        sbw_sequence( 1, true , &newdr2, &resb);
-        res |= (uint16_t)resb << 15;
-
-        drout[0] = bitswap(res >> 8);
-        drout[1] = bitswap(res & 0xff);
-    } else {
-        uint32_t nbytes = (nbits + 7) >> 3;
-
-        // DR content is MSB-first instead of LSB-first (IR is the latter)
-        bitswap_n(nbytes, drin);
-
-        // n-1 data bits with TMS=0
-        // 1 data bit with TMS=1 (to exit1-dr)
-        uint8_t newdr2, resb = 0;
-        newdr2 = drin[nbytes - 1] >> ((nbits - 1) & 7);
-        sbw_sequence(nbits - 1, false, drin, drout);
-        sbw_sequence(1, true , &newdr2, &resb);
-        drout[nbytes - 1] |= resb << ((nbits - 1) & 7);
-
-        bitswap_n(nbytes, drout);
-    }
-
-    // TMS=1 (to update-dr)
-    // TMS=0 (to run-test/idle)
-    const uint8_t tms_seq_2a = 0x01;
-    const uint8_t tms_seq_2b = 0x01 >> 1;
-    sbw_tms_sequence(1, true, &tms_seq_2a);
-    sbw_tms_sequence(1, sbw_get_last_tclk(), &tms_seq_2b);
-}
-
--- a/bsp/rp2040/m_isp/pinout.h
+++ b/bsp/rp2040/m_isp/pinout.h
@ -1,83 +0,0 @@
-// vim: set et ts=8:
-
-#ifndef PINOUT_H_
-#define PINOUT_H_
-
-// NOTE NOTE NOTE: as mode2 shares some stuff with mode1 (CMSIS-DAP, UART),
-//                 make sure that pinouts are compatible etc.
-
-// UART config
-#define PINOUT_UART_TX         8
-#define PINOUT_UART_RX         9
-/*#define PINOUT_UART_CTS       10
-#define PINOUT_UART_RTS       11*/
-#define PINOUT_UART_INTERFACE uart1
-#define PINOUT_UART_BAUDRATE  115200
-
-// JTAG config
-#define PINOUT_JTAG_TCK    2  // == SWCLK
-#define PINOUT_JTAG_TMS    3  // == SWDIO
-#define PINOUT_JTAG_TDI    4
-#define PINOUT_JTAG_TDO    5
-#define PINOUT_JTAG_nTRST  6
-#define PINOUT_JTAG_nRESET 7
-#define PINOUT_JTAG_PIO_DEV pio0
-#define PINOUT_JTAG_SWO_DEV pio0
-
-// SBW config
-#define PINOUT_SBW_PIO pio1
-#define PINOUT_SBW_TCK  10
-#define PINOUT_SBW_TDIO 11
-
-// LED config
-
-// you can change these two as you like
-#define PINOUT_LED_CONNECTED 1
-#define PINOUT_LED_RUNNING   0
-
-#ifndef PINOUT_LED
-#ifndef PICO_DEFAULT_LED_PIN
-#error "PICO_DEFAULT_LED_PIN is not defined, run PICOPROBE_LED=<led_pin> cmake"
-#elif PICO_DEFAULT_LED_PIN == -1
-#error "PICO_DEFAULT_LED_PIN is defined as -1, run PICOPROBE_LED=<led_pin> cmake"
-#else
-#define PINOUT_LED PICO_DEFAULT_LED_PIN
-#endif
-#endif /* PICOPROBE_LED */
-
-/*
- * HARDWARE RESOURCE USAGE:
- *
- * IRQ:
- *   DMA0	DAP-UART
- *   UART1	DAP-UART
- *
- * DMA: (max. 12)
- *   DAP-UART	2
- *   SWO-UART	1
- *   SWO-MC	1
- *
- * PIO:
- *   PIO0: (max. 4 SM, max. 32 insn)
- *     JTAG	1	6
- *     SWD	1	11
- *     SWO	2	6 (manchester) + 9 (uart)
- *
- *     PIO0 IS NOW FULL!
- *   PIO1: (max. 4 SM, max. 32 insn)
- *     SBW	1	32
- *
- * UART: stdio
- *   0: stdio
- *   1: USB-CDC/DAP-UART
- *
- * SPI:
- *
- * I2C:
- *
- * ADC:
- *
- */
-
-#endif
-
--- a/bsp/rp2040/m_isp/sbw.pio
+++ b/bsp/rp2040/m_isp/sbw.pio
@ -1,206 +0,0 @@
-
-.program sbw
-.side_set 1
-
-; Pin assignments:
-; - SBWTCK is side-set pin 0
-; - SBWTDIO is OUT/IN pin 0
-;
-; Autopush and autopull must be enabled, set to 8
-; SBWTDIO input should not be guarded with sync flipflops, as TDO xfers are synchronous
-
-PUBLIC start:
-    ; SBWTCK hi to not lose/reset debug mode
-    pull                        side 1 ; clear leftover OSR bits, pull in new data
-startloop:
-    out exec, 16                side 1 ; use for set y, 0/1 ; in x, num ; jmp addr
-    jmp startloop               side 1
-
-; "subroutine" "calling convention"
-; * set y, 0/1 : initial TMS (sbw_seq) / TDI (sbw_tms_seq) / TCLK (sbw_tclk_burst) value
-; * in  x, num : number of JTAG cycles (sbw_seq/sbw_tms_seq) / TCLK half-cycles (sbw_tclk_burst)
-; * jmp subroutine
-; * ^ all 'side 1'
-
-; y: static TMS value to use
-; x: number of JTAG clock cycles minus one
-; TDI output gets sourced bit by bit from the TX FIFO
-; TDO input gets sent bit by bit to the RX FIFO
-PUBLIC sbw_seq:
-sbw_seq_iter:
-    ; tms slot:
-        set pindirs, 1          side 1      ; SBWTDIO is now output
-        mov pins,    y          side 1      ; output static TMS value
-        nop                     side 0 [1]  ; target reads TMS at falling edge
-    ; tdi slot:
-        out pins, 1             side 1 [1]  ; output TDI from FIFO
-        nop                     side 0 [1]  ; target reads TDI at falling edge
-    ; tdo slot:
-        set pindirs, 0          side 1 [1]  ; we need some clock pulse
-        nop                     side 0      ; give target some time to drive IO
-        in pins, 1              side 0      ; input TDO
-
-        jmp x--, sbw_seq_iter   side 1      ; also gives target some time to stop driving IO
-        ; NOTE: there's a glitch: if TMS=TDO!=TDI and !=TDI_next,
-        ;       then there's a short glitch (wrong voltage level) in the
-        ;       TDO->TMS high clock phase. it's benign.
-
-    push                        side 1      ; flush ISR
-    jmp start                   side 1
-
-
-; y: static TDI value to use
-; x: number of JTAG clock cycles minus one
-; TMS output gets sourced bit by bit from the TX FIFO
-PUBLIC sbw_tms_seq:
-    ; tms slot
-        set pindirs, 1          side 1      ; SBWTDIO is now output
-        out pins, 1             side 1      ; output TMS from FIFO
-        nop                     side 0 [1]  ; target reads TMS at falling edge
-    ; tdi slot
-        mov pins, y             side 1 [1]  ; output static TDI value
-        nop                     side 0 [1]  ; target reads TDI at falling edge
-    ; tdo slot
-        set pindirs, 0          side 1 [1]
-        jmp x--, sbw_tms_seq    side 0 [1]  ; ignore returned TDO, go back
-
-    jmp start                   side 1
-
-
-; stationary SBWTCK values are:
-;             jmp
-; TMS TDI TDO |TMS TDI TDO
-; |   |   |   ||   |   |
-; 1100110011001110011001100 ...
-;
-; a full cycle takes 13 cycles, a single slot 4
-; the SM divider should be sysclk/(4*baudrate)
-; baudrate shouldn't exceed 20 MHz (typical value is 18 MHz?)
-; SBWTCK LOW phases shouldn't exceed 7us, so the baudrate should be at least 40 kHz
-
-; y: initial/previous TCLK value
-; x: number of TCLK *half*-cycles minus one!
-; TCLK values get sourced from the TX FIFO. best is to use a DMA with fixed
-;      source address, with value 0x55/0xaa (depending on y) for strobes, or
-;      0x00/0xff (and x=0) for a single set/clear.
-; alternatively, one could set the "load-bearing instruction" to a
-; "set pins, 0/1 side 1 [12]" for a fixed value useful for single sets/clears
-PUBLIC sbw_tclk_burst:
-        set pindirs, 1          side 1     ; SBWTDIO is now output
-    ; tms slot:
-        set pins,   0           side 1     ; stay in run-test/idle TAP state
-        nop                     side 0     ; target reads TMS at falling edge
-        mov pins,   y           side 0     ; during low phase, prepare TCLK
-    ; tdi slot:
-        nop                     side 1 [6] ; wait a bit
-    burst_loop:
-;PUBLIC sbw_tclk_burst_loadbearing_insn:
-        out pins, 1             side 1 [6] ; in the middle of TDI hiphase: do TCLK
-        jmp x--, burst_loop     side 1 [6]
-        nop                     side 0 [1] ; need a low clock edge for TDO
-    ; tdo slot:
-        set pindirs, 0          side 1 [1]
-        nop                     side 0 [1]
-
-   ;jmp start                   side 1 ; not needed because of wrapping
-; 32 insns -- filling one entire PIO instruction memory
-
-; TODO: update this paragraph
-; a full TCLK cycle in this burst mode takes 24 PIOSM cycles. at a "standard"
-; baudrate of 18 MHz, this ends up being 375 kHz, which is in the required
-; range when doing eg. flash programming. at the max baudrate (20 MHz), the
-; TCLK speed is 417 kHz, which is still ok. max TCLK would be, if PIOSM freq is
-; 125 MHz, 2.6 MHz, which is good enough imo
-; TODO: determine minimum baudrate for stable TCLK (12 MHz?)
-
-
-% c-sdk {
-static inline void sbw_pio_init(PIO pio, uint sm, uint prog_offs,
-        float freq, uint pin_sbwclk, uint pin_sbwio) {
-    if (freq < 72e3) freq = 72e3;
-    if (freq > 20e6) freq = 20e6;
-
-    pio_sm_set_enabled(pio, sm, false);
-
-    pio_sm_config c = sbw_program_get_default_config(prog_offs);
-    sm_config_set_out_pins(&c, pin_sbwio, 1);
-    sm_config_set_set_pins(&c, pin_sbwio, 1);
-    sm_config_set_in_pins(&c, pin_sbwio);
-    sm_config_set_sideset_pins(&c, pin_sbwclk);
-    sm_config_set_out_shift(&c, false, true, 8);
-    sm_config_set_in_shift(&c, false, true, 8);
-    sm_config_set_clkdiv(&c, (float)clock_get_hz(clk_sys) / (4 * freq));
-    pio_sm_init(pio, sm, prog_offs, &c);
-
-    // SBWTDIO is low, SBWTCK is high, SBWTDIO is input by default
-    pio_sm_set_pindirs_with_mask(pio, sm,
-            (1u << pin_sbwclk) | (1u << pin_sbwio), (1u << pin_sbwclk) | (1u << pin_sbwio));
-
-    // need to do an extra set with sideset to make sure clk stays high and
-    // there's no glitch
-    pio_sm_exec(pio, sm, pio_encode_set(pio_pins, 1) | pio_encode_sideset(1, 1));
-
-    // SBW is mostly synchronous, bypass input synchroniser to reduce delay
-    hw_set_bits(&pio->input_sync_bypass, 1u << pin_sbwio);
-
-    gpio_set_pulls(pin_sbwio, false, true); // SBWTDIO is pulldn
-
-    pio_sm_set_enabled(pio, sm, true);
-
-    // set padsbank func to PIO *after* initing PIO, otherwise a glitch occurs
-    pio_gpio_init(pio, pin_sbwclk);
-    pio_gpio_init(pio, pin_sbwio );
-}
-
-// meant for sbw_seq/sbw_tms_seq
-static inline void sbw_pio_set_baudrate(PIO pio, uint sm, float freq) {
-    if (freq < 72e3) freq = 72e3;
-    if (freq > 20e6) freq = 20e6;
-
-    pio_sm_set_clkdiv(pio, sm, (float)clock_get_hz(clk_sys) / (4 * freq));
-}
-
-// meant for sbw_tclk_burst
-static inline void sbw_pio_set_tclkfreq(PIO pio, uint sm, float freq) {
-    if (freq < 250e3) freq = 250e3;
-    if (freq > 450e3) freq = 450e3;
-
-    pio_sm_set_clkdiv(pio, sm, (float)clock_get_hz(clk_sys) / (24 * freq));
-}
-
-static inline uint16_t sbw_pio_gen_setx(uint x) {
-    return pio_encode_set(pio_x, x) | pio_encode_sideset(1, 1) | (1<<12);
-}
-static inline uint16_t sbw_pio_gen_sety(uint y) {
-    return pio_encode_set(pio_y, y) | pio_encode_sideset(1, 1) | (1<<12);
-}
-static inline uint16_t sbw_pio_gen_outx(uint bits) {
-    return pio_encode_out(pio_x, bits) | pio_encode_sideset(1, 1) | (1<<12);
-}
-// subroutine is one of "sbw_offset_sbw_seq", "sbw_offset_sbw_tms_seq", "sbw_offset_sbw_tclk_burst"
-static inline uint16_t sbw_pio_gen_jmp(uint subroutine) {
-    return pio_encode_jmp(subroutine) | pio_encode_sideset(1, 1) | (1<<12);
-}
-
-/*static inline uint16_t sbw_pio_loadbearing_gen_outpins(void) {
-    return pio_encode_out(pio_pins, 1) | pio_encode_sideset(1, 1) | pio_encode_delay(12) | (1<<12);
-}
-static inline uint16_t sbw_pio_loadbearing_gen_setpins(uint value) {
-    return pio_encode_set(pio_pins, value) | pio_encode_sideset(1, 1) | pio_encode_delay(12) | (1<<12);
-}
-
-static inline void sbw_pio_loadbearing_set_outpins(PIO pio) {
-    pio->instr_mem[sbw_offset_sbw_tclk_burst_loadbearing_insn] =
-        sbw_pio_loadbearing_gen_outpins();
-}
-static inline void sbw_pio_loadbearing_set_setpins(PIO pio, uint value) {
-    pio->instr_mem[sbw_offset_sbw_tclk_burst_loadbearing_insn] =
-        sbw_pio_loadbearing_gen_setpins(value);
-}*/
-
-/*static inline bool sbw_pio_is_idle(PIO pio, uint sm, uint offset) {
-    return pio_sm_get_pc(pio, sm) < sbw_offset_sbw_seq + offset;
-}*/
-
-%}
-
--- a/bsp/rp2040/m_isp/sbw_hw.c
+++ b/bsp/rp2040/m_isp/sbw_hw.c
@ -1,335 +0,0 @@
-// vim: set et:
-
-#include <stdio.h>
-
-#include <hardware/clocks.h>
-#include <hardware/dma.h>
-#include <hardware/gpio.h>
-#include <hardware/pio.h>
-#include <hardware/pio_instructions.h>
-#include <hardware/timer.h>
-
-#include "m_isp/pinout.h"
-#include "m_isp/sbw_hw.h"
-
-#include "sbw.pio.h"
-
-void sbw_preinit(bool nrst) {
-    //SLAU320AJ 2.3.1.1 ; SLAS722G p36:
-    // TEST/SBWTCK low for >100 us: reset debug state
-    // set nRST/NMI/SBWTDIO low: avoid sending an NMI when the debugger detaches
-    // TEST/SBWTCK high for >1 us : signal that we want a debugger
-    // nRST/NMI/SBWTDIO high: we want SBW
-    // TEST low for >0.025us <7us: latch on "we want SBW" signal
-    // TEST high again for >1 us: ready to SBW
-
-    // old impl, doesn't work, don't use
-    /*// TCK, TDIO now low
-    busy_wait_ms(4);//busy_wait_us_32(150); // reset debug state while keeping CPU in reset
-
-    gpio_put(PINOUT_SBW_TCK , true ); // we want a debugger
-    busy_wait_us_32(1);
-    gpio_put(PINOUT_SBW_TDIO, true ); // we want SBW
-    busy_wait_ms(20);//busy_wait_us_32(1);
-    gpio_put(PINOUT_SBW_TCK , false); // latch "we want SBW"
-    busy_wait_us_32(3);
-    gpio_put(PINOUT_SBW_TCK , true ); // start SBW stuff
-    busy_wait_ms(5);//busy_wait_us_32(100); // wait a bit more*/
-
-#if 1
-    (void)nrst; // always assumed nrst=false here :/
-    // from slau320 sources
-    //gpio_put(PINOUT_SBW_TCK , false);
-    gpio_put(PINOUT_SBW_TDIO, true ); // FIXME: ummmm TCK ???
-    gpio_put(PINOUT_SBW_TDIO, false);
-    gpio_set_dir(PINOUT_SBW_TCK , true);
-    gpio_set_dir(PINOUT_SBW_TDIO, true);
-    gpio_set_function(PINOUT_SBW_TCK , GPIO_FUNC_SIO);
-    gpio_set_function(PINOUT_SBW_TDIO, GPIO_FUNC_SIO);
-
-    gpio_put(PINOUT_SBW_TCK , false);
-    gpio_put(PINOUT_SBW_TDIO, true);
-    busy_wait_ms(4); // reset TEST logic
-    gpio_put(PINOUT_SBW_TDIO, true);
-    busy_wait_us_32(1);
-    gpio_put(PINOUT_SBW_TCK , true);
-    busy_wait_ms(20); // activate TEST logic
-
-    // "phase 1"
-    gpio_put(PINOUT_SBW_TDIO, true);
-    busy_wait_us_32(60);
-
-    // "phase 2"
-    gpio_put(PINOUT_SBW_TCK , false);
-
-    // "phase 3"
-    busy_wait_us_32(1);
-
-    // "phase 4"
-    gpio_put(PINOUT_SBW_TCK , true);
-    busy_wait_us_32(60);
-
-    // "phase 5"
-    busy_wait_ms(5);
-
-
-    //// new impl:
-#else
-    // from MSP430.DLL 'BIOS' (FETUIF?) sources
-    // can handle SBW/JTAG selection and nRST stuff
-    // ... but it doesn't seem to work
-
-    // TEST = TCK
-    // nRESET = TDIO = NMI
-    gpio_put(PINOUT_SBW_TCK , true/*false*/); // tck = test
-    gpio_put(PINOUT_SBW_TDIO, nrst/*true*/);
-    gpio_set_dir(PINOUT_SBW_TCK , true);
-    gpio_set_dir(PINOUT_SBW_TDIO, true);
-    gpio_set_function(PINOUT_SBW_TCK , GPIO_FUNC_SIO);
-    gpio_set_function(PINOUT_SBW_TDIO, GPIO_FUNC_SIO);
-    busy_wait_ms(4/*1*/); // 4?
-
-    gpio_put(PINOUT_SBW_TDIO, nrst);
-    busy_wait_us_32(1);
-    gpio_put(PINOUT_SBW_TCK , true);
-    // activate test logic
-    busy_wait_ms(20/*100*/); // 20 should be ok here I think?
-
-    // "phase 1"
-    gpio_put(PINOUT_SBW_TDIO, true); // false here if you want JTAG
-    busy_wait_us_32(40); // 60?
-
-    // "phase 2"
-    gpio_put(PINOUT_SBW_TCK, false); // ??? // true for JTAG?
-
-    // "phase 3"
-    // something (TDIO hi?) to do if RSTLOW & JTAG?
-    busy_wait_us_32(1);
-
-    // "phase 4"
-    gpio_put(PINOUT_SBW_TCK , true); // ??? // false for JTAG?
-    busy_wait_us_32(40/*60*/); // 40 should be ok here I think?
-
-    // phase 5
-    // something (TDIO hi?) to do if RSTHIGH & JTAG?
-    busy_wait_ms(5);
-#endif
-}
-
-static int sbw_piosm = -1, sbw_offset = -1;
-
-static bool last_tclk = true;
-static uint8_t last_tdi = 0xff, last_tms = 0xff;
-
-bool sbw_init(void) {
-    if (sbw_piosm >= 0 || sbw_offset >= 0) return false;
-
-    if (!pio_can_add_program(PINOUT_SBW_PIO, &sbw_program)) return false;
-    sbw_offset = pio_add_program(PINOUT_SBW_PIO, &sbw_program);
-
-    sbw_piosm = pio_claim_unused_sm(PINOUT_SBW_PIO, false);
-    if (sbw_piosm < 0) {
-        pio_remove_program(PINOUT_SBW_PIO, &sbw_program, sbw_offset);
-        sbw_offset = -1;
-        return false;
-    }
-
-    // need to start at 50 kHz: fuse check needs TMS cycles with a low phase
-    // of at least 5us. 50 kHz is below the required time (the actual maximum
-    // frequency would be around 80 kHz), but the exact frequency doesn't
-    // matter much as we'll switch to a higher one once the check has been
-    // completed
-    sbw_pio_init(PINOUT_SBW_PIO, sbw_piosm, sbw_offset, 50e3,
-            PINOUT_SBW_TCK, PINOUT_SBW_TDIO);
-
-    last_tdi = last_tms = 0xff;
-    last_tclk = true;
-
-    return true;
-}
-
-void sbw_deinit(void) {
-    if (sbw_piosm >= 0) {
-        pio_sm_set_enabled(PINOUT_SBW_PIO, sbw_piosm, false);
-        pio_sm_unclaim(PINOUT_SBW_PIO, sbw_piosm);
-        sbw_piosm = -1;
-    }
-
-    if (sbw_offset >= 0) {
-        pio_remove_program(PINOUT_SBW_PIO, &sbw_program, sbw_offset);
-        sbw_offset = -1;
-    }
-}
-
-void sbw_set_freq(bool tclk, float freq) {
-    if (tclk) {
-        sbw_pio_set_tclkfreq(PINOUT_SBW_PIO, sbw_piosm, freq);
-    } else {
-        sbw_pio_set_baudrate(PINOUT_SBW_PIO, sbw_piosm, freq);
-    }
-}
-
-static uint8_t bitswap(uint8_t in) {
-    static const uint8_t lut[16] = {
-        0x0, 0x8, 0x4, 0xc, 0x2, 0xa, 0x6, 0xe,
-        0x1, 0x9, 0x5, 0xd, 0x3, 0xb, 0x7, 0xf
-    };
-
-    return (lut[in&0xf] << 4) | lut[in>>4];
-}
-
-#define piosm_txf(width) (*(io_wo_##width *)&PINOUT_SBW_PIO->txf[sbw_piosm])
-#define piosm_rxf(width) (*(io_ro_##width *)&PINOUT_SBW_PIO->rxf[sbw_piosm])
-#define piosm_txf_wait() while (pio_sm_is_tx_fifo_full(PINOUT_SBW_PIO, sbw_piosm)) tight_loop_contents()
-
-bool sbw_get_last_tms(void) { return last_tms; }
-bool sbw_get_last_tdi(void) { return last_tdi; }
-bool sbw_get_last_tclk(void) { return last_tclk; }
-
-void sbw_sequence(uint32_t ncyc, bool tms, const uint8_t* tdi, uint8_t* tdo) {
-    if (ncyc == 0) return;
-
-    uint32_t nbytes = (ncyc + 7) >> 3;
-    uint32_t last_shift = (8 - ncyc) & 7;
-
-    uint32_t txremain = nbytes,
-             rxremain = last_shift ? nbytes : (nbytes + 1);
-
-    // initial TMS value in y
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_sety(tms ? 1 : 0);
-
-    // number of cycles in x
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_outx(32);
-    piosm_txf_wait();
-    piosm_txf(32) = ncyc - 1;
-
-    // jmp to correct subroutine
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_jmp(sbw_offset_sbw_seq + sbw_offset);
-
-    for (size_t oi = 0, ii = 0; txremain || rxremain; tight_loop_contents()) {
-        if (txremain && !pio_sm_is_tx_fifo_full(PINOUT_SBW_PIO, sbw_piosm)) {
-            piosm_txf(8) = bitswap(tdi ? tdi[ii] : last_tdi);
-            --txremain;
-            ++ii;
-        }
-
-        if (rxremain && !pio_sm_is_rx_fifo_empty(PINOUT_SBW_PIO, sbw_piosm)) {
-            uint8_t ov = piosm_rxf(8);
-            --rxremain;
-
-            if (tdo && oi < nbytes) {
-                if (last_shift && oi == nbytes - 1) {
-                    tdo[oi] = bitswap(ov) >> last_shift;
-                } else {
-                    tdo[oi] = bitswap(ov);
-                }
-                ++oi;
-            }
-        }
-    }
-
-    //while (!sbw_pio_is_idle(PINOUT_SBW_PIO, sbw_piosm, sbw_offset)) tight_loop_contents();
-
-    if (tdi) last_tdi = (tdi[nbytes - 1] & (1 << (ncyc & 7))) ? 0xff : 0;
-    last_tms = tms ? 0xff : 0;
-}
-
-void sbw_tms_sequence(uint32_t ncyc, bool tdi, const uint8_t* tms) {
-    if (ncyc == 0 || !tms) return;
-
-    uint32_t nbytes = (ncyc + 7) >> 3;
-    uint32_t txremain = nbytes;
-
-    // initial TDI value in y
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_sety(tdi ? 1 : 0);
-
-    // number of cycles in x
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_outx(32);
-    piosm_txf_wait();
-    piosm_txf(32) = ncyc - 1;
-
-    // jmp to correct subroutine
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_jmp(sbw_offset_sbw_tms_seq + sbw_offset);
-
-    for (size_t ii = 0; txremain; tight_loop_contents()) {
-        if (txremain && !pio_sm_is_tx_fifo_full(PINOUT_SBW_PIO, sbw_piosm)) {
-            piosm_txf(8) = bitswap(tms[ii]);
-            --txremain;
-            ++ii;
-        }
-    }
-
-    //while (!sbw_pio_is_idle(PINOUT_SBW_PIO, sbw_piosm, sbw_offset)) tight_loop_contents();
-
-    last_tdi = tdi ? 0xff : 0;
-    last_tms = (tms[nbytes - 1] & (1 << (ncyc & 7))) ? 0xff : 0;
-}
-
-void sbw_clrset_tclk(bool value) {
-    //sbw_pio_loadbearing_set_setpins(PINOUT_SBW_PIO, value ? 1 : 0); // new value
-
-    // pre-TCLK value
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_sety(last_tclk ? 1 : 0);
-
-    // only one TCLK
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_setx(0);
-
-    // jmp to subroutine
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_jmp(sbw_offset_sbw_tclk_burst + sbw_offset);
-
-    // always use out pins, 1
-    piosm_txf_wait();
-    piosm_txf(8) = value ? 0xff : 0;
-
-    // wait until done
-    /*while ( sbw_pio_is_idle(PINOUT_SBW_PIO, sbw_piosm, sbw_offset)) tight_loop_contents();
-    while (!sbw_pio_is_idle(PINOUT_SBW_PIO, sbw_piosm, sbw_offset)) tight_loop_contents();*/
-
-    last_tclk = value;
-}
-
-void sbw_tclk_burst(uint32_t ncyc) {
-    //sbw_pio_loadbearing_set_outpins(PINOUT_SBW_PIO);
-
-    uint32_t txremain = ((ncyc + 7) >> 3) * 2 - 1;
-
-    // MSB-first
-    uint8_t pattern = last_tclk ? 0x55 : 0xaa;
-
-    // pre-TCLK value
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_sety(last_tclk ? 1 : 0);
-
-    // number of TCLK half-cycles in x
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_outx(32);
-    piosm_txf_wait();
-    piosm_txf(32) = ncyc*2 - 1;
-
-    // jmp to subroutine
-    piosm_txf_wait();
-    piosm_txf(16) = sbw_pio_gen_jmp(sbw_offset_sbw_tclk_burst + sbw_offset);
-
-    for (; txremain; tight_loop_contents()) {
-        if (txremain && !pio_sm_is_tx_fifo_full(PINOUT_SBW_PIO, sbw_piosm)) {
-            piosm_txf(8) = pattern;
-            --txremain;
-        }
-    }
-
-    // wait until done
-    /*while ( sbw_pio_is_idle(PINOUT_SBW_PIO, sbw_piosm, sbw_offset)) tight_loop_contents();
-    while (!sbw_pio_is_idle(PINOUT_SBW_PIO, sbw_piosm, sbw_offset)) tight_loop_contents();*/
-
-    // last_tclk doesn't change - always an even number of TCLK half-cycles
-}
-
--- a/bsp/rp2040/m_isp/sbw_hw.h
+++ b/bsp/rp2040/m_isp/sbw_hw.h
@ -1,38 +0,0 @@
-
-#ifndef BSP_RP2040_SBW_HW_H
-#define BSP_RP2040_SBW_HW_H
-
-#include <stdint.h>
-#include <stdbool.h>
-/*#include <hardware/pio.h>*/
-
-/*#define PINOUT_SBW_PIO pio0
-#define PINOUT_SBW_TCK  8
-#define PINOUT_SBW_TDIO 9*/
-
-//extern int sbw_piosm, sbw_offset;
-
-// does the debug handshake/SBW setup thingy, call before sbw_init()
-void sbw_preinit(bool nrst);
-
-bool sbw_init(void);
-void sbw_deinit(void);
-
-void sbw_set_freq(bool tclk, float freq);
-
-bool sbw_get_last_tms(void);
-bool sbw_get_last_tdi(void);
-bool sbw_get_last_tclk(void);
-
-void sbw_sequence(uint32_t ncyc, bool tms, const uint8_t* tdi, uint8_t* tdo);
-void sbw_tms_sequence(uint32_t ncyc, bool tdi, const uint8_t* tms);
-
-void sbw_clrset_tclk(bool tclk);
-
-static inline void sbw_clr_tclk(void) { sbw_clrset_tclk(false); }
-static inline void sbw_set_tclk(void) { sbw_clrset_tclk(true ); }
-
-void sbw_tclk_burst(uint32_t ncyc);
-
-#endif
-
--- a/host/dpctl/init.py
+++ b/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")
--- a/host/dpctl/commands.py
+++ b/host/dpctl/commands.py
@ -10,7 +10,6 @@ from .protocol import *

 FEATURES_OF_MODE = {
    1: ["UART", "CMSIS-DAP", "SPI", "I2C", "temperature sensor", "1-wire"],
-    2: ["UART", "CMSIS-DAP", "MehFET"],
    3: ["JTAG", "SWD"],
    4: ["SUMP"]
 }
@ -86,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()
--- a/host/dpctl/protocol.py
+++ b/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:
--- a/pico_sdk_import.cmake
+++ b/pico_sdk_import.cmake
@ -1,62 +0,0 @@
-# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
-
-# This can be dropped into an external project to help locate this SDK
-# It should be include()ed prior to project()
-
-if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
-    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
-    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
-endif ()
-
-if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
-    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
-    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
-endif ()
-
-if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
-    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
-    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
-endif ()
-
-set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
-set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
-set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
-
-if (NOT PICO_SDK_PATH)
-    if (PICO_SDK_FETCH_FROM_GIT)
-        include(FetchContent)
-        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
-        if (PICO_SDK_FETCH_FROM_GIT_PATH)
-            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
-        endif ()
-        FetchContent_Declare(
-                pico_sdk
-                GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
-                GIT_TAG master
-        )
-        if (NOT pico_sdk)
-            message("Downloading Raspberry Pi Pico SDK")
-            FetchContent_Populate(pico_sdk)
-            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
-        endif ()
-        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
-    else ()
-        message(FATAL_ERROR
-                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
-                )
-    endif ()
-endif ()
-
-get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
-if (NOT EXISTS ${PICO_SDK_PATH})
-    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
-endif ()
-
-set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
-if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
-    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
-endif ()
-
-set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
-
-include(${PICO_SDK_INIT_CMAKE_FILE})
--- a/src/.gitignore
+++ b/src/.gitignore
@ -1 +1 @@
-_*_test/
+_storage_test/
--- a/src/m_default/_default.c
+++ b/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"
@ -21,10 +22,6 @@
 /* temperature sensor */
 #include "m_default/tempsensor.h"

-// TODO: CMSIS-DAP USB bulk:
-// * DAP_ExecuteCommand (returns response size)
-// * interface: vendor, 0.0 subclass/proto, EP1, CMSIS-DAP in name
-
 enum m_default_cmds {
    mdef_cmd_spi = mode_cmd__specific,
    mdef_cmd_i2c,
@ -39,13 +36,13 @@ enum m_default_feature {
    mdef_feat_tempsense = 1<<4,
 };

-#ifdef DBOARD_HAS_UART
-extern cothread_t m_def_uartthread;
-extern uint8_t m_def_uartstack[THREAD_STACK_SIZE];
-__attribute__((__weak__)) cothread_t m_def_uartthread;
-__attribute__((__weak__)) uint8_t m_def_uartstack[THREAD_STACK_SIZE];
+static bool data_dirty = false;

-__attribute__((__weak__)) void m_def_uart_thread_fn(void) {
+#ifdef DBOARD_HAS_UART
+static cothread_t uartthread;
+static uint8_t    uartstack[THREAD_STACK_SIZE];
+
+static void uart_thread_fn(void) {
    cdc_uart_init();
    thread_yield();
    while (1) {
@ -53,9 +50,6 @@ __attribute__((__weak__)) void m_def_uart_thread_fn(void) {
        thread_yield();
    }
 }
-#define uartthread m_def_uartthread
-#define uartstack m_def_uartstack
-#define uart_thread_fn m_def_uart_thread_fn
 #endif

 #ifdef DBOARD_HAS_SPI
@ -90,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?
@ -104,41 +109,6 @@ static void leave_cb(void) {
 #endif
 }

-void dap_do_bulk_stuff(int itf) {
-    // FIXME: move to a separate file, maybe
-    /*static*/ uint8_t rx_buf[DAP_PACKET_SIZE];
-    /*static*/ uint8_t tx_buf[DAP_PACKET_SIZE];
-    //static uint32_t rxpos = 0;
-    const uint32_t rxpos=0;
-
-    if (tud_vendor_n_mounted(itf) && tud_vendor_n_available(itf)) {
-        memset(rx_buf, 0, DAP_PACKET_SIZE);
-        uint32_t avail = tud_vendor_n_read(itf, &rx_buf[rxpos], sizeof rx_buf - rxpos);
-        uint32_t pos2 = rxpos + avail;
-
-        if (avail) {
-            memset(tx_buf, 0, DAP_PACKET_SIZE);
-            uint32_t res = DAP_ExecuteCommand(&rx_buf[rxpos], tx_buf);
-
-            uint16_t respcount = (uint16_t)res,
-                     reqcount = (uint16_t)(res>>16);
-            //printf("avail=%04lx resp=%04x req=%04x rxpos=%lx\n", avail, respcount, reqcount, rxpos);
-
-            if (reqcount > pos2) { // command requires more data than available, so, welp
-                tx_buf[0] = rx_buf[0]; // something
-                tx_buf[1] = 0xff;
-                tud_vendor_n_write(itf, tx_buf, 2);
-                //rxpos=0;//pos2;
-            } else {
-                tud_vendor_n_write(itf, tx_buf, respcount);
-                //memmove(rx_buf, &rx_buf[rxpos+reqcount], DAP_PACKET_SIZE - reqcount);
-                //rxpos = 0;
-            }
-            //printf("->rxpos=%lx\n", rxpos);
-        }
-    }
-}
-
 static void task_cb(void) {
 #ifdef DBOARD_HAS_UART
    tud_task();
@ -148,8 +118,6 @@ static void task_cb(void) {
    tud_task();
    thread_enter(serprogthread);
 #endif
-
-    dap_do_bulk_stuff(VND_N_CMSISDAP);
 }

 static void handle_cmd_cb(uint8_t cmd) {
@ -190,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
@ -202,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
@ -231,16 +205,12 @@ enum {

    STRID_IF_VND_CFG,
    STRID_IF_HID_CMSISDAP,
-    STRID_IF_VND_CMSISDAP,
    STRID_IF_VND_I2CTINYUSB,
    STRID_IF_CDC_UART,
    STRID_IF_CDC_SERPROG,
    STRID_IF_CDC_STDIO,
 };
 enum {
-#ifdef DBOARD_HAS_CMSISDAP
-    ITF_NUM_VND_CMSISDAP,
-#endif
 #if CFG_TUD_VENDOR > 0
    ITF_NUM_VND_CFG,
 #endif
@ -275,7 +245,6 @@ enum {
        + TUD_I2CTINYUSB_LEN
 #endif
 #ifdef DBOARD_HAS_CMSISDAP
-        + TUD_VENDOR_DESC_LEN
        + TUD_HID_INOUT_DESC_LEN
 #endif
 #ifdef DBOARD_HAS_UART
@ -289,20 +258,29 @@ enum {
 #endif
 };

-#define EPNUM_VND_DAP_OUT       0x01
-#define EPNUM_VND_DAP_IN        0x81
-#define EPNUM_VND_CFG_OUT       0x02/*-1*/
-#define EPNUM_VND_CFG_IN        0x82/*-1*/
-#define EPNUM_HID_CMSISDAP      0x03/*-1*/
-#define EPNUM_CDC_UART_OUT      0x04/*-1*/
-#define EPNUM_CDC_UART_IN       0x84/*-1*/
-#define EPNUM_CDC_UART_NOTIF    0x85/*-1*/
-#define EPNUM_CDC_SERPROG_OUT   0x06/*-1*/
-#define EPNUM_CDC_SERPROG_IN    0x86/*-1*/
-#define EPNUM_CDC_SERPROG_NOTIF 0x87/*-1*/
-#define EPNUM_CDC_STDIO_OUT     0x08/*-1*/
-#define EPNUM_CDC_STDIO_IN      0x88/*-1*/
-#define EPNUM_CDC_STDIO_NOTIF   0x89/*-1*/
+#define EPNUM_VND_CFG_OUT       0x01
+#define EPNUM_VND_CFG_IN        0x81
+#define EPNUM_HID_CMSISDAP      0x02
+#define EPNUM_CDC_UART_OUT      0x03
+#define EPNUM_CDC_UART_IN       0x83
+#define EPNUM_CDC_UART_NOTIF    0x84
+#define EPNUM_CDC_SERPROG_OUT   0x05
+#define EPNUM_CDC_SERPROG_IN    0x85
+#define EPNUM_CDC_SERPROG_NOTIF 0x86
+#define EPNUM_CDC_STDIO_OUT     0x07
+#define EPNUM_CDC_STDIO_IN      0x87
+#define EPNUM_CDC_STDIO_NOTIF   0x88
+
+/*#define EPNUM_CDC_UART_OUT      0x02
+#define EPNUM_CDC_UART_IN       0x82
+#define EPNUM_CDC_UART_NOTIF    0x83
+#define EPNUM_HID_CMSISDAP      0x04
+#define EPNUM_CDC_SERPROG_OUT   0x05
+#define EPNUM_CDC_SERPROG_IN    0x85
+#define EPNUM_CDC_SERPROG_NOTIF 0x86
+#define EPNUM_CDC_STDIO_OUT     0x07
+#define EPNUM_CDC_STDIO_IN      0x87
+#define EPNUM_CDC_STDIO_NOTIF   0x88*/

 // clang-format off
 #if CFG_TUD_HID > 0
@ -315,11 +293,6 @@ 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),

-#ifdef DBOARD_HAS_CMSISDAP
-    TUD_VENDOR_DESCRIPTOR_EX(ITF_NUM_VND_CMSISDAP, STRID_IF_VND_CMSISDAP, EPNUM_VND_DAP_OUT,
-        EPNUM_VND_DAP_IN, CFG_TUD_VENDOR_RX_BUFSIZE, 0, 0),
-#endif
-
 #if CFG_TUD_VENDOR > 0
    TUD_VENDOR_DESCRIPTOR_EX(ITF_NUM_VND_CFG, STRID_IF_VND_CFG, EPNUM_VND_CFG_OUT,
        EPNUM_VND_CFG_IN, CFG_TUD_VENDOR_RX_BUFSIZE, VND_CFG_SUBCLASS, VND_CFG_PROTOCOL),
@ -363,7 +336,6 @@ static const char* string_desc_arr[] = {
    // max string length check:  |||||||||||||||||||||||||||||||
    [STRID_IF_VND_CFG  ]      = "Device cfg/ctl interface",
    [STRID_IF_HID_CMSISDAP]   = "CMSIS-DAP HID interface",
-    [STRID_IF_VND_CMSISDAP]   = "CMSIS-DAP bulk interface",
    [STRID_IF_VND_I2CTINYUSB] = "I2C-Tiny-USB interface",
    [STRID_IF_CDC_UART]       = "UART CDC interface",
    [STRID_IF_CDC_SERPROG]    = "Serprog CDC interface",
@ -400,7 +372,6 @@ static void my_hid_set_report_cb(uint8_t instance, uint8_t report_id,
    (void)report_id;
    (void)report_type;

-    memset(tx_buffer, 0, CFG_TUD_HID_EP_BUFSIZE);
    DAP_ProcessCommand(rx_buffer, tx_buffer);

    tud_hid_report(0, tx_buffer, response_size);
@ -436,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 = {
@ -451,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,
@ -469,16 +460,3 @@ struct mode m_01_default = {
 };
 // clang-format on

-#if defined(DBOARD_HAS_UART)
-// FIXME: use mode-specific callback here?
-void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts) {
-    (void)dtr;
-
-    if (mode_current != &m_01_default) return;
-
-    if (itf == CDC_N_UART) {
-        cdc_uart_set_hwflow(rts);
-    }
-}
-#endif
-
--- a/src/m_default/cdc.h
+++ b/src/m_default/cdc.h
@ -2,7 +2,7 @@
 #ifndef CDC_H_
 #define CDC_H_

-/*#include "m_default/bsp-feature.h"*/
+#include "m_default/bsp-feature.h"

 /* BSP function prototypes for various USB-CDC interfaces */
 #ifdef DBOARD_HAS_UART
--- a/src/m_default/cdc_serprog.c
+++ b/src/m_default/cdc_serprog.c
@ -2,7 +2,6 @@

 #include <stdio.h>

-#include "tusb_config.h"
 #include <tusb.h>

 #include "m_default/bsp-feature.h"
@ -58,7 +57,6 @@ uint32_t sp_spi_get_buf_limit(void) {
    return sizeof(rx_buf) - 1;
 }

-// TODO: this is duplicated several times over the codebase, maybe reduce this
 static uint8_t read_byte_cdc(void) {
    while (rxavail <= 0) {
        if (!tud_cdc_n_connected(CDC_N_SERPROG) || !tud_cdc_n_available(CDC_N_SERPROG)) {
--- a/src/m_default/vnd_i2ctinyusb.c
+++ b/src/m_default/vnd_i2ctinyusb.c
@ -11,7 +11,6 @@

 #include <hardware/i2c.h>

-#include "tusb_config.h"
 #include <tusb.h>
 #include <device/usbd_pvt.h>

--- a/src/m_isp/_isp.c
+++ b/src/m_isp/_isp.c
@ -1,350 +0,0 @@
-// vim: set et:
-
-/* UGLY HACK */
-#define BSP_FEATURE_M_DEFAULT_H_
-
-#include <tusb.h>
-
-#include "mode.h"
-#include "thread.h"
-#include "usbstdio.h"
-#include "vnd_cfg.h"
-
-#include "m_isp/bsp-feature.h"
-
-/* CMSIS-DAP */
-#include "DAP_config.h" /* ARM code *assumes* this is included prior to DAP.h */
-#include "DAP.h"
-/* CDC UART */
-#include "m_default/cdc.h" /* yeah just reuse this one */
-/* MehFET */
-#include "m_isp/mehfet.h"
-
-enum m_isp_cmds {
-    misp_cmd_idk = mode_cmd__specific,
-};
-enum m_isp_feature {
-    misp_feat_uart      = 1<<0,
-    misp_feat_cmsisdap  = 1<<1,
-    misp_feat_mehfet    = 1<<2,
-};
-
-#ifdef DBOARD_HAS_UART
-extern cothread_t m_def_uartthread;
-extern uint8_t m_def_uartstack[THREAD_STACK_SIZE];
-__attribute__((__weak__)) cothread_t m_def_uartthread;
-__attribute__((__weak__)) uint8_t m_def_uartstack[THREAD_STACK_SIZE];
-
-__attribute__((__weak__)) void m_def_uart_thread_fn(void) {
-    cdc_uart_init();
-    thread_yield();
-    while (1) {
-        cdc_uart_task();
-        thread_yield();
-    }
-}
-#define uartthread m_def_uartthread
-#define uartstack m_def_uartstack
-#define uart_thread_fn m_def_uart_thread_fn
-#endif
-
-#ifdef DBOARD_HAS_MEHFET
-static cothread_t mehfetthread;
-static uint8_t mehfetstack[THREAD_STACK_SIZE];
-
-static void mehfet_thread_fn(void) {
-    mehfet_init();
-    thread_yield();
-    while (1) {
-        mehfet_task();
-        thread_yield();
-    }
-}
-#endif
-
-static void enter_cb(void) {
-#ifdef USE_USBCDC_FOR_STDIO
-    stdio_usb_set_itf_num(CDC_N_STDIO);
-#endif
-    vnd_cfg_set_itf_num(VND_N_CFG);
-
-    // TODO: CMSISDAP?
-
-    // HACK: we init UART stuff first: UART inits gpio 10,11 pinmux fn to UART
-    //       flow control signals, which is ok for mode1, but conflicts with
-    //       stuff in mode2
-#ifdef DBOARD_HAS_UART
-    uartthread = co_derive(uartstack, sizeof uartstack, uart_thread_fn);
-    thread_enter(uartthread);  // will call cdc_uart_init() on correct thread
-#endif
-#ifdef DBOARD_HAS_MEHFET
-    mehfetthread = co_derive(mehfetstack, sizeof mehfetstack, mehfet_thread_fn);
-    thread_enter(mehfetthread);
-#endif
-}
-static void leave_cb(void) {
-    // TODO: CMSISDAP?
-
-#ifdef DBOARD_HAS_MEHFET
-    mehfet_deinit();
-#endif
-#ifdef DBOARD_HAS_UART
-    cdc_uart_deinit();
-#endif
-}
-
-void dap_do_bulk_stuff(int itf);
-
-static void task_cb(void) {
-#ifdef DBOARD_HAS_UART
-    tud_task();
-    thread_enter(uartthread);
-#endif
-#ifdef DBOARD_HAS_MEHFET
-    tud_task();
-    thread_enter(mehfetthread);
-#endif
-
-    dap_do_bulk_stuff(VND_N_CMSISDAP);
-}
-
-static void handle_cmd_cb(uint8_t cmd) {
-    uint8_t resp = 0;
-
-    switch (cmd) {
-    case mode_cmd_get_features:
-#ifdef DBOARD_HAS_UART
-        resp |= misp_feat_uart;
-#endif
-#ifdef DBOARD_HAS_CMSISDAP
-        resp |= misp_feat_cmsisdap;
-#endif
-#ifdef DBOARD_HAS_MEHFET
-        resp |= misp_feat_mehfet;
-#endif
-        vnd_cfg_write_resp(cfg_resp_ok, 1, &resp);
-        break;
-    default:
-        vnd_cfg_write_strf(cfg_resp_illcmd, "unknown mode1 command %02x", cmd);
-        break;
-    }
-}
-
-enum {
-    STRID_LANGID = 0,
-    STRID_MANUFACTURER,
-    STRID_PRODUCT,
-    STRID_SERIAL,
-
-    STRID_CONFIG,
-
-    STRID_IF_VND_CFG,
-    STRID_IF_HID_CMSISDAP,
-    STRID_IF_VND_CMSISDAP,
-    STRID_IF_CDC_UART,
-    STRID_IF_VND_MEHFET,
-    STRID_IF_CDC_STDIO,
-};
-enum {
-#ifdef DBOARD_HAS_CMSISDAP
-    ITF_NUM_VND_CMSISDAP,
-#endif
-#if CFG_TUD_VENDOR > 0
-    ITF_NUM_VND_CFG,
-#endif
-#ifdef DBOARD_HAS_CMSISDAP
-    ITF_NUM_HID_CMSISDAP,
-#endif
-#ifdef DBOARD_HAS_UART
-    ITF_NUM_CDC_UART_COM,
-    ITF_NUM_CDC_UART_DATA,
-#endif
-#ifdef DBOARD_HAS_MEHFET
-    ITF_NUM_VND_MEHFET,
-#endif
-#ifdef USE_USBCDC_FOR_STDIO
-    ITF_NUM_CDC_STDIO_COM,
-    ITF_NUM_CDC_STDIO_DATA,
-#endif
-
-    ITF_NUM__TOTAL
-};
-enum {
-    CONFIG_TOTAL_LEN
-        = TUD_CONFIG_DESC_LEN
-#if CFG_TUD_VENDOR > 0
-        + TUD_VENDOR_DESC_LEN
-#endif
-#ifdef DBOARD_HAS_CMSISDAP
-        + TUD_VENDOR_DESC_LEN
-        + TUD_HID_INOUT_DESC_LEN
-#endif
-#ifdef DBOARD_HAS_UART
-        + TUD_CDC_DESC_LEN
-#endif
-#ifdef DBOARD_HAS_MEHFET
-        + TUD_VENDOR_DESC_LEN
-#endif
-#ifdef USE_USBCDC_FOR_STDIO
-        + TUD_CDC_DESC_LEN
-#endif
-};
-
-#define EPNUM_VND_DAP_OUT       0x01
-#define EPNUM_VND_DAP_IN        0x81
-#define EPNUM_VND_CFG_OUT       0x02/*-1*/
-#define EPNUM_VND_CFG_IN        0x82/*-1*/
-#define EPNUM_HID_CMSISDAP      0x03/*-1*/
-#define EPNUM_CDC_UART_OUT      0x04/*-1*/
-#define EPNUM_CDC_UART_IN       0x84/*-1*/
-#define EPNUM_CDC_UART_NOTIF    0x85/*-1*/
-#define EPNUM_VND_MEHFET_OUT    0x06/*-1*/
-#define EPNUM_VND_MEHFET_IN     0x86/*-1*/
-#define EPNUM_CDC_STDIO_OUT     0x07/*-1*/
-#define EPNUM_CDC_STDIO_IN      0x87/*-1*/
-#define EPNUM_CDC_STDIO_NOTIF   0x88/*-1*/
-
-// clang-format off
-#if CFG_TUD_HID > 0
-static const uint8_t desc_hid_report[] = { // ugh
-    TUD_HID_REPORT_DESC_GENERIC_INOUT(CFG_TUD_HID_EP_BUFSIZE)
-};
-#endif
-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),
-
-#ifdef DBOARD_HAS_CMSISDAP
-    TUD_VENDOR_DESCRIPTOR_EX(ITF_NUM_VND_CMSISDAP, STRID_IF_VND_CMSISDAP, EPNUM_VND_DAP_OUT,
-        EPNUM_VND_DAP_IN, CFG_TUD_VENDOR_RX_BUFSIZE, 0, 0),
-#endif
-
-#if CFG_TUD_VENDOR > 0
-    TUD_VENDOR_DESCRIPTOR_EX(ITF_NUM_VND_CFG, STRID_IF_VND_CFG, EPNUM_VND_CFG_OUT,
-        EPNUM_VND_CFG_IN, CFG_TUD_VENDOR_RX_BUFSIZE, VND_CFG_SUBCLASS, VND_CFG_PROTOCOL),
-#endif
-
-#ifdef DBOARD_HAS_CMSISDAP
-    TUD_HID_INOUT_DESCRIPTOR(ITF_NUM_HID_CMSISDAP, STRID_IF_HID_CMSISDAP,
-        0 /*HID_PROTOCOL_NONE*/, sizeof(desc_hid_report), EPNUM_HID_CMSISDAP,
-        0x80 | (EPNUM_HID_CMSISDAP + 0), CFG_TUD_HID_EP_BUFSIZE, 1),
-#endif
-
-#ifdef DBOARD_HAS_UART
-    TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_UART_COM, STRID_IF_CDC_UART, EPNUM_CDC_UART_NOTIF,
-        CFG_TUD_CDC_RX_BUFSIZE, EPNUM_CDC_UART_OUT, EPNUM_CDC_UART_IN, CFG_TUD_CDC_RX_BUFSIZE),
-#endif
-
-#ifdef DBOARD_HAS_MEHFET
-    TUD_VENDOR_DESCRIPTOR_EX(ITF_NUM_VND_MEHFET, STRID_IF_VND_MEHFET, EPNUM_VND_MEHFET_OUT,
-        EPNUM_VND_MEHFET_IN, CFG_TUD_VENDOR_RX_BUFSIZE, '4', '3'),
-#endif
-
-#ifdef USE_USBCDC_FOR_STDIO
-    TUD_CDC_DESCRIPTOR(ITF_NUM_CDC_STDIO_COM, STRID_IF_CDC_STDIO, EPNUM_CDC_STDIO_NOTIF,
-        CFG_TUD_CDC_RX_BUFSIZE, EPNUM_CDC_STDIO_OUT, EPNUM_CDC_STDIO_IN, CFG_TUD_CDC_RX_BUFSIZE),
-#endif
-};
-static const char* string_desc_arr[] = {
-    NULL,
-
-    // no hw info here, or the name will be too long >__>
-    // CMSIS-DAP spec:
-    // "The Product String must contain 'CMSIS-DAP' somewhere in the string.
-    //  This is used by the debuggers to identify a CMSIS-DAP compliant Debug
-    //  Unit that is connected to a host computer."
-    [STRID_PRODUCT]           = INFO_PRODUCT_BARE " CMSIS-DAP",
-    [STRID_CONFIG]            = "Configuration descriptor",
-    // max string length check:  |||||||||||||||||||||||||||||||
-    [STRID_IF_VND_CFG  ]      = "Device cfg/ctl interface",
-    [STRID_IF_HID_CMSISDAP]   = "CMSIS-DAP HID interface",
-    [STRID_IF_VND_CMSISDAP]   = "CMSIS-DAP bulk interface",
-    [STRID_IF_CDC_UART]       = "UART CDC interface",
-    [STRID_IF_VND_MEHFET]     = "MehFET MSP430 debug interface",
-#ifdef USE_USBCDC_FOR_STDIO
-    [STRID_IF_CDC_STDIO]      = "stdio CDC interface (debug)",
-#endif
-};
-// clang-format on
-
-#ifdef DBOARD_HAS_CMSISDAP
-static const uint8_t* my_hid_descriptor_report_cb(uint8_t instance) {
-    (void)instance;
-
-    return desc_hid_report;
-}
-/*static uint16_t my_hid_get_report_cb(uint8_t instance, uint8_t report_id,
-        hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen) {
-    // TODO not implemented
-    (void)instance;
-    (void)report_id;
-    (void)report_type;
-    (void)buffer;
-    (void)reqlen;
-
-    return 0;
-}*/
-
-static void my_hid_set_report_cb(uint8_t instance, uint8_t report_id,
-        hid_report_type_t report_type, uint8_t const* rx_buffer, uint16_t bufsize) {
-    static uint8_t tx_buffer[CFG_TUD_HID_EP_BUFSIZE];
-    uint32_t       response_size = TU_MIN(CFG_TUD_HID_EP_BUFSIZE, bufsize);
-
-    (void)instance;
-    (void)report_id;
-    (void)report_type;
-
-    memset(tx_buffer, 0, CFG_TUD_HID_EP_BUFSIZE);
-    DAP_ProcessCommand(rx_buffer, tx_buffer);
-
-    tud_hid_report(0, tx_buffer, response_size);
-}
-#endif
-
-#if CFG_TUD_CDC > 0
-static void my_cdc_line_coding_cb(uint8_t itf, cdc_line_coding_t const* line_coding) {
-    switch (itf) {
-#ifdef DBOARD_HAS_UART
-        case CDC_N_UART:
-            cdc_uart_set_coding(line_coding->bit_rate, line_coding->stop_bits,
-                    line_coding->parity, line_coding->data_bits);
-            break;
-#endif
-#ifdef USE_USBCDC_FOR_STDIO
-        case CDC_N_STDIO:
-            stdio_usb_line_coding_cb(line_coding);
-            break;
-#endif
-    }
-}
-#endif
-
-extern struct mode m_02_isp;
-// clang-format off
-struct mode m_02_isp = {
-    .name = "In-system-programming/debugging mode",
-    .version = 0x0010,
-    .n_string_desc = sizeof(string_desc_arr)/sizeof(string_desc_arr[0]),
-
-    .usb_desc = desc_configuration,
-    .string_desc = string_desc_arr,
-
-    .enter = enter_cb,
-    .leave = leave_cb,
-    .task  = task_cb,
-    .handle_cmd = handle_cmd_cb,
-
-#if defined(DBOARD_HAS_CMSISDAP) && CFG_TUD_HID > 0
-#if 0
-    .tud_hid_get_report_cb = my_hid_get_report_cb,
-#endif
-    .tud_hid_set_report_cb = my_hid_set_report_cb,
-    .tud_hid_descriptor_report_cb = my_hid_descriptor_report_cb,
-#endif
-
-#if CFG_TUD_CDC > 0
-    .tud_cdc_line_coding_cb = my_cdc_line_coding_cb,
-#endif
-};
-// clang-format on
-
--- a/src/m_isp/mehfet.c
+++ b/src/m_isp/mehfet.c
@ -1,441 +0,0 @@
-// vim: set et:
-
-#include <string.h>
-
-#include <tusb.h>
-
-#include "info.h"
-#include "thread.h"
-
-#include "m_isp/bsp-feature.h"
-#include "m_isp/mehfet.h"
-
-struct cmdlen {
-    uint32_t len;
-    uint8_t cmd;
-};
-
-#define BUFSIZE 256
-
-static uint8_t rx_buf[BUFSIZE];
-static uint8_t tx_buf[BUFSIZE];
-
-static uint32_t rxavail, rxpos, txpos;
-
-// TODO: this is duplicated several times over the codebase, maybe reduce this
-static uint8_t read_byte(void) {
-    while (rxavail <= 0) {
-        if (!tud_vendor_n_mounted(VND_N_MEHFET) || !tud_vendor_n_available(VND_N_MEHFET)) {
-            thread_yield();
-            continue;
-        }
-
-        rxpos   = 0;
-        rxavail = tud_vendor_n_read(VND_N_MEHFET, rx_buf, sizeof rx_buf);
-
-        if (rxavail == 0) thread_yield();
-    }
-
-    uint8_t rv = rx_buf[rxpos];
-    ++rxpos;
-    --rxavail;
-
-    return rv;
-}
-/*static void drop_incoming(void) {
-    rxavail = 0;
-    rxpos = 0;
-
-    // empty tinyusb internal buffer
-    if (tud_vendor_n_mounted(VND_N_MEHFET)) {
-        while (tud_vendor_n_available(VND_N_MEHFET)) {
-            tud_vendor_n_read(VND_N_MEHFET, rx_buf, sizeof rx_buf);
-        }
-    }
-}*/
-static void write_flush(void) {
-    // TODO: is this needed?
-    while (tud_vendor_n_write_available(VND_N_MEHFET) < txpos) {
-        thread_yield();
-    }
-
-    tud_vendor_n_write(VND_N_MEHFET, tx_buf, txpos);
-    txpos = 0;
-}
-static void write_byte(uint8_t v) {
-    if (txpos == sizeof tx_buf) {
-        write_flush();
-    }
-
-    tx_buf[txpos] = v;
-    ++txpos;
-}
-
-////////////////
-
-static uint32_t plpos;
-static struct cmdlen read_cmd_len(void) {
-    uint8_t cmd = read_byte(),
-            lastbyte = cmd;
-    uint32_t l = 0;
-
-    //printf("cmd=%02x\n", cmd);
-
-    for (size_t i = 0; (i < 4) && (lastbyte & 0x80); ++i) {
-        lastbyte = read_byte();
-        //printf("lenbyte=%02x\n");
-
-        uint8_t mask = (i == 3) ? 0xff : 0x7f;
-        l |= (lastbyte & mask) << (i * 7);
-    }
-
-    //printf("len=0x%x\n");
-    plpos = 0;
-    return (struct cmdlen){ .len = l, .cmd = cmd & 0x7f };
-}
-
-static inline uint8_t read_pl(void) {
-    ++plpos;
-    return read_byte();
-}
-static void flush_pl(uint32_t len) {
-    while (plpos < len) read_byte();
-}
-
-static void write_resp(enum mehfet_status stat, size_t resplen, const uint8_t* resp) {
-    //if (stat != mehfet_ok) drop_incoming();
-
-    write_byte((stat & 0x7f) | (resplen ? 0x80 : 0));
-
-    for (size_t i = 0, len2 = resplen; (i < 4) && len2; ++i) {
-        uint8_t nextv;
-        if (i == 3) {
-            nextv = (uint8_t)len2;
-        } else {
-            nextv = len2 & 0x7f;
-            if ((len2 >> 7) != 0) nextv |= 0x80;
-        }
-        len2 >>= 7;
-
-        write_byte(nextv);
-    }
-
-    for (size_t i = 0; i < resplen; ++i)
-        write_byte(resp[i]);
-
-    write_flush();
-}
-static void write_resp_str(enum mehfet_status stat, const char* str) {
-    write_resp(stat, strlen(str)+1 /* include null terminator */, (const uint8_t*)str);
-}
-
-///////////////
-
-static uint8_t connstat;
-
-void mehfet_init(void) {
-    rxavail = 0;
-    rxpos   = 0;
-    txpos   = 0;
-    plpos   = 0;
-
-    connstat = mehfet_conn_none;
-
-    mehfet_hw_init();
-}
-void mehfet_deinit(void) {
-    if (connstat != mehfet_conn_none) {
-        mehfet_hw_disconnect();
-        connstat = mehfet_conn_none;
-    }
-
-    mehfet_hw_deinit();
-}
-
-void mehfet_task(void) {
-    struct cmdlen cmdhdr = read_cmd_len();
-
-    switch (cmdhdr.cmd) {
-    case mehfet_info:
-        //printf("in info cmd\n");
-        if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "Info takes no parameters");
-        else {
-            // TODO: add flag once Loop has been implemented
-            uint32_t caps = mehfet_hw_get_caps() /*| mehfet_cap_has_loop*/;
-            uint16_t ver = MEHFET_PROTO_VER;
-            uint8_t pktbuf_l2 = __builtin_ctz(sizeof tx_buf);
-            const char* name = INFO_PRODUCT(INFO_BOARDNAME);
-
-            size_t bufsize = strlen(name) + 1 + 8;
-            if (bufsize > 256) // uuuuuh, (stack size is 512b)
-                write_resp_str(mehfet_error, "not enough space for info buffer");
-
-            uint8_t buf[bufsize];
-            buf[0] = (caps >>  0) & 0xff;
-            buf[1] = (caps >>  8) & 0xff;
-            buf[2] = (caps >> 16) & 0xff;
-            buf[3] = (caps >> 24) & 0xff;
-
-            buf[4] = (ver >> 0) & 0xff;
-            buf[5] = (ver >> 8) & 0xff;
-
-            buf[6] = pktbuf_l2;
-            buf[7] = 0; // reserved
-
-            memcpy(&buf[8], name, bufsize - 8);
-
-            //printf("infocmd done\n");
-            write_resp(mehfet_ok, bufsize, buf);
-        }
-        break;
-    case mehfet_status:
-        if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "Status takes no parameters");
-        else write_resp(mehfet_ok, 1, &connstat);
-        break;
-    case mehfet_connect:
-        if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "Connect takes one parameter byte");
-        else if (connstat != mehfet_conn_none) {
-            write_resp_str(mehfet_badstate, "Already connected");
-        } else {
-            enum mehfet_conn conn = read_pl();
-
-            // check if we can actually do this
-            enum mehfet_conn conntyp = conn & mehfet_conn_typemask;
-            switch (conntyp) {
-            case mehfet_conn_jtag_noentry:
-                if (!(mehfet_hw_get_caps() & mehfet_cap_jtag_noentry)) {
-                    write_resp(mehfet_nocaps, 0, NULL);
-                    goto EXIT;
-                }
-                break;
-            case mehfet_conn_jtag_entryseq:
-                if (!(mehfet_hw_get_caps() & mehfet_cap_jtag_entryseq)) {
-                    write_resp(mehfet_nocaps, 0, NULL);
-                    goto EXIT;
-                }
-                break;
-            case mehfet_conn_sbw_entryseq:
-                if (!(mehfet_hw_get_caps() & mehfet_cap_sbw_entryseq)) {
-                    write_resp(mehfet_nocaps, 0, NULL);
-                    goto EXIT;
-                }
-                break;
-            case mehfet_conn_auto:
-                if (mehfet_hw_get_caps() & mehfet_cap_sbw_entryseq)
-                    conntyp = mehfet_conn_sbw_entryseq;
-                else if (mehfet_hw_get_caps() & mehfet_cap_jtag_entryseq)
-                    conntyp = mehfet_conn_jtag_entryseq;
-                else if (mehfet_hw_get_caps() & mehfet_cap_jtag_noentry)
-                    conntyp = mehfet_conn_jtag_noentry;
-                else {
-                    write_resp_str(mehfet_nocaps, "Connect: no mode implemented in hardware...");
-                    goto EXIT;
-                }
-                break;
-            default:
-                write_resp_str(mehfet_badargs, "Connect: invalid connection mode");
-                goto EXIT;
-            }
-
-            const char* resp = mehfet_hw_connect(conntyp | (conn & mehfet_conn_nrstmask));
-            if (!resp) {
-                connstat = conntyp;
-                write_resp(mehfet_ok, 0, NULL);
-            } else
-                write_resp_str(mehfet_error, resp);
-        }
-        break;
-    case mehfet_disconnect:
-        if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "Disconnect takes no parameters");
-        else {
-            if (connstat != mehfet_conn_none) mehfet_hw_disconnect();
-            connstat = mehfet_conn_none;
-
-            write_resp(mehfet_ok, 0, NULL);
-        }
-        break;
-    case mehfet_delay:
-        if (cmdhdr.len != 4) write_resp_str(mehfet_badargs, "Delay takes 4 parameter bytes");
-        else {
-            uint32_t v = 0;
-            v |= (uint32_t)read_pl() << 0 ;
-            v |= (uint32_t)read_pl() << 8 ;
-            v |= (uint32_t)read_pl() << 16;
-            v |= (uint32_t)read_pl() << 24;
-
-            bool exact = v & (1u<<31), us = v & (1u<<30);
-            v &= (1u << 30) - 1;
-
-            if (exact) {
-                if (us) mehfet_hw_delay_us(v);
-                else mehfet_hw_delay_ms(v);
-            } else {
-                mehfet_hw_timer_start(us, v);
-                while (!mehfet_hw_timer_reached()) thread_yield();
-            }
-
-            write_resp(mehfet_ok, 0, NULL);
-        }
-        break;
-    case mehfet_set_clkspeed:
-        if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "SetClkSpeed takes one parameter byte");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            mehfet_hw_set_clkspeed(read_pl() != 0);
-            write_resp(mehfet_ok, 0, NULL);
-        }
-        break;
-    case mehfet_get_old_lines:
-        if (cmdhdr.len != 0) write_resp_str(mehfet_badargs, "GetOldLines takes no parameters");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            uint8_t v = mehfet_hw_get_old_lines();
-            write_resp(mehfet_ok, 1, &v);
-        }
-        break;
-    case mehfet_tdio_seq:
-        if (cmdhdr.len < 6) write_resp_str(mehfet_badargs,
-                "TdioSequence: need at least a TMS level, number of cycles and some TDI data (at least 6 bytes)");
-        // stack size is 512b
-        else if (cmdhdr.len > 128 + 5) write_resp_str(mehfet_badargs,
-                "TdioSequence: too much data to process, can do max. 1024 bits (128B) at once");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            uint32_t ncyc = 0;
-            ncyc |= (uint32_t)read_pl() << 0 ;
-            ncyc |= (uint32_t)read_pl() << 8 ;
-            ncyc |= (uint32_t)read_pl() << 16;
-            ncyc |= (uint32_t)read_pl() << 24;
-
-            bool tmslvl = read_pl() != 0;
-
-            size_t nbytes = (ncyc + 7) >> 3;
-            if (nbytes != cmdhdr.len - 5) {
-                write_resp_str(mehfet_badargs, "TdioSequence: bad ncyc<->payload length");
-            } else {
-                uint8_t tdi_stuff[nbytes], tdo_stuff[nbytes];
-
-                for (size_t i = 0; i < nbytes; ++i) tdi_stuff[i] = read_pl();
-
-                mehfet_hw_tdio_seq(ncyc, tmslvl, tdi_stuff, tdo_stuff);
-
-                write_resp(mehfet_ok, nbytes, tdo_stuff);
-            }
-        }
-        break;
-    case mehfet_tms_seq:
-        if (cmdhdr.len < 6) write_resp_str(mehfet_badargs,
-                "TmsSequence: need a TDI level, the number of cycles and some TMS data (at least 6 bytes)");
-        // stack size is 512b
-        else if (cmdhdr.len > 256 + 5) write_resp_str(mehfet_badargs,
-                "TmsSequence: too much data to process, can do max. 2048 bits (256B) at once");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            uint32_t ncyc = 0;
-            ncyc |= (uint32_t)read_pl() << 0 ;
-            ncyc |= (uint32_t)read_pl() << 8 ;
-            ncyc |= (uint32_t)read_pl() << 16;
-            ncyc |= (uint32_t)read_pl() << 24;
-
-            bool tdilvl = read_pl() != 0;
-
-            size_t nbytes = (ncyc + 7) >> 3;
-            if (nbytes != cmdhdr.len - 5) {
-                write_resp_str(mehfet_badargs, "TmsSequence: bad ncyc<->payload length");
-            } else {
-                uint8_t tms_stuff[nbytes];
-
-                for (size_t i = 0; i < nbytes; ++i) tms_stuff[i] = read_pl();
-
-                mehfet_hw_tms_seq(ncyc, tdilvl, tms_stuff);
-
-                write_resp(mehfet_ok, 0, NULL);
-            }
-        }
-        break;
-    case mehfet_tclk_edge:
-        if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "TclkEdge takes one parameter byte");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            uint8_t newtclk = read_pl();
-            mehfet_hw_tclk_edge(newtclk);
-            write_resp(mehfet_ok, 0, NULL);
-        }
-        break;
-    case mehfet_tclk_burst:
-        if (cmdhdr.len != 4) write_resp_str(mehfet_badargs, "TclkBurst takes 4 parameter bytes");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            uint32_t ncyc = 0;
-            ncyc |= (uint32_t)read_pl() << 0 ;
-            ncyc |= (uint32_t)read_pl() << 8 ;
-            ncyc |= (uint32_t)read_pl() << 16;
-            ncyc |= (uint32_t)read_pl() << 24;
-
-            mehfet_hw_tclk_burst(ncyc);
-            write_resp(mehfet_ok, 0, NULL);
-        }
-        break;
-
-    case mehfet_reset_tap:
-        if (!(mehfet_hw_get_caps() & mehfet_cap_has_reset_tap)) write_resp(mehfet_nocaps, 0, NULL);
-        else if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "ResetTAP takes one parameter byte");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            uint8_t v = mehfet_hw_reset_tap(read_pl());
-            write_resp(mehfet_ok, 1, &v);
-        }
-        break;
-    case mehfet_irshift:
-        if (!(mehfet_hw_get_caps() & mehfet_cap_has_irshift)) write_resp(mehfet_nocaps, 0, NULL);
-        else if (cmdhdr.len != 1) write_resp_str(mehfet_badargs, "IRshift takes one parameter byte");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            uint8_t newir = read_pl();
-            uint8_t oldir = mehfet_hw_shift_ir(newir);
-            write_resp(mehfet_ok, 1, &oldir);
-        }
-        break;
-    case mehfet_drshift:
-        if (!(mehfet_hw_get_caps() & mehfet_cap_has_drshift)) write_resp(mehfet_nocaps, 0, NULL);
-        else if (cmdhdr.len < 5) write_resp_str(mehfet_badargs,
-                "DRshift takes at least a bit count and some data (at least 5 bytes)");
-        else if (cmdhdr.len > 128 + 4) write_resp_str(mehfet_badargs,
-                "DRshift: too much data to process, can do max. 1024 bits (128B)");
-        else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        else {
-            uint32_t nbits = 0;
-            nbits |= (uint32_t)read_pl() << 0 ;
-            nbits |= (uint32_t)read_pl() << 8 ;
-            nbits |= (uint32_t)read_pl() << 16;
-            nbits |= (uint32_t)read_pl() << 24;
-
-            size_t nbytes = (nbits + 7) >> 3;
-            if (nbytes != cmdhdr.len - 4) {
-                write_resp_str(mehfet_badargs, "DRshift: bad nbits<->payload length");
-            } else {
-                uint8_t newdr[nbytes], olddr[nbytes];
-
-                for (size_t i = 0; i < nbytes; ++i) newdr[i] = read_pl();
-
-                mehfet_hw_shift_dr(nbits, newdr, olddr);
-
-                write_resp(mehfet_ok, nbytes, olddr);
-            }
-        }
-        break;
-
-    case mehfet_loop:
-        // TODO
-        write_resp_str(mehfet_nocaps, "not implemented yet, sorry");
-        //else if (connstat == mehfet_conn_none) write_resp(mehfet_badstate, 0, NULL);
-        break;
-
-    default:
-        write_resp(mehfet_invalidcmd, 0, NULL);
-    }
-
-EXIT:
-    flush_pl(cmdhdr.len);
-}
-
--- a/src/m_isp/mehfet.h
+++ b/src/m_isp/mehfet.h
@ -1,114 +0,0 @@
-// vim: set et:
-
-#ifndef M_ISP_MEHFET
-#define M_ISP_MEHFET
-
-#include <stdint.h>
-#include <stdbool.h>
-
-#define MEHFET_PROTO_VER 0x0001
-
-void mehfet_init(void);
-void mehfet_deinit(void);
-void mehfet_task(void);
-
-enum mehfet_cmd {
-    mehfet_info          = 0x01,
-    mehfet_status        = 0x02,
-    mehfet_connect       = 0x03,
-    mehfet_disconnect    = 0x04,
-    mehfet_delay         = 0x05,
-    mehfet_set_clkspeed  = 0x06,
-    mehfet_get_old_lines = 0x07,
-    mehfet_tdio_seq      = 0x08,
-    mehfet_tms_seq       = 0x09,
-    mehfet_tclk_edge     = 0x0a,
-    mehfet_tclk_burst    = 0x0b,
-    mehfet_reset_tap     = 0x0c,
-    mehfet_irshift       = 0x0d,
-    mehfet_drshift       = 0x0e,
-    mehfet_loop          = 0x0f,
-};
-
-enum mehfet_status {
-    mehfet_ok = 0x00,
-
-    mehfet_badargs    = 0x7b,
-    mehfet_nocaps     = 0x7c,
-    mehfet_badstate   = 0x7d,
-    mehfet_invalidcmd = 0x7e,
-    mehfet_error      = 0x7f
-};
-
-enum mehfet_caps {
-    mehfet_cap_jtag_noentry  = 1<<0,
-    mehfet_cap_jtag_entryseq = 1<<1,
-    mehfet_cap_sbw_entryseq  = 1<<2,
-
-    mehfet_cap_has_reset_tap = 1<< 8,
-    mehfet_cap_has_irshift   = 1<< 9,
-    mehfet_cap_has_drshift   = 1<<10,
-    mehfet_cap_has_loop      = 1<<11,
-};
-
-enum mehfet_conn {
-    mehfet_conn_none = 0,
-    mehfet_conn_auto = 0,
-
-    mehfet_conn_jtag_noentry  = 1,
-    mehfet_conn_jtag_entryseq = 2,
-    mehfet_conn_sbw_entryseq  = 3,
-
-    mehfet_conn_typemask   = 0x7f,
-    mehfet_conn_nrstmask   = 0x80
-};
-
-enum mehfet_resettap_flags {
-    mehfet_rsttap_do_reset  = 1<<0, // reset TAP to run-test/idle state
-    mehfet_rsttap_fuse_do   = 1<<1, // perform fuse check procedure (TMS pulses) on target
-    mehfet_rsttap_fuse_read = 1<<2, // check whether the JTAG fuse has been blown
-    mehfet_rsttap_highspeed = 1<<3, // can move to high-speed transport afterwards
-                                    // (fuse check procedure is max 50 kHz)
-};
-enum mehfet_resettap_status {
-    mehfet_rsttap_fuse_blown = 0x80
-};
-
-// hw routines
-
-void mehfet_hw_init(void);
-void mehfet_hw_deinit(void);
-
-__attribute__((__const__))
-enum mehfet_caps mehfet_hw_get_caps(void);
-
-const char* /*error string, NULL if no error*/ mehfet_hw_connect(enum mehfet_conn conn);
-void mehfet_hw_disconnect(void);
-
-void mehfet_hw_delay_ms(uint32_t t);
-void mehfet_hw_delay_us(uint32_t t);
-void mehfet_hw_timer_start(bool us, uint32_t to_reach);
-bool mehfet_hw_timer_reached(void);
-
-void mehfet_hw_set_clkspeed(bool fast);
-uint8_t mehfet_hw_get_old_lines(void);
-
-void mehfet_hw_tdio_seq(uint32_t ncyc, bool tmslvl, const uint8_t* tdi, uint8_t* tdo);
-void mehfet_hw_tms_seq(uint32_t ncyc, bool tdilvl, const uint8_t* tms);
-void mehfet_hw_tclk_edge(bool newtclk);
-void mehfet_hw_tclk_burst(uint32_t ncyc);
-
-enum mehfet_resettap_status mehfet_hw_reset_tap(enum mehfet_resettap_flags flags);
-uint8_t mehfet_hw_shift_ir(uint8_t newir);
-// drin is not const here, as the implementation may want to shuffle stuff around
-void mehfet_hw_shift_dr(uint32_t nbits, uint8_t* drin, uint8_t* drout);
-static inline uint16_t mehfet_hw_shift_dr16(uint16_t newdr) {
-    uint8_t drin[2], drout[2];
-    drin[0] = (uint8_t)newdr;
-    drin[1] = (uint8_t)(newdr >> 8);
-    mehfet_hw_shift_dr(16, drin, drout);
-    return drout[0] | ((uint16_t)drout[1] << 8);
-}
-
-#endif
-
--- a/src/m_sump/_sump.c
+++ b/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,
--- a/src/m_sump/cdc_sump.c
+++ b/src/m_sump/cdc_sump.c
@ -27,7 +27,6 @@
 */

 #include <assert.h>
-
 #include <tusb.h>

 #include "alloc.h"
--- 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"
@ -14,16 +15,6 @@
 static cothread_t vndcfg_thread;
 static uint8_t    vndcfg_stack[THREAD_STACK_SIZE];

-// shrug, somewhere
-uint8_t bitswap(uint8_t in) {
-    static const uint8_t lut[16] = {
-        0x0, 0x8, 0x4, 0xc, 0x2, 0xa, 0x6, 0xe,
-        0x1, 0x9, 0x5, 0xd, 0x3, 0xb, 0x7, 0xf
-    };
-
-    return (lut[in&0xf] << 4) | lut[in>>4];
-}
-
 static void vndcfg_thread_fn(void) {
    vnd_cfg_init();
    thread_yield();
@ -41,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

--- a/src/mode.h
+++ b/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;
--- a/src/modeset.c
+++ b/src/modeset.c
@ -5,14 +5,15 @@
 #include "alloc.h"
 #include "board.h" /* bsp_reset_bootloader() */
 #include "mode.h"
+#include "storage.h"

-extern struct mode m_01_default, m_02_isp, m_03_jscan, m_04_sump;
+extern struct mode m_01_default, m_03_jscan, m_04_sump;

 // clang-format off
 const struct mode* const mode_list[16] = {
    NULL, // dummy 0 entry
    &m_01_default, // entry 1 CANNOT be NULL!
-    &m_02_isp, //NULL, // mode 2 (hw chip programming stuff) not implemented yet
+    NULL, // mode 2 (hw chip programming stuff) not implemented yet
    &m_03_jscan,
    &m_04_sump,
    NULL, // terminating entry
@ -37,9 +38,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;
@ -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
+
--- a/src/storage.c
+++ b/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 */
+
--- a/src/storage.h
+++ b/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
+
--- a/src/storage_internal.h
+++ b/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
+
--- a/src/storage_save.c
+++ b/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
+
--- a/src/tusb_config.h
+++ b/src/tusb_config.h
@ -51,14 +51,14 @@ extern "C" {
 // Default to Highspeed for MCU with internal HighSpeed PHY (can be port specific), otherwise
 // FullSpeed
 #ifndef BOARD_DEVICE_RHPORT_SPEED
-/*#if (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX ||      \
+#if (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX ||      \
        CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX || CFG_TUSB_MCU == OPT_MCU_NUC505 || \
        CFG_TUSB_MCU == OPT_MCU_CXD56) \

 #define BOARD_DEVICE_RHPORT_SPEED OPT_MODE_HIGH_SPEED
-#else*/
+#else
 #define BOARD_DEVICE_RHPORT_SPEED OPT_MODE_FULL_SPEED
-//#endif
+#endif
 #endif

 // Device mode with rhport and speed defined by board.mk
@ -106,7 +106,7 @@ extern "C" {
 //------------- CLASS -------------//
 #define CFG_TUD_MSC  0
 #define CFG_TUD_MIDI 0
-#define CFG_TUD_ECM_RNDIS  0
+#define CFG_TUD_NET  0
 // see also: bsp/<family>/bsp-info.h

 #define CFG_TUD_HID_EP_BUFSIZE 64
--- a/src/util.h
+++ b/src/util.h
@ -15,7 +15,5 @@ uint8_t get_unique_id_u8 (uint8_t * desc_str);
 uint8_t get_unique_id_u16(uint16_t* desc_str);
 // clang-format on

-uint8_t bitswap(uint8_t in);
-
 #endif

--- a/src/vnd_cfg.c
+++ b/src/vnd_cfg.c
@ -12,12 +12,14 @@

 #if CFG_TUD_VENDOR > 0
 static uint8_t rx_buf[CFG_TUD_VENDOR_TX_BUFSIZE];
-static uint8_t tx_buf[CFG_TUD_VENDOR_RX_BUFSIZE];
+static uint8_t tx_buf[CFG_TUD_VENDOR_TX_BUFSIZE];

 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;
@ -30,7 +32,6 @@ void vnd_cfg_set_itf_num(int itf) {
    VND_N_CFG = itf;
 }

-// TODO: this is duplicated several times over the codebase, maybe reduce this
 uint8_t vnd_cfg_read_byte(void) {
    while (rxavail <= 0) {
        if (!tud_vendor_n_mounted(VND_N_CFG) || !tud_vendor_n_available(VND_N_CFG)) {
@ -192,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;
--- a/src/vnd_cfg.h
+++ b/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
--- a/2
+++ b/2
@ -1 +1 @@
-Subproject commit 4bfab30c02279a0530e1a56f4a7c539f2d35a293
+Subproject commit d49938d0f5052bce70e55c652b657c0a6a7e84fe
Author	SHA1	Message	Date
Triss	493be92bd2	better dpctl output for persistent storage	2021-09-25 13:58:06 +02:00
Triss	383dcc4ea9	clear remaining 32 reserved bytes	2021-09-25 13:58:06 +02:00
Triss	07db20ecb6	get storage to work now	2021-09-25 13:58:06 +02:00
Triss	6a4eafb96e	dpctl storage stuff (untested)	2021-09-25 13:58:06 +02:00
Triss	9d2a970161	storage impl for modes	2021-09-25 13:58:06 +02:00
Triss	1318e0f9c2	i think thats a bug	2021-09-25 13:58:06 +02:00
Triss	4015b7bf42	PERSISTENT_STORAGE usage flag	2021-09-25 13:58:05 +02:00
Triss	befced7132	finish storage base	2021-09-25 13:57:14 +02:00
Triss	36b55483a8	fix compilation, more stuff	2021-09-25 13:57:14 +02:00
Triss	3c3795f8cb	initial persistent storage code	2021-09-25 13:57:14 +02:00