[Backport v3.7.99-ncs1-branch] Nrfx 6553 fix tls handling for s2ram

arch/arm/core/cortex_m/pm_s2ram.S

@@ -70,7 +70,9 @@ SECTION_FUNC(TEXT, arch_pm_s2ram_suspend)
 	/*
 	 * Mark entering suspend to RAM.
 	 */
-	bl pm_s2ram_mark_set
+	mov 	r1, lr
+	bl 	pm_s2ram_mark_set
+	mov	lr, r1
 
 	/*
 	 * Call the system_off function passed as parameter. This should never
@@ -86,7 +88,9 @@ SECTION_FUNC(TEXT, arch_pm_s2ram_suspend)
 	/*
 	 * Reset the marking of suspend to RAM, return is ignored.
 	 */
-	bl pm_s2ram_mark_check_and_clear
+	mov 	r1, lr
+	bl	pm_s2ram_mark_check_and_clear
+	mov 	lr, r1
 
 	/* Move system_off back to r0 as return value */
 	mov	r0, r4
@@ -99,16 +103,13 @@ GTEXT(arch_pm_s2ram_resume)
 SECTION_FUNC(TEXT, arch_pm_s2ram_resume)
 	/*
 	 * Check if reset occurred after suspending to RAM.
-	 * Store LR to ensure we can continue boot when we are not suspended
-	 * to RAM. In addition to LR, R0 is pushed too, to ensure "SP mod 8 = 0",
-	 * as stated by ARM rule 6.2.1.2 for AAPCS32.
 	 */
-	push    {r0, lr}
-	bl      pm_s2ram_mark_check_and_clear
-	cmp	r0, #0x1
-	pop     {r0, lr}
-	beq	resume
-	bx	lr
+	mov 	r1, lr
+	bl 	pm_s2ram_mark_check_and_clear
+	mov 	lr, r1
+	cmp     r0, #0x1
+	beq     resume
+	bx      lr
 
 resume:
 	/*

arch/arm/core/cortex_m/pm_s2ram.c

@@ -22,20 +22,44 @@ __noinit _cpu_context_t _cpu_context;
  */
 static __noinit uint32_t marker;
 
-void pm_s2ram_mark_set(void)
+void __attribute__((naked)) pm_s2ram_mark_set(void)
 {
-	marker = MAGIC;
+	__asm__ volatile(
+		/* Set the marker to MAGIC value */
+		"str	%[_magic_val], [%[_marker]]\n"
+
+		"bx	lr\n"
+		:
+		: [_magic_val] "r"(MAGIC), [_marker] "r"(&marker)
+		: "r1", "r4", "memory");
 }
 
-bool pm_s2ram_mark_check_and_clear(void)
+bool __attribute__((naked)) pm_s2ram_mark_check_and_clear(void)
 {
-	if (marker == MAGIC) {
-		marker = 0;
+	__asm__ volatile(
+		/* Set return value to 0 */
+		"mov	r0, #0\n"
+
+		/* Check the marker */
+		"ldr	r3, [%[_marker]]\n"
+		"cmp	r3, %[_magic_val]\n"
+		"bne	exit\n"
+
+		/*
+		 * Reset the marker
+		 */
+		"str	r0, [%[_marker]]\n"
 
-		return true;
-	}
+		/*
+		 * Set return value to 1
+		 */
+		"mov	r0, #1\n"
 
-	return false;
+		"exit:\n"
+		"bx lr\n"
+		:
+		: [_magic_val] "r"(MAGIC), [_marker] "r"(&marker)
+		: "r0", "r1", "r3", "r4", "memory");
 }
 
 #endif /* CONFIG_PM_S2RAM_CUSTOM_MARKING */

include/zephyr/arch/common/pm_s2ram.h

@@ -65,6 +65,11 @@ int arch_pm_s2ram_suspend(pm_s2ram_system_off_fn_t system_off);
  *
  * Default implementation is setting a magic word in RAM. CONFIG_PM_S2RAM_CUSTOM_MARKING
  * allows custom implementation.
+ * The following requirements must be fulfilled:
+ * - the function cannot use stack (asm function or function with 'naked' attribute)
+ * - the content of the R1 and R4 registers must remain unchanged
+ * - returning from the function should be performed with the `bx lr` instruction
+ *
  */
 void pm_s2ram_mark_set(void);
 
@@ -76,6 +81,11 @@ void pm_s2ram_mark_set(void);
  *
  * Default implementation is checking a magic word in RAM. CONFIG_PM_S2RAM_CUSTOM_MARKING
  * allows custom implementation.
+ * The following requirements must be fulfilled:
+ * - the function cannot use stack (most likely asm function)
+ * - the content of the R1 and R4 registers must remain unchanged
+ * - the function's return value is passed by R0 register
+ * - returning from the function should be performed with the `bx lr` instruction
  *
  * @retval true if marking is found which indicates resuming after suspend-to-RAM.
  * @retval false if marking is not found which indicates standard boot.

soc/nordic/nrf54h/pm_s2ram.c

@@ -127,25 +127,48 @@ int soc_s2ram_suspend(pm_s2ram_system_off_fn_t system_off)
 	return ret;
 }
 
-void pm_s2ram_mark_set(void)
+void __attribute__((naked)) pm_s2ram_mark_set(void)
 {
 	/* empty */
+	__asm__ volatile("bx	lr\n");
 }
 
-bool pm_s2ram_mark_check_and_clear(void)
+bool __attribute__((naked)) pm_s2ram_mark_check_and_clear(void)
 {
-	bool unretained_wake;
-	bool restore_valid;
-	uint32_t reset_reason = nrf_resetinfo_resetreas_local_get(NRF_RESETINFO);
+	__asm__ volatile(
+		/* Set return value to 0 */
+		"mov	r0, #0\n"
 
-	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);
+		/* Load and check RESETREAS register */
+		"ldr	r3, [%[resetinfo_addr], %[resetreas_offs]]\n"
+		"cmp	r3, %[resetreas_unretained_mask]\n"
+
+		"bne	exit\n"
+
+		/* Clear RESETREAS register */
+		"str	r0, [%[resetinfo_addr], %[resetreas_offs]]\n"
+
+		/* Load RESTOREVALID register */
+		"ldr	r3, [%[resetinfo_addr], %[restorevalid_offs]]\n"
+
+		/* Clear RESTOREVALID */
+		"str	r0, [%[resetinfo_addr], %[restorevalid_offs]]\n"
+
+		/* Check RESTOREVALID register */
+		"cmp	r3, %[restorevalid_present_mask]\n"
+		"bne	exit\n"
+
+		/* Set return value to 1 */
+		"mov	r0, #1\n"
 
-	restore_valid = nrf_resetinfo_restore_valid_check(NRF_RESETINFO);
-	nrf_resetinfo_restore_valid_set(NRF_RESETINFO, false);
+		"exit:\n"
+		"bx	lr\n"
+		:
+		: [resetinfo_addr] "r"(NRF_RESETINFO),
+		  [resetreas_offs] "r"(offsetof(NRF_RESETINFO_Type, RESETREAS.LOCAL)),
+		  [resetreas_unretained_mask] "r"(NRF_RESETINFO_RESETREAS_LOCAL_UNRETAINED_MASK),
+		  [restorevalid_offs] "r"(offsetof(NRF_RESETINFO_Type, RESTOREVALID)),
+		  [restorevalid_present_mask] "r"(RESETINFO_RESTOREVALID_RESTOREVALID_Msk)
 
-	return (unretained_wake & restore_valid) ? true : false;
+		: "r0", "r1", "r3", "r4", "memory");
 }