From 505f57dd0053a07a8fc158b4ce63b7280a5de5de Mon Sep 17 00:00:00 2001
From: Eero Nurkkala <eero.nurkkala@offcode.fi>
Date: Wed, 15 May 2024 14:30:12 +0300
Subject: [PATCH] arm64: introduce MMU support for EL3

Currently MMU supports only EL1.  Introduce EL3 support as well.

Signed-off-by: Eero Nurkkala <eero.nurkkala@offcode.fi>
---
 arch/arm64/src/common/arm64_mmu.c | 58 ++++++++++++++++++++++++++++---
 1 file changed, 54 insertions(+), 4 deletions(-)

diff --git a/arch/arm64/src/common/arm64_mmu.c b/arch/arm64/src/common/arm64_mmu.c
index 20e35ea80f..3c73097563 100644
--- a/arch/arm64/src/common/arm64_mmu.c
+++ b/arch/arm64/src/common/arm64_mmu.c
@@ -523,6 +523,40 @@ static void setup_page_tables(void)
     }
 }
 
+#ifdef CONFIG_ARCH_BOOT_EL3
+static void enable_mmu_el3(unsigned int flags)
+{
+  uint64_t value;
+  UNUSED(flags);
+
+  /* Set MAIR, TCR and TBBR registers */
+
+  write_sysreg(MEMORY_ATTRIBUTES, mair_el3);
+  write_sysreg(get_tcr(3), tcr_el3);
+  write_sysreg(((uint64_t)base_xlat_table), ttbr0_el3);
+
+  /* Ensure these changes are seen before MMU is enabled */
+
+  ARM64_DSB();
+  ARM64_ISB();
+
+  /* Enable the MMU and data cache */
+
+  value = read_sysreg(sctlr_el3);
+  write_sysreg((value | SCTLR_M_BIT
+#ifndef CONFIG_ARM64_DCACHE_DISABLE
+               | SCTLR_C_BIT
+#endif
+               ), sctlr_el3);
+
+  /* Ensure the MMU enable takes effect immediately */
+
+  ARM64_ISB();
+#ifdef CONFIG_MMU_DEBUG
+  sinfo("MMU enabled with dcache\n");
+#endif
+}
+#else
 static void enable_mmu_el1(unsigned int flags)
 {
   uint64_t value;
@@ -555,6 +589,7 @@ static void enable_mmu_el1(unsigned int flags)
   sinfo("MMU enabled with dcache\n");
 #endif
 }
+#endif
 
 /***************************************************************************
  * Public Functions
@@ -588,19 +623,30 @@ int arm64_mmu_set_memregion(const struct arm_mmu_region *region)
 int arm64_mmu_init(bool is_primary_core)
 {
   uint64_t val;
+  uint64_t el;
   unsigned flags = 0;
 
-  /* Current MMU code supports only EL1 */
+  /* Current MMU code supports EL1 and EL3 */
 
-  __asm__ volatile ("mrs %0, CurrentEL" : "=r" (val));
+  __asm__ volatile ("mrs %0, CurrentEL" : "=r" (el));
 
-  __MMU_ASSERT(GET_EL(val) == MODE_EL1,
+#ifdef CONFIG_ARCH_BOOT_EL3
+  __MMU_ASSERT(GET_EL(el) == MODE_EL3,
+               "Exception level not EL3, MMU not enabled!\n");
+
+  /* Ensure that MMU is already not enabled */
+
+  __asm__ volatile ("mrs %0, sctlr_el3" : "=r" (val));
+  __MMU_ASSERT((val & SCTLR_M_BIT) == 0, "MMU is already enabled\n");
+#else
+  __MMU_ASSERT(GET_EL(el) == MODE_EL1,
                "Exception level not EL1, MMU not enabled!\n");
 
   /* Ensure that MMU is already not enabled */
 
   __asm__ volatile ("mrs %0, sctlr_el1" : "=r" (val));
   __MMU_ASSERT((val & SCTLR_M_BIT) == 0, "MMU is already enabled\n");
+#endif
 
 #ifdef CONFIG_MMU_DEBUG
   sinfo("xlat tables:\n");
@@ -617,9 +663,13 @@ int arm64_mmu_init(bool is_primary_core)
       setup_page_tables();
     }
 
-  /* currently only EL1 is supported */
+  /* Currently EL1 and EL3 are supported */
 
+#ifdef CONFIG_ARCH_BOOT_EL3
+  enable_mmu_el3(flags);
+#else
   enable_mmu_el1(flags);
+#endif
 
   return 0;
 }