1 files changed, 59 insertions, 14 deletions

diff --git a/platforms/chibios/bootloaders/stm32_dfu.c b/platforms/chibios/bootloaders/stm32_dfu.c
index f845bf21e9..8cf640f7f9 100644
--- a/platforms/chibios/bootloaders/stm32_dfu.c
+++ b/platforms/chibios/bootloaders/stm32_dfu.c
@@ -1,4 +1,4 @@
-/* Copyright 2021 QMK
+/* Copyright 2021-2023 QMK
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -15,12 +15,17 @@
  */
 
 #include "bootloader.h"
+#include "util.h"
 
 #include <ch.h>
 #include <hal.h>
 #include "wait.h"
 
-extern uint32_t __ram0_end__;
+#ifndef STM32_BOOTLOADER_RAM_SYMBOL
+#    define STM32_BOOTLOADER_RAM_SYMBOL __ram0_end__
+#endif
+
+extern uint32_t STM32_BOOTLOADER_RAM_SYMBOL;
 
 #ifndef STM32_BOOTLOADER_DUAL_BANK
 #    define STM32_BOOTLOADER_DUAL_BANK FALSE
@@ -72,10 +77,25 @@ void enter_bootloader_mode_if_requested(void) {}
 /* This code should be checked whether it runs correctly on platforms */
 #    define SYMVAL(sym) (uint32_t)(((uint8_t *)&(sym)) - ((uint8_t *)0))
 #    define BOOTLOADER_MAGIC 0xDEADBEEF
-#    define MAGIC_ADDR (unsigned long *)(SYMVAL(__ram0_end__) - 4)
+#    define MAGIC_ADDR (unsigned long *)(SYMVAL(STM32_BOOTLOADER_RAM_SYMBOL) - 4)
+
+__attribute__((weak)) void bootloader_marker_enable(void) {
+    uint32_t *marker = (uint32_t *)MAGIC_ADDR;
+    *marker          = BOOTLOADER_MAGIC; // set magic flag => reset handler will jump into boot loader
+}
+
+__attribute__((weak)) bool bootloader_marker_active(void) {
+    const uint32_t *marker = (const uint32_t *)MAGIC_ADDR;
+    return (*marker == BOOTLOADER_MAGIC) ? true : false;
+}
+
+__attribute__((weak)) void bootloader_marker_disable(void) {
+    uint32_t *marker = (uint32_t *)MAGIC_ADDR;
+    *marker          = 0;
+}
 
 __attribute__((weak)) void bootloader_jump(void) {
-    *MAGIC_ADDR = BOOTLOADER_MAGIC; // set magic flag => reset handler will jump into boot loader
+    bootloader_marker_enable();
     NVIC_SystemReset();
 }
 
@@ -84,18 +104,43 @@ __attribute__((weak)) void mcu_reset(void) {
 }
 
 void enter_bootloader_mode_if_requested(void) {
-    unsigned long *check = MAGIC_ADDR;
-    if (*check == BOOTLOADER_MAGIC) {
-        *check = 0;
-        __set_CONTROL(0);
-        __set_MSP(*(__IO uint32_t *)STM32_BOOTLOADER_ADDRESS);
+    if (bootloader_marker_active()) {
+        bootloader_marker_disable();
+
+        __disable_irq();
+
+#    if defined(QMK_MCU_ARCH_CORTEX_M7)
+        SCB_DisableDCache();
+        SCB_DisableICache();
+#    endif
+
+#    if defined(__MPU_PRESENT) && (__MPU_PRESENT == 1U)
+        ARM_MPU_Disable();
+#    endif
+
+        SysTick->CTRL = 0;
+        SysTick->VAL  = 0;
+        SysTick->LOAD = 0;
+
+        // Clear interrupt enable and interrupt pending registers
+        for (int i = 0; i < ARRAY_SIZE(NVIC->ICER); i++) {
+            NVIC->ICER[i] = 0xFFFFFFFF;
+            NVIC->ICPR[i] = 0xFFFFFFFF;
+        }
+
         __enable_irq();
 
-        typedef void (*BootJump_t)(void);
-        BootJump_t boot_jump = *(BootJump_t *)(STM32_BOOTLOADER_ADDRESS + 4);
-        boot_jump();
-        while (1)
-            ;
+        struct system_memory_vector_t {
+            uint32_t stack_top;
+            void (*entrypoint)(void);
+        };
+        const struct system_memory_vector_t *bootloader = (const struct system_memory_vector_t *)(STM32_BOOTLOADER_ADDRESS);
+
+        // Jump to bootloader
+        __set_MSP(bootloader->stack_top);
+        bootloader->entrypoint();
+        while (true) {
+        }
     }
 }
 #endif