From a36b154fd187e9d548d81bdefdfcdec296890a92 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Krzysztof=20Chru=C5=9Bci=C5=84ski?=
 <krzysztof.chruscinski@nordicsemi.no>
Date: Mon, 7 Apr 2025 15:13:19 +0200
Subject: [PATCH] soc: nordic: nrf54h: power: Enable cache as early as possible
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Add nrf_cache_power_up and nrf_cache_power_down functions. In case of
s2ram power up cache as early as possible, before restoring ARM core
registers. It improves restore time from 180 us to 33 us.

As a minor optimization nrf_memconf_ramblock_control_mask_enable_set is
used which allows to control ram blocks for icache and dcache in a
single register write.

Signed-off-by: Krzysztof Chruściński <krzysztof.chruscinski@nordicsemi.no>
---
 soc/nordic/nrf54h/pm_s2ram.c |  3 ++
 soc/nordic/nrf54h/power.c    | 67 ++++++++++++++++++++----------------
 soc/nordic/nrf54h/power.h    |  5 +++
 3 files changed, 46 insertions(+), 29 deletions(-)

diff --git a/soc/nordic/nrf54h/pm_s2ram.c b/soc/nordic/nrf54h/pm_s2ram.c
index bb54161e6a5..56e5e080233 100644
--- a/soc/nordic/nrf54h/pm_s2ram.c
+++ b/soc/nordic/nrf54h/pm_s2ram.c
@@ -9,6 +9,7 @@
 #include <zephyr/sys/util.h>
 #include <hal/nrf_resetinfo.h>
 #include "pm_s2ram.h"
+#include "power.h"
 
 #include <cmsis_core.h>
 
@@ -170,6 +171,8 @@ int soc_s2ram_suspend(pm_s2ram_system_off_fn_t system_off)
 		return ret;
 	}
 
+	nrf_power_up_cache();
+
 	mpu_resume(&backup_data.mpu_context);
 	nvic_resume(&backup_data.nvic_context);
 	scb_resume(&backup_data.scb_context);
diff --git a/soc/nordic/nrf54h/power.c b/soc/nordic/nrf54h/power.c
index e06262c26aa..e1263be0d0e 100644
--- a/soc/nordic/nrf54h/power.c
+++ b/soc/nordic/nrf54h/power.c
@@ -18,42 +18,50 @@
 
 extern sys_snode_t soc_node;
 
+static void nrf_power_down_cache(void)
+{
+	static const uint32_t msk =
+		(IS_ENABLED(CONFIG_DCACHE) ? BIT(RAMBLOCK_CONTROL_BIT_DCACHE) : 0) |
+		(IS_ENABLED(CONFIG_ICACHE) ? BIT(RAMBLOCK_CONTROL_BIT_ICACHE) : 0);
+
+	if (msk == 0) {
+		return;
+	}
+
+	/* Functions are non-empty only if cache is enabled.
+	 * Data cache disabling include flushing.
+	 */
+	sys_cache_data_disable();
+	sys_cache_instr_disable();
+	nrf_memconf_ramblock_control_mask_enable_set(NRF_MEMCONF, RAMBLOCK_POWER_ID, msk, false);
+}
+
+void nrf_power_up_cache(void)
+{
+	static const uint32_t msk =
+		(IS_ENABLED(CONFIG_DCACHE) ? BIT(RAMBLOCK_CONTROL_BIT_DCACHE) : 0) |
+		(IS_ENABLED(CONFIG_ICACHE) ? BIT(RAMBLOCK_CONTROL_BIT_ICACHE) : 0);
+
+	if (msk == 0) {
+		return;
+	}
+
+	nrf_memconf_ramblock_control_mask_enable_set(NRF_MEMCONF, RAMBLOCK_POWER_ID, msk, true);
+	sys_cache_instr_enable();
+	sys_cache_data_enable();
+}
+
 static void common_suspend(void)
 {
-	if (IS_ENABLED(CONFIG_DCACHE)) {
-		/* Flush, disable and power down DCACHE */
-		sys_cache_data_flush_all();
-		sys_cache_data_disable();
-		nrf_memconf_ramblock_control_enable_set(NRF_MEMCONF, RAMBLOCK_POWER_ID,
-							RAMBLOCK_CONTROL_BIT_DCACHE, false);
-	}
-
-	if (IS_ENABLED(CONFIG_ICACHE)) {
-		/* Disable and power down ICACHE */
-		sys_cache_instr_disable();
-		nrf_memconf_ramblock_control_enable_set(NRF_MEMCONF, RAMBLOCK_POWER_ID,
-							RAMBLOCK_CONTROL_BIT_ICACHE, false);
-	}
-
 	soc_lrcconf_poweron_release(&soc_node, NRF_LRCCONF_POWER_DOMAIN_0);
+	nrf_power_down_cache();
 }
 
 static void common_resume(void)
 {
-	if (IS_ENABLED(CONFIG_ICACHE)) {
-		/* Power up and re-enable ICACHE */
-		nrf_memconf_ramblock_control_enable_set(NRF_MEMCONF, RAMBLOCK_POWER_ID,
-							RAMBLOCK_CONTROL_BIT_ICACHE, true);
-		sys_cache_instr_enable();
-	}
-
-	if (IS_ENABLED(CONFIG_DCACHE)) {
-		/* Power up and re-enable DCACHE */
-		nrf_memconf_ramblock_control_enable_set(NRF_MEMCONF, RAMBLOCK_POWER_ID,
-							RAMBLOCK_CONTROL_BIT_DCACHE, true);
-		sys_cache_data_enable();
-	}
-
+	/* Common part does not include cache enabling. In case of s2ram it is done
+	 * as early as possible to speed up the process.
+	 */
 	soc_lrcconf_poweron_request(&soc_node, NRF_LRCCONF_POWER_DOMAIN_0);
 }
 
@@ -112,6 +120,7 @@ static void s2idle_exit(uint8_t substate_id)
 	case 1: /* Substate for idle with cache retained - not implemented yet. */
 		break;
 	case 2: /* Substate for idle with cache disabled. */
+		nrf_power_up_cache();
 		common_resume();
 #if !defined(CONFIG_SOC_NRF54H20_CPURAD)
 		soc_lrcconf_poweron_release(&soc_node, NRF_LRCCONF_POWER_MAIN);
diff --git a/soc/nordic/nrf54h/power.h b/soc/nordic/nrf54h/power.h
index 021c20ec4dc..87489574e72 100644
--- a/soc/nordic/nrf54h/power.h
+++ b/soc/nordic/nrf54h/power.h
@@ -17,4 +17,9 @@
  */
 void nrf_poweroff(void);
 
+/**
+ * @brief Power up and enable instruction and data cache.
+ */
+void nrf_power_up_cache(void);
+
 #endif /* _ZEPHYR_SOC_ARM_NORDIC_NRF_POWER_H_ */