diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index 02d2b35..fcfb9a3 100644
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -20,13 +20,13 @@ jobs:
       - name: Install Arm GNU Toolchain (arm-none-eabi-gcc)
         uses: carlosperate/arm-none-eabi-gcc-action@v1
         with:
-          release: '12.3.Rel1'
+          release: '13.2.Rel1'
 
       # Since we reference stable versions of Pico-SDK and pico-extras, we can cache their downloads.
       # If this were to reference changing branches (like "master"), this caching step must be removed!!!
       - name: Cache Pico-SDK and Extras 
         id: cache-sdk
-        uses: actions/cache@v3
+        uses: actions/cache@v4
         with:
           path: ${{github.workspace}}/pico/
           key: ${{ env.PICOSDK_VER }}-${{ env.PICOEXTRA_VER }}          
@@ -52,58 +52,18 @@ jobs:
       - name: Patch Pico SDK to allow LTO
         run: sed -i 's/WRAPPER_FUNC(x) __wrap_/WRAPPER_FUNC(x) __attribute__((used)) __wrap_/' ${{env.PICO_SDK_PATH}}/src/rp2_common/pico_platform/include/pico/platform.h
 
-      - name: Build GUS Firmware
+      - name: Build Mega Firmware
         shell: bash
         working-directory: ${{github.workspace}}/build
         run: |
           mkdir -p $OUTPUT_DIR
-          cmake $GITHUB_WORKSPACE/sw -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DPROJECT_TYPE="GUS" -DUSE_LTO=1 -DUSB_JOYSTICK=1
+          cmake $GITHUB_WORKSPACE/sw -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DPROJECT_TYPE="MULTIFW" -DUSE_LTO=1 -DUSB_JOYSTICK=1
           cmake --build . --config $BUILD_TYPE --parallel $(nproc)
-          cp picogus.uf2 $OUTPUT_DIR/pg-gus.uf2
-
-      - name: Build SB Firmware
-        working-directory: ${{github.workspace}}/build
-        shell: bash
-        run: |
-          cmake $GITHUB_WORKSPACE/sw -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DPROJECT_TYPE="SB" -DUSE_LTO=1 -DUSB_JOYSTICK=1
-          cmake --build . --config $BUILD_TYPE --parallel $(nproc)
-          cp picogus.uf2 $OUTPUT_DIR/pg-sb.uf2
-
-      - name: Build MPU401 Firmware
-        working-directory: ${{github.workspace}}/build
-        shell: bash
-        run: |
-          cmake $GITHUB_WORKSPACE/sw -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DPROJECT_TYPE="MPU" -DUSE_LTO=1 -DUSB_JOYSTICK=1
-          cmake --build . --config $BUILD_TYPE --parallel $(nproc)
-          cp picogus.uf2 $OUTPUT_DIR/pg-mpu.uf2
-
-      - name: Build Tandy Firmware
-        working-directory: ${{github.workspace}}/build
-        shell: bash
-        run: |
-          cmake $GITHUB_WORKSPACE/sw -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DPROJECT_TYPE="TANDY" -DUSE_LTO=1 -DUSB_JOYSTICK=1
-          cmake --build . --config $BUILD_TYPE --parallel $(nproc)
-          cp picogus.uf2 $OUTPUT_DIR/pg-tandy.uf2
-
-      - name: Build CMS Firmware
-        working-directory: ${{github.workspace}}/build
-        shell: bash
-        run: |
-          cmake $GITHUB_WORKSPACE/sw -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DPROJECT_TYPE="CMS" -DUSE_LTO=1 -DUSB_JOYSTICK=1
-          cmake --build . --config $BUILD_TYPE --parallel $(nproc)
-          cp picogus.uf2 $OUTPUT_DIR/pg-cms.uf2
-
-      - name: Build Joystick Only Firmware
-        working-directory: ${{github.workspace}}/build
-        shell: bash
-        run: |
-          cmake $GITHUB_WORKSPACE/sw -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DPROJECT_TYPE="JOY" -DUSE_LTO=1
-          cmake --build . --config $BUILD_TYPE --parallel $(nproc)
-          cp picogus.uf2 $OUTPUT_DIR/pg-joyex.uf2
+          cp picogus.uf2 $OUTPUT_DIR/picogus.uf2
 
       # will generate PicoGUS Firmwares.zip as downloadable artifact with all .uf2 files
       - name: Upload All Firmwares
-        uses: actions/upload-artifact@v3
+        uses: actions/upload-artifact@v4
         with:
           name: PicoGUS Firmwares
           path: ${{env.OUTPUT_DIR}}
@@ -134,7 +94,7 @@ jobs:
 
       # will generate pgusinit.zip as downloadable artifact with pgusinit.exe
       - name: Upload pgusinit
-        uses: actions/upload-artifact@v3
+        uses: actions/upload-artifact@v4
         with:
           name: pgusinit
           path: ${{env.OUTPUT_DIR}}/pgusinit.exe
@@ -159,12 +119,12 @@ jobs:
         run: |
           awk '/^# v${{ steps.tag.outputs.version }}/{a=1;next}/^# /{a=0}a' sw/CHANGELOG.md > release-body.md
       - name: Download firmware artifact
-        uses: actions/download-artifact@v3
+        uses: actions/download-artifact@v4
         with:
           name: PicoGUS Firmwares
           path: ${{env.STAGING_DIR}}
       - name: Download pgusinit artifact
-        uses: actions/download-artifact@v3
+        uses: actions/download-artifact@v4
         with:
           name: pgusinit
           path: ${{env.STAGING_DIR}}
diff --git a/.gitignore b/.gitignore
index 78d10b7..ee565cf 100644
--- a/.gitignore
+++ b/.gitignore
@@ -8,4 +8,7 @@
 build/
 *.kicad_prl
 fp-info-cache
-.vscode
\ No newline at end of file
+.vscode
+
+firmware.ld
+flash_firmware.h
diff --git a/sw/uf2.h b/multifw/uf2.h
similarity index 100%
rename from sw/uf2.h
rename to multifw/uf2.h
diff --git a/sw/uf2create.c b/multifw/uf2create.c
similarity index 89%
rename from sw/uf2create.c
rename to multifw/uf2create.c
index 2d70567..b5a76dc 100644
--- a/sw/uf2create.c
+++ b/multifw/uf2create.c
@@ -84,6 +84,11 @@ int main(int argc, char *argv[])
 
 	fclose(f_output);
 
+        printf("Creating all.bin");
+        f_output = fopen("all.bin", "wb");
+        fwrite(flashbuffer, total_size, 1, f_output);
+        fclose(f_output);
+
 	printf("Done!\n");
 
 	return 0;
diff --git a/pgusinit/pgusinit.c b/pgusinit/pgusinit.c
index ebfe069..bc53155 100755
--- a/pgusinit/pgusinit.c
+++ b/pgusinit/pgusinit.c
@@ -31,8 +31,18 @@ typedef enum {
     JOYSTICK_ONLY_MODE = 0x0f
 } card_mode_t;
 
+static const char *fwnames[8] = {
+    "INVALID",
+    "GUS",
+    "SB",
+    "MPU",
+    "TANDY",
+    "CMS",
+    "JOY"
+};
+
 void banner(void) {
-    printf("PicoGUSinit v2.1.6 (c) 2024 Ian Scott - licensed under the GNU GPL v2\n\n");
+    printf("PicoGUSinit v3.0.0 (c) 2024 Ian Scott - licensed under the GNU GPL v2\n\n");
 }
 
 const char* usage_by_card[] = {
@@ -60,9 +70,8 @@ void usage(char *argv0, card_mode_t mode) {
     fprintf(stderr, "    /j        - enable USB joystick support\n");
     fprintf(stderr, "    /v x      - set volume of the wavetable header. Scale 0-100, Default: 100\n");
     fprintf(stderr, "                (for PicoGUS v2.0 boards only)\n");
-    fprintf(stderr, "    /m x [d]  - change card mode to x (1=gus,2=sb,3=mpu,4=tandy,5=cms,6=joy)\n");
-    fprintf(stderr, "                the optional parameter 'd' makes the mode permanent at boot\n");
-    fprintf(stderr, "                (Only if pg-multi.uf2 is flashed)\n");
+    fprintf(stderr, "    /m x      - change card mode to x (gus, sb, mpu, tandy, cms, joy)\n");
+    fprintf(stderr, "    /s        - save settings to the card to persist on system boot\n");
     if (mode > GUS_MODE && mode < JOYSTICK_ONLY_MODE) {
         fprintf(stderr, "Sound Blaster/AdLib, MPU-401, Tandy, CMS modes only:\n");
         fprintf(stderr, "    /p x - set the (hex) base port address of the emulated card. Defaults:\n");
@@ -76,7 +85,7 @@ void usage(char *argv0, card_mode_t mode) {
     }
     if (mode == MPU_MODE) {
         fprintf(stderr, "MPU-401 mode only:\n");
-        fprintf(stderr, "    /s   - delay SYSEX (for rev.0 Roland MT-32)\n");
+        fprintf(stderr, "    /x   - delay SYSEX (for rev.0 Roland MT-32)\n");
         fprintf(stderr, "    /n   - fake all notes off (for Roland RA-50)\n");
         //              "................................................................................\n"
     }
@@ -174,7 +183,7 @@ void print_firmware_string(void) {
 }
 
 
-bool wait_for_read(uint8_t value) {
+bool wait_for_read(const uint8_t value) {
     for (uint32_t i = 0; i < 6000000; ++i) {    // Up to 6000000, for bigger fws like pg-multi.uf2, waiting for flash erase. If not, timeout and error.
         if (inp(DATA_PORT_HIGH) == value) {
             return true;
@@ -183,25 +192,29 @@ bool wait_for_read(uint8_t value) {
     return false;
 }
 
+void write_settings(void) {
+    outp(CONTROL_PORT, 0xE1); // Select save settings register
+    outp(DATA_PORT_HIGH, 0xff);
+    printf("Settings saved to flash.\n");
+    delay(100);
+}
+
 // For multifw
-int reboot_to_firmware(uint8_t value, int mode) {
-
-    const char *fwnames[6];
-    fwnames[0] = "GUS";
-    fwnames[1] = "SB";
-    fwnames[2] = "MPU";
-    fwnames[3] = "TANDY";
-    fwnames[4] = "CMS";
-    fwnames[5] = "JOY";
-
+int reboot_to_firmware(const uint8_t value, const bool permanent) {
     outp(CONTROL_PORT, 0xCC); // Knock on the door...
 
     outp(CONTROL_PORT, 0xE0); // Select firmware selection register
-    outpw(DATA_PORT_LOW, (0x0000 + value + (mode << 8))); // Send firmware number and permanent flag
+    outp(DATA_PORT_HIGH, value); // Send firmware number and permanent flag
+            delay(100);
 
-    printf("\nMode change requested. Rebooting to fw: %d (%s)...\n", value, fwnames[value-1]);
-    if (mode)
-            printf("%s mode selected as permanent on system boot.\n", fwnames[value-1]);
+    printf("\nMode change requested.\n");
+    if (permanent) {
+        write_settings();
+    }
+    printf("Rebooting to fw: %s...\n", fwnames[value]);
+    outp(CONTROL_PORT, 0xE2); // Select reboot register
+    outp(DATA_PORT_HIGH, 0xff);
+    delay(100);
 
     // Wait for card to reboot to new firmware
     if (!wait_for_read(0xDD)) {
@@ -327,8 +340,9 @@ int main(int argc, char* argv[]) {
     uint8_t mpu_fakeallnotesoff = 0;
     uint8_t enable_joystick = 0;
     uint8_t adlib_wait = 0;
-    uint8_t fwnum = 7;
-    int pMode = 0;
+    char mode_name[8] = {0};
+    int fw_num = 8;
+    bool permanent = false;
 
     banner();
     // Get magic value from port on PicoGUS that is not on real GUS
@@ -359,16 +373,11 @@ int main(int argc, char* argv[]) {
                 usage(argv[0], mode);
                 return 255;
             }
-            e = sscanf(argv[++i], "%hu", &fwnum);
-            if (e != 1 || fwnum < 1 || fwnum> 6) {
+            e = sscanf(argv[++i], "%7s", mode_name);
+            if (e != 1) {
                 usage(argv[0], mode);
                 return 5;
             }
-            if (argc >= i + 2) {
-                if (!stricmp(argv[++i], "d")) {
-                    pMode = 1;
-                }               
-            }
         } else if (stricmp(argv[i], "/4") == 0) {
             force_44k = 1;
         } else if (stricmp(argv[i], "/a") == 0) {
@@ -423,7 +432,7 @@ int main(int argc, char* argv[]) {
                 usage(argv[0], mode);
                 return 4;
             }
-        } else if (stricmp(argv[i], "/s") == 0) {
+        } else if (stricmp(argv[i], "/x") == 0) {
             mpu_delaysysex = 1;
         } else if (stricmp(argv[i], "/n") == 0) {
             mpu_fakeallnotesoff = 1;
@@ -437,6 +446,8 @@ int main(int argc, char* argv[]) {
                 usage(argv[0], mode);
                 return 5;
             }
+        } else if (stricmp(argv[i], "/s") == 0) {
+            permanent = true;
         }
         ++i;
     }
@@ -456,8 +467,19 @@ int main(int argc, char* argv[]) {
         return write_firmware(fw_filename, protocol_got);
     }
 
-    if (fwnum < 7) {
-        return reboot_to_firmware(fwnum, pMode);
+    if (mode_name[0]) {
+        int i;
+        for (i = 1; i < 8; ++i) {
+            if (strnicmp(fwnames[i], mode_name, 7) == 0) {
+                fw_num = i;
+                break;
+            }
+        }
+        if (i == 8) {
+            usage(argv[0], mode);
+            return 255;
+        }
+        return reboot_to_firmware(fw_num, permanent);
     }
 
     outp(CONTROL_PORT, 0xf0); // Select hardware type register
@@ -558,5 +580,9 @@ int main(int argc, char* argv[]) {
     }
     printf("PicoGUS initialized!\n");
 
+    if (permanent) {
+        write_settings();
+    }
+
     return 0;
 }
diff --git a/sw/CMakeLists.txt b/sw/CMakeLists.txt
index a97a292..31429bb 100644
--- a/sw/CMakeLists.txt
+++ b/sw/CMakeLists.txt
@@ -3,7 +3,6 @@
 cmake_minimum_required(VERSION 3.13)
 
 
-
 # Initialise pico_sdk from installed location
 # (note this can come from environment, CMake cache etc)
 # to manually set Pico SDK path, uncomment this with your path.
@@ -14,18 +13,15 @@ cmake_minimum_required(VERSION 3.13)
 include(pico_sdk_import.cmake)
 
 project(picogus
-    VERSION "1.2.6"
+    VERSION "2.0.0"
     LANGUAGES C CXX ASM
 )
 
-if( NOT (PROJECT_TYPE STREQUAL "BOOT"))
-
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_CXX_STANDARD 17)
 
 include(pico_extras_import.cmake)
 
-
 set(PICO_BOARD_HEADER_DIRS ${CMAKE_CURRENT_LIST_DIR})
 set(PICO_BOARD picogus2)
 set(PICO_COPY_TO_RAM 1)
@@ -34,15 +30,11 @@ set(PICO_COPY_TO_RAM 1)
 # This is needed to support tusb_xinput
 set(PICO_TINYUSB_PATH ${CMAKE_CURRENT_LIST_DIR}/tinyusb)
 
-endif() #NOT BOOT
-
 #set(PICO_DEOPTIMIZED_DEBUG "1")
 #set(CMAKE_BUILD_TYPE "Debug")
 set(CMAKE_BUILD_TYPE "Release")
 #set(SKIP_PICO_MALLOC 1)
 
-
-
 if(NOT PROJECT_TYPE)
     set(PROJECT_TYPE "GUS")
     message("PROJECT_TYPE not set, defaulting to ${PROJECT_TYPE}")
@@ -52,69 +44,100 @@ endif()
 pico_sdk_init()
 
 ################################################################################
-# Helper function
+# Helper function for setting linker script
 function(set_linker_script TARGET script)
-    target_link_directories(${TARGET} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
-    pico_set_linker_script(${TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/${script})
+    target_link_directories(${TARGET} PRIVATE ${CMAKE_SOURCE_DIR}/multifw)
+    pico_set_linker_script(${TARGET} ${script})
 
     # Add dependencies on the 'included' linker scripts so that the target gets
     # rebuilt if they are changed
-    pico_add_link_depend(${TARGET} ${CMAKE_CURRENT_SOURCE_DIR}/${script})
+    pico_add_link_depend(${TARGET} ${script})
 endfunction()
 
-if(PROJECT_TYPE STREQUAL "BOOT")
-
-add_executable(bootloader)
-target_sources(bootloader PRIVATE bootloader.c)
-target_link_libraries(bootloader PRIVATE pico_stdlib hardware_flash pico_multicore)
-pico_add_extra_outputs(bootloader)
-set_linker_script(bootloader bootloader.ld)
-
-else()
-################################################################################
-
-set(USB_JOYSTICK 1) 
-
-if(USE_LTO)
-    # Pico SDK must be patched to allow for LTO so it is not enabled by default.
-    # To patch to Pico SDK, run:
-    # sed -i 's/WRAPPER_FUNC(x) __wrap_/WRAPPER_FUNC(x) __attribute__((used)) __wrap_/' "$PICO_SDK_PATH"/src/rp2_common/pico_platform/include/pico/platform.h
-    add_compile_options(-flto=jobserver)
-endif()
-
-# Add executable. Default name is the project name, version 0.1
-add_executable(picogus picogus.cpp)
-pico_set_program_version(picogus ${CMAKE_PROJECT_VERSION})
 
+# Global definitions across all targets
 add_compile_definitions(
+    RP2_CLOCK_SPEED=366000
+    PICO_DEFAULT_UART=0
+    PICO_DEFAULT_UART_TX_PIN=28
+    PICO_DEFAULT_UART_RX_PIN=-1
+    PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS=250
+)
+
+################################################################################
+# Helper function for common firmware build
+function(config_target TARGET_NAME MULTIFW)
+    # Add executable. Default name is the project name, version 0.1
+    add_executable(${TARGET_NAME} picogus.cpp)
+    pico_set_program_version(${TARGET_NAME} ${CMAKE_PROJECT_VERSION})
+
+    target_compile_definitions(${TARGET_NAME} PRIVATE
         USE_AUDIO_I2S=1
         PICO_AUDIO_I2S_DATA_PIN=16
         PICO_AUDIO_I2S_CLOCK_PIN_BASE=17
-        PICO_DEFAULT_UART=0
-        PICO_DEFAULT_UART_TX_PIN=28
-        PICO_DEFAULT_UART_RX_PIN=-1
         PICO_AUDIO_I2S_PIO=1
-        # USE_IRQ=1
         USE_ALARM=1
-        PICO_STDIO_USB_CONNECT_WAIT_TIMEOUT_MS=250
         M62429_PIO=1
-)
+    )
 
-target_sources(picogus PRIVATE pico_reflash.c)
-
-target_compile_definitions(picogus PRIVATE PICO_FLASH_SPI_CLKDIV=4)
-
-if(PROJECT_TYPE STREQUAL "GUS")
-    # target_compile_options(picogus PRIVATE -save-temps -fverbose-asm)
-    if(NOT GUS_DEFAULT_PORT)
-        set(GUS_DEFAULT_PORT "0x240")
-        message("GUS_DEFAULT_PORT not set, defaulting to ${GUS_DEFAULT_PORT}")
+    if(USE_LTO)
+        # Pico SDK must be patched to allow for LTO so it is not enabled by default.
+        # To patch to Pico SDK, run:
+        # sed -i 's/WRAPPER_FUNC(x) __wrap_/WRAPPER_FUNC(x) __attribute__((used)) __wrap_/' "$PICO_SDK_PATH"/src/rp2_common/pico_platform/include/pico/platform.h
+        target_compile_options(${TARGET_NAME} PRIVATE -flto=jobserver)
     endif()
-    pico_set_program_name(picogus "picogus-gus")
-    target_sources(picogus PRIVATE gusplay.cpp pico_pic.c isa_dma.c)
-    target_compile_definitions(picogus PRIVATE
+
+    target_sources(${TARGET_NAME} PRIVATE pico_reflash.c flash_settings.c)
+    # target_compile_options(${TARGET_NAME} PRIVATE -save-temps -fverbose-asm)
+    if(MULTIFW)
+        set_linker_script(${TARGET_NAME} ${CMAKE_BINARY_DIR}/${TARGET_NAME}.ld)
+    endif()
+
+    if(USB_JOYSTICK)
+        # Joystick stuff
+        target_sources(${TARGET_NAME} PRIVATE joy_hid/joy_hid.c)
+        target_link_libraries(${TARGET_NAME} hardware_pwm)
+        target_include_directories(${TARGET_NAME} PUBLIC ${CMAKE_CURRENT_LIST_DIR}/joy_hid)
+        target_link_libraries(${TARGET_NAME} tinyusb_host)
+        target_link_libraries(${TARGET_NAME} xinput_host_app_driver)
+        target_compile_definitions(${TARGET_NAME} PRIVATE USB_JOYSTICK=1)
+    endif()
+
+    target_sources(${TARGET_NAME} PRIVATE M62429/M62429.cpp)
+    pico_generate_pio_header(${TARGET_NAME} ${CMAKE_CURRENT_LIST_DIR}/M62429/M62429.pio)
+    pico_generate_pio_header(${TARGET_NAME} ${CMAKE_CURRENT_LIST_DIR}/isa_io.pio)
+
+    target_link_libraries(
+        ${TARGET_NAME}
+        pico_stdlib
+        pico_multicore
+        pico_audio_i2s
+        pico_flash
+        hardware_pio
+        hardware_pll
+        hardware_flash
+        hardware_adc
+        hardware_exception
+    )
+    pico_add_extra_outputs(${TARGET_NAME})
+endfunction()
+
+if(USB_JOYSTICK)
+    # Joystick stuff
+    add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/tusb_xinput)
+endif()
+add_subdirectory(rp2040-psram)
+add_subdirectory(opl)
+
+
+################################################################################
+# Build GUS firmware
+function(build_gus TARGET_NAME MULTIFW)
+    config_target(${TARGET_NAME} ${MULTIFW})
+    pico_set_program_name(${TARGET_NAME} "picogus-gus")
+    target_sources(${TARGET_NAME} PRIVATE gusplay.cpp pico_pic.c isa_dma.c)
+    target_compile_definitions(${TARGET_NAME} PRIVATE
         SOUND_GUS=1
-        GUS_DEFAULT_PORT=${GUS_DEFAULT_PORT}
         PSRAM=1
         TEST_PSRAM=1
         PSRAM_ASYNC=1
@@ -133,33 +156,21 @@ if(PROJECT_TYPE STREQUAL "GUS")
         SCALE_22K_TO_44K=1
         # FORCE_28CH_27CH=1
     )
-    add_subdirectory(rp2040-psram)
-    target_link_libraries(picogus rp2040-psram hardware_interp)
-    # pico_generate_pio_header(picogus ${CMAKE_CURRENT_LIST_DIR}/psram_spi.pio)
-    set(ASYNC_UART 0)
-    if(ASYNC_UART)
-        add_compile_definitions(
-            ASYNC_UART=1
-        )
-        pico_enable_stdio_uart(picogus 0)
-    else()
-        pico_enable_stdio_uart(picogus 1)
-    endif()
-    pico_enable_stdio_usb(picogus 0)
-    pico_enable_stdio_semihosting(picogus 0)
+    pico_generate_pio_header(${TARGET_NAME} ${CMAKE_CURRENT_LIST_DIR}/isa_dma.pio)
+    target_link_libraries(${TARGET_NAME} rp2040-psram hardware_interp)
+    pico_enable_stdio_uart(${TARGET_NAME} 1)
+    pico_enable_stdio_usb(${TARGET_NAME} 0)
+    pico_enable_stdio_semihosting(${TARGET_NAME} 0)
+endfunction()
 
-    set_linker_script(picogus firmware1.ld)
-endif()
-
-if(PROJECT_TYPE STREQUAL "SB")
-    pico_set_program_name(picogus "picogus-sb")
-    target_sources(picogus PRIVATE sbdsp.cpp sbplay.cpp pico_pic.c isa_dma.c)    
-    
-    target_link_libraries(
-        picogus        
-        opl        
-    )
-    target_compile_definitions(picogus PRIVATE
+################################################################################
+# Build SB firmware
+function(build_sb TARGET_NAME MULTIFW)
+    config_target(${TARGET_NAME} ${MULTIFW})
+    pico_set_program_name(${TARGET_NAME} "picogus-sb")
+    target_sources(${TARGET_NAME} PRIVATE sbdsp.cpp sbplay.cpp pico_pic.c isa_dma.c)    
+    target_link_libraries(${TARGET_NAME} opl)
+    target_compile_definitions(${TARGET_NAME} PRIVATE
         SOUND_SB=1
         # SOUND_OPL=1
         SOUND_DSP=1
@@ -168,137 +179,212 @@ if(PROJECT_TYPE STREQUAL "SB")
         EMU8950_ASM=1
         # PICOW=1
     )
-    add_subdirectory(opl)
-    pico_enable_stdio_uart(picogus 1)
-    pico_enable_stdio_usb(picogus 0)
-    pico_enable_stdio_semihosting(picogus 0)
+    pico_generate_pio_header(${TARGET_NAME} ${CMAKE_CURRENT_LIST_DIR}/isa_dma.pio)
+    pico_enable_stdio_uart(${TARGET_NAME} 1)
+    pico_enable_stdio_usb(${TARGET_NAME} 0)
+    pico_enable_stdio_semihosting(${TARGET_NAME} 0)
+endfunction()
 
-    set_linker_script(picogus firmware2.ld)
-endif()
-
-if(PROJECT_TYPE STREQUAL "MPU")
-    pico_set_program_name(picogus "picogus-mpu401")
-    target_compile_definitions(picogus PRIVATE
+################################################################################
+# Build MPU firmware
+function(build_mpu TARGET_NAME MULTIFW)
+    config_target(${TARGET_NAME} ${MULTIFW})
+    pico_set_program_name(${TARGET_NAME} "picogus-mpu401")
+    target_compile_definitions(${TARGET_NAME} PRIVATE
         SOUND_MPU=1
     )
-    target_sources(picogus PRIVATE
+    target_sources(${TARGET_NAME} PRIVATE
         pico_pic.c
         mpu401/mpu401.c
         mpu401/midi.c
     )
-    target_sources(picogus PRIVATE mpuplay.cpp)
+    target_sources(${TARGET_NAME} PRIVATE mpuplay.cpp)
     # Use USB stdio because UART is used by MIDI, but only if USB joysick is disabled
-    pico_enable_stdio_uart(picogus 0)
+    pico_enable_stdio_uart(${TARGET_NAME} 0)
     if(USB_JOYSTICK)
-        pico_enable_stdio_usb(picogus 0)
+        pico_enable_stdio_usb(${TARGET_NAME} 0)
     else()
-        pico_enable_stdio_usb(picogus 1)
+        pico_enable_stdio_usb(${TARGET_NAME} 1)
     endif()
-    pico_enable_stdio_semihosting(picogus 0)
+    pico_enable_stdio_semihosting(${TARGET_NAME} 0)
+endfunction()
 
-    set_linker_script(picogus firmware3.ld)
-endif()
-
-if(PROJECT_TYPE STREQUAL "TANDY")
-    pico_set_program_name(picogus "picogus-tandy")
-    target_compile_definitions(picogus PRIVATE
+################################################################################
+# Build Tandy firmware
+function(build_tandy TARGET_NAME MULTIFW)
+    config_target(${TARGET_NAME} ${MULTIFW})
+    pico_set_program_name(${TARGET_NAME} "picogus-tandy")
+    target_compile_definitions(${TARGET_NAME} PRIVATE
         SOUND_TANDY=1
         # SQUARE_FLOAT_OUTPUT=1
     )
-    target_sources(picogus PRIVATE
+    target_sources(${TARGET_NAME} PRIVATE
         pico_pic.c
         square/square.cpp
     )
-    target_sources(picogus PRIVATE tandyplay.cpp)
-    pico_enable_stdio_uart(picogus 1)
-    pico_enable_stdio_usb(picogus 0)
-    pico_enable_stdio_semihosting(picogus 0)
+    target_sources(${TARGET_NAME} PRIVATE tandyplay.cpp)
+    pico_enable_stdio_uart(${TARGET_NAME} 1)
+    pico_enable_stdio_usb(${TARGET_NAME} 0)
+    pico_enable_stdio_semihosting(${TARGET_NAME} 0)
+endfunction()
 
-    set_linker_script(picogus firmware4.ld)
-endif()
-
-if(PROJECT_TYPE STREQUAL "CMS")
-    pico_set_program_name(picogus "picogus-cms")
-    target_compile_definitions(picogus PRIVATE
+################################################################################
+# Build CMS firmware
+function(build_cms TARGET_NAME MULTIFW)
+    config_target(${TARGET_NAME} ${MULTIFW})
+    pico_set_program_name(${TARGET_NAME} "picogus-cms")
+    target_compile_definitions(${TARGET_NAME} PRIVATE
         SOUND_CMS=1
-        # SQUARE_FLOAT_OUTPUT=1
     )
-
-    target_sources(picogus PRIVATE
+    target_sources(${TARGET_NAME} PRIVATE
         pico_pic.c
         square/square.cpp
     )
+    target_sources(${TARGET_NAME} PRIVATE cmsplay.cpp)
+    pico_enable_stdio_uart(${TARGET_NAME} 1)
+    pico_enable_stdio_usb(${TARGET_NAME} 0)
+    pico_enable_stdio_semihosting(${TARGET_NAME} 0)
+endfunction()
 
-    target_sources(picogus PRIVATE cmsplay.cpp)
-    pico_enable_stdio_uart(picogus 1)
-    pico_enable_stdio_usb(picogus 0)
-    pico_enable_stdio_semihosting(picogus 0)
-
-    set_linker_script(picogus firmware5.ld)
-endif()
-
-if(PROJECT_TYPE STREQUAL "JOY")
-    pico_set_program_name(picogus "picogus-joy")
+################################################################################
+# Build joystick-only firmware
+function(build_joy TARGET_NAME MULTIFW)
+    config_target(${TARGET_NAME} ${MULTIFW})
+    pico_set_program_name(${TARGET_NAME} "picogus-joy")
     set(USB_JOYSTICK 1)
-    target_compile_definitions(picogus PRIVATE
+    target_compile_definitions(${TARGET_NAME} PRIVATE
         USB_JOYSTICK_ONLY=1
     )
  
-    target_sources(picogus PRIVATE
+    target_sources(${TARGET_NAME} PRIVATE
         usbplay.cpp
     )
-     pico_enable_stdio_uart(picogus 1)
-     pico_enable_stdio_usb(picogus 0)
-     pico_enable_stdio_semihosting(picogus 0)
+     pico_enable_stdio_uart(${TARGET_NAME} 1)
+     pico_enable_stdio_usb(${TARGET_NAME} 0)
+     pico_enable_stdio_semihosting(${TARGET_NAME} 0)
+endfunction()
 
-    set_linker_script(picogus firmware6.ld)
-endif()
 
-if(USB_JOYSTICK)
-    # Joystick stuff
-    target_sources(picogus PRIVATE joy_hid/joy_hid.c)
-    target_link_libraries(picogus hardware_pwm)
-    target_include_directories(picogus PUBLIC ${CMAKE_CURRENT_LIST_DIR}/joy_hid)
-    target_link_libraries(picogus tinyusb_host)
-    add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/tusb_xinput)
-    target_link_libraries(picogus xinput_host_app_driver)
-    target_compile_definitions(picogus PRIVATE USB_JOYSTICK=1)
-endif()
-
-target_sources(picogus PRIVATE M62429/M62429.cpp)
-pico_generate_pio_header(picogus ${CMAKE_CURRENT_LIST_DIR}/M62429/M62429.pio)
-
-pico_generate_pio_header(picogus ${CMAKE_CURRENT_LIST_DIR}/isa_io.pio)
-pico_generate_pio_header(picogus ${CMAKE_CURRENT_LIST_DIR}/isa_dma.pio)
-
-target_link_libraries(
-    picogus
-    pico_stdlib
-    pico_multicore
-    pico_audio_i2s
-    hardware_pio
-    hardware_pll
-    hardware_flash
-    hardware_adc
-)
-
-if(ASYNC_UART)
-    add_subdirectory(stdio_async_uart)
-    target_link_libraries(
-        picogus
-        stdio_async_uart
+################################################################################
+# Build "multifw" - all firmwares in one UF2
+if(PROJECT_TYPE STREQUAL "MULTIFW")
+    set(FW_TARGET picogus)
+    add_custom_command(
+        OUTPUT
+            "${CMAKE_BINARY_DIR}/generated/multifw/flash_firmware.h"
+            "${CMAKE_BINARY_DIR}/pg-gus-multifw.ld"
+            "${CMAKE_BINARY_DIR}/pg-sb-multifw.ld"
+            "${CMAKE_BINARY_DIR}/pg-mpu-multifw.ld"
+            "${CMAKE_BINARY_DIR}/pg-tandy-multifw.ld"
+            "${CMAKE_BINARY_DIR}/pg-cms-multifw.ld"
+            "${CMAKE_BINARY_DIR}/pg-joy-multifw.ld"
+        COMMAND
+            ${CMAKE_COMMAND}
+            -DFLASH_FIRMWARE_H_IN="${CMAKE_SOURCE_DIR}/multifw/flash_firmware.h.in"
+            -DFLASH_FIRMWARE_H="${CMAKE_BINARY_DIR}/generated/multifw/flash_firmware.h"
+            -DFW_LD_IN="${CMAKE_SOURCE_DIR}/multifw/firmware.ld.in"
+            -DFW_1_BIN="${CMAKE_BINARY_DIR}/pg-gus.bin"
+            -DFW_1_LD="${CMAKE_BINARY_DIR}/pg-gus-multifw.ld"
+            -DFW_2_BIN="${CMAKE_BINARY_DIR}/pg-sb.bin"
+            -DFW_2_LD="${CMAKE_BINARY_DIR}/pg-sb-multifw.ld"
+            -DFW_3_BIN="${CMAKE_BINARY_DIR}/pg-mpu.bin"
+            -DFW_3_LD="${CMAKE_BINARY_DIR}/pg-mpu-multifw.ld"
+            -DFW_4_BIN="${CMAKE_BINARY_DIR}/pg-tandy.bin"
+            -DFW_4_LD="${CMAKE_BINARY_DIR}/pg-tandy-multifw.ld"
+            -DFW_5_BIN="${CMAKE_BINARY_DIR}/pg-cms.bin"
+            -DFW_5_LD="${CMAKE_BINARY_DIR}/pg-cms-multifw.ld"
+            -DFW_6_BIN="${CMAKE_BINARY_DIR}/pg-joy.bin"
+            -DFW_6_LD="${CMAKE_BINARY_DIR}/pg-joy-multifw.ld"
+            -P ${CMAKE_SOURCE_DIR}/multifw/generate_ld.cmake
+        DEPENDS
+            "${CMAKE_SOURCE_DIR}/multifw/firmware.ld.in"
+            pg-gus
+            pg-sb
+            pg-mpu
+            pg-tandy
+            pg-cms
+            pg-joy
     )
+    add_custom_target(generate_flash_firmware_h DEPENDS
+        "${CMAKE_BINARY_DIR}/generated/multifw/flash_firmware.h"
+        "${CMAKE_BINARY_DIR}/pg-gus-multifw.ld"
+        "${CMAKE_BINARY_DIR}/pg-sb-multifw.ld"
+        "${CMAKE_BINARY_DIR}/pg-mpu-multifw.ld"
+        "${CMAKE_BINARY_DIR}/pg-tandy-multifw.ld"
+        "${CMAKE_BINARY_DIR}/pg-cms-multifw.ld"
+        "${CMAKE_BINARY_DIR}/pg-joy-multifw.ld"
+    )
+
+    build_gus(pg-gus FALSE)
+    build_gus(pg-gus-multifw TRUE)
+    add_dependencies(pg-gus-multifw pg-gus generate_flash_firmware_h)
+    build_sb(pg-sb FALSE)
+    build_sb(pg-sb-multifw TRUE)
+    add_dependencies(pg-sb-multifw pg-sb generate_flash_firmware_h)
+    build_mpu(pg-mpu FALSE)
+    build_mpu(pg-mpu-multifw TRUE)
+    add_dependencies(pg-mpu-multifw pg-mpu generate_flash_firmware_h)
+    build_tandy(pg-tandy FALSE)
+    build_tandy(pg-tandy-multifw TRUE)
+    add_dependencies(pg-tandy-multifw pg-tandy generate_flash_firmware_h)
+    build_cms(pg-cms FALSE)
+    build_cms(pg-cms-multifw TRUE)
+    add_dependencies(pg-cms-multifw pg-cms generate_flash_firmware_h)
+    build_joy(pg-joy FALSE)
+    build_joy(pg-joy-multifw TRUE)
+    add_dependencies(pg-joy-multifw pg-joy generate_flash_firmware_h)
+
+    add_executable(bootloader)
+    target_sources(bootloader PRIVATE multifw/bootloader.c flash_settings.c)
+    target_link_libraries(bootloader PRIVATE pico_stdlib hardware_flash pico_flash hardware_exception)
+    pico_add_extra_outputs(bootloader)
+    # set_linker_script(bootloader ${CMAKE_SOURCE_DIR}/multifw/bootloader.ld)
+    target_include_directories(bootloader PRIVATE ${CMAKE_BINARY_DIR}/generated/multifw)
+    pico_enable_stdio_uart(bootloader 1)
+    pico_enable_stdio_usb(bootloader 0)
+    pico_enable_stdio_semihosting(bootloader 0)
+    add_dependencies(bootloader generate_flash_firmware_h)
+
+    if (NOT UF2CREATE_FOUND)
+        set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/multifw)
+        # Needs to be an external project so it can be built as a native binary outside of the Pico SDK
+        find_package(UF2CREATE)
+        # Rebuild uf2create if flash_firmware.h is regenerated
+        ExternalProject_Add_StepDependencies(UF2CREATEBuild build generate_flash_firmware_h)
+    endif()
+    add_custom_command(
+        OUTPUT picogus.uf2
+        DEPENDS bootloader pg-gus-multifw pg-sb-multifw pg-mpu-multifw pg-tandy-multifw pg-cms-multifw pg-joy-multifw
+        COMMAND UF2CREATE bootloader.bin pg-gus-multifw.bin pg-sb-multifw.bin pg-mpu-multifw.bin pg-tandy-multifw.bin pg-cms-multifw.bin pg-joy-multifw.bin picogus.uf2
+    )
+    add_custom_target(picogus ALL DEPENDS picogus.uf2)
+
+################################################################################
+# Builds for individual "legacy" firmware modes
+elseif(PROJECT_TYPE STREQUAL "GUS")
+    set(FW_TARGET pg-gus)
+    build_gus(pg-gus FALSE)
+elseif(PROJECT_TYPE STREQUAL "SB")
+    set(FW_TARGET pg-sb)
+    build_sb(pg-sb FALSE)
+elseif(PROJECT_TYPE STREQUAL "MPU")
+    set(FW_TARGET pg-mpu)
+    build_mpu(pg-mpu FALSE)
+elseif(PROJECT_TYPE STREQUAL "TANDY")
+    set(FW_TARGET pg-tandy)
+    build_tandy(pg-tandy FALSE)
+elseif(PROJECT_TYPE STREQUAL "CMS")
+    set(FW_TARGET pg-cms)
+    build_cms(pg-cms FALSE)
+elseif(PROJECT_TYPE STREQUAL "JOY")
+    set(FW_TARGET pg-joyex)
+    build_joy(pg-joyex FALSE)
 endif()
 
-pico_add_extra_outputs(picogus)
-# print size
-add_custom_command(TARGET ${PROJECT_NAME} POST_BUILD
-    COMMAND arm-none-eabi-size --format=berkeley "${PROJECT_NAME}.elf")
-
+################################################################################
+# Optional target to program built firmware via openocd
 add_custom_target(
     program
-    DEPENDS picogus
-    COMMAND openocd -f interface/cmsis-dap.cfg -f target/rp2040.cfg -c "adapter speed 5000\; program picogus.elf verify reset exit"
+    DEPENDS ${FW_TARGET}
+    COMMAND openocd -f interface/cmsis-dap.cfg -f target/rp2040.cfg -c "adapter speed 5000\; program ${FW_TARGET}.bin verify reset exit 0x10000000"
 )
-
-endif()  # endif BOOTLOADER
diff --git a/sw/bootloader.c b/sw/bootloader.c
deleted file mode 100644
index 268694b..0000000
--- a/sw/bootloader.c
+++ /dev/null
@@ -1,74 +0,0 @@
-// By Jeroen Taverne
-// New features and mods by smymm
-
-#include "pico/stdlib.h"
-#include "hardware/structs/watchdog.h"
-#include <hardware/flash.h>
-#include "pico/multicore.h"
-#include "flash_firmware.h"
-
-static uint32_t sStart = 0;
-static const uint32_t permOffset = (2048 * 1024) - FLASH_PAGE_SIZE;   //  last page of a 2M flash (adjust for other flash sizes if needed)
-static const uint32_t offset[NR_OF_FIRMWARES] = {FLASH_FIRMWARE1, FLASH_FIRMWARE2, FLASH_FIRMWARE3, FLASH_FIRMWARE4, FLASH_FIRMWARE5, FLASH_FIRMWARE6};
-
-uint8_t read_permMode(void)
-{
-    const uint8_t *pMode = (const uint8_t *) (XIP_BASE + permOffset);
-    uint8_t pModeByte = *pMode;
-
-    if (pModeByte >= 1 && pModeByte <= NR_OF_FIRMWARES)
-        return (pModeByte - 1);
-    else
-        return 0;   // No valid value, boot 1st fw.
-}
-
-void write_permMode(uint8_t pModeFw)  // warning!! erasing last 4kb sector of the flash, and writing last page
-{
-    uint8_t data[FLASH_PAGE_SIZE] = {0};
-
-    data[0] = pModeFw;
-
-    uint32_t ints = save_and_disable_interrupts();
-    flash_range_erase((permOffset + FLASH_PAGE_SIZE - FLASH_SECTOR_SIZE), FLASH_SECTOR_SIZE);    // last sector
-    restore_interrupts(ints);
-    sleep_ms(100);
-    ints = save_and_disable_interrupts();
-    flash_range_program(permOffset, data, FLASH_PAGE_SIZE);     // last page
-    restore_interrupts(ints);
-}
-
-int main(void)
-{
-	uint8_t firmware_nr = (uint8_t) (0x000000FF & watchdog_hw->scratch[3]);
-    uint8_t firmware_mode = (uint8_t) (0x000000FF & (watchdog_hw->scratch[3] >> 8));
-
-	if (firmware_nr > NR_OF_FIRMWARES) {
-		firmware_nr = 0;
-	}
-
-    if (firmware_mode == 1 && firmware_nr > 0) // Perm mode requested. Write fw num to flash.
-        write_permMode(firmware_nr);
-
-    if (firmware_nr == 0)   // Try to boot from permanent storage if no change requested / cold boot / bad value
-    {
-        sStart = offset[read_permMode()] + XIP_BASE;
-    } else {    // Mode change requested by pgusinit
-    	sStart = offset[firmware_nr-1] + XIP_BASE;
-    }
-
-	// Jump to application
-	asm volatile
-	(
-	    "mov r0, %[start]\n"
-	    "ldr r1, =%[vtable]\n"
-	    "str r0, [r1]\n"
-	    "ldmia r0, {r0, r1}\n"
-	    "msr msp, r0\n"
-	    "bx r1\n"
-	    :
-	    : [start] "r" (sStart + 0x100), [vtable] "X" (PPB_BASE + M0PLUS_VTOR_OFFSET)
-	    :
-	);
-
-	return 0;
-}
diff --git a/sw/cmsplay.cpp b/sw/cmsplay.cpp
index cb7ea58..dfbc25a 100644
--- a/sw/cmsplay.cpp
+++ b/sw/cmsplay.cpp
@@ -16,8 +16,8 @@
 #endif
 
 #include "pico/stdlib.h"
-
 #include "pico/audio_i2s.h"
+#include "pico/flash.h"
 
 #ifdef USB_JOYSTICK
 #include "tusb.h"
@@ -78,6 +78,7 @@ struct audio_buffer_pool *init_audio() {
 
 void play_cms() {
     puts("starting core 1 CMS");
+    flash_safe_execute_core_init();
 
 #ifdef USB_JOYSTICK
     // Init TinyUSB for joystick support
diff --git a/sw/firmware1.ld b/sw/firmware1.ld
deleted file mode 100644
index 53f9ed9..0000000
--- a/sw/firmware1.ld
+++ /dev/null
@@ -1,6 +0,0 @@
-MEMORY
-{
-    FLASH(rx) : ORIGIN = 0x10008000, LENGTH = 256k
-}
-
-INCLUDE fw_memmap_c2ram.ld
diff --git a/sw/firmware2.ld b/sw/firmware2.ld
deleted file mode 100644
index 9f2c470..0000000
--- a/sw/firmware2.ld
+++ /dev/null
@@ -1,6 +0,0 @@
-MEMORY
-{
-    FLASH(rx) : ORIGIN = 0x10040000, LENGTH = 256k
-}
-
-INCLUDE fw_memmap_c2ram.ld
diff --git a/sw/firmware3.ld b/sw/firmware3.ld
deleted file mode 100644
index 9a135d4..0000000
--- a/sw/firmware3.ld
+++ /dev/null
@@ -1,6 +0,0 @@
-MEMORY
-{
-    FLASH(rx) : ORIGIN = 0x10080000, LENGTH = 256k
-}
-
-INCLUDE fw_memmap_c2ram.ld
diff --git a/sw/firmware4.ld b/sw/firmware4.ld
deleted file mode 100644
index a960274..0000000
--- a/sw/firmware4.ld
+++ /dev/null
@@ -1,6 +0,0 @@
-MEMORY
-{
-    FLASH(rx) : ORIGIN = 0x100c0000, LENGTH = 256k
-}
-
-INCLUDE fw_memmap_c2ram.ld
diff --git a/sw/firmware5.ld b/sw/firmware5.ld
deleted file mode 100644
index 5f14cc9..0000000
--- a/sw/firmware5.ld
+++ /dev/null
@@ -1,6 +0,0 @@
-MEMORY
-{
-    FLASH(rx) : ORIGIN = 0x10100000, LENGTH = 256k
-}
-
-INCLUDE fw_memmap_c2ram.ld
diff --git a/sw/firmware6.ld b/sw/firmware6.ld
deleted file mode 100644
index 96cf888..0000000
--- a/sw/firmware6.ld
+++ /dev/null
@@ -1,6 +0,0 @@
-MEMORY
-{
-    FLASH(rx) : ORIGIN = 0x10140000, LENGTH = 256k
-}
-
-INCLUDE fw_memmap_c2ram.ld
diff --git a/sw/flash_settings.c b/sw/flash_settings.c
new file mode 100644
index 0000000..4cf631c
--- /dev/null
+++ b/sw/flash_settings.c
@@ -0,0 +1,105 @@
+#include <string.h>
+#include <stdio.h>
+
+#include "flash_settings.h"
+#include "hardware/flash.h"
+#include "hardware/sync.h"
+#include "pico/stdlib.h"
+#include "pico/flash.h"
+
+static const Settings defaultSettings = {
+    .magic = SETTINGS_MAGIC,
+    .version = SETTINGS_VERSION,
+    .startupMode = 1,
+    .Global = {
+        .waveTableVolume = 100
+    },
+    .Joy = {
+        .basePort = 0xffff
+    },
+    .GUS = {
+        .basePort = 0x240,
+        .audioBuffer = 4,
+        .dmaInterval = 0,
+        .force44k = false
+    },
+    .SB = {
+        .basePort = 0x220,
+        .oplBasePort = 0x388,
+        .oplSpeedSensitive = false
+    },
+    .MPU = {
+        .basePort = 0x330,
+        .delaySysex = false,
+        .fakeAllNotesOff = false
+    },
+    .CMS = {
+        .basePort = 0x220
+    },
+    .Tandy = {
+        .basePort = 0x2c0
+    }
+};
+
+
+#define SETTINGS_SECTOR (PICO_FLASH_SIZE_BYTES - FLASH_SECTOR_SIZE)   //  last sector of a 2M flash (adjust for other flash sizes if needed)
+void loadSettings(Settings* settings)
+{
+    printf("copying settings to %u from %u with size %u\n", settings, XIP_BASE + SETTINGS_SECTOR, sizeof(Settings));
+    memcpy(settings, (void *)(XIP_BASE + SETTINGS_SECTOR), sizeof(Settings));
+    /* stdio_flush(); */
+
+    if (settings->magic != SETTINGS_MAGIC || settings->version != SETTINGS_VERSION) {
+        // Initialize settings to default
+        puts("default settings");
+        stdio_flush();
+        memcpy(settings, &defaultSettings, sizeof(Settings));
+        return;
+    }
+    stdio_flush();
+}
+
+
+static void saveSettingsSafe(void* data)
+{
+    putchar('-');
+    // Clock down the RP2040 so the flash at its default 1/2 clock divider is within spec (<=133MHz)
+    set_sys_clock_khz(240000, true);
+    flash_range_erase(SETTINGS_SECTOR, FLASH_SECTOR_SIZE);    // last sector
+    putchar('-');
+    flash_range_program(SETTINGS_SECTOR, data, FLASH_PAGE_SIZE);
+    set_sys_clock_khz(RP2_CLOCK_SPEED, true);
+    printf("settings saved");
+}
+
+void saveSettings(const Settings* settings)
+{
+    uint8_t data[FLASH_PAGE_SIZE] = {0};
+    static_assert(sizeof(Settings) < FLASH_PAGE_SIZE, "Settings struct doesn't fit inside one flash page");
+    memcpy(data, settings, sizeof(Settings));
+    printf("doing settings save: ");
+    int result = flash_safe_execute(saveSettingsSafe, data, 100);
+    if (result) {
+        printf("uh oh... %d", result);
+    }
+    /*
+    // Stop second core
+    multicore_reset_core1();
+    // Clock down the RP2040 so the flash at its default 1/2 clock divider is within spec (<=133MHz)
+    set_sys_clock_khz(240000, true);
+
+    putchar('/');
+    uint32_t ints = save_and_disable_interrupts();
+    flash_range_erase(SETTINGS_SECTOR, FLASH_SECTOR_SIZE);    // last sector
+    putchar('/');
+    flash_range_program(SETTINGS_SECTOR, data, FLASH_PAGE_SIZE);
+    set_sys_clock_khz(RP2_CLOCK_SPEED, true);
+    restore_interrupts(ints);
+    printf("settings saved");
+    */
+}
+
+void resetSettings(void)
+{
+    saveSettings(&defaultSettings);
+}
diff --git a/sw/flash_settings.h b/sw/flash_settings.h
new file mode 100644
index 0000000..dce333f
--- /dev/null
+++ b/sw/flash_settings.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#define SETTINGS_MAGIC 0x70677573  // "pgus" in ascii
+#define SETTINGS_VERSION 1
+
+// Settings struct has generous padding for future settings by aligning to 4 bytes
+typedef struct Settings {
+    uint32_t magic;  // should be "pgus" in ascii (0x7069636F)
+    uint8_t version;
+    uint8_t startupMode;
+    struct {
+        uint8_t waveTableVolume;
+    } Global;
+    struct {
+        uint16_t basePort;
+    } Joy;
+    struct {
+        uint16_t basePort;
+        uint8_t audioBuffer;
+        uint8_t dmaInterval;
+        bool force44k : 1;
+    } GUS;
+    struct {
+        uint16_t basePort;
+        uint16_t oplBasePort;
+        bool oplSpeedSensitive : 1;
+    } SB;
+    struct {
+        uint16_t basePort;
+        bool delaySysex : 1;
+        bool fakeAllNotesOff : 1;
+    } MPU;
+    struct {
+        uint16_t basePort;
+    } CMS;
+    struct {
+        uint16_t basePort;
+    } Tandy;
+} Settings;
+
+
+void loadSettings(Settings* settings);
+void saveSettings(const Settings* settings);
+void resetSettings(void);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/sw/gusplay.cpp b/sw/gusplay.cpp
index 7a049d4..76f2310 100644
--- a/sw/gusplay.cpp
+++ b/sw/gusplay.cpp
@@ -15,12 +15,15 @@
 
 #include "pico/stdlib.h"
 #include "pico/audio_i2s.h"
+#include "pico/flash.h"
 
 #ifdef USE_ALARM
 #include "pico_pic.h"
 #endif
 
+#ifdef USB_JOYSTICK
 #include "tusb.h"
+#endif
 
 #ifdef PSRAM
 #include "psram_spi.h"
@@ -80,6 +83,7 @@ struct audio_buffer_pool *init_audio() {
 // void __xip_cache("my_sub_section") (play_gus)(void) {
 void play_gus() {
     puts("starting core 1");
+    flash_safe_execute_core_init();
     uint32_t start, end;
 
 #ifdef USE_ALARM
@@ -117,8 +121,10 @@ void play_gus() {
 #endif
 #endif
     GUS_Setup();
+#ifdef USB_JOYSTICK
     // Init TinyUSB for joystick support
     tuh_init(BOARD_TUH_RHPORT);
+#endif
 
     struct audio_buffer_pool *ap = init_audio();
     for (;;) {
@@ -150,7 +156,9 @@ void play_gus() {
         */
         // gpio_xor_mask(1u << PICO_DEFAULT_LED_PIN);
         give_audio_buffer(ap, buffer);
+#ifdef USB_JOYSTICK
         // Service TinyUSB events
         tuh_task();
+#endif
     }
 }
diff --git a/sw/mpuplay.cpp b/sw/mpuplay.cpp
index 5a74479..e095711 100644
--- a/sw/mpuplay.cpp
+++ b/sw/mpuplay.cpp
@@ -1,5 +1,10 @@
 #include <stdio.h>
 
+#include "flash_settings.h"
+extern Settings settings;
+
+#include "pico/flash.h"
+
 #ifdef USE_ALARM
 #include "pico_pic.h"
 #endif
@@ -12,6 +17,7 @@
 
 void play_mpu() {
     puts("starting core 1 MPU");
+    flash_safe_execute_core_init();
 
 #ifdef USB_JOYSTICK
     // Init TinyUSB for joystick support
@@ -23,7 +29,7 @@ void play_mpu() {
     PIC_Init();
     puts("pic inited on core 1");
 #endif
-    MPU401_Init(false, false);
+    MPU401_Init(settings.MPU.delaySysex, settings.MPU.fakeAllNotesOff);
 
     for (;;) {
         send_midi_byte();				// see if we need to send a byte	
diff --git a/sw/multifw/CMakeLists.txt b/sw/multifw/CMakeLists.txt
new file mode 100644
index 0000000..e4102db
--- /dev/null
+++ b/sw/multifw/CMakeLists.txt
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.29)
+project(multifw)
+
+# add_executable(uf2create)
+# target_sources(uf2create PRIVATE uf2create.c)
+
+add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/.. picogus)
+
+add_custom_command(
+    OUTPUT pg-multi.uf2
+    COMMAND uf2create picogus.bin pg-multi.uf2
+    DEPENDS picogus
+)
+add_custom_target(pg-multi
+    DEPENDS pg-multi.uf2
+)
diff --git a/sw/multifw/FindUF2CREATE.cmake b/sw/multifw/FindUF2CREATE.cmake
new file mode 100644
index 0000000..ecdd5bf
--- /dev/null
+++ b/sw/multifw/FindUF2CREATE.cmake
@@ -0,0 +1,46 @@
+# Finds (or builds) the uf2create executable
+#
+# This will define the following variables
+#
+#    UF2CREATE_FOUND
+#
+# and the following imported targets
+#
+#     UF2CREATE
+#
+
+if (NOT UF2CREATE_FOUND)
+    # todo we would like to use pckgconfig to look for it first
+    # see https://pabloariasal.github.io/2018/02/19/its-time-to-do-cmake-right/
+
+    include(ExternalProject)
+
+    set(UF2CREATE_SOURCE_DIR ${CMAKE_SOURCE_DIR}/multifw/uf2create)
+    set(UF2CREATE_BINARY_DIR ${CMAKE_BINARY_DIR}/uf2create)
+
+    set(UF2CREATE_BUILD_TARGET UF2CREATEBuild)
+    set(UF2CREATE_TARGET UF2CREATE)
+
+    if (NOT TARGET ${UF2CREATE_BUILD_TARGET})
+        pico_message_debug("UF2CREATE will need to be built")
+        ExternalProject_Add(${UF2CREATE_BUILD_TARGET}
+                PREFIX uf2create
+                SOURCE_DIR ${UF2CREATE_SOURCE_DIR}
+                BINARY_DIR ${UF2CREATE_BINARY_DIR}
+                CMAKE_ARGS "-DCMAKE_MAKE_PROGRAM:FILEPATH=${CMAKE_MAKE_PROGRAM}"
+                CMAKE_CACHE_ARGS "-DMULTIFW_INCLUDE:STRING=${CMAKE_BINARY_DIR}/generated/multifw"
+                BUILD_ALWAYS 1 # force dependency checking
+                INSTALL_COMMAND ""
+                )
+    endif()
+
+    set(UF2CREATE_EXECUTABLE ${UF2CREATE_BINARY_DIR}/uf2create)
+    if(NOT TARGET ${UF2CREATE_TARGET})
+        add_executable(${UF2CREATE_TARGET} IMPORTED)
+    endif()
+    set_property(TARGET ${UF2CREATE_TARGET} PROPERTY IMPORTED_LOCATION
+            ${UF2CREATE_EXECUTABLE})
+
+    add_dependencies(${UF2CREATE_TARGET} ${UF2CREATE_BUILD_TARGET})
+    set(UF2CREATE_FOUND 1)
+endif()
diff --git a/sw/multifw/bootloader.c b/sw/multifw/bootloader.c
new file mode 100644
index 0000000..742d013
--- /dev/null
+++ b/sw/multifw/bootloader.c
@@ -0,0 +1,65 @@
+// By Jeroen Taverne
+// New features and mods by smymm
+
+#include "pico/stdlib.h"
+#include "hardware/structs/watchdog.h"
+#include <hardware/flash.h>
+#include "flash_firmware.h"
+#include "../flash_settings.h"
+#include <stdio.h>
+
+static uint32_t sStart = 0;
+static const uint32_t offset[NR_OF_FIRMWARES] = {FLASH_FIRMWARE1, FLASH_FIRMWARE2, FLASH_FIRMWARE3, FLASH_FIRMWARE4, FLASH_FIRMWARE5, FLASH_FIRMWARE6};
+
+uint8_t read_permMode(void)
+{
+    printf("getsettings... \n");
+    Settings settings;
+    loadSettings(&settings);
+    printf("gotsettings\n");
+    uint8_t pModeByte = settings.startupMode;
+
+    if (pModeByte >= 1 && pModeByte <= NR_OF_FIRMWARES)
+        return (pModeByte - 1);
+    else
+        return 0;   // No valid value, boot 1st fw.
+}
+
+
+int main(void)
+{
+    stdio_init_all();
+    printf("picogus bootloader\n");
+    uint8_t firmware_nr = (uint8_t) (0x000000FF & watchdog_hw->scratch[3]);
+    printf("firmware_nr: %u\n", firmware_nr);
+    stdio_flush();
+
+    if (firmware_nr > NR_OF_FIRMWARES) {
+        firmware_nr = 0;
+    }
+
+    if (firmware_nr == 0)   // Try to boot from permanent storage if no change requested / cold boot / bad value
+    {
+        sStart = offset[read_permMode()] + XIP_BASE;
+    } else {    // Mode change requested by pgusinit
+    	sStart = offset[firmware_nr-1] + XIP_BASE;
+    }
+    printf("sStart: %x\n", sStart);
+    stdio_flush();
+
+    // Jump to application
+    asm volatile
+    (
+         "mov r0, %[start]\n"
+         "ldr r1, =%[vtable]\n"
+         "str r0, [r1]\n"
+         "ldmia r0, {r0, r1}\n"
+         "msr msp, r0\n"
+         "bx r1\n"
+         :
+         : [start] "r" (sStart + 0x100), [vtable] "X" (PPB_BASE + M0PLUS_VTOR_OFFSET)
+         :
+    );
+
+    return 0;
+}
diff --git a/sw/multifw/bootloader.ld b/sw/multifw/bootloader.ld
new file mode 100644
index 0000000..8adc8db
--- /dev/null
+++ b/sw/multifw/bootloader.ld
@@ -0,0 +1,251 @@
+/* Based on GCC ARM embedded samples.
+   Defines the following symbols for use by code:
+    __exidx_start
+    __exidx_end
+    __etext
+    __data_start__
+    __preinit_array_start
+    __preinit_array_end
+    __init_array_start
+    __init_array_end
+    __fini_array_start
+    __fini_array_end
+    __data_end__
+    __bss_start__
+    __bss_end__
+    __end__
+    end
+    __HeapLimit
+    __StackLimit
+    __StackTop
+    __stack (== StackTop)
+*/
+
+MEMORY
+{
+    FLASH(rx) : ORIGIN = 0x10000000, LENGTH = 32k
+    RAM(rwx) : ORIGIN =  0x20000000, LENGTH = 256k
+    SCRATCH_X(rwx) : ORIGIN = 0x20040000, LENGTH = 4k
+    SCRATCH_Y(rwx) : ORIGIN = 0x20041000, LENGTH = 4k
+}
+
+ENTRY(_entry_point)
+
+SECTIONS
+{
+    /* Second stage bootloader is prepended to the image. It must be 256 bytes big
+       and checksummed. It is usually built by the boot_stage2 target
+       in the Raspberry Pi Pico SDK
+    */
+
+    .flash_begin : {
+        __flash_binary_start = .;
+    } > FLASH
+
+    .boot2 : {
+        __boot2_start__ = .;
+        KEEP (*(.boot2))
+        __boot2_end__ = .;
+    } > FLASH
+
+    ASSERT(__boot2_end__ - __boot2_start__ == 256,
+        "ERROR: Pico second stage bootloader must be 256 bytes in size")
+
+    /* The second stage will always enter the image at the start of .text.
+       The debugger will use the ELF entry point, which is the _entry_point
+       symbol if present, otherwise defaults to start of .text.
+       This can be used to transfer control back to the bootrom on debugger
+       launches only, to perform proper flash setup.
+    */
+
+    .text : {
+        __logical_binary_start = .;
+        KEEP (*(.vectors))
+        KEEP (*(.binary_info_header))
+        __binary_info_header_end = .;
+        KEEP (*(.reset))
+        /* TODO revisit this now memset/memcpy/float in ROM */
+        /* bit of a hack right now to exclude all floating point and time critical (e.g. memset, memcpy) code from
+         * FLASH ... we will include any thing excluded here in .data below by default */
+        *(.init)
+        *(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .text*)
+        *(.fini)
+        /* Pull all c'tors into .text */
+        *crtbegin.o(.ctors)
+        *crtbegin?.o(.ctors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors)
+        *(SORT(.ctors.*))
+        *(.ctors)
+        /* Followed by destructors */
+        *crtbegin.o(.dtors)
+        *crtbegin?.o(.dtors)
+        *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors)
+        *(SORT(.dtors.*))
+        *(.dtors)
+
+        *(.eh_frame*)
+        . = ALIGN(4);
+    } > FLASH
+
+    .rodata : {
+        *(EXCLUDE_FILE(*libgcc.a: *libc.a:*lib_a-mem*.o *libm.a:) .rodata*)
+        . = ALIGN(4);
+        *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.flashdata*)))
+        . = ALIGN(4);
+    } > FLASH
+
+    .ARM.extab :
+    {
+        *(.ARM.extab* .gnu.linkonce.armextab.*)
+    } > FLASH
+
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > FLASH
+    __exidx_end = .;
+
+    /* Machine inspectable binary information */
+    . = ALIGN(4);
+    __binary_info_start = .;
+    .binary_info :
+    {
+        KEEP(*(.binary_info.keep.*))
+        *(.binary_info.*)
+    } > FLASH
+    __binary_info_end = .;
+    . = ALIGN(4);
+
+   .ram_vector_table (NOLOAD): {
+        *(.ram_vector_table)
+    } > RAM
+
+    .data : {
+        __data_start__ = .;
+        *(vtable)
+
+        *(.time_critical*)
+
+        /* remaining .text and .rodata; i.e. stuff we exclude above because we want it in RAM */
+        *(.text*)
+        . = ALIGN(4);
+        *(.rodata*)
+        . = ALIGN(4);
+
+        *(.data*)
+
+        . = ALIGN(4);
+        *(.after_data.*)
+        . = ALIGN(4);
+        /* preinit data */
+        PROVIDE_HIDDEN (__mutex_array_start = .);
+        KEEP(*(SORT(.mutex_array.*)))
+        KEEP(*(.mutex_array))
+        PROVIDE_HIDDEN (__mutex_array_end = .);
+
+        . = ALIGN(4);
+        /* preinit data */
+        PROVIDE_HIDDEN (__preinit_array_start = .);
+        KEEP(*(SORT(.preinit_array.*)))
+        KEEP(*(.preinit_array))
+        PROVIDE_HIDDEN (__preinit_array_end = .);
+
+        . = ALIGN(4);
+        /* init data */
+        PROVIDE_HIDDEN (__init_array_start = .);
+        KEEP(*(SORT(.init_array.*)))
+        KEEP(*(.init_array))
+        PROVIDE_HIDDEN (__init_array_end = .);
+
+        . = ALIGN(4);
+        /* finit data */
+        PROVIDE_HIDDEN (__fini_array_start = .);
+        *(SORT(.fini_array.*))
+        *(.fini_array)
+        PROVIDE_HIDDEN (__fini_array_end = .);
+
+        *(.jcr)
+        . = ALIGN(4);
+        /* All data end */
+        __data_end__ = .;
+    } > RAM AT> FLASH
+    /* __etext is (for backwards compatibility) the name of the .data init source pointer (...) */
+    __etext = LOADADDR(.data);
+
+    .uninitialized_data (NOLOAD): {
+        . = ALIGN(4);
+        *(.uninitialized_data*)
+    } > RAM
+
+    /* Start and end symbols must be word-aligned */
+    .scratch_x : {
+        __scratch_x_start__ = .;
+        *(.scratch_x.*)
+        . = ALIGN(4);
+        __scratch_x_end__ = .;
+    } > SCRATCH_X AT > FLASH
+    __scratch_x_source__ = LOADADDR(.scratch_x);
+
+    .scratch_y : {
+        __scratch_y_start__ = .;
+        *(.scratch_y.*)
+        . = ALIGN(4);
+        __scratch_y_end__ = .;
+    } > SCRATCH_Y AT > FLASH
+    __scratch_y_source__ = LOADADDR(.scratch_y);
+
+    .bss  : {
+        . = ALIGN(4);
+        __bss_start__ = .;
+        *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.bss*)))
+        *(COMMON)
+        . = ALIGN(4);
+        __bss_end__ = .;
+    } > RAM
+
+    .heap (NOLOAD):
+    {
+        __end__ = .;
+        end = __end__;
+        KEEP(*(.heap*))
+        __HeapLimit = .;
+    } > RAM
+
+    /* .stack*_dummy section doesn't contains any symbols. It is only
+     * used for linker to calculate size of stack sections, and assign
+     * values to stack symbols later
+     *
+     * stack1 section may be empty/missing if platform_launch_core1 is not used */
+
+    /* by default we put core 0 stack at the end of scratch Y, so that if core 1
+     * stack is not used then all of SCRATCH_X is free.
+     */
+    .stack1_dummy (NOLOAD):
+    {
+        *(.stack1*)
+    } > SCRATCH_X
+    .stack_dummy (NOLOAD):
+    {
+        KEEP(*(.stack*))
+    } > SCRATCH_Y
+
+    .flash_end : {
+        PROVIDE(__flash_binary_end = .);
+    } > FLASH
+
+    /* stack limit is poorly named, but historically is maximum heap ptr */
+    __StackLimit = ORIGIN(RAM) + LENGTH(RAM);
+    __StackOneTop = ORIGIN(SCRATCH_X) + LENGTH(SCRATCH_X);
+    __StackTop = ORIGIN(SCRATCH_Y) + LENGTH(SCRATCH_Y);
+    __StackOneBottom = __StackOneTop - SIZEOF(.stack1_dummy);
+    __StackBottom = __StackTop - SIZEOF(.stack_dummy);
+    PROVIDE(__stack = __StackTop);
+
+    /* Check if data + heap + stack exceeds RAM limit */
+    ASSERT(__StackLimit >= __HeapLimit, "region RAM overflowed")
+
+    ASSERT( __binary_info_header_end - __logical_binary_start <= 256, "Binary info must be in first 256 bytes of the binary")
+    /* todo assert on extra code */
+}
+
diff --git a/sw/multifw/firmware.ld.in b/sw/multifw/firmware.ld.in
new file mode 100644
index 0000000..ebbda4b
--- /dev/null
+++ b/sw/multifw/firmware.ld.in
@@ -0,0 +1,6 @@
+MEMORY
+{
+    FLASH(rx) : ORIGIN = 0x10000000 + ${FW_ORIGIN}, LENGTH = ${FW_SIZE_ALIGNED}
+}
+
+INCLUDE fw_memmap_c2ram.ld
diff --git a/sw/flash_firmware.h b/sw/multifw/flash_firmware.h.in
similarity index 51%
rename from sw/flash_firmware.h
rename to sw/multifw/flash_firmware.h.in
index 38b3ec6..81fb6b2 100644
--- a/sw/flash_firmware.h
+++ b/sw/multifw/flash_firmware.h.in
@@ -7,9 +7,9 @@
 // ld files for picogus firmwares/modes are based on memmap_copy_to_ram.ld as they run with set(PICO_COPY_TO_RAM 1)
 
 #define NR_OF_FIRMWARES 6
-#define FLASH_FIRMWARE1 0x00008000
-#define FLASH_FIRMWARE2 0x00040000
-#define FLASH_FIRMWARE3 0x00080000
-#define FLASH_FIRMWARE4 0x000c0000
-#define FLASH_FIRMWARE5 0x00100000
-#define FLASH_FIRMWARE6 0x00140000
+#define FLASH_FIRMWARE1 ${FW_1_ORIGIN}
+#define FLASH_FIRMWARE2 ${FW_2_ORIGIN}
+#define FLASH_FIRMWARE3 ${FW_3_ORIGIN}
+#define FLASH_FIRMWARE4 ${FW_4_ORIGIN}
+#define FLASH_FIRMWARE5 ${FW_5_ORIGIN}
+#define FLASH_FIRMWARE6 ${FW_6_ORIGIN}
diff --git a/sw/fw_memmap_c2ram.ld b/sw/multifw/fw_memmap_c2ram.ld
similarity index 100%
rename from sw/fw_memmap_c2ram.ld
rename to sw/multifw/fw_memmap_c2ram.ld
diff --git a/sw/multifw/generate_ld.cmake b/sw/multifw/generate_ld.cmake
new file mode 100644
index 0000000..5842261
--- /dev/null
+++ b/sw/multifw/generate_ld.cmake
@@ -0,0 +1,30 @@
+# Script to generate the linker scripts for each firmware with the firmware size and offset (aligned to 1024 bytes)
+# As well as the flash_firmware.h file used by the bootloader and uf2create utility
+
+# Initial fixed offset of 32768 for the bootloader
+set(FW_ORIGIN "32768")
+
+macro(generate_ld_file FW_FILE FW_LD_FILE)
+    file(SIZE ${FW_FILE} FW_SIZE)
+    math(EXPR FW_SIZE_ALIGNED "(${FW_SIZE} / 1024) * 1024 + 1024")
+    # file(REMOVE ${FW_LD_FILE})
+    configure_file(${FW_LD_IN} ${FW_LD_FILE})
+    list(APPEND FW_ORIGINS "${FW_ORIGIN}")
+    math(EXPR FW_ORIGIN "${FW_ORIGIN} + ${FW_SIZE_ALIGNED}")
+endmacro()
+
+generate_ld_file(${FW_1_BIN} ${FW_1_LD})
+generate_ld_file(${FW_2_BIN} ${FW_2_LD})
+generate_ld_file(${FW_3_BIN} ${FW_3_LD})
+generate_ld_file(${FW_4_BIN} ${FW_4_LD})
+generate_ld_file(${FW_5_BIN} ${FW_5_LD})
+generate_ld_file(${FW_6_BIN} ${FW_6_LD})
+list(GET FW_ORIGINS 0 FW_1_ORIGIN)
+list(GET FW_ORIGINS 1 FW_2_ORIGIN)
+list(GET FW_ORIGINS 2 FW_3_ORIGIN)
+list(GET FW_ORIGINS 3 FW_4_ORIGIN)
+list(GET FW_ORIGINS 4 FW_5_ORIGIN)
+list(GET FW_ORIGINS 5 FW_6_ORIGIN)
+
+file(REMOVE ${FLASH_FIRMWARE_H})
+configure_file(${FLASH_FIRMWARE_H_IN} ${FLASH_FIRMWARE_H})
diff --git a/sw/multifw/uf2create/CMakeLists.txt b/sw/multifw/uf2create/CMakeLists.txt
new file mode 100644
index 0000000..b7e414d
--- /dev/null
+++ b/sw/multifw/uf2create/CMakeLists.txt
@@ -0,0 +1,7 @@
+cmake_minimum_required(VERSION 3.29)
+project(uf2create)
+
+set(CMAKE_C_STANDARD 11)
+
+add_executable(uf2create uf2create.c)
+target_include_directories(uf2create PRIVATE ${MULTIFW_INCLUDE})
diff --git a/sw/multifw/uf2create/uf2.h b/sw/multifw/uf2create/uf2.h
new file mode 100644
index 0000000..d00773a
--- /dev/null
+++ b/sw/multifw/uf2create/uf2.h
@@ -0,0 +1,47 @@
+/*
+* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _BOOT_UF2_H
+#define _BOOT_UF2_H
+
+#include <stdint.h>
+#include <assert.h>
+
+/** \file uf2.h
+*  \defgroup boot_uf2 boot_uf2
+*
+* Header file for the UF2 format supported by an RP2040 in BOOTSEL mode.
+*/
+
+#define UF2_MAGIC_START0 0x0A324655u
+#define UF2_MAGIC_START1 0x9E5D5157u
+#define UF2_MAGIC_END    0x0AB16F30u
+
+#define UF2_FLAG_NOT_MAIN_FLASH     0x00000001u
+#define UF2_FLAG_FILE_CONTAINER     0x00001000u
+#define UF2_FLAG_FAMILY_ID_PRESENT  0x00002000u
+#define UF2_FLAG_MD5_PRESENT        0x00004000u
+
+#define RP2040_FAMILY_ID 0xe48bff56
+
+struct uf2_block
+{
+	// 32 byte header
+	uint32_t magic_start0;
+	uint32_t magic_start1;
+	uint32_t flags;
+	uint32_t target_addr;
+	uint32_t payload_size;
+	uint32_t block_no;
+	uint32_t num_blocks;
+	uint32_t file_size; // or familyID;
+	uint8_t  data[476];
+	uint32_t magic_end;
+};
+
+static_assert(sizeof(struct uf2_block) == 512, "uf2_block not sector sized");
+
+#endif
diff --git a/sw/multifw/uf2create/uf2create.c b/sw/multifw/uf2create/uf2create.c
new file mode 100644
index 0000000..7a89d5c
--- /dev/null
+++ b/sw/multifw/uf2create/uf2create.c
@@ -0,0 +1,95 @@
+// By Jeroen Taverne
+
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include "uf2.h"
+#include "flash_firmware.h"
+
+#define FLASH_SIZE 0x200000
+#define PAYLOAD_SIZE 0x100
+#define INPUT_FILES (NR_OF_FIRMWARES + 1)
+
+int main(int argc, char *argv[])
+{
+	int total_size = 0;
+	static uint8_t flashbuffer[FLASH_SIZE];
+	uint32_t offset[INPUT_FILES] = {0, FLASH_FIRMWARE1, FLASH_FIRMWARE2, FLASH_FIRMWARE3, FLASH_FIRMWARE4, FLASH_FIRMWARE5, FLASH_FIRMWARE6};
+	struct uf2_block uf2;
+
+	FILE *f_input, *f_output;
+
+	if (argc != INPUT_FILES+2) {
+		fprintf(stderr, "Usage: %s <%d binary input files> <UF2 output file>\n", argv[0], INPUT_FILES);
+		return -1;
+	}
+
+	memset(flashbuffer, 0xff, FLASH_SIZE);
+
+	for (int i=0; i<INPUT_FILES; i++) {
+		int arg = i + 1;
+		printf("Opening %s for read\n", argv[arg]);
+		f_input = fopen(argv[arg], "rb");
+		if (f_input == NULL) {
+			fprintf(stderr, "%s: can't open %s for reading\n", argv[0], argv[arg]);
+			return -1;
+		}
+		fseek(f_input, 0L, SEEK_END);
+		uint32_t size = ftell(f_input);
+		fseek(f_input, 0L, SEEK_SET);
+		if ((size + offset[i]) > FLASH_SIZE) {
+			fprintf(stderr, "Too big to fit in FLASH!\n");
+			return -1;
+		}
+		for (int j=0; j<size; j++) {
+			if (flashbuffer[offset[i] + j] != 0xff) {
+				fprintf(stderr, "Files will overlap in FLASH!\n");
+				return -1;
+			}
+		}
+		fread(&flashbuffer[offset[i]], size, 1, f_input);
+		fclose(f_input);
+		if (size > 0) {
+			size += offset[i];
+			if (size > total_size) {
+				total_size = size;
+			}
+		}
+	}
+
+	int arg = INPUT_FILES + 1;
+	printf("Opening %s for write\n", argv[arg]);
+	f_output = fopen(argv[arg], "wb");
+
+	uf2.magic_start0 = UF2_MAGIC_START0;
+	uf2.magic_start1 = UF2_MAGIC_START1;
+	uf2.flags = UF2_FLAG_FAMILY_ID_PRESENT;
+	uf2.file_size = RP2040_FAMILY_ID;
+	uf2.payload_size = PAYLOAD_SIZE;
+	uf2.block_no = 0;
+	uf2.num_blocks = (total_size + PAYLOAD_SIZE - 1) / PAYLOAD_SIZE;
+	memset(uf2.data, 0, sizeof(uf2.data));
+	uf2.magic_end = UF2_MAGIC_END;
+
+	int index = 0;
+	while (index < total_size) {
+		uf2.target_addr = 0x10000000 + index;
+		memcpy(uf2.data, &flashbuffer[index], PAYLOAD_SIZE);
+		fwrite(&uf2, sizeof(uf2), 1, f_output);
+		uf2.block_no++;
+		index += PAYLOAD_SIZE;
+	}
+
+	fclose(f_output);
+
+        printf("Creating picogus.bin\n");
+        f_output = fopen("picogus.bin", "wb");
+        fwrite(flashbuffer, total_size, 1, f_output);
+        fclose(f_output);
+
+	printf("Done!\n");
+
+	return 0;
+}
diff --git a/sw/picogus.cpp b/sw/picogus.cpp
index a42540d..6fff5dd 100644
--- a/sw/picogus.cpp
+++ b/sw/picogus.cpp
@@ -9,6 +9,7 @@
 #include "hardware/clocks.h"
 
 #include "pico_reflash.h"
+#include "flash_settings.h"
 
 // For multifw
 #include "hardware/watchdog.h"
@@ -48,8 +49,8 @@ M62429* m62429;
 
 #ifdef SOUND_SB
 #include "opl.h"
-static uint16_t basePort = 0x220u;
-static uint16_t sb_port_test = basePort >> 4;
+static uint16_t basePort;
+static uint16_t sb_port_test;
 
 void play_adlib(void);
 extern "C" int OPL_Pico_Init(unsigned int);
@@ -61,8 +62,8 @@ extern void sbdsp_write(uint8_t address, uint8_t value);
 extern uint8_t sbdsp_read(uint8_t address);
 extern void sbdsp_init();
 extern void sbdsp_process();
-static uint16_t adlib_basePort = 0x388u;
-static bool adlib_wait = false;
+static uint16_t adlib_basePort;
+static bool adlib_wait;
 #endif
 
 #ifdef SOUND_GUS
@@ -70,21 +71,21 @@ static bool adlib_wait = false;
 
 #include "isa_dma.h"
 dma_inst_t dma_config;
-static uint16_t basePort = GUS_DEFAULT_PORT;
-static uint16_t gus_port_test = basePort >> 4 | 0x10;
+static uint16_t basePort;
+static uint16_t gus_port_test;
 void play_gus(void);
 #endif
 
 
 #ifdef SOUND_MPU
 #include "mpu401/export.h"
-static uint16_t basePort = 0x330u;
+static uint16_t basePort;
 void play_mpu(void);
 #endif
 
 #ifdef SOUND_TANDY
 #include "square/square.h"
-static uint16_t basePort = 0x2c0u;
+static uint16_t basePort;
 void play_tandy(void);
 
 #include "cmd_buffers.h"
@@ -92,7 +93,7 @@ tandy_buffer_t tandy_buffer = { {0}, 0, 0 };
 #endif
 
 #ifdef SOUND_CMS
-static uint16_t basePort = 0x220u;
+static uint16_t basePort;
 void play_cms(void);
 static uint8_t cms_detect = 0xFF;
 
@@ -101,7 +102,7 @@ cms_buffer_t cms_buffer = { {0}, 0, 0 };
 #endif
 
 #ifdef USB_JOYSTICK
-static uint16_t joyPort = 0xffffu;
+static uint16_t joyPort;
 #ifdef USB_JOYSTICK_ONLY
 void play_usb(void);
 #endif
@@ -121,6 +122,10 @@ static bool control_active = false;
 static uint8_t sel_reg = 0;
 static uint16_t cur_data = 0;
 static uint32_t cur_read = 0;
+static bool queueSaveSettings = false;
+static bool queueReboot = false;
+
+Settings settings;
 
 #define IOW_PIO_SM 0
 #define IOR_PIO_SM 1
@@ -165,6 +170,8 @@ __force_inline void select_picogus(uint8_t value) {
         break;
 #endif
     case 0xE0: // Select firmware boot mode register
+    case 0xE1: // Select save settings register
+    case 0xE2: // Select reboot register
         break;
     case 0xF0: // Hardware version
         break;
@@ -183,11 +190,6 @@ __force_inline void write_picogus_low(uint8_t value) {
     case 0x05: // Adlib Base port
         basePort_tmp = value;
         break;
-    // For multifw
-    case 0xE0:
-        // set firmware num and perm flag
-        multifw_tmp = value;
-        break;
     }
 }
 
@@ -198,43 +200,62 @@ __force_inline void write_picogus_high(uint8_t value) {
         basePort = (value << 8) | basePort_tmp;
 #endif
 #ifdef SOUND_GUS
+        settings.GUS.basePort = basePort;
         gus_port_test = basePort >> 4 | 0x10;
         // GUS_SetPort(basePort);
 #endif
 #ifdef SOUND_SB
+        settings.SB.basePort = basePort;
         sb_port_test = basePort >> 4;
+#endif
+#ifdef SOUND_MPU
+        settings.MPU.basePort = basePort;
+#endif
+#ifdef SOUND_TANDY
+        settings.Tandy.basePort = basePort;
+#endif
+#ifdef SOUND_CMS
+        settings.CMS.basePort = basePort;
 #endif
         break;
     case 0x05: // Adlib Base port
 #ifdef SOUND_SB
         adlib_basePort = (value << 8) | basePort_tmp;
+        settings.SB.oplBasePort = adlib_basePort;
 #endif
         break;
     case 0x0f: // enable joystick
 #ifdef USB_JOYSTICK
         joyPort = value ? 0x201u : 0xffff;
+        settings.Joy.basePort = joyPort;
 #endif
         break;
 #ifdef SOUND_GUS
     case 0x10: // Audio buffer size
         // Value is sent by pgusinit as the size - 1, so we need to add 1 back to it
         GUS_SetAudioBuffer(value + 1);
+        settings.GUS.audioBuffer = value + 1;
         break;
     case 0x11: // DMA interval
         GUS_SetDMAInterval(value);
+        settings.GUS.dmaInterval = value;
         break;
     case 0x12: // Force 44k output
         GUS_SetFixed44k(value);
+        settings.GUS.force44k = value;
         break;
 #endif
     case 0x20: // Wavetable mixer volume
         if (BOARD_TYPE == PICOGUS_2) {
             m62429->setVolume(M62429_BOTH, value);
+            settings.Global.waveTableVolume = value;
         }
         break;
 #ifdef SOUND_MPU
     case 0x21: // MIDI emulation flags
         MPU401_Init(value & 0x1 /* delaysysex */, value & 0x2 /* fakeallnotesoff */);
+        settings.MPU.delaySysex = value & 0x1;
+        settings.MPU.fakeAllNotesOff = value & 0x2;
         break;
 #endif
 #ifdef SOUND_SB
@@ -244,10 +265,17 @@ __force_inline void write_picogus_high(uint8_t value) {
 #endif
     // For multifw
     case 0xE0:
-            // set firmware num, perm flag and reboot
-	        watchdog_hw->scratch[3] = ((value << 8) | multifw_tmp);
-	        watchdog_reboot(0, 0, 0);
-            break;
+        // set firmware num, perm flag and reboot
+        settings.startupMode = value;
+        printf("requesting startup mode: %u\n", value);
+        break;
+    case 0xE1:
+        queueSaveSettings = true;
+        break;
+    case 0xE2:
+        watchdog_hw->scratch[3] = settings.startupMode;
+        printf("rebooting into mode: %u\n", settings.startupMode);
+        watchdog_reboot(0, 0, 0);
     case 0xff: // Firmware write
         pico_firmware_write(value);
         break;
@@ -346,6 +374,30 @@ __force_inline uint8_t read_picogus_high(void) {
     }
 }
 
+
+void processSettings(void) {
+#ifdef SOUND_SB
+    basePort = settings.SB.basePort;
+    sb_port_test = basePort >> 4;
+    adlib_basePort = settings.SB.oplBasePort;
+    adlib_wait = settings.SB.oplSpeedSensitive;
+#endif
+#ifdef SOUND_GUS
+    basePort = settings.GUS.basePort;
+    gus_port_test = basePort >> 4 | 0x10;
+#endif
+#ifdef SOUND_MPU
+    basePort = settings.MPU.basePort;
+#endif
+#ifdef SOUND_CMS
+    basePort = settings.CMS.basePort;
+#endif
+#ifdef SOUND_TANDY
+    basePort = settings.Tandy.basePort;
+#endif
+}
+
+
 static constexpr uint32_t IO_WAIT = 0xffffffffu;
 static constexpr uint32_t IO_END = 0x0u;
 // OR with 0x0000ff00 is required to set pindirs in the PIO
@@ -530,6 +582,10 @@ __force_inline void handle_iow(void) {
     }
     // Fallthrough if no match, or for slow write, reset PIO
     pio_sm_put(pio0, IOW_PIO_SM, IO_END);
+    if (queueSaveSettings) {
+        saveSettings(&settings);
+        queueSaveSettings = false;
+    }
 }
 
 __force_inline void handle_ior(void) {
@@ -658,7 +714,7 @@ void err_blink(void) {
 #include "pico_pic.h"
 #endif
 
-constexpr uint32_t rp2_clock = 366000;
+constexpr uint32_t rp2_clock = RP2_CLOCK_SPEED;
 constexpr float psram_clkdiv = (float)rp2_clock / 200000.0;
 constexpr float pwm_clkdiv = (float)rp2_clock / 22727.27;
 constexpr float iow_clkdiv = (float)rp2_clock / 183000.0;
@@ -692,6 +748,10 @@ int main()
         puts("I was reset due the debug port");
     }
 
+    // Load settings from flash
+    loadSettings(&settings);
+    processSettings();
+
     // Determine board type. GPIO 29 is grounded on PicoGUS v2.0, and on a Pico, it's VSYS/3 (~1.666V)
     // GPIO 25 must be high to read GPIO 29 on the Pico W
     gpio_init(25);
@@ -760,11 +820,7 @@ int main()
         m62429->begin(24, 25, pio1, -1);
 #else
         m62429->begin(24, 25);
-#endif
-#ifdef SOUND_MPU
-        m62429->setVolume(M62429_BOTH, 100);
-#else // SOUND_MPU
-        m62429->setVolume(M62429_BOTH, 0);
+        m62429->setVolume(M62429_BOTH, settings.Global.waveTableVolume);
 #endif // SOUND_MPU
     }
 
@@ -831,6 +887,9 @@ int main()
 #ifdef SOUND_GUS
     puts("Creating GUS");
     GUS_OnReset();
+    GUS_SetAudioBuffer(settings.GUS.audioBuffer);
+    GUS_SetDMAInterval(settings.GUS.dmaInterval);
+    GUS_SetFixed44k(settings.GUS.force44k);
     multicore_launch_core1(&play_gus);
 #endif // SOUND_GUS
 
diff --git a/sw/release/create_release.sh b/sw/release/create_release.sh
index 6ee2bd1..479a511 100755
--- a/sw/release/create_release.sh
+++ b/sw/release/create_release.sh
@@ -16,25 +16,28 @@ build () {
     local PROJECT_TYPE=$1
     local FW_SUFFIX=$2
     local EXTRA_CMAKE="$3"
+    local EXTRA_BUILD="$4"
 
     cd "$BUILD_DIR"
 #    cmake .. --fresh -DPROJECT_TYPE=$PROJECT_TYPE $EXTRA_CMAKE
-    cmake --clean-first .. -DPROJECT_TYPE=$PROJECT_TYPE $EXTRA_CMAKE
-    make clean && make -j
+    cmake .. -DPROJECT_TYPE=$PROJECT_TYPE $EXTRA_CMAKE
+    cmake --build . -j $(nproc) $EXTRA_BUILD
     if [[ ! $2 == 'BOOT' ]]; then
-        cp picogus.bin "$STAGING_DIR"/pg-$FW_SUFFIX.bin
-        cp picogus.uf2 "$STAGING_DIR"/pg-$FW_SUFFIX.uf2
-        cd -
+        if [[ $FW_SUFFIX ]]; then
+            cp picogus.bin "$STAGING_DIR"/pg-$FW_SUFFIX.bin
+            cp picogus.uf2 "$STAGING_DIR"/pg-$FW_SUFFIX.uf2
+        fi
     else
         cp bootloader.bin "$STAGING_DIR"/bootloader.bin 
         gcc ../uf2create.c -o uf2create
     fi
+    cd -
 }
 
 # Build pgusinit
 cd "$SW_HOME"/../pgusinit
-#export WATCOM="$(readlink -f "$SW_HOME"/../../watcom)"
-export WATCOM="/usr/bin/watcom"
+export WATCOM="$(readlink -f "$SW_HOME"/../../watcom)"
+# export WATCOM="/usr/bin/watcom"
 if [[ $OSTYPE == 'darwin'* ]]; then
     # Assuming Apple ARM 
     BINDIR="$WATCOM"/armo64
@@ -51,13 +54,45 @@ cp README.md "$STAGING_DIR"
 cd -
 
 # Build picogus releases
-build BOOT "BOOT"  # BOOTLOADER for multifw
-build GUS "gus" "-DGUS_DEFAULT_PORT=0x240"
-build SB "sb"
-build MPU "mpu"
-build TANDY "tandy"
-build CMS "cms"
-build JOY "joy"
+BOOTLOADER_SIZE=32768
+build GUS "" "--fresh" "--clean-first"
+FW_1_SIZE=$((($(wc -c <"$BUILD_DIR"/picogus.bin) / 1024) * 1024 + 1024))
+export FW_1_ORIGIN=$BOOTLOADER_SIZE
+FW_SIZE=$FW_1_SIZE FW_ORIGIN=$FW_1_ORIGIN envsubst <../firmware.ld.in >../firmware.ld
+build GUS "gus" "--fresh -D MULTIFW=1" "--clean-first"
+
+build SB "" "--fresh" "--clean-first"
+FW_2_SIZE=$((($(wc -c <"$BUILD_DIR"/picogus.bin) / 1024) * 1024 + 1024))
+export FW_2_ORIGIN=$(($FW_1_ORIGIN + $FW_1_SIZE))
+FW_SIZE=$FW_2_SIZE FW_ORIGIN=$FW_2_ORIGIN envsubst <../firmware.ld.in >../firmware.ld
+build SB "sb" "--fresh -D MULTIFW=1" "--clean-first"
+
+build MPU "" "--fresh" "--clean-first"
+FW_3_SIZE=$((($(wc -c <"$BUILD_DIR"/picogus.bin) / 1024) * 1024 + 1024))
+export FW_3_ORIGIN=$(($FW_2_ORIGIN + $FW_2_SIZE))
+FW_SIZE=$FW_3_SIZE FW_ORIGIN=$FW_3_ORIGIN envsubst <../firmware.ld.in >../firmware.ld
+build MPU "mpu" "--fresh -D MULTIFW=1" "--clean-first"
+
+build TANDY "" "--fresh" "--clean-first"
+FW_4_SIZE=$((($(wc -c <"$BUILD_DIR"/picogus.bin) / 1024) * 1024 + 1024))
+export FW_4_ORIGIN=$(($FW_3_ORIGIN + $FW_3_SIZE))
+FW_SIZE=$FW_4_SIZE FW_ORIGIN=$FW_4_ORIGIN envsubst <../firmware.ld.in >../firmware.ld
+build TANDY "tandy" "--fresh -D MULTIFW=1" "--clean-first"
+
+build CMS "" "--fresh" "--clean-first"
+FW_5_SIZE=$((($(wc -c <"$BUILD_DIR"/picogus.bin) / 1024) * 1024 + 1024))
+export FW_5_ORIGIN=$(($FW_4_ORIGIN + $FW_4_SIZE))
+FW_SIZE=$FW_5_SIZE FW_ORIGIN=$FW_5_ORIGIN envsubst <../firmware.ld.in >../firmware.ld
+build CMS "cms" "--fresh -D MULTIFW=1" "--clean-first"
+
+build JOY "" "--fresh" "--clean-first"
+FW_6_SIZE=$((($(wc -c <"$BUILD_DIR"/picogus.bin) / 1024) * 1024 + 1024))
+export FW_6_ORIGIN=$(($FW_5_ORIGIN + $FW_5_SIZE))
+FW_SIZE=$FW_6_SIZE FW_ORIGIN=$FW_6_ORIGIN envsubst <../firmware.ld.in >../firmware.ld
+build JOY "joy" "--fresh -D MULTIFW=1" "--clean-first"
+
+envsubst < ../flash_firmware.h.in > ../flash_firmware.h
+build BOOT "BOOT" "--fresh" "--clean-first" # BOOTLOADER for multifw
 
 # Get release version
 cd "$BUILD_DIR"
@@ -67,7 +102,7 @@ cd -
 #bin files for multi-firmware
 cd "$STAGING_DIR"
 "$BUILD_DIR"/uf2create bootloader.bin pg-gus.bin pg-sb.bin pg-mpu.bin pg-tandy.bin pg-cms.bin pg-joy.bin pg-multi.uf2
-rm *.bin
+# rm *.bin
 cd -
 
 # Create zip file
diff --git a/sw/sbplay.cpp b/sw/sbplay.cpp
index d13c459..0be708a 100644
--- a/sw/sbplay.cpp
+++ b/sw/sbplay.cpp
@@ -15,8 +15,8 @@
 #endif
 
 #include "pico/stdlib.h"
-
 #include "pico/audio_i2s.h"
+#include "pico/flash.h"
 
 #include "opl.h"
 extern "C" void OPL_Pico_simple(int16_t *buffer, uint32_t nsamples);
@@ -90,6 +90,7 @@ struct audio_buffer_pool *init_audio() {
 
 void play_adlib() {
     puts("starting core 1");
+    flash_safe_execute_core_init();
     uint32_t start, end;
 
 #ifdef USE_ALARM
diff --git a/sw/tandyplay.cpp b/sw/tandyplay.cpp
index c1e41a5..ba85491 100644
--- a/sw/tandyplay.cpp
+++ b/sw/tandyplay.cpp
@@ -15,8 +15,8 @@
 #endif
 
 #include "pico/stdlib.h"
-
 #include "pico/audio_i2s.h"
+#include "pico/flash.h"
 
 #include "square/square.h"
 
@@ -75,6 +75,7 @@ struct audio_buffer_pool *init_audio() {
 
 void play_tandy() {
     puts("starting core 1 tandy");
+    flash_safe_execute_core_init();
 #ifdef USB_JOYSTICK
     // Init TinyUSB for joystick support
     tuh_init(BOARD_TUH_RHPORT);