STM32WBAX PM rework to support STOP/STDBY modes

dts/arm/st/wba/stm32wba.dtsi

@@ -63,7 +63,11 @@
 				power-state-name = "suspend-to-ram";
 				substate-id = <1>;
 				min-residency-us = <1000>;
-				exit-latency-us = <50>;
+				/*
+				 * From DS: worst case Twu_stdby with retention = 51.49 us
+				 * Add extra margin to take resume code into account.
+				 */
+				exit-latency-us = <150>;
 			};
 		};
 	};

soc/st/stm32/stm32wbax/Kconfig

@@ -18,3 +18,6 @@ config SOC_SERIES_STM32WBAX
 	select HAS_PM
 	select HAS_POWEROFF
 	select SOC_EARLY_INIT_HOOK
+	select PM_DEVICE if PM
+	select PM_DEVICE_RUNTIME if PM
+	select PM_DEVICE_SYSTEM_MANAGED if PM

soc/st/stm32/stm32wbax/hci_if/linklayer_plat_adapt.c

@@ -10,7 +10,8 @@
 
 #include <stm32_backup_domain.h>
 
-#include "scm.h"
+#include <app_conf.h>
+#include <bsp.h>
 
 #define LOG_LEVEL CONFIG_SOC_LOG_LEVEL
 LOG_MODULE_REGISTER(linklayer_plat_adapt);
@@ -245,17 +246,13 @@ void LINKLAYER_PLAT_StartRadioEvt(void)
 	__HAL_RCC_RADIO_CLK_SLEEP_ENABLE();
 
 	NVIC_SetPriority((IRQn_Type)RADIO_INTR_NUM, RADIO_INTR_PRIO_HIGH_Z);
-
-	scm_notifyradiostate(SCM_RADIO_ACTIVE);
 }
 
 void LINKLAYER_PLAT_StopRadioEvt(void)
 {
 	__HAL_RCC_RADIO_CLK_SLEEP_DISABLE();
 
 	NVIC_SetPriority((IRQn_Type)RADIO_INTR_NUM, RADIO_INTR_PRIO_LOW_Z);
-
-	scm_notifyradiostate(SCM_RADIO_NOT_ACTIVE);
 }
 
 /* Link Layer notification for RCO calibration start */

soc/st/stm32/stm32wbax/power.c

@@ -10,6 +10,12 @@
 #include <zephyr/init.h>
 #include <zephyr/arch/common/pm_s2ram.h>
 #include <zephyr/drivers/timer/system_timer.h>
+#include <zephyr/arch/arm/cortex_m/fpu.h>
+#include <zephyr/arch/arm/cortex_m/scb.h>
+#include <zephyr/arch/arm/mpu/arm_mpu.h>
+#include <kernel_arch_func.h>
+
+#include <cmsis_core.h>
 
 #include <stm32wbaxx_ll_bus.h>
 #include <stm32wbaxx_ll_cortex.h>
@@ -18,69 +24,75 @@
 #include <stm32wbaxx_ll_system.h>
 #include <clock_control/clock_stm32_ll_common.h>
 
-#ifdef CONFIG_BT_STM32WBA
-#include "scm.h"
-#endif
-
 #include <zephyr/logging/log.h>
 
 LOG_MODULE_DECLARE(soc, CONFIG_SOC_LOG_LEVEL);
 
-void stm32_power_init(void);
 
-static void set_mode_stop(uint8_t substate_id)
-{
+#define HSE_ON (READ_BIT(RCC->CR, RCC_CR_HSEON) == RCC_CR_HSEON)
 
-	LL_PWR_ClearFlag_STOP();
-	LL_RCC_ClearResetFlags();
+static uint32_t ram_waitstates_backup;
+static uint32_t flash_latency_backup;
 
-	/* Erratum 2.2.15:
-	 * Disabling ICACHE is required before entering stop mode
-	 */
-	sys_cache_instr_disable();
-
-#ifdef CONFIG_BT_STM32WBA
-	scm_setwaitstates(LP);
+#if defined(CONFIG_PM_S2RAM)
+static struct fpu_ctx_full fpu_state;
+static struct scb_context scb_state;
+static struct z_mpu_context_retained mpu_state;
 #endif
-	/* Set SLEEPDEEP bit of Cortex System Control Register */
+
+static int enter_low_power_mode(void)
+{
+	uint32_t basepri;
+
 	LL_LPM_EnableDeepSleep();
 
 	while (LL_PWR_IsActiveFlag_ACTVOS() == 0) {
 	}
 
-	switch (substate_id) {
-	case 1: /* enter STOP0 mode */
-		LL_PWR_SetPowerMode(LL_PWR_MODE_STOP0);
-		break;
-	case 2: /* enter STOP1 mode */
-		LL_PWR_SetPowerMode(LL_PWR_MODE_STOP1);
-		break;
-	default:
-		LOG_DBG("Unsupported power state substate-id %u", substate_id);
-		break;
-	}
-}
+	/*
+	 * Prevent spurious entry in low-power state if any
+	 * interrupt is currently pending. The WFI instruction
+	 * will not enter low-power state if an interrupt with
+	 * higher priority than BASEPRI is pending, regardless
+	 * of the value of PRIMASK.
+	 *
+	 * When this function executes, Zephyr's arch_irq_lock() has
+	 * configured BASEPRI to mask interrupts. Set PRIMASK to keep
+	 * interrupts masked then lower BASEPRI to 0, after saving its
+	 * current value, so that any pending interrupt is seen by WFI
+	 * and inhibits low-power state entry.
+	 */
+	__disable_irq();
+	basepri = __get_BASEPRI();
+	__set_BASEPRI(0);
+	__ISB();
+	__DSB();
 
-#if defined(CONFIG_PM_S2RAM)
-static int suspend_to_ram(void)
-{
-	LL_LPM_EnableDeepSleep();
+	/* Attempt entry in low-power state */
+	__WFI();
 
-	while (LL_PWR_IsActiveFlag_ACTVOS() == 0) {
-	}
+	/*
+	 * If entry in STOP mode was attempted, execution
+	 * resumes here regardless of success or failure.
+	 * If entry in STANDBY mode was attempted, execution
+	 * will reach this point only if entry in low-power
+	 * state did not occur. Restore BASEPRI and clear
+	 * PRIMASK then return an error to indicate that
+	 * entry in STANDBY mode failed (return value of
+	 * this function is ignored for STOP mode entry).
+	 */
+	__set_BASEPRI(basepri);
+	__enable_irq();
 
-	/* Select mode entry : WFE or WFI and enter the CPU selected mode */
-	k_cpu_idle();
+	/* Disable SLEEPDEEP at Cortex-M level */
+	LL_LPM_EnableSleep();
 
-	return 0;
+	return -EBUSY;
 }
 
-static void set_mode_suspend_to_ram(void)
+#if defined(CONFIG_PM_S2RAM)
+static void set_mode_suspend_to_ram_enter(void)
 {
-	/* Enable SRAM full retention */
-	LL_PWR_SetSRAM1SBRetention(LL_PWR_SRAM1_SB_FULL_RETENTION);
-	LL_PWR_SetSRAM2SBRetention(LL_PWR_SRAM2_SB_FULL_RETENTION);
-
 	/* Enable RTC wakeup
 	 * This configures an internal pin that generates an event to wakeup the system
 	 */
@@ -97,88 +109,124 @@ static void set_mode_suspend_to_ram(void)
 	/* Select standby mode */
 	LL_PWR_SetPowerMode(LL_PWR_MODE_STANDBY);
 
+	/* Save FPU, SCB and MPU states */
+	z_arm_save_fp_context(&fpu_state);
+	z_arm_save_scb_context(&scb_state);
+#if defined(CONFIG_ARM_MPU)
+	z_arm_save_mpu_context(&mpu_state);
+#endif /* CONFIG_ARM_MPU */
 	/* Save context and enter Standby mode */
-	arch_pm_s2ram_suspend(suspend_to_ram);
+	arch_pm_s2ram_suspend(enter_low_power_mode);
+}
+
+static void set_mode_suspend_to_ram_exit(void)
+{
+	/* Save MPU, SCB and FPU states */
+#if defined(CONFIG_ARM_MPU)
+	z_arm_restore_mpu_context(&mpu_state);
+#endif /* CONFIG_ARM_MPU */
+	z_arm_restore_scb_context(&scb_state);
+	z_arm_restore_fp_context(&fpu_state);
+
+	__HAL_RCC_PWR_CLK_ENABLE();
+	__HAL_RCC_RAMCFG_CLK_ENABLE();
 
-	/* Execution is restored at this point after wake up */
 	/* Restore system clock as soon as we exit standby mode */
 	stm32_clock_control_standby_exit();
+
+	if (LL_PWR_IsActiveFlag_SB() || !HSE_ON) {
+		stm32_clock_control_init(NULL);
+	}
+	/* Resume configuration */
+	stm32wba_init();
+	stm32_power_init();
 }
 #endif
 
-/* Invoke Low Power/System Off specific Tasks */
-void pm_state_set(enum pm_state state, uint8_t substate_id)
+static void set_mode_stop_enter(uint8_t substate_id)
 {
-	switch (state) {
-	case PM_STATE_SUSPEND_TO_IDLE:
-		set_mode_stop(substate_id);
-
-		/* Select mode entry : WFE or WFI and enter the CPU selected mode */
-		k_cpu_idle();
+	LL_PWR_ClearFlag_STOP();
+	LL_RCC_ClearResetFlags();
 
+	/* Erratum 2.2.15:
+	 * Disabling ICACHE is required before entering stop mode
+	 */
+	sys_cache_instr_disable();
+	/* If stop mode is greater than 1, manage flash latency and RAMCFG */
+	if (substate_id > 1) {
+		flash_latency_backup = __HAL_FLASH_GET_LATENCY();
+		if (flash_latency_backup < FLASH_LATENCY_1) {
+			/* Set flash latency to 1, since system will restart from HSI16 */
+			__HAL_FLASH_SET_LATENCY(FLASH_LATENCY_1);
+			while (__HAL_FLASH_GET_LATENCY() != FLASH_LATENCY_1) {
+			}
+		}
+		ram_waitstates_backup = READ_BIT(RAMCFG_SRAM1->CR, RAMCFG_CR_WSC);
+		MODIFY_REG(RAMCFG_SRAM1->CR, RAMCFG_CR_WSC, RAMCFG_WAITSTATE_1);
+		MODIFY_REG(RAMCFG_SRAM2->CR, RAMCFG_CR_WSC, RAMCFG_WAITSTATE_1);
+	}
+	switch (substate_id) {
+	case 1:
+		LL_PWR_SetPowerMode(LL_PWR_MODE_STOP0);
 		break;
-#if defined(CONFIG_PM_S2RAM)
-	case PM_STATE_SUSPEND_TO_RAM:
-		set_mode_suspend_to_ram();
+	case 2:
+		LL_PWR_SetPowerMode(LL_PWR_MODE_STOP1);
 		break;
-#endif
 	default:
-		LOG_DBG("Unsupported power state %u", state);
+		LOG_DBG("Unsupported power state substate-id %u", substate_id);
 		return;
 	}
+
+	enter_low_power_mode();
 }
 
-/* Handle SOC specific activity after Low Power Mode Exit */
-void pm_state_exit_post_ops(enum pm_state state, uint8_t substate_id)
+static void set_mode_stop_exit(uint8_t substate_id)
 {
-#ifdef CONFIG_BT_STM32WBA
-	if (LL_PWR_IsActiveFlag_STOP() == 1U) {
-		scm_setup();
+	if (LL_PWR_IsActiveFlag_STOP() || !HSE_ON) {
+		/* Reconfigure the clock (incl. RAM/FLASH latency) */
+		stm32_clock_control_init(NULL);
 	} else {
-		scm_setwaitstates(RUN);
+		/* Stop mode skipped */
+		if (substate_id > 1) {
+			__HAL_FLASH_SET_LATENCY(flash_latency_backup);
+			while (__HAL_FLASH_GET_LATENCY() != flash_latency_backup) {
+			}
+			MODIFY_REG(RAMCFG_SRAM1->CR, RAMCFG_CR_WSC, ram_waitstates_backup);
+			MODIFY_REG(RAMCFG_SRAM2->CR, RAMCFG_CR_WSC, ram_waitstates_backup);
+		}
 	}
-#endif
 
+	/* Erratum 2.2.15:
+	 * Enable ICACHE when exiting stop mode
+	 */
+	sys_cache_instr_enable();
+}
+
+/* Invoke Low Power/System Off specific Tasks */
+void pm_state_set(enum pm_state state, uint8_t substate_id)
+{
 	switch (state) {
 	case PM_STATE_SUSPEND_TO_IDLE:
-		if (substate_id <= 2) {
-			/* Erratum 2.2.15:
-			 * Enable ICACHE when exiting stop mode
-			 */
-			sys_cache_instr_enable();
-
-			LL_LPM_DisableSleepOnExit();
-			LL_LPM_EnableSleep();
-		} else {
-			LOG_DBG("Unsupported power substate-id %u",
-							substate_id);
-		}
+		set_mode_stop_enter(substate_id);
+		/* Resume here */
+		set_mode_stop_exit(substate_id);
 		break;
-	case PM_STATE_SUSPEND_TO_RAM:
 #if defined(CONFIG_PM_S2RAM)
-		stm32wba_init();
-		stm32_power_init();
-
-		LL_LPM_DisableSleepOnExit();
-		LL_LPM_EnableSleep();
-#else
-		LOG_DBG("Suspend to RAM needs CONFIG_PM_S2RAM to be enabled");
-#endif
+	case PM_STATE_SUSPEND_TO_RAM:
+		set_mode_suspend_to_ram_enter();
+		/* Resume here */
+		set_mode_suspend_to_ram_exit();
 		break;
-	case PM_STATE_STANDBY:
-		__fallthrough;
-	case PM_STATE_SUSPEND_TO_DISK:
-		__fallthrough;
+#endif
 	default:
 		LOG_DBG("Unsupported power state %u", state);
-		break;
+		return;
 	}
+}
 
-	/* When BLE is enabled, clock restoration is performed by SCM */
-#if !defined(CONFIG_BT_STM32WBA)
-	stm32_clock_control_init(NULL);
-#endif
-
+/* Handle SOC specific activity after Low Power Mode Exit */
+void pm_state_exit_post_ops(enum pm_state state, uint8_t substate_id)
+{
 	/*
 	 * System is now in active mode.
 	 * Reenable interrupts which were disabled
@@ -191,10 +239,6 @@ void pm_state_exit_post_ops(enum pm_state state, uint8_t substate_id)
 void stm32_power_init(void)
 {
 
-#ifdef CONFIG_BT_STM32WBA
-	scm_init();
-#endif
-
 	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_PWR);
 
 #ifdef CONFIG_DEBUG
@@ -205,6 +249,13 @@ void stm32_power_init(void)
 	LL_DBGMCU_DisableDBGStandbyMode();
 #endif
 
+	/* Enable SRAM full retention */
+	LL_PWR_SetSRAM1SBRetention(LL_PWR_SRAM1_SB_FULL_RETENTION);
+	LL_PWR_SetSRAM2SBRetention(LL_PWR_SRAM2_SB_FULL_RETENTION);
+
+	/* Enable Radio RAM full retention */
+	LL_PWR_SetRadioSBRetention(LL_PWR_RADIO_SB_FULL_RETENTION);
+
 	/* Enabling  Ultra Low power mode */
 	LL_PWR_EnableUltraLowPowerMode();
 

west.yml

@@ -245,7 +245,7 @@ manifest:
       groups:
         - hal
     - name: hal_stm32
-      revision: 465fb02d6e3e09f61d887f8a1a5049cfaf85a19d
+      revision: 3a4b521441607646ec43bece2c53f333ce7f9b69
       path: modules/hal/stm32
       groups:
         - hal