From 79599a15d4c6e7b39c77f13ad579c0a603e1c3a6 Mon Sep 17 00:00:00 2001
From: Erwan Gouriou <erwan.gouriou@st.com>
Date: Wed, 28 Sep 2022 09:22:07 +0200
Subject: [PATCH] soc: stm32: stmw32wba: Get stop mode compatible with BLE

Supporting Stop1 mode while BLE RF is enabled requires some specific
adaptation and usage of STM32WBA Cube BLE controller scm API.

scm (Secure clock manager) is in charge of switching clock depending
on RF status and should be informed of PM stop modes scheduling.

Signed-off-by: Erwan Gouriou <erwan.gouriou@st.com>
---
 soc/arm/st_stm32/stm32wba/Kconfig.series |  1 +
 soc/arm/st_stm32/stm32wba/power.c        | 73 ++++++++++++++++++------
 2 files changed, 58 insertions(+), 16 deletions(-)

diff --git a/soc/arm/st_stm32/stm32wba/Kconfig.series b/soc/arm/st_stm32/stm32wba/Kconfig.series
index 28edaf07790..5cdaaa7e672 100644
--- a/soc/arm/st_stm32/stm32wba/Kconfig.series
+++ b/soc/arm/st_stm32/stm32wba/Kconfig.series
@@ -15,6 +15,7 @@ config SOC_SERIES_STM32WBAX
 	select ARMV8_M_DSP
 	select CPU_CORTEX_M_HAS_DWT
 	select HAS_STM32CUBE
+	select USE_STM32_HAL_PWR_EX
 	select HAS_PM
 	help
 	  Enable support for STM32WBA MCU series
diff --git a/soc/arm/st_stm32/stm32wba/power.c b/soc/arm/st_stm32/stm32wba/power.c
index 018c27ee0b8..89d9f5e72ed 100644
--- a/soc/arm/st_stm32/stm32wba/power.c
+++ b/soc/arm/st_stm32/stm32wba/power.c
@@ -8,19 +8,45 @@
 #include <soc.h>
 #include <zephyr/init.h>
 
-#include <stm32wbaxx_ll_utils.h>
 #include <stm32wbaxx_ll_bus.h>
 #include <stm32wbaxx_ll_cortex.h>
 #include <stm32wbaxx_ll_pwr.h>
+#include <stm32wbaxx_ll_icache.h>
 #include <stm32wbaxx_ll_rcc.h>
 #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 set_mode_stop(uint8_t substate_id)
 {
+
+	LL_PWR_ClearFlag_STOP();
+	LL_RCC_ClearResetFlags();
+
+	/* Erratum 2.2.15:
+	 * Disabling ICACHE is required before entering stop mode
+	 */
+	LL_ICACHE_Disable();
+	while (LL_ICACHE_IsEnabled() == 1U) {
+	}
+
+
+#ifdef CONFIG_BT_STM32WBA
+	scm_setwaitstates(LP);
+#endif
+	/* Set SLEEPDEEP bit of Cortex System Control Register */
+	LL_LPM_EnableDeepSleep();
+
+	while (LL_PWR_IsActiveFlag_ACTVOS() == 0) {
+	}
+
 	switch (substate_id) {
 	case 1: /* enter STOP0 mode */
 		LL_PWR_SetPowerMode(LL_PWR_MODE_STOP0);
@@ -34,13 +60,6 @@ void set_mode_stop(uint8_t substate_id)
 	}
 }
 
-void set_mode_standby(uint8_t substate_id)
-{
-	ARG_UNUSED(substate_id);
-	/* Select standby mode */
-	LL_PWR_SetPowerMode(LL_PWR_MODE_STANDBY);
-}
-
 /* Invoke Low Power/System Off specific Tasks */
 void pm_state_set(enum pm_state state, uint8_t substate_id)
 {
@@ -49,17 +68,13 @@ void pm_state_set(enum pm_state state, uint8_t substate_id)
 		set_mode_stop(substate_id);
 		break;
 	case PM_STATE_STANDBY:
-		/* To be tested */
-		set_mode_standby(substate_id);
-		break;
+		/* Not supported today */
+		__fallthrough;
 	default:
 		LOG_DBG("Unsupported power state %u", state);
 		return;
 	}
 
-	/* Set SLEEPDEEP bit of Cortex System Control Register */
-	LL_LPM_EnableDeepSleep();
-
 	/* Select mode entry : WFE or WFI and enter the CPU selected mode */
 	k_cpu_idle();
 }
@@ -67,6 +82,21 @@ void pm_state_set(enum pm_state state, uint8_t substate_id)
 /* Handle SOC specific activity after Low Power Mode Exit */
 void pm_state_exit_post_ops(enum pm_state state, uint8_t substate_id)
 {
+	/* Erratum 2.2.15:
+	 * Enable ICACHE when exiting stop mode
+	 */
+	LL_ICACHE_Enable();
+	while (LL_ICACHE_IsEnabled() == 0U) {
+	}
+
+#ifdef CONFIG_BT_STM32WBA
+	if (LL_PWR_IsActiveFlag_STOP() == 1U) {
+		scm_setup();
+	} else {
+		scm_setwaitstates(RUN);
+	}
+#endif
+
 	switch (state) {
 	case PM_STATE_SUSPEND_TO_IDLE:
 		if (substate_id <= 2) {
@@ -87,8 +117,11 @@ void pm_state_exit_post_ops(enum pm_state state, uint8_t substate_id)
 		LOG_DBG("Unsupported power state %u", state);
 		break;
 	}
-	/* need to restore the clock */
+
+	/* When BLE is enabled, clock restoration is performed by SCM */
+#if !defined(CONFIG_BT_STM32WBA)
 	stm32_clock_control_init(NULL);
+#endif
 
 	/*
 	 * System is now in active mode.
@@ -101,9 +134,17 @@ void pm_state_exit_post_ops(enum pm_state state, uint8_t substate_id)
 /* Initialize STM32 Power */
 static int stm32_power_init(void)
 {
-	/* enable Power clock */
+
+#ifdef CONFIG_BT_STM32WBA
+	scm_init();
+#endif
+
 	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_PWR);
 
+	LL_PWR_EnableUltraLowPowerMode();
+
+	LL_FLASH_EnableSleepPowerDown();
+
 	return 0;
 }