From fa3ad399d72e93371ae706da5216a68fa40d27a3 Mon Sep 17 00:00:00 2001
From: Yatao Li <yatao.li@live.com>
Date: Sun, 15 Jun 2025 02:50:03 +0800
Subject: [PATCH] sd_boot: handle bootmode

---
 Code/pico_multi_booter/sd_boot/CMakeLists.txt |  3 +-
 .../pico_multi_booter/sd_boot/i2ckbd/i2ckbd.c | 22 ++++++++
 .../pico_multi_booter/sd_boot/i2ckbd/i2ckbd.h |  3 +-
 Code/pico_multi_booter/sd_boot/main.c         | 52 +++++++++++++++++++
 4 files changed, 78 insertions(+), 2 deletions(-)

diff --git a/Code/pico_multi_booter/sd_boot/CMakeLists.txt b/Code/pico_multi_booter/sd_boot/CMakeLists.txt
index 8a34451..cb42486 100644
--- a/Code/pico_multi_booter/sd_boot/CMakeLists.txt
+++ b/Code/pico_multi_booter/sd_boot/CMakeLists.txt
@@ -40,9 +40,10 @@ target_compile_options(${APP_NAME} PRIVATE
         -Wall
         -Wno-unused-variable
         -Wno-unused-function
+        -DPICO_STDIO_USB_ENABLE_RESET_VIA_VENDOR_INTERFACE=0
 ) 
 
-pico_enable_stdio_usb(${APP_NAME} 0)
+pico_enable_stdio_usb(${APP_NAME} 1)
 pico_enable_stdio_uart(${APP_NAME} 1)
 
 pico_add_extra_outputs(${APP_NAME})
diff --git a/Code/pico_multi_booter/sd_boot/i2ckbd/i2ckbd.c b/Code/pico_multi_booter/sd_boot/i2ckbd/i2ckbd.c
index 1b32456..f90c3b5 100644
--- a/Code/pico_multi_booter/sd_boot/i2ckbd/i2ckbd.c
+++ b/Code/pico_multi_booter/sd_boot/i2ckbd/i2ckbd.c
@@ -83,3 +83,25 @@ int read_battery() {
     }
     return -1;
 }
+
+int read_bootmode() {
+    int retval;
+    unsigned char msg[2];
+    msg[0] = 0x0e; // REG_ID_BOOT
+
+    if (i2c_inited == 0) return -1;
+
+    retval = i2c_write_timeout_us(I2C_KBD_MOD, I2C_KBD_ADDR, msg, 1, false, 500000);
+    if (retval == PICO_ERROR_GENERIC || retval == PICO_ERROR_TIMEOUT) {
+        DEBUG_PRINT("Boot I2C write err\n");
+        return -1;
+    }
+    sleep_ms(16);
+    retval = i2c_read_timeout_us(I2C_KBD_MOD, I2C_KBD_ADDR, (unsigned char *) msg, 2, false, 500000);
+    if (retval == PICO_ERROR_GENERIC || retval == PICO_ERROR_TIMEOUT || msg[0] != 0x0e) {
+        DEBUG_PRINT("Boot I2C read err\n");
+        return -1;
+    }
+
+    return msg[1];
+}
diff --git a/Code/pico_multi_booter/sd_boot/i2ckbd/i2ckbd.h b/Code/pico_multi_booter/sd_boot/i2ckbd/i2ckbd.h
index 6ced07b..dabb8d5 100644
--- a/Code/pico_multi_booter/sd_boot/i2ckbd/i2ckbd.h
+++ b/Code/pico_multi_booter/sd_boot/i2ckbd/i2ckbd.h
@@ -16,9 +16,10 @@
 void init_i2c_kbd();
 int read_i2c_kbd();
 int read_battery();
+int read_bootmode();
 
 #define bitRead(value, bit) (((value) >> (bit)) & 0x01)
 #define bitClear(value, bit) ((value) &= ~(1 << (bit)))
 #define bitSet(value, bit) ((value) |= (1 << (bit)))
 
-#endif
\ No newline at end of file
+#endif
diff --git a/Code/pico_multi_booter/sd_boot/main.c b/Code/pico_multi_booter/sd_boot/main.c
index 4f88392..445bb73 100644
--- a/Code/pico_multi_booter/sd_boot/main.c
+++ b/Code/pico_multi_booter/sd_boot/main.c
@@ -15,6 +15,8 @@
 #include <stdio.h>
 #include <string.h>
 #include "pico/stdlib.h"
+#include "pico/bootrom.h"
+#include "pico/usb_reset_interface.h"
 #include "hardware/gpio.h"
 #include "hardware/clocks.h"
 #include "debug.h"
@@ -202,6 +204,35 @@ static bool is_valid_application(uint32_t *app_location)
     return true;
 }
 
+void boot_default()
+{
+    DEBUG_PRINT("entering boot_default\n");
+    // Get the pointer to the application flash area
+    uint32_t *app_location = (uint32_t *)(XIP_BASE + SD_BOOT_FLASH_OFFSET);
+    launch_application_from(app_location);
+    // We should never reach here
+    while (1)
+    {
+        tight_loop_contents();
+    }
+}
+
+void boot_fwupdate()
+{
+    DEBUG_PRINT("entering boot_fwupdate\n");
+    lcd_init();
+    lcd_clear();
+
+    draw_rect_spi(20, 140, 300, 180, WHITE);
+    lcd_set_cursor(30, 150);
+    lcd_print_string_color((char *)"FIRMWARE UPDATE", BLACK, WHITE);
+
+    sleep_ms(2000);
+
+    uint gpio_mask = 0u;
+    reset_usb_boot(gpio_mask, PICO_STDIO_USB_RESET_BOOTSEL_INTERFACE_DISABLE_MASK);
+}
+
 int load_firmware_by_path(const char *path)
 {
     text_directory_ui_set_status("STAT: Flashing firmware...");
@@ -298,6 +329,27 @@ int main()
     gpio_pull_up(SD_DET_PIN); // Enable pull-up resistor
 
     keypad_init();
+
+    // Check bootmode now: 0=default, 1=sdcard, 2=fwupdate
+    int bootmode = read_bootmode();
+    DEBUG_PRINT("bootmode = %d\n", bootmode);
+    switch(bootmode) {
+      case 0:
+        // BOOTMODE_DEFAULT
+        boot_default();
+        break;
+      case 2:
+        // BOOTMODE_FWUPDATE
+        boot_fwupdate();
+        break;
+      case 1:
+        // BOOTMODE_SDCARD
+      default:
+        break;
+    }
+
+    // BEGIN SDCARD BOOT
+
     lcd_init();
     lcd_clear();
 	text_directory_ui_pre_init();