From fb0a4d31919521b6332cd464b600ac607e7718f6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Manuel=20Arg=C3=BCelles?= <manuel.arguelles@nxp.com>
Date: Sat, 18 Mar 2023 00:00:00 +0000
Subject: [PATCH] soc: fvp_aemv8r_aarch32: enable caches at init
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Enable at SoC boot time when enabled through Kconfig. Cache management
API is not used since it could be built without its support enabled.

Signed-off-by: Manuel Argüelles <manuel.arguelles@nxp.com>
---
 soc/arm/arm/fvp_aemv8r_aarch32/CMakeLists.txt |  1 +
 soc/arm/arm/fvp_aemv8r_aarch32/Kconfig.soc    |  1 +
 soc/arm/arm/fvp_aemv8r_aarch32/soc.c          | 27 +++++++++++++++++++
 3 files changed, 29 insertions(+)
 create mode 100644 soc/arm/arm/fvp_aemv8r_aarch32/soc.c

diff --git a/soc/arm/arm/fvp_aemv8r_aarch32/CMakeLists.txt b/soc/arm/arm/fvp_aemv8r_aarch32/CMakeLists.txt
index 1e9b4a9e9e9cf1..01b4ede7dfa5b2 100644
--- a/soc/arm/arm/fvp_aemv8r_aarch32/CMakeLists.txt
+++ b/soc/arm/arm/fvp_aemv8r_aarch32/CMakeLists.txt
@@ -2,3 +2,4 @@
 # SPDX-License-Identifier: Apache-2.0
 
 zephyr_library_sources_ifdef(CONFIG_ARM_MPU arm_mpu_regions.c)
+zephyr_library_sources(soc.c)
diff --git a/soc/arm/arm/fvp_aemv8r_aarch32/Kconfig.soc b/soc/arm/arm/fvp_aemv8r_aarch32/Kconfig.soc
index 28929d1ee6c685..3b6441c707b1ff 100644
--- a/soc/arm/arm/fvp_aemv8r_aarch32/Kconfig.soc
+++ b/soc/arm/arm/fvp_aemv8r_aarch32/Kconfig.soc
@@ -12,5 +12,6 @@ config SOC_FVP_AEMV8R_AARCH32
 	select CPU_HAS_MPU
 	select GIC_V3
 	select GIC_SINGLE_SECURITY_STATE
+	select PLATFORM_SPECIFIC_INIT
 
 endchoice
diff --git a/soc/arm/arm/fvp_aemv8r_aarch32/soc.c b/soc/arm/arm/fvp_aemv8r_aarch32/soc.c
new file mode 100644
index 00000000000000..f447b73bbf6efd
--- /dev/null
+++ b/soc/arm/arm/fvp_aemv8r_aarch32/soc.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright 2023 NXP
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/kernel.h>
+#include <zephyr/arch/arm/aarch32/cortex_a_r/cmsis.h>
+
+void z_arm_platform_init(void)
+{
+	if (IS_ENABLED(CONFIG_ICACHE)) {
+		if (!(__get_SCTLR() & SCTLR_I_Msk)) {
+			L1C_InvalidateICacheAll();
+			__set_SCTLR(__get_SCTLR() | SCTLR_I_Msk);
+			__ISB();
+		}
+	}
+
+	if (IS_ENABLED(CONFIG_DCACHE)) {
+		if (!(__get_SCTLR() & SCTLR_C_Msk)) {
+			L1C_InvalidateDCacheAll();
+			__set_SCTLR(__get_SCTLR() | SCTLR_C_Msk);
+			__DSB();
+		}
+	}
+}