diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig index b9fc7bdb06..f2c95c734f 100644 --- a/arch/arm64/Kconfig +++ b/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--- diff --git a/arch/arm64/src/common/arm64_arch.h b/arch/arm64/src/common/arm64_arch.h index de47814d9d..d329365303 100644 --- a/arch/arm64/src/common/arm64_arch.h +++ b/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 */ diff --git a/arch/arm64/src/common/arm64_boot.c b/arch/arm64/src/common/arm64_boot.c index 42df595307..80ec0094e7 100644 --- a/arch/arm64/src/common/arm64_boot.c +++ b/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 */ diff --git a/arch/arm64/src/common/arm64_fork.c b/arch/arm64/src/common/arm64_fork.c index aea3d57044..f1520b9b13 100644 --- a/arch/arm64/src/common/arm64_fork.c +++ b/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); diff --git a/arch/arm64/src/common/arm64_head.S b/arch/arm64/src/common/arm64_head.S index f10dcd4005..34b8f2f9f1 100644 --- a/arch/arm64/src/common/arm64_head.S +++ b/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: diff --git a/arch/arm64/src/common/arm64_initialstate.c b/arch/arm64/src/common/arm64_initialstate.c index b54532ba71..7773ca2204 100644 --- a/arch/arm64/src/common/arm64_initialstate.c +++ b/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); diff --git a/arch/arm64/src/common/arm64_schedulesigaction.c b/arch/arm64/src/common/arm64_schedulesigaction.c index 255a564e42..9d8ba15996 100644 --- a/arch/arm64/src/common/arm64_schedulesigaction.c +++ b/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; diff --git a/arch/arm64/src/common/arm64_vector_table.S b/arch/arm64/src/common/arm64_vector_table.S index 1f9fd33243..c37add5c78 100644 --- a/arch/arm64/src/common/arm64_vector_table.S +++ b/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 */ diff --git a/arch/arm64/src/common/arm64_vectors.S b/arch/arm64/src/common/arm64_vectors.S index e6541b519c..cf6958d9b3 100644 --- a/arch/arm64/src/common/arm64_vectors.S +++ b/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