From d74dd85a1601f9b9010feb4437de343f39ec344b Mon Sep 17 00:00:00 2001
From: Adam Kondraciuk <adam.kondraciuk@nordicsemi.no>
Date: Thu, 17 Oct 2024 13:12:01 +0200
Subject: [PATCH] [nrf fromlist] soc: nordic: s2ram: Align s2ram marking
 procedures

Rework Nordic specific S2RAM marking procedures.
The S2RAM marking procedures must not disrupt the stack due to
the TLS pointer not yet being initialized during their execution.

Upstream PR: https://github.com/zephyrproject-rtos/zephyr/pull/80039

Signed-off-by: Adam Kondraciuk <adam.kondraciuk@nordicsemi.no>
---
 soc/nordic/common/CMakeLists.txt              |  1 +
 soc/nordic/common/pm_s2ram.S                  | 70 +++++++++++++++++++
 .../nrf54h/Kconfig.defconfig.nrf54h20_cpuapp  | 20 ++++++
 soc/nordic/nrf54h/pm_s2ram.c                  | 23 ------
 4 files changed, 91 insertions(+), 23 deletions(-)
 create mode 100644 soc/nordic/common/pm_s2ram.S

diff --git a/soc/nordic/common/CMakeLists.txt b/soc/nordic/common/CMakeLists.txt
index 4e000006d41..a848f912a56 100644
--- a/soc/nordic/common/CMakeLists.txt
+++ b/soc/nordic/common/CMakeLists.txt
@@ -14,6 +14,7 @@ endif()
 zephyr_library_sources_ifdef(CONFIG_POWEROFF poweroff.c)
 if(CONFIG_ARM)
   zephyr_library_sources_ifdef(CONFIG_NRF_PLATFORM_HALTIUM soc_lrcconf.c)
+  zephyr_library_sources_ifdef(CONFIG_PM_S2RAM_CUSTOM_MARKING pm_s2ram.S)
 endif()
 
 if((CONFIG_SOC_SERIES_NRF54HX OR CONFIG_SOC_SERIES_NRF92X) AND CONFIG_CPU_HAS_CUSTOM_FIXED_SOC_MPU_REGIONS)
diff --git a/soc/nordic/common/pm_s2ram.S b/soc/nordic/common/pm_s2ram.S
new file mode 100644
index 00000000000..39bf23849a7
--- /dev/null
+++ b/soc/nordic/common/pm_s2ram.S
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2024 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+ /**
+ * @file
+ * @brief Nordic suspend-to-RAM code (S2RAM)
+ */
+
+#include <zephyr/toolchain.h>
+#include <zephyr/arch/cpu.h>
+#include <zephyr/arch/common/pm_s2ram.h>
+
+
+GTEXT(pm_s2ram_mark_set)
+SECTION_FUNC(TEXT, pm_s2ram_mark_set)
+	/*
+	 * Do nothing
+	 */
+	bx	lr
+
+GTEXT(pm_s2ram_mark_check_and_clear)
+SECTION_FUNC(TEXT, pm_s2ram_mark_check_and_clear)
+	/*
+	 * Set return value to 0
+	 */
+	mov	r0, #0
+
+	/*
+	 * Load check RESETREAS register
+	 */
+	ldr	r2, =CONFIG_NRF_RESETINFO_PERIPH_ADDRESS
+	ldr 	r5, =CONFIG_NRF_RESETREAS_LOCAL_REG_OFFSET
+	ldr 	r3, [r5, r2]
+	ldr 	r6, =CONFIG_NRF_RESETREAS_LOCAL_UNRETAINEDWAKE_FIELD
+	cmp	r3, r6
+
+	bne	exit
+
+	/*
+	 * Clear RESETREAS
+	 */
+	str	r0, [r5, r2]
+
+	/*
+	 * Load RESTOREVALID register
+	 */
+	ldr 	r5, =CONFIG_NRF_RESTOREVALID_REG_OFFSET
+	ldr 	r3, [r5, r2]
+
+	/*
+	 * Clear RESTOREVALID
+	 */
+	str	r0, [r5, r2]
+
+	/*
+	 * Check RESTOREVALID register
+	 */
+	ldr 	r5, =CONFIG_NRF_RESTOREVALID_PRESENT_FIELD
+	cmp	r3, r5
+	bne	exit
+
+	/*
+	 * Set return value to 1
+	 */
+	mov	r0, #1
+exit:
+	bx lr
diff --git a/soc/nordic/nrf54h/Kconfig.defconfig.nrf54h20_cpuapp b/soc/nordic/nrf54h/Kconfig.defconfig.nrf54h20_cpuapp
index 597f7fbea22..2f0cfe46630 100644
--- a/soc/nordic/nrf54h/Kconfig.defconfig.nrf54h20_cpuapp
+++ b/soc/nordic/nrf54h/Kconfig.defconfig.nrf54h20_cpuapp
@@ -14,4 +14,24 @@ config NRF_REGTOOL_GENERATE_UICR
 config NRF_REGTOOL_GENERATE_BICR
 	default y
 
+config NRF_RESETINFO_PERIPH_ADDRESS
+	hex
+	default 0x5201E000
+
+config NRF_RESETREAS_LOCAL_REG_OFFSET
+	hex
+	default 0x4A4
+
+config NRF_RESTOREVALID_REG_OFFSET
+	hex
+	default 0x4C0
+
+config NRF_RESETREAS_LOCAL_UNRETAINEDWAKE_FIELD
+	hex
+	default 0x10
+
+config NRF_RESTOREVALID_PRESENT_FIELD
+	hex
+	default 0x1
+
 endif # SOC_NRF54H20_CPUAPP || SOC_NRF54H20_ENGB_CPUAPP
diff --git a/soc/nordic/nrf54h/pm_s2ram.c b/soc/nordic/nrf54h/pm_s2ram.c
index 10cdd36a762..9263f6c2c37 100644
--- a/soc/nordic/nrf54h/pm_s2ram.c
+++ b/soc/nordic/nrf54h/pm_s2ram.c
@@ -126,26 +126,3 @@ int soc_s2ram_suspend(pm_s2ram_system_off_fn_t system_off)
 
 	return ret;
 }
-
-void pm_s2ram_mark_set(void)
-{
-	/* empty */
-}
-
-bool pm_s2ram_mark_check_and_clear(void)
-{
-	bool unretained_wake;
-	bool restore_valid;
-	uint32_t reset_reason = nrf_resetinfo_resetreas_local_get(NRF_RESETINFO);
-
-	if (reset_reason != NRF_RESETINFO_RESETREAS_LOCAL_UNRETAINED_MASK) {
-		return false;
-	}
-	unretained_wake = reset_reason & NRF_RESETINFO_RESETREAS_LOCAL_UNRETAINED_MASK;
-	nrf_resetinfo_resetreas_local_set(NRF_RESETINFO, 0);
-
-	restore_valid = nrf_resetinfo_restore_valid_check(NRF_RESETINFO);
-	nrf_resetinfo_restore_valid_set(NRF_RESETINFO, false);
-
-	return (unretained_wake & restore_valid) ? true : false;
-}