From d7a0b4fa932c93141268c54849453765210be5c3 Mon Sep 17 00:00:00 2001
From: Gerard Marull-Paretas <gerard@teslabs.com>
Date: Tue, 8 Aug 2023 10:54:11 +0200
Subject: [PATCH] soc: arm: st_stm32: u5: add support for sys_poweroff

Add support for the sys_poweroff hook, re-using code from the SOFT_OFF
state.

Signed-off-by: Gerard Marull-Paretas <gerard@teslabs.com>
---
 soc/arm/st_stm32/stm32u5/CMakeLists.txt |  2 ++
 soc/arm/st_stm32/stm32u5/Kconfig.series |  1 +
 soc/arm/st_stm32/stm32u5/power.c        | 13 -------------
 soc/arm/st_stm32/stm32u5/poweroff.c     | 21 +++++++++++++++++++++
 4 files changed, 24 insertions(+), 13 deletions(-)
 create mode 100644 soc/arm/st_stm32/stm32u5/poweroff.c

diff --git a/soc/arm/st_stm32/stm32u5/CMakeLists.txt b/soc/arm/st_stm32/stm32u5/CMakeLists.txt
index 59be7817eab..8f4b837538d 100644
--- a/soc/arm/st_stm32/stm32u5/CMakeLists.txt
+++ b/soc/arm/st_stm32/stm32u5/CMakeLists.txt
@@ -8,3 +8,5 @@ zephyr_sources(
 zephyr_sources_ifdef(CONFIG_PM
   power.c
   )
+
+zephyr_sources_ifdef(CONFIG_POWEROFF poweroff.c)
diff --git a/soc/arm/st_stm32/stm32u5/Kconfig.series b/soc/arm/st_stm32/stm32u5/Kconfig.series
index 99835997a6a..81094811b38 100644
--- a/soc/arm/st_stm32/stm32u5/Kconfig.series
+++ b/soc/arm/st_stm32/stm32u5/Kconfig.series
@@ -16,5 +16,6 @@ config SOC_SERIES_STM32U5X
 	select CPU_CORTEX_M_HAS_DWT
 	select HAS_STM32CUBE
 	select HAS_PM
+	select HAS_POWEROFF
 	help
 	  Enable support for STM32U5 MCU series
diff --git a/soc/arm/st_stm32/stm32u5/power.c b/soc/arm/st_stm32/stm32u5/power.c
index 4d6034a8c47..f880ffa1d08 100644
--- a/soc/arm/st_stm32/stm32u5/power.c
+++ b/soc/arm/st_stm32/stm32u5/power.c
@@ -54,13 +54,6 @@ void set_mode_standby(uint8_t substate_id)
 	LL_PWR_SetPowerMode(LL_PWR_STANDBY_MODE);
 }
 
-void set_mode_shutdown(uint8_t substate_id)
-{
-	ARG_UNUSED(substate_id);
-	/* Select shutdown mode */
-	LL_PWR_SetPowerMode(LL_PWR_SHUTDOWN_MODE);
-}
-
 /* Invoke Low Power/System Off specific Tasks */
 void pm_state_set(enum pm_state state, uint8_t substate_id)
 {
@@ -72,9 +65,6 @@ void pm_state_set(enum pm_state state, uint8_t substate_id)
 		/* To be tested */
 		set_mode_standby(substate_id);
 		break;
-	case PM_STATE_SOFT_OFF:
-		set_mode_shutdown(substate_id);
-		break;
 	default:
 		LOG_DBG("Unsupported power state %u", state);
 		return;
@@ -102,9 +92,6 @@ void pm_state_exit_post_ops(enum pm_state state, uint8_t substate_id)
 	case PM_STATE_STANDBY:
 		/* To be tested */
 		LL_LPM_EnableSleep();
-	case PM_STATE_SOFT_OFF:
-		/* We should not get there */
-		__fallthrough;
 	case PM_STATE_SUSPEND_TO_RAM:
 		__fallthrough;
 	case PM_STATE_SUSPEND_TO_DISK:
diff --git a/soc/arm/st_stm32/stm32u5/poweroff.c b/soc/arm/st_stm32/stm32u5/poweroff.c
new file mode 100644
index 00000000000..191230be523
--- /dev/null
+++ b/soc/arm/st_stm32/stm32u5/poweroff.c
@@ -0,0 +1,21 @@
+/*
+ * Copyright (c) 2023 Nordic Semiconductor ASA
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/kernel.h>
+#include <zephyr/sys/poweroff.h>
+#include <zephyr/toolchain.h>
+
+#include <stm32_ll_cortex.h>
+#include <stm32_ll_pwr.h>
+
+void z_sys_poweroff(void)
+{
+	LL_PWR_SetPowerMode(LL_PWR_SHUTDOWN_MODE);
+	LL_LPM_EnableDeepSleep();
+
+	k_cpu_idle();
+
+	CODE_UNREACHABLE;
+}