2 files changed, 58 insertions, 2 deletions

diff --git a/platforms/chibios/bootloaders/rp2040.c b/platforms/chibios/bootloaders/rp2040.c
new file mode 100644
index 0000000000..bedc00f32e
--- /dev/null
+++ b/platforms/chibios/bootloaders/rp2040.c
@@ -0,0 +1,56 @@
+// Copyright 2022 Stefan Kerkmann
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include "quantum.h"
+#include "hal.h"
+#include "bootloader.h"
+#include "pico/bootrom.h"
+
+#if !defined(RP2040_BOOTLOADER_DOUBLE_TAP_RESET_LED)
+#    define RP2040_BOOTLOADER_DOUBLE_TAP_RESET_LED_MASK 0U
+#else
+#    define RP2040_BOOTLOADER_DOUBLE_TAP_RESET_LED_MASK (1U << RP2040_BOOTLOADER_DOUBLE_TAP_RESET_LED)
+#endif
+
+__attribute__((weak)) void mcu_reset(void) {
+    NVIC_SystemReset();
+}
+void bootloader_jump(void) {
+    reset_usb_boot(RP2040_BOOTLOADER_DOUBLE_TAP_RESET_LED_MASK, 0U);
+}
+
+void enter_bootloader_mode_if_requested(void) {}
+
+#if defined(RP2040_BOOTLOADER_DOUBLE_TAP_RESET)
+#    if !defined(RP2040_BOOTLOADER_DOUBLE_TAP_RESET_TIMEOUT)
+#        define RP2040_BOOTLOADER_DOUBLE_TAP_RESET_TIMEOUT 200U
+#    endif
+
+// Needs to be located in a RAM section that is never initialized on boot to
+// preserve its value on reset
+static volatile uint32_t __attribute__((section(".ram0.bootloader_magic"))) magic_location;
+const uint32_t magic_token = 0xCAFEB0BA;
+
+// We can not use the __early_init / enter_bootloader_mode_if_requested hook as
+// we depend on an already initialized system with usable memory regions and
+// populated function pointer tables to the optimized math functions in the
+// bootrom. This function is called just prior to main.
+void __late_init(void) {
+    // All clocks have to be enabled before jumping to the bootloader function,
+    // otherwise the bootrom will be stuck infinitely.
+    clocks_init();
+
+    if (magic_location != magic_token) {
+        magic_location = magic_token;
+        // ChibiOS is not initialized at this point, so sleeping is only
+        // possible via busy waiting.
+        wait_us(RP2040_BOOTLOADER_DOUBLE_TAP_RESET_TIMEOUT * 1000U);
+        magic_location = 0;
+        return;
+    }
+
+    magic_location = 0;
+    reset_usb_boot(RP2040_BOOTLOADER_DOUBLE_TAP_RESET_LED_MASK, 0U);
+}
+
+#endif
diff --git a/platforms/chibios/bootloaders/stm32_dfu.c b/platforms/chibios/bootloaders/stm32_dfu.c
index ff866bd2bc..7b4ab86033 100644
--- a/platforms/chibios/bootloaders/stm32_dfu.c
+++ b/platforms/chibios/bootloaders/stm32_dfu.c
@@ -38,7 +38,7 @@ extern uint32_t __ram0_end__;
 #    endif
 
 #    ifndef STM32_BOOTLOADER_DUAL_BANK_DELAY
-#        define STM32_BOOTLOADER_DUAL_BANK_DELAY 100000
+#        define STM32_BOOTLOADER_DUAL_BANK_DELAY 100
 #    endif
 
 __attribute__((weak)) void bootloader_jump(void) {
@@ -55,7 +55,7 @@ __attribute__((weak)) void bootloader_jump(void) {
 #    endif
 
     // Wait for a while for the capacitor to charge
-    wait_ms(100);
+    wait_ms(STM32_BOOTLOADER_DUAL_BANK_DELAY);
 
     // Issue a system reset to get the ROM bootloader to execute, with BOOT0 high
     NVIC_SystemReset();