arm64: reset: flush D-Cache before it is disabled

In the commit 573a712 patch "arm64:
reset: disable cache and MMU for safety", it disables D-Cache and MMU
for safety, but in some cases, for example the code is loaded into memory
by hardware debugger, we need to flush D-Cache before disable it in
order to make sure the data is coherent in the system, otherwise it
will report "Synchronous Abort" when D-Cache is disabled.

Signed-off-by: Jiafei Pan <[email protected]>

Author: JiafeiPan

arch/arm64/core/Kconfig

@@ -364,4 +364,13 @@ config ARM64_DCACHE_ALL_OPS
 	  Enable this option to provide the data cache APIs to flush or
 	  invalidate all data caches.
 
+config ARM64_BOOT_DISABLE_DCACHE
+	bool "Disable data cache before enable MMU when booting from EL2"
+	depends on ARM64_DCACHE_ALL_OPS
+	help
+	  To make it safe, if data cache is enabled in case of Zephyr is booting
+	  from EL2, enable this option, it will clean and invalidate all data
+	  cache and then disable data cache, it will will be re-enabled after
+	  MMU is configured and enabled.
+
 endif # CPU_CORTEX_A || CPU_AARCH64_CORTEX_R

arch/arm64/core/reset.c

@@ -5,6 +5,7 @@
  */
 
 #include <kernel_internal.h>
+#include <zephyr/arch/cache.h>
 #include <zephyr/sys/barrier.h>
 #include "boot.h"
 
@@ -122,6 +123,21 @@ void z_arm64_el2_init(void)
 	uint64_t reg;
 
 	reg = read_sctlr_el2();
+#ifdef CONFIG_ARM64_BOOT_DISABLE_DCACHE
+	/* Disable D-Cache if it is enabled, it will re-enabled when MMU is enabled at EL1 */
+	if (reg & SCTLR_C_BIT) {
+		/* Clean and invalidate the data cache before disabling it to ensure memory
+		 * remains coherent.
+		 */
+		arch_dcache_flush_and_invd_all();
+		barrier_isync_fence_full();
+		/* Disable D-Cache and MMU for EL2 */
+		reg &= ~(SCTLR_C_BIT | SCTLR_M_BIT);
+		write_sctlr_el2(reg);
+		/* Invalidate TLB entries */
+		__asm__ volatile("dsb ishst; tlbi alle2; dsb ish; isb" : : : "memory");
+	}
+#endif
 	reg |= (SCTLR_EL2_RES1 |	/* RES1 */
 		SCTLR_I_BIT |		/* Enable i-cache */
 		SCTLR_SA_BIT);		/* Enable SP alignment check */