diff --git a/ports/stm32/Makefile b/ports/stm32/Makefile index fd76f20a6..cdccfddcf 100644 --- a/ports/stm32/Makefile +++ b/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)) diff --git a/ports/stm32/powerctrl.c b/ports/stm32/powerctrl.c index 253f1056c..137312ade 100644 --- a/ports/stm32/powerctrl.c +++ b/ports/stm32/powerctrl.c @@ -519,10 +519,103 @@ 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()) { - return -MP_EINVAL; + 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. diff --git a/ports/stm32/powerctrl.h b/ports/stm32/powerctrl.h index 9f223e794..eedc448b2 100644 --- a/ports/stm32/powerctrl.h +++ b/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); diff --git a/ports/stm32/powerctrlboot.c b/ports/stm32/powerctrlboot.c index 880e43e04..caa563292 100644 --- a/ports/stm32/powerctrlboot.c +++ b/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);