Introduce pm config

arch/arm/core/cortex_m/pm_s2ram.S

@@ -14,12 +14,11 @@
 #include <zephyr/arch/cpu.h>
 #include <zephyr/arch/common/pm_s2ram.h>
 
-#define MAGIC (0xDABBAD00)
-
 _ASM_FILE_PROLOGUE
 
+GTEXT(pm_s2ram_mark_set)
+GTEXT(pm_s2ram_mark_check_and_clear)
 GDATA(_cpu_context)
-GDATA(marker)
 
 SECTION_FUNC(TEXT, arch_pm_s2ram_suspend)
 	/*
@@ -64,11 +63,9 @@ SECTION_FUNC(TEXT, arch_pm_s2ram_suspend)
 	str	r2, [r1, #___cpu_context_t_control_OFFSET]
 
 	/*
-	 * Set the marker to MAGIC value
+	 * Mark entering suspend to RAM.
 	 */
-	ldr	r1, =marker
-	ldr	r2, =MAGIC
-	str	r2, [r1]
+	bl pm_s2ram_mark_set
 
 	/*
 	 * Call the system_off function passed as parameter. This should never
@@ -82,35 +79,29 @@ SECTION_FUNC(TEXT, arch_pm_s2ram_suspend)
 	 */
 
 	/*
-	 * Reset the marker
+	 * Reset the marking of suspend to RAM, return is ignored.
 	 */
-	ldr	r1, =marker
-	mov	r2, #0x0
-	str	r2, [r1]
+	push	{r0}
+	bl pm_s2ram_mark_check_and_clear
+	pop	{r0}
 
 	pop	{r4-r12, lr}
 	bx	lr
 
+
 GTEXT(arch_pm_s2ram_resume)
 SECTION_FUNC(TEXT, arch_pm_s2ram_resume)
 	/*
-	 * Check if the marker is set
+	 * Check if reset occurred after suspending to RAM.
 	 */
-	ldr	r0, =marker
-	ldr	r0, [r0]
-	ldr	r1, =MAGIC
-	cmp	r0, r1
+	push    {lr}
+	bl      pm_s2ram_mark_check_and_clear
+	cmp	r0, #0x1
+	pop     {lr}
 	beq	resume
 	bx	lr
 
 resume:
-	/*
-	 * Reset the marker
-	 */
-	ldr	r0, =marker
-	mov	r1, #0x0
-	str	r1, [r0]
-
 	/*
 	 * Restore the CPU context
 	 */

arch/arm/core/cortex_m/pm_s2ram.c

@@ -9,12 +9,150 @@
 
 #include <zephyr/arch/common/pm_s2ram.h>
 
+#define NVIC_MEMBER_SIZE(member) ARRAY_SIZE(((NVIC_Type *)0)->member)
+
+/* Currently dynamic regions are only used in case of userspace or stack guard and
+ * stack guard is not used by default on Cortex-M33 because there is a dedicated
+ * mechanism for stack overflow detection. Unless those condition change we don't
+ * need to store MPU content, it can just be reinitialized on resuming.
+ */
+#define MPU_USE_DYNAMIC_REGIONS IS_ENABLED(CONFIG_USERSPACE) || IS_ENABLED(CONFIG_MPU_STACK_GUARD)
+#define MPU_NUM_REGIONS DT_PROP(DT_INST(0, arm_armv8m_mpu), arm_num_mpu_regions)
+
+#define MAGIC (0xDABBAD00)
+
+typedef struct {
+	/* NVIC components backuped into RAM. */
+	uint32_t ISER[NVIC_MEMBER_SIZE(ISER)];
+	uint32_t ISPR[NVIC_MEMBER_SIZE(ISPR)];
+	uint32_t IABR[NVIC_MEMBER_SIZE(IABR)];
+	uint8_t  IPR[NVIC_MEMBER_SIZE(IPR)];
+} _nvic_context_t;
+
+typedef struct {
+	uint32_t RNR;
+	uint32_t RBAR[MPU_NUM_REGIONS];
+	uint32_t RLAR[MPU_NUM_REGIONS];
+	uint32_t MAIR0;
+	uint32_t MAIR1;
+	uint32_t CTRL;
+} _mpu_context_t;
+
 /**
  * CPU context for S2RAM
  */
 __noinit _cpu_context_t _cpu_context;
 
+/**
+ * NVIC context for S2RAM
+ */
+
+struct backup {
+	_nvic_context_t nvic_context;
+	_mpu_context_t  mpu_context;
+};
+
+static __noinit struct backup backup_data;
+
+extern void z_arm_configure_static_mpu_regions(void);
+extern int z_arm_mpu_init(void);
+
+/* MPU registers cannot be simply copied because content of RBARx RLARx registers
+ * depends on region which is selected by RNR register.
+ */
+static void mpu_suspend(_mpu_context_t *backup)
+{
+	if (!MPU_USE_DYNAMIC_REGIONS) {
+		return;
+	}
+
+	backup->RNR = MPU->RNR;
+
+	for (uint8_t i = 0; i < MPU_NUM_REGIONS; i++) {
+		MPU->RNR = i;
+		backup->RBAR[i] = MPU->RBAR;
+		backup->RLAR[i] = MPU->RLAR;
+	}
+	backup->MAIR0 = MPU->MAIR0;
+	backup->MAIR1 = MPU->MAIR1;
+	backup->CTRL = MPU->CTRL;
+}
+
+static void mpu_resume(_mpu_context_t *backup)
+{
+	if (!MPU_USE_DYNAMIC_REGIONS) {
+		z_arm_mpu_init();
+		z_arm_configure_static_mpu_regions();
+		return;
+	}
+
+	uint32_t rnr = backup->RNR;
+
+	for (uint8_t i = 0; i < MPU_NUM_REGIONS; i++) {
+		MPU->RNR = i;
+		MPU->RBAR = backup->RBAR[i];
+		MPU->RLAR = backup->RLAR[i];
+	}
+
+	MPU->MAIR0 = backup->MAIR0;
+	MPU->MAIR1 = backup->MAIR1;
+	MPU->RNR = rnr;
+	MPU->CTRL = backup->CTRL;
+}
+
+static void nvic_suspend(_nvic_context_t *backup)
+{
+	memcpy(backup->ISER, (uint32_t *)NVIC->ISER, sizeof(NVIC->ISER));
+	memcpy(backup->ISPR, (uint32_t *)NVIC->ISPR, sizeof(NVIC->ISPR));
+	memcpy(backup->IABR, (uint32_t *)NVIC->IABR, sizeof(NVIC->IABR));
+	memcpy(backup->IPR, (uint32_t *)NVIC->IPR, sizeof(NVIC->IPR));
+}
+
+static void nvic_resume(_nvic_context_t *backup)
+{
+	memcpy((uint32_t *)NVIC->ISER, backup->ISER, sizeof(NVIC->ISER));
+	memcpy((uint32_t *)NVIC->ISPR, backup->ISPR, sizeof(NVIC->ISPR));
+	memcpy((uint32_t *)NVIC->IABR, backup->IABR, sizeof(NVIC->IABR));
+	memcpy((uint32_t *)NVIC->IPR, backup->IPR, sizeof(NVIC->IPR));
+}
+
+int pm_s2ram_suspend(pm_s2ram_system_off_fn_t system_off)
+{
+	int ret;
+
+	nvic_suspend(&backup_data.nvic_context);
+	mpu_suspend(&backup_data.mpu_context);
+	ret = arch_pm_s2ram_suspend(system_off);
+	if (ret < 0) {
+		return ret;
+	}
+
+	mpu_resume(&backup_data.mpu_context);
+	nvic_resume(&backup_data.nvic_context);
+
+	return ret;
+}
+
+#ifndef CONFIG_PM_S2RAM_CUSTOM_MARKING
 /**
  * S2RAM Marker
  */
-__noinit uint32_t marker;
+static __noinit uint32_t marker;
+
+void pm_s2ram_mark_set(void)
+{
+	marker = MAGIC;
+}
+
+bool pm_s2ram_mark_check_and_clear(void)
+{
+	if (marker == MAGIC) {
+		marker = 0;
+
+		return true;
+	}
+
+	return false;
+}
+
+#endif /* CONFIG_PM_S2RAM_CUSTOM_MARKING */

drivers/cache/Kconfig.nrf

@@ -7,3 +7,9 @@ config CACHE_NRF_CACHE
 	depends on HAS_NRFX && CACHE_MANAGEMENT
 	help
 	  Enable support for the nRF cache driver.
+
+config CACHE_NRF_PATCH_LINEADDR
+	bool "Patch lineaddr"
+	default y if SOC_NRF54H20
+	help
+	  Manually set 28th bit in the LINEADDR in Trustzone Secure build.

drivers/cache/cache_nrf.c

@@ -14,10 +14,8 @@ LOG_MODULE_REGISTER(cache_nrfx, CONFIG_CACHE_LOG_LEVEL);
 #define NRF_ICACHE NRF_CACHE
 #endif
 
-#define CACHE_LINE_SIZE		     32
 #define CACHE_BUSY_RETRY_INTERVAL_US 10
 
-static struct k_spinlock lock;
 
 enum k_nrf_cache_op {
 	/*
@@ -55,7 +53,6 @@ static inline bool is_cache_busy(NRF_CACHE_Type *cache)
 static inline void wait_for_cache(NRF_CACHE_Type *cache)
 {
 	while (is_cache_busy(cache)) {
-		k_busy_wait(CACHE_BUSY_RETRY_INTERVAL_US);
 	}
 }
 
@@ -68,14 +65,6 @@ static inline int _cache_all(NRF_CACHE_Type *cache, enum k_nrf_cache_op op)
 		return -ENOTSUP;
 	}
 
-	k_spinlock_key_t key = k_spin_lock(&lock);
-
-	/*
-	 * Invalidating the whole cache is dangerous. For good measure
-	 * disable the cache.
-	 */
-	nrf_cache_disable(cache);
-
 	wait_for_cache(cache);
 
 	switch (op) {
@@ -102,66 +91,68 @@ static inline int _cache_all(NRF_CACHE_Type *cache, enum k_nrf_cache_op op)
 
 	wait_for_cache(cache);
 
-	nrf_cache_enable(cache);
-
-	k_spin_unlock(&lock, key);
-
 	return 0;
 }
 
 static inline void _cache_line(NRF_CACHE_Type *cache, enum k_nrf_cache_op op, uintptr_t line_addr)
 {
-	wait_for_cache(cache);
+	do {
+		wait_for_cache(cache);
 
-	nrf_cache_lineaddr_set(cache, line_addr);
+		nrf_cache_lineaddr_set(cache, line_addr);
 
-	switch (op) {
+		switch (op) {
 
 #if NRF_CACHE_HAS_TASK_CLEAN
-	case K_NRF_CACHE_CLEAN:
-		nrf_cache_task_trigger(cache, NRF_CACHE_TASK_CLEANLINE);
-		break;
+		case K_NRF_CACHE_CLEAN:
+			nrf_cache_task_trigger(cache, NRF_CACHE_TASK_CLEANLINE);
+			break;
 #endif
 
-	case K_NRF_CACHE_INVD:
-		nrf_cache_task_trigger(cache, NRF_CACHE_TASK_INVALIDATELINE);
-		break;
+		case K_NRF_CACHE_INVD:
+			nrf_cache_task_trigger(cache, NRF_CACHE_TASK_INVALIDATELINE);
+			break;
 
 #if NRF_CACHE_HAS_TASK_FLUSH
-	case K_NRF_CACHE_FLUSH:
-		nrf_cache_task_trigger(cache, NRF_CACHE_TASK_FLUSHLINE);
-		break;
+		case K_NRF_CACHE_FLUSH:
+			nrf_cache_task_trigger(cache, NRF_CACHE_TASK_FLUSHLINE);
+			break;
 #endif
 
-	default:
-		break;
-	}
-
-	wait_for_cache(cache);
+		default:
+			break;
+		}
+	} while (nrf_cache_lineaddr_get(cache) != line_addr);
 }
 
 static inline int _cache_range(NRF_CACHE_Type *cache, enum k_nrf_cache_op op, void *addr,
 			       size_t size)
 {
 	uintptr_t line_addr = (uintptr_t)addr;
-	uintptr_t end_addr = line_addr + size;
+	uintptr_t end_addr;
+
+	/* Some SOCs has a bug that requires to set 28th bit in the address on
+	 * Trustzone secure builds.
+	 */
+	if (IS_ENABLED(CONFIG_CACHE_NRF_PATCH_LINEADDR) &&
+	    !IS_ENABLED(CONFIG_TRUSTED_EXECUTION_NONSECURE)) {
+		line_addr |= BIT(28);
+	}
+
+	end_addr = line_addr + size;
 
 	/*
 	 * Align address to line size
 	 */
-	line_addr &= ~(CACHE_LINE_SIZE - 1);
+	line_addr &= ~(CONFIG_DCACHE_LINE_SIZE - 1);
 
 	do {
-		k_spinlock_key_t key = k_spin_lock(&lock);
-
 		_cache_line(cache, op, line_addr);
-
-		k_spin_unlock(&lock, key);
-
-		line_addr += CACHE_LINE_SIZE;
-
+		line_addr += CONFIG_DCACHE_LINE_SIZE;
 	} while (line_addr < end_addr);
 
+	wait_for_cache(cache);
+
 	return 0;
 }
 
@@ -192,11 +183,6 @@ void cache_data_enable(void)
 	nrf_cache_enable(NRF_DCACHE);
 }
 
-void cache_data_disable(void)
-{
-	nrf_cache_disable(NRF_DCACHE);
-}
-
 int cache_data_flush_all(void)
 {
 #if NRF_CACHE_HAS_TASK_CLEAN
@@ -206,6 +192,14 @@ int cache_data_flush_all(void)
 #endif
 }
 
+void cache_data_disable(void)
+{
+	if (nrf_cache_enable_check(NRF_DCACHE)) {
+		(void)cache_data_flush_all();
+	}
+	nrf_cache_disable(NRF_DCACHE);
+}
+
 int cache_data_invd_all(void)
 {
 	return _cache_checks(NRF_DCACHE, K_NRF_CACHE_INVD, NULL, 0, false);