2 files changed, 34 insertions, 1 deletions
diff --git a/src/stm32/stm32g0.c b/src/stm32/stm32g0.c
index f38fc9c5..36520dfb 100644
--- a/src/stm32/stm32g0.c
+++ b/src/stm32/stm32g0.c
@@ -147,7 +147,6 @@ usb_request_bootloader(void)
 void
 armcm_main(void)
 {
-    check_usb_dfu_bootloader();
     SCB->VTOR = (uint32_t)VectorTable;
 
     // Reset clock registers (in case bootloader has changed them)
@@ -164,6 +163,8 @@ armcm_main(void)
     RCC->APBENR1 = 0x00000000;
     RCC->APBENR2 = 0x00000000;
 
+    check_usb_dfu_bootloader();
+
     // Set flash latency
     FLASH->ACR = (2<<FLASH_ACR_LATENCY_Pos) | FLASH_ACR_ICEN | FLASH_ACR_PRFTEN;
 
diff --git a/src/stm32/stm32h7.c b/src/stm32/stm32h7.c
index d6a32d96..bb0fe454 100644
--- a/src/stm32/stm32h7.c
+++ b/src/stm32/stm32h7.c
@@ -6,6 +6,7 @@
 
 #include "autoconf.h" // CONFIG_CLOCK_REF_FREQ
 #include "board/armcm_boot.h" // VectorTable
+#include "board/irq.h" // irq_disable
 #include "board/armcm_reset.h" // try_request_canboot
 #include "command.h" // DECL_CONSTANT_STR
 #include "internal.h" // get_pclock_frequency
@@ -187,11 +188,36 @@ clock_setup(void)
  * USB bootloader
  ****************************************************************/
 
+#define USB_BOOT_FLAG_ADDR (CONFIG_RAM_START + CONFIG_RAM_SIZE - 1024)
+#define USB_BOOT_FLAG 0x55534220424f4f54 // "USB BOOT"
+
+// Flag that bootloader is desired and reboot
+static void
+usb_reboot_for_dfu_bootloader(void)
+{
+    irq_disable();
+    *(uint64_t*)USB_BOOT_FLAG_ADDR = USB_BOOT_FLAG;
+    NVIC_SystemReset();
+}
+
+// Check if rebooting into system DFU Bootloader
+static void
+check_usb_dfu_bootloader(void)
+{
+    if (!CONFIG_USBSERIAL || *(uint64_t*)USB_BOOT_FLAG_ADDR != USB_BOOT_FLAG)
+        return;
+    *(uint64_t*)USB_BOOT_FLAG_ADDR = 0;
+    uint32_t *sysbase = (uint32_t*)0x1FF09800;
+    asm volatile("mov sp, %0\n bx %1"
+                 : : "r"(sysbase[0]), "r"(sysbase[1]));
+}
+
 // Handle USB reboot requests
 void
 usb_request_bootloader(void)
 {
     try_request_canboot();
+    usb_reboot_for_dfu_bootloader();
 }
 
 
@@ -205,8 +231,14 @@ armcm_main(void)
 {
     // Run SystemInit() and then restore VTOR
     SystemInit();
+    RCC->D1CCIPR = 0x00000000;
+    RCC->D2CCIP1R = 0x00000000;
+    RCC->D2CCIP2R = 0x00000000;
+    RCC->D3CCIPR = 0x00000000;
     SCB->VTOR = (uint32_t)VectorTable;
 
+    check_usb_dfu_bootloader();
+
     clock_setup();
 
     sched_main();