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>

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;

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;

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);

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;

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
 

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
 

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