diff --git a/arch/arm/core/aarch64/macro_priv.inc b/arch/arm/core/aarch64/macro_priv.inc index e711504ef25..7810399a2dc 100644 --- a/arch/arm/core/aarch64/macro_priv.inc +++ b/arch/arm/core/aarch64/macro_priv.inc @@ -18,6 +18,9 @@ */ .macro z_arm64_enter_exc xreg0, xreg1 + /* Switch to SP_EL0 */ + msr spsel, #0 + /* * Two things can happen to the remaining registers: * diff --git a/arch/arm/core/aarch64/reset.S b/arch/arm/core/aarch64/reset.S index c330be2284c..3a97be8a6da 100644 --- a/arch/arm/core/aarch64/reset.S +++ b/arch/arm/core/aarch64/reset.S @@ -59,7 +59,9 @@ SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start) msr vbar_el3, x19 isb - /* Setup a stack for EL3 (SP_ELx) */ + /* Switch to SP_EL0 and setup the stack */ + msr spsel, #0 + ldr x0, =(z_interrupt_stacks) add x0, x0, #(CONFIG_ISR_STACK_SIZE) mov sp, x0 @@ -121,7 +123,9 @@ SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start) msr vbar_el1, x19 isb - /* Setup the stack (SP_ELx) */ + /* Switch to SP_EL0 and setup the stack */ + msr spsel, #0 + ldr x0, =(z_interrupt_stacks) add x0, x0, #(CONFIG_ISR_STACK_SIZE) mov sp, x0 diff --git a/arch/arm/core/aarch64/thread.c b/arch/arm/core/aarch64/thread.c index dc0028be4fd..66e0aca00d4 100644 --- a/arch/arm/core/aarch64/thread.c +++ b/arch/arm/core/aarch64/thread.c @@ -41,7 +41,7 @@ void arch_new_thread(struct k_thread *thread, k_thread_stack_t *stack, * - SPSR_ELn: to enable IRQs (we are masking FIQs). */ pInitCtx->elr = (uint64_t)z_thread_entry; - pInitCtx->spsr = SPSR_MODE_EL1H | DAIF_FIQ; + pInitCtx->spsr = SPSR_MODE_EL1T | DAIF_FIQ; /* * We are saving SP to pop out entry and parameters when going through diff --git a/arch/arm/core/aarch64/vector_table.S b/arch/arm/core/aarch64/vector_table.S index 914acaac340..5a0153fd373 100644 --- a/arch/arm/core/aarch64/vector_table.S +++ b/arch/arm/core/aarch64/vector_table.S @@ -62,11 +62,16 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table) /* Current EL with SP0 / Synchronous */ .align 7 - b . + b z_arm64_sync_exc /* Current EL with SP0 / IRQ */ .align 7 - b . + +#ifdef CONFIG_GEN_SW_ISR_TABLE + b _isr_wrapper +#else + b z_irq_spurious +#endif /* Current EL with SP0 / FIQ */ .align 7 @@ -74,7 +79,7 @@ SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table) /* Current EL with SP0 / SError */ .align 7 - b . + b z_arm64_serror /* Current EL with SPx / Synchronous */ .align 7 diff --git a/include/arch/arm/aarch64/cpu.h b/include/arch/arm/aarch64/cpu.h index 4ab3d3a9e24..3d7458db6bb 100644 --- a/include/arch/arm/aarch64/cpu.h +++ b/include/arch/arm/aarch64/cpu.h @@ -20,6 +20,8 @@ #define DAIF_DBG BIT(9) #define DAIF_MASK (0xf << 6) +#define SPSR_MODE_EL0T (0x0) +#define SPSR_MODE_EL1T (0x4) #define SPSR_MODE_EL1H (0x5) #define SCTLR_EL3_RES1 (BIT(29) | BIT(28) | BIT(23) | \