1 files changed, 18 insertions, 0 deletions
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;