sd_boot: handle bootmode

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();