From d830f2ee3d28d26bf5fd65df2010f9c614409787 Mon Sep 17 00:00:00 2001
From: Francois Ramu <francois.ramu@st.com>
Date: Thu, 25 Mar 2021 15:39:43 +0100
Subject: [PATCH] soc: arm: stm32l5 serie with low power management

The low power modes are available on the stm32l5 soc
with the mcu STOP0/1/2 modes, depending on the CONFIG_PM

Signed-off-by: Francois Ramu <francois.ramu@st.com>
---
 soc/arm/st_stm32/stm32l5/CMakeLists.txt |   4 +
 soc/arm/st_stm32/stm32l5/power.c        | 123 ++++++++++++++++++++++++
 2 files changed, 127 insertions(+)
 create mode 100644 soc/arm/st_stm32/stm32l5/power.c

diff --git a/soc/arm/st_stm32/stm32l5/CMakeLists.txt b/soc/arm/st_stm32/stm32l5/CMakeLists.txt
index ac3ba70ace6..59be7817eab 100644
--- a/soc/arm/st_stm32/stm32l5/CMakeLists.txt
+++ b/soc/arm/st_stm32/stm32l5/CMakeLists.txt
@@ -4,3 +4,7 @@ zephyr_include_directories(${ZEPHYR_BASE}/drivers)
 zephyr_sources(
   soc.c
   )
+
+zephyr_sources_ifdef(CONFIG_PM
+  power.c
+  )
diff --git a/soc/arm/st_stm32/stm32l5/power.c b/soc/arm/st_stm32/stm32l5/power.c
new file mode 100644
index 00000000000..59fb816bf0e
--- /dev/null
+++ b/soc/arm/st_stm32/stm32l5/power.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2021 STMicroelectronics.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#include <zephyr.h>
+#include <power/power.h>
+#include <soc.h>
+#include <init.h>
+
+#include <stm32l5xx_ll_utils.h>
+#include <stm32l5xx_ll_bus.h>
+#include <stm32l5xx_ll_cortex.h>
+#include <stm32l5xx_ll_pwr.h>
+#include <stm32l5xx_ll_rcc.h>
+#include <stm32l5xx_ll_system.h>
+#include <clock_control/clock_stm32_ll_common.h>
+
+#include <logging/log.h>
+LOG_MODULE_DECLARE(soc, CONFIG_SOC_LOG_LEVEL);
+
+/* select MSI as wake-up system clock if configured, HSI otherwise */
+#if defined(CONFIG_CLOCK_STM32_SYSCLK_SRC_MSI)
+#define RCC_STOP_WAKEUPCLOCK_SELECTED LL_RCC_STOP_WAKEUPCLOCK_MSI
+#else
+#define RCC_STOP_WAKEUPCLOCK_SELECTED LL_RCC_STOP_WAKEUPCLOCK_HSI
+#endif
+
+/* Invoke Low Power/System Off specific Tasks */
+void pm_power_state_set(struct pm_state_info info)
+{
+	if (info.state != PM_STATE_SUSPEND_TO_IDLE) {
+		LOG_DBG("Unsupported power state %u", info.state);
+		return;
+	}
+
+	switch (info.substate_id) {
+	case 1: /* this corresponds to the STOP0 mode: */
+		/* ensure the proper wake-up system clock */
+		LL_RCC_SetClkAfterWakeFromStop(RCC_STOP_WAKEUPCLOCK_SELECTED);
+		/* enter STOP0 mode */
+		LL_PWR_SetPowerMode(LL_PWR_MODE_STOP0);
+		LL_LPM_EnableDeepSleep();
+		/* enter SLEEP mode : WFE or WFI */
+		k_cpu_idle();
+		break;
+	case 2: /* this corresponds to the STOP1 mode: */
+		/* ensure the proper wake-up system clock */
+		LL_RCC_SetClkAfterWakeFromStop(RCC_STOP_WAKEUPCLOCK_SELECTED);
+		/* enter STOP1 mode */
+		LL_PWR_SetPowerMode(LL_PWR_MODE_STOP1);
+		LL_LPM_EnableDeepSleep();
+		/* enter SLEEP mode : WFE or WFI */
+		k_cpu_idle();
+		break;
+	case 3: /* this corresponds to the STOP2 mode: */
+		/* ensure the proper wake-up system clock */
+		LL_RCC_SetClkAfterWakeFromStop(RCC_STOP_WAKEUPCLOCK_SELECTED);
+#ifdef PWR_CR1_RRSTP
+		LL_PWR_DisableSRAM3Retention();
+#endif /* PWR_CR1_RRSTP */
+		/* enter STOP2 mode */
+		LL_PWR_SetPowerMode(LL_PWR_MODE_STOP2);
+		LL_LPM_EnableDeepSleep();
+		/* enter SLEEP mode : WFE or WFI */
+		k_cpu_idle();
+		break;
+	default:
+		LOG_DBG("Unsupported power state substate-id %u",
+			info.substate_id);
+		break;
+	}
+}
+
+/* Handle SOC specific activity after Low Power Mode Exit */
+void pm_power_state_exit_post_ops(struct pm_state_info info)
+{
+	if (info.state != PM_STATE_SUSPEND_TO_IDLE) {
+		LOG_DBG("Unsupported power substate-id %u", info.state);
+	} else {
+		switch (info.substate_id) {
+		case 1:	/* STOP0 */
+			__fallthrough;
+		case 2:	/* STOP1 */
+			__fallthrough;
+		case 3:	/* STOP2 */
+			LL_LPM_DisableSleepOnExit();
+			LL_LPM_EnableSleep();
+			break;
+		default:
+			LOG_DBG("Unsupported power substate-id %u",
+				info.substate_id);
+			break;
+		}
+		/* need to restore the clock */
+		stm32_clock_control_init(NULL);
+	}
+
+	/*
+	 * System is now in active mode.
+	 * Reenable interrupts which were disabled
+	 * when OS started idling code.
+	 */
+	irq_unlock(0);
+}
+
+/* Initialize STM32 Power */
+static int stm32_power_init(const struct device *dev)
+{
+	ARG_UNUSED(dev);
+
+	/* enable Power clock */
+	LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR);
+
+#ifdef CONFIG_DEBUG
+	/* Enable the Debug Module during all and any Low power mode */
+	LL_DBGMCU_EnableDBGStopMode();
+#endif /* CONFIG_DEBUG */
+
+	return 0;
+}
+
+SYS_INIT(stm32_power_init, POST_KERNEL, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);