1 files changed, 54 insertions, 0 deletions
diff --git a/ports/stm32/powerctrl.c b/ports/stm32/powerctrl.c
index c3792be3e..2ad242600 100644
--- a/ports/stm32/powerctrl.c
+++ b/ports/stm32/powerctrl.c
@@ -30,6 +30,60 @@
 #include "rtc.h"
 #include "genhdr/pllfreqtable.h"
 
+#if defined(STM32H7)
+#define RCC_SR          RSR
+#define RCC_SR_SFTRSTF  RCC_RSR_SFTRSTF
+#define RCC_SR_RMVF     RCC_RSR_RMVF
+#else
+#define RCC_SR          CSR
+#define RCC_SR_SFTRSTF  RCC_CSR_SFTRSTF
+#define RCC_SR_RMVF     RCC_CSR_RMVF
+#endif
+
+// Location in RAM of bootloader state (just after the top of the stack)
+extern uint32_t _estack[];
+#define BL_STATE ((uint32_t*)&_estack)
+
+NORETURN void powerctrl_mcu_reset(void) {
+    BL_STATE[1] = 1; // invalidate bootloader address
+    #if __DCACHE_PRESENT == 1
+    SCB_CleanDCache();
+    #endif
+    NVIC_SystemReset();
+}
+
+NORETURN void powerctrl_enter_bootloader(uint32_t r0, uint32_t bl_addr) {
+    BL_STATE[0] = r0;
+    BL_STATE[1] = bl_addr;
+    #if __DCACHE_PRESENT == 1
+    SCB_CleanDCache();
+    #endif
+    NVIC_SystemReset();
+}
+
+static __attribute__((naked)) void branch_to_bootloader(uint32_t r0, uint32_t bl_addr) {
+    __asm volatile (
+        "ldr r2, [r1, #0]\n"    // get address of stack pointer
+        "msr msp, r2\n"         // get stack pointer
+        "ldr r2, [r1, #4]\n"    // get address of destination
+        "bx r2\n"               // branch to bootloader
+    );
+}
+
+void powerctrl_check_enter_bootloader(void) {
+    uint32_t bl_addr = BL_STATE[1];
+    BL_STATE[1] = 1; // invalidate bootloader address
+    if ((bl_addr & 0xfff) == 0 && (RCC->RCC_SR & RCC_SR_SFTRSTF)) {
+        // Reset by NVIC_SystemReset with bootloader data set -> branch to bootloader
+        RCC->RCC_SR = RCC_SR_RMVF;
+        #if defined(STM32F0) || defined(STM32F4) || defined(STM32L4)
+        __HAL_SYSCFG_REMAPMEMORY_SYSTEMFLASH();
+        #endif
+        uint32_t r0 = BL_STATE[0];
+        branch_to_bootloader(r0, bl_addr);
+    }
+}
+
 #if !defined(STM32F0)
 
 // Assumes that PLL is used as the SYSCLK source