arm64: provide EL3 support

This provides means to run NuttX completely in EL3.  This may
be useful with NuttX based bootloaders that are executed from
OCRAM.  Instead of SPL/U-boot combo, NuttX may replace SPL
completely.

Signed-off-by: Eero Nurkkala <eero.nurkkala@offcode.fi>

arch/arm64/Kconfig

@@ -146,6 +146,15 @@ config ARCH_HAVE_EL3
 		runing at EL3 is not necessary and system register for EL3
 		is not accessible
 
+config ARCH_BOOT_EL3
+	bool "Boot in EL3"
+	default n
+	depends on ARCH_HAVE_EL3
+	---help---
+		If NuttX works as the primary bootloader, give option to
+		stay in EL3. This will prevent it to switching into EL2/EL1
+		levels.
+
 config ARCH_SET_VMPIDR_EL2
 	bool "Set VMPIDR_EL2 at EL2 stage"
 	---help---

arch/arm64/src/common/arm64_arch.h

@@ -119,6 +119,8 @@
 #define SPSR_MODE_EL1H      (0x5)
 #define SPSR_MODE_EL2T      (0x8)
 #define SPSR_MODE_EL2H      (0x9)
+#define SPSR_MODE_EL3T      (0xc)
+#define SPSR_MODE_EL3H      (0xd)
 #define SPSR_MODE_MASK      (0xf)
 
 /* CurrentEL: Current Exception Level */

arch/arm64/src/common/arm64_boot.c

@@ -78,6 +78,11 @@ void arm64_boot_el3_init(void)
   reg = 0U;               /* Reset */
   reg |= SCR_NS_BIT;      /* EL2 / EL3 non-secure */
   reg |= (SCR_RES1 |      /* RES1 */
+  #ifdef CONFIG_ARCH_BOOT_EL3
+          SCR_IRQ_BIT |   /* Route IRQs to EL3 */
+          SCR_FIQ_BIT |   /* Route FIQs to EL3 */
+          SCR_EA_BIT |    /* Route EAs to EL3 */
+  #endif
           SCR_RW_BIT |    /* EL2 execution state is AArch64 */
           SCR_ST_BIT |    /* Do not trap EL1 accesses to timer */
           SCR_HCE_BIT |   /* Do not trap HVC */

arch/arm64/src/common/arm64_fork.c

@@ -225,7 +225,11 @@ pid_t arm64_fork(const struct fork_s *context)
   pforkctx->regs[REG_X28]  = context->regs[FORK_REG_X28];
   pforkctx->regs[REG_X29]  = newfp;
 
+#ifdef CONFIG_ARCH_BOOT_EL3
+  pforkctx->spsr = SPSR_MODE_EL3H;
+#else
   pforkctx->spsr = SPSR_MODE_EL1H;
+#endif
 
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   pforkctx->spsr       |= (DAIF_IRQ_BIT | DAIF_FIQ_BIT);

arch/arm64/src/common/arm64_head.S

@@ -235,6 +235,15 @@ switch_el:
 
     bl    arm64_boot_el3_init
 
+#ifdef CONFIG_ARCH_BOOT_EL3
+    msr   SPSel, #1
+
+    /* Set SP_EL3 (with SPSel = 1) */
+
+    mov   sp, x24
+    b     el3_boot
+#endif
+
     /* Get next EL */
 
     adr   x0, switch_el
@@ -268,6 +277,8 @@ switch_el:
 
     msr   SPSel, #1
     msr   DAIFClr, #(DAIFCLR_ABT_BIT)
+
+el3_boot:
     isb
 
 jump_to_c_entry:

arch/arm64/src/common/arm64_initialstate.c

@@ -74,9 +74,13 @@ void arm64_new_task(struct tcb_s * tcb)
   memset(pinitctx, 0, sizeof(struct regs_context));
   pinitctx->elr       = (uint64_t)tcb->start;
 
-  /* Keep using SP_EL1 */
+  /* Keep using SP_EL1 or SP_EL3 */
 
+#ifdef CONFIG_ARCH_BOOT_EL3
+  pinitctx->spsr      = SPSR_MODE_EL3H;
+#else
   pinitctx->spsr      = SPSR_MODE_EL1H;
+#endif
 
 #ifdef CONFIG_SUPPRESS_INTERRUPTS
   pinitctx->spsr       |= (DAIF_IRQ_BIT | DAIF_FIQ_BIT);

arch/arm64/src/common/arm64_schedulesigaction.c

@@ -74,7 +74,11 @@ void arm64_init_signal_process(struct tcb_s *tcb, struct regs_context *regs)
 
   /* Keep using SP_EL1 */
 
+#ifdef CONFIG_ARCH_BOOT_EL3
+  psigctx->spsr      = SPSR_MODE_EL3H | DAIF_FIQ_BIT | DAIF_IRQ_BIT;
+#else
   psigctx->spsr      = SPSR_MODE_EL1H | DAIF_FIQ_BIT | DAIF_IRQ_BIT;
+#endif
   psigctx->sp_elx    = (uint64_t)stack_ptr;
   psigctx->sp_el0    = (uint64_t)psigctx;
   psigctx->exe_depth = 1;

arch/arm64/src/common/arm64_vector_table.S

@@ -69,8 +69,13 @@
     stp    x30, \xreg0,  [sp, #8 * REG_X30]
 
     /* ELR and SPSR */
+#ifdef CONFIG_ARCH_BOOT_EL3
+    mrs    \xreg0, elr_el3
+    mrs    \xreg1, spsr_el3
+#else
     mrs    \xreg0, elr_el1
     mrs    \xreg1, spsr_el1
+#endif
     stp    \xreg0, \xreg1,  [sp, #8 * REG_ELR]
 
     /* increment exception depth */
@@ -246,8 +251,13 @@ arm64_exit_exc_fpu_done:
     /* restore spsr and elr at el1*/
 
     ldp    x0, x1,   [sp, #8 * REG_ELR]
+#ifdef CONFIG_ARCH_BOOT_EL3
+    msr    elr_el3,   x0
+    msr    spsr_el3,  x1
+#else
     msr    elr_el1,   x0
     msr    spsr_el1,  x1
+#endif
 
     /* decrement exception depth */
 

arch/arm64/src/common/arm64_vectors.S

@@ -92,8 +92,13 @@ SECTION_FUNC(text, up_saveusercontext)
     stp    x30, x4,  [x0, #8 * REG_X30]
 
     /* ELR and SPSR */
+#ifdef CONFIG_ARCH_BOOT_EL3
+    mrs    x4,  elr_el3
+    mrs    x5,  spsr_el3
+#else
     mrs    x4,  elr_el1
     mrs    x5,  spsr_el1
+#endif
     stp    x4,  x5,  [x0, #8 * REG_ELR]
 
     arm64_exception_context_save x4 x5 x0
@@ -181,7 +186,11 @@ GTEXT(arm64_sync_exc)
 SECTION_FUNC(text, arm64_sync_exc)
     /* checking the EC value to see which exception need to be handle */
 
+#ifdef CONFIG_ARCH_BOOT_EL3
+    mrs    x0, esr_el3
+#else
     mrs    x0, esr_el1
+#endif
     lsr    x1, x0, #26
 
 #ifdef CONFIG_ARCH_FPU