From 5995fb526158ae224dddacef212c1d553d6a6306 Mon Sep 17 00:00:00 2001
From: Damien George <damien@micropython.org>
Date: Wed, 16 Feb 2022 22:22:04 +1100
Subject: [PATCH] stm32/mboot: Allow a board more control over entry
 initialisation.

If MBOOT_BOARD_ENTRY_INIT is defined by a board then that function must now
make sure system clocks are configured, eg by calling mboot_entry_init().

Signed-off-by: Damien George <damien@micropython.org>
---
 ports/stm32/mboot/main.c  | 13 +------------
 ports/stm32/mboot/mboot.h | 22 ++++++++++++++++++++--
 2 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/ports/stm32/mboot/main.c b/ports/stm32/mboot/main.c
index aec31cbc20..8d6e28f189 100644
--- a/ports/stm32/mboot/main.c
+++ b/ports/stm32/mboot/main.c
@@ -1507,14 +1507,7 @@ void stm32_main(uint32_t initial_r0) {
 
 enter_bootloader:
 
-    // Init subsystems (mboot_get_reset_mode() may call these, calling them again is ok)
-    led_init();
-
-    // set the system clock to be HSE
-    SystemClock_Config();
-
-    // Ensure IRQs are enabled (needed coming out of ST bootloader on H7)
-    __set_PRIMASK(0);
+    MBOOT_BOARD_ENTRY_INIT(&initial_r0);
 
     #if USE_USB_POLLING
     // irqs with a priority value greater or equal to "pri" will be disabled
@@ -1524,10 +1517,6 @@ enter_bootloader:
     __ASM volatile ("msr basepri_max, %0" : : "r" (pri) : "memory");
     #endif
 
-    #if defined(MBOOT_BOARD_ENTRY_INIT)
-    MBOOT_BOARD_ENTRY_INIT(initial_r0);
-    #endif
-
     #if defined(MBOOT_SPIFLASH_ADDR)
     MBOOT_SPIFLASH_SPIFLASH->config = MBOOT_SPIFLASH_CONFIG;
     mp_spiflash_init(MBOOT_SPIFLASH_SPIFLASH);
diff --git a/ports/stm32/mboot/mboot.h b/ports/stm32/mboot/mboot.h
index 3a27ce08ef..254c842653 100644
--- a/ports/stm32/mboot/mboot.h
+++ b/ports/stm32/mboot/mboot.h
@@ -26,8 +26,7 @@
 #ifndef MICROPY_INCLUDED_STM32_MBOOT_MBOOT_H
 #define MICROPY_INCLUDED_STM32_MBOOT_MBOOT_H
 
-#include <stdint.h>
-#include <stddef.h>
+#include "py/mphal.h"
 
 // Use this to tag global static data in RAM that doesn't need to be zeroed on startup
 #define SECTION_NOZERO_BSS __attribute__((section(".nozero_bss")))
@@ -39,6 +38,10 @@
 #define NORETURN __attribute__((noreturn))
 #define MP_ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
 
+#ifndef MBOOT_BOARD_ENTRY_INIT
+#define MBOOT_BOARD_ENTRY_INIT mboot_entry_init
+#endif
+
 enum {
     MBOOT_ERRNO_FLASH_ERASE_DISALLOWED = 200,
     MBOOT_ERRNO_FLASH_ERASE_FAILED,
@@ -86,6 +89,8 @@ enum {
 extern uint8_t _estack[ELEM_DATA_SIZE];
 
 void systick_init(void);
+void led_init(void);
+void SystemClock_Config(void);
 
 uint32_t get_le32(const uint8_t *b);
 void led_state_all(unsigned int mask);
@@ -101,4 +106,17 @@ int do_write(uint32_t addr, const uint8_t *src8, size_t len);
 const uint8_t *elem_search(const uint8_t *elem, uint8_t elem_id);
 int fsload_process(void);
 
+static inline void mboot_entry_init(uint32_t *initial_r0) {
+    // Init subsystems (mboot_get_reset_mode() may call these, calling them again is ok)
+    led_init();
+
+    // set the system clock to be HSE
+    SystemClock_Config();
+
+    #if defined(STM32H7)
+    // Ensure IRQs are enabled (needed coming out of ST bootloader on H7)
+    __set_PRIMASK(0);
+    #endif
+}
+
 #endif // MICROPY_INCLUDED_STM32_MBOOT_MBOOT_H