stm32/powerctrlboot: Provide custom SystemInit for WB55.

Signed-off-by: Andrew Leech <andrew.leech@planetinnovation.com.au>
Signed-off-by: Damien George <damien@micropython.org>

7 files changed, 35 insertions, 6 deletions

diff --git a/ports/stm32/main.c b/ports/stm32/main.c
index 66dcb86dd..87799d2d3 100644
--- a/ports/stm32/main.c
+++ b/ports/stm32/main.c
@@ -298,6 +298,9 @@ STATIC bool init_sdcard_fs(void) {
 #endif
 
 void stm32_main(uint32_t reset_mode) {
+    // Low-level MCU initialisation.
+    stm32_system_init();
+
     #if !defined(STM32F0) && defined(MICROPY_HW_VTOR)
     // Change IRQ vector table if configured differently
     SCB->VTOR = MICROPY_HW_VTOR;
diff --git a/ports/stm32/mboot/main.c b/ports/stm32/mboot/main.c
index 74c70e76e..59106beb7 100644
--- a/ports/stm32/mboot/main.c
+++ b/ports/stm32/mboot/main.c
@@ -1311,6 +1311,9 @@ extern PCD_HandleTypeDef pcd_fs_handle;
 extern PCD_HandleTypeDef pcd_hs_handle;
 
 void stm32_main(uint32_t initial_r0) {
+    // Low-level MCU initialisation.
+    stm32_system_init();
+
     #if defined(STM32H7)
     // Configure write-once power options, and wait for voltage levels to be ready
     PWR->CR3 = PWR_CR3_LDOEN;
diff --git a/ports/stm32/powerctrl.h b/ports/stm32/powerctrl.h
index eedc448b2..e9adc5296 100644
--- a/ports/stm32/powerctrl.h
+++ b/ports/stm32/powerctrl.h
@@ -29,6 +29,14 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#if defined(STM32WB)
+void stm32_system_init(void);
+#else
+static inline void stm32_system_init(void) {
+    SystemInit();
+}
+#endif
+
 void SystemClock_Config(void);
 
 NORETURN void powerctrl_mcu_reset(void);
diff --git a/ports/stm32/powerctrlboot.c b/ports/stm32/powerctrlboot.c
index b78fcae4b..912c8633a 100644
--- a/ports/stm32/powerctrlboot.c
+++ b/ports/stm32/powerctrlboot.c
@@ -28,6 +28,24 @@
 #include "irq.h"
 #include "powerctrl.h"
 
+#if defined(STM32WB)
+void stm32_system_init(void) {
+    if (RCC->CR == 0x00000560 && RCC->CFGR == 0x00070005) {
+        // Wake from STANDBY with HSI enabled as system clock.  The second core likely
+        // also needs HSI to remain enabled, so do as little as possible here.
+        #if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+        // set CP10 and CP11 Full Access.
+        SCB->CPACR |= (3 << (10 * 2)) | (3 << (11 * 2));
+        #endif
+        // Disable all interrupts.
+        RCC->CIER = 0x00000000;
+    } else {
+        // Other start-up (eg POR), use standard system init code.
+        SystemInit();
+    }
+}
+#endif
+
 void powerctrl_config_systick(void) {
     // Configure SYSTICK to run at 1kHz (1ms interval)
     SysTick->CTRL |= SYSTICK_CLKSOURCE_HCLK;
diff --git a/ports/stm32/resethandler.s b/ports/stm32/resethandler.s
index 7f0973346..e0e59321f 100644
--- a/ports/stm32/resethandler.s
+++ b/ports/stm32/resethandler.s
@@ -62,8 +62,7 @@ Reset_Handler:
     cmp  r1, r2
     bcc  .bss_zero_loop
 
-    /* Initialise the system and jump to the main code */
-    bl   SystemInit
+    /* Jump to the main code */
     mov  r0, r4
     b    stm32_main
 
diff --git a/ports/stm32/resethandler_m0.s b/ports/stm32/resethandler_m0.s
index bb88e8dae..868d6da90 100644
--- a/ports/stm32/resethandler_m0.s
+++ b/ports/stm32/resethandler_m0.s
@@ -66,8 +66,7 @@ Reset_Handler:
     cmp  r1, r2
     bcc  .bss_zero_loop
 
-    /* Initialise the system and jump to the main code */
-    bl   SystemInit
+    /* Jump to the main code */
     mov  r0, r4
     bl   stm32_main
 
diff --git a/ports/stm32/resethandler_m3.s b/ports/stm32/resethandler_m3.s
index 05a44306e..f29821696 100644
--- a/ports/stm32/resethandler_m3.s
+++ b/ports/stm32/resethandler_m3.s
@@ -67,8 +67,7 @@ Reset_Handler:
     cmp  r1, r2
     bcc  .bss_zero_loop
 
-    /* Initialise the system and jump to the main code */
-    bl   SystemInit
+    /* Jump to the main code */
     mov  r0, r4
     bl   stm32_main