stm32/powerctrl: Support changing frequency on WB MCUs.

This allows changing the frequency to: 100kHz, 200kHz, 400kHz, 800kHz,
1MHz, 2MHz, 4MHz, 8MHz, 16MHz, 32MHz, 64MHz.  For frequencies 2MHz and
below, low power run (LPR) mode is enabled automatically.

Signed-off-by: Damien George <damien@micropython.org>

ports/stm32/Makefile

@@ -100,7 +100,7 @@ CFLAGS_MCU_h7 = $(CFLAGS_CORTEX_M) -mtune=cortex-m7 -mcpu=cortex-m7
 CFLAGS_MCU_wb = $(CFLAGS_CORTEX_M) -mtune=cortex-m4 -mcpu=cortex-m4
 
 CFLAGS += $(INC) -Wall -Wpointer-arith -Werror -Wdouble-promotion -Wfloat-conversion -std=gnu99 -nostdlib $(CFLAGS_MOD) $(CFLAGS_EXTRA)
-CFLAGS += -D$(CMSIS_MCU)
+CFLAGS += -D$(CMSIS_MCU) -DUSE_FULL_LL_DRIVER
 CFLAGS += $(CFLAGS_MCU_$(MCU_SERIES))
 CFLAGS += $(COPT)
 CFLAGS += -I$(BOARD_DIR)
@@ -396,6 +396,7 @@ HAL_SRC_C += $(addprefix $(HAL_DIR)/Src/stm32$(MCU_SERIES)xx_,\
 	hal_tim.c \
 	hal_tim_ex.c \
 	hal_uart.c \
+	ll_utils.c \
 	)
 
 ifeq ($(MCU_SERIES),$(filter $(MCU_SERIES),f4 f7 h7 l0 l4 wb))

ports/stm32/powerctrl.c

@@ -519,11 +519,104 @@ set_clk:
 
 #elif defined(STM32WB)
 
+#include "stm32wbxx_ll_utils.h"
+
+#define LPR_THRESHOLD (2000000)
+#define VOS2_THRESHOLD (16000000)
+
+enum {
+    SYSCLK_MODE_NONE,
+    SYSCLK_MODE_MSI,
+    SYSCLK_MODE_HSE_64M,
+};
+
 int powerctrl_set_sysclk(uint32_t sysclk, uint32_t ahb, uint32_t apb1, uint32_t apb2) {
-    // For now it's not supported to change SYSCLK (only bus dividers).
-    if (sysclk != HAL_RCC_GetSysClockFreq()) {
+    int sysclk_mode = SYSCLK_MODE_NONE;
+    uint32_t msirange = 0;
+    uint32_t sysclk_cur = HAL_RCC_GetSysClockFreq();
+
+    if (sysclk == sysclk_cur) {
+        // SYSCLK does not need changing.
+    } else if (sysclk == 64000000) {
+        sysclk_mode = SYSCLK_MODE_HSE_64M;
+    } else {
+        for (msirange = 0; msirange < MP_ARRAY_SIZE(MSIRangeTable); ++msirange) {
+            if (MSIRangeTable[msirange] != 0 && sysclk == MSIRangeTable[msirange]) {
+                sysclk_mode = SYSCLK_MODE_MSI;
+                break;
+            }
+        }
+
+        if (sysclk_mode == SYSCLK_MODE_NONE) {
+            // Unsupported SYSCLK value.
             return -MP_EINVAL;
         }
+    }
+
+    // Exit LPR if SYSCLK will increase beyond threshold.
+    if (LL_PWR_IsEnabledLowPowerRunMode()) {
+        if (sysclk > LPR_THRESHOLD) {
+            if (sysclk_cur < LPR_THRESHOLD) {
+                // Must select MSI=LPR_THRESHOLD=2MHz to exit LPR.
+                LL_RCC_MSI_SetRange(LL_RCC_MSIRANGE_5);
+            }
+
+            // Exit LPR and wait for the regulator to be ready.
+            LL_PWR_ExitLowPowerRunMode();
+            while (!LL_PWR_IsActiveFlag_REGLPF()) {
+            }
+        }
+    }
+
+    // Select VOS1 if SYSCLK will increase beyond threshold.
+    if (sysclk > VOS2_THRESHOLD) {
+        LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1);
+        while (LL_PWR_IsActiveFlag_VOS()) {
+        }
+    }
+
+    if (sysclk_mode == SYSCLK_MODE_HSE_64M) {
+        SystemClock_Config();
+    } else if (sysclk_mode == SYSCLK_MODE_MSI) {
+        // Set flash latency to maximum to ensure the latency is large enough for
+        // both the current SYSCLK and the SYSCLK that will be selected below.
+        LL_FLASH_SetLatency(LL_FLASH_LATENCY_3);
+        while (LL_FLASH_GetLatency() != LL_FLASH_LATENCY_3) {
+        }
+
+        // Before changing the MSIRANGE value, if MSI is on then it must also be ready.
+        while ((RCC->CR & (RCC_CR_MSIRDY | RCC_CR_MSION)) == RCC_CR_MSION) {
+        }
+        LL_RCC_MSI_SetRange(msirange << RCC_CR_MSIRANGE_Pos);
+
+        // Clock SYSCLK from MSI.
+        LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_MSI);
+        while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_MSI) {
+        }
+
+        // Disable PLL to decrease power consumption.
+        LL_RCC_PLL_Disable();
+        while (LL_RCC_PLL_IsReady() != 0) {
+        }
+        LL_RCC_PLL_DisableDomain_SYS();
+
+        // Select VOS2 if possible.
+        if (sysclk <= VOS2_THRESHOLD) {
+            LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE2);
+        }
+
+        // Enter LPR if possible.
+        if (sysclk <= LPR_THRESHOLD) {
+            LL_PWR_EnterLowPowerRunMode();
+        }
+
+        // Configure flash latency for the new SYSCLK.
+        LL_SetFlashLatency(sysclk);
+
+        // Update HAL state and SysTick.
+        SystemCoreClockUpdate();
+        powerctrl_config_systick();
+    }
 
     // Return straightaway if the clocks are already at the desired frequency.
     if (ahb == HAL_RCC_GetHCLKFreq()

ports/stm32/powerctrl.h

@@ -35,6 +35,7 @@ NORETURN void powerctrl_mcu_reset(void);
 NORETURN void powerctrl_enter_bootloader(uint32_t r0, uint32_t bl_addr);
 void powerctrl_check_enter_bootloader(void);
 
+void powerctrl_config_systick(void);
 int powerctrl_rcc_clock_config_pll(RCC_ClkInitTypeDef *rcc_init, uint32_t sysclk_mhz, bool need_pllsai);
 int powerctrl_set_sysclk(uint32_t sysclk, uint32_t ahb, uint32_t apb1, uint32_t apb2);
 void powerctrl_enter_stop_mode(void);

ports/stm32/powerctrlboot.c

@@ -28,7 +28,7 @@
 #include "irq.h"
 #include "powerctrl.h"
 
-static inline void powerctrl_config_systick(void) {
+void powerctrl_config_systick(void) {
     // Configure SYSTICK to run at 1kHz (1ms interval)
     SysTick->CTRL |= SYSTICK_CLKSOURCE_HCLK;
     SysTick_Config(HAL_RCC_GetHCLKFreq() / 1000);