From 745d83adac60727dee4b041b219531b67f4a0257 Mon Sep 17 00:00:00 2001
From: Florin Maticu <florin.maticu@gmail.com>
Date: Sun, 18 Sep 2022 16:51:12 +0200
Subject: [PATCH] DFU mode implementation for STM32F4 MCU.

---
 .../stm/common-hal/microcontroller/__init__.c | 14 ++++++--
 ports/stm/supervisor/port.c                   | 32 +++++++++++++++++++
 2 files changed, 44 insertions(+), 2 deletions(-)

diff --git a/ports/stm/common-hal/microcontroller/__init__.c b/ports/stm/common-hal/microcontroller/__init__.c
index 3bb850f5d3..c399951f54 100644
--- a/ports/stm/common-hal/microcontroller/__init__.c
+++ b/ports/stm/common-hal/microcontroller/__init__.c
@@ -36,7 +36,7 @@
 #include "shared-bindings/microcontroller/__init__.h"
 #include "shared-bindings/microcontroller/Pin.h"
 #include "shared-bindings/microcontroller/Processor.h"
-
+#include "supervisor/port.h"
 #include "supervisor/filesystem.h"
 #include "supervisor/shared/safe_mode.h"
 
@@ -73,15 +73,25 @@ void common_hal_mcu_enable_interrupts(void) {
     __enable_irq();
 }
 
+static bool next_reset_to_bootloader = false;
+
 void common_hal_mcu_on_next_reset(mcu_runmode_t runmode) {
     if (runmode == RUNMODE_SAFE_MODE) {
         safe_mode_on_next_reset(PROGRAMMATIC_SAFE_MODE);
     }
+    if (runmode == RUNMODE_BOOTLOADER) {
+        next_reset_to_bootloader = true;
+    }
 }
 
 void common_hal_mcu_reset(void) {
     filesystem_flush(); // TODO: implement as part of flash improvements
-    NVIC_SystemReset();
+
+    if (next_reset_to_bootloader) {
+        reset_to_bootloader();
+    } else {
+        NVIC_SystemReset();
+    }
 }
 
 // The singleton microcontroller.Processor object, bound to microcontroller.cpu
diff --git a/ports/stm/supervisor/port.c b/ports/stm/supervisor/port.c
index c3f1fee190..7f31f3c7e5 100644
--- a/ports/stm/supervisor/port.c
+++ b/ports/stm/supervisor/port.c
@@ -276,7 +276,39 @@ void reset_port(void) {
 }
 
 void reset_to_bootloader(void) {
+
+/*
+From STM AN2606:
+Before jumping to bootloader user must:
+• Disable all peripheral clocks
+• Disable used PLL
+• Disable interrupts
+• Clear pending interrupts
+System memory boot mode can be exited by getting out from bootloader activation
+condition and generating hardware reset or using Go command to execute user code
+*/
+    HAL_RCC_DeInit();
+    HAL_DeInit();
+
+    // disable all interupts
+    __disable_irq();
+
+    // Clear all pending interrupts
+    for (uint8_t i = 0; i < (sizeof(NVIC->ICPR) / NVIC->ICPR[0]); ++i) {
+        NVIC->ICPR[i] = 0xFFFFFFFF;
+    }
+    // information about jump addresses has been taken from STM AN2606.
+    #if defined(STM32F4)
+    __set_MSP(*((uint32_t *)0x1FFF0000));
+    ((void (*)(void)) * ((uint32_t *)0x1FFF0004))();
+    #else
+    // DFU mode for STM32 variant note implemented.
     NVIC_SystemReset();
+    #endif
+
+    while (true) {
+        asm ("nop;");
+    }
 }
 
 void reset_cpu(void) {