Add support for handling EL3.

A7-A9X has an EL3. Handle it (without SMP support).

Signed-off-by: Nick Chan <towinchenmi@gmail.com>

m1n1-raw.ld

@@ -1,6 +1,7 @@
 ENTRY(_start)
 
 _stack_size = 0x20000;
+_stack_size_el3 = 0x8000;
 
 /* We are actually relocatable */
 . = 0;
@@ -71,6 +72,12 @@ SECTIONS {
         *(COMMON)
         _bss_end = .;
     } : data
+    .stack_el3 : ALIGN(0x4000) {
+        PROVIDE(_stack_top_el3 = .);
+        . += _stack_size_el3 - 8;
+        QUAD(0x5176694b43415453);
+        PROVIDE(_stack_bot_el3 = .);
+    }
     .stack : ALIGN(0x4000) {
         PROVIDE(_stack_top = .);
         . += _stack_size - 8;

m1n1.ld

@@ -4,6 +4,7 @@ ENTRY(_start)
 _va_base = 0xFFFFFE0007004000;
 
 _stack_size = 0x20000;
+_stack_size_el3 = 0x8000;
 
 _max_payload_size = 64*1024*1024;
 
@@ -176,6 +177,10 @@ SECTIONS {
         *(COMMON)
         . = ALIGN(0x4000);
         _bss_end = .;
+        PROVIDE(_stack_top_el3 = .);
+        . += _stack_size_el3;
+        PROVIDE(_stack_bot_el3 = .);
+        . = ALIGN(0x4000);
         PROVIDE(_stack_top = .);
         . += _stack_size;
         PROVIDE(_stack_bot = .);

src/chickens.c

@@ -69,7 +69,7 @@ void init_t6021_avalanche(int rev);
 void init_t6031_sawtooth(void);
 void init_t6031_everest(int rev);
 
-bool cpufeat_actlr_el2;
+bool cpufeat_actlr_el2, cpufeat_fast_ipi;
 
 const char *init_cpu(void)
 {

src/exception.c

@@ -30,6 +30,8 @@ static char *m_table[0x10] = {
     [0x05] = "EL1h", //
     [0x08] = "EL2t", //
     [0x09] = "EL2h", //
+    [0x0c] = "EL3t", //
+    [0x0d] = "EL3h", //
 };
 
 static char *gl_m_table[0x10] = {
@@ -114,6 +116,8 @@ static const char *get_exception_level(void)
             return "EL1";
         else if (lvl == 0x08)
             return "EL2";
+        else if (lvl == 0x0c)
+            return "EL3";
     }
 
     return "?";
@@ -121,7 +125,10 @@ static const char *get_exception_level(void)
 
 void exception_initialize(void)
 {
-    msr(VBAR_EL1, _vectors_start);
+    if (in_el3())
+        msr(VBAR_EL3, _vectors_start);
+    else
+        msr(VBAR_EL1, _vectors_start);
 
     // Clear FIQ sources
     msr(CNTP_CTL_EL0, 7L);
@@ -130,9 +137,12 @@ void exception_initialize(void)
         msr(CNTP_CTL_EL02, 7L);
         msr(CNTV_CTL_EL02, 7L);
     }
-    reg_clr(SYS_IMP_APL_PMCR0, PMCR0_IACT | PMCR0_IMODE_MASK);
+
-    reg_clr(SYS_IMP_APL_UPMCR0, UPMCR0_IMODE_MASK);
+    if (cpufeat_fast_ipi) {
-    msr(SYS_IMP_APL_IPI_SR_EL1, IPI_SR_PENDING);
+        reg_clr(SYS_IMP_APL_PMCR0, PMCR0_IACT | PMCR0_IMODE_MASK);
+        reg_clr(SYS_IMP_APL_UPMCR0, UPMCR0_IMODE_MASK);
+        msr(SYS_IMP_APL_IPI_SR_EL1, IPI_SR_PENDING);
+    }
 
     if (is_boot_cpu())
         msr(DAIF, 0 << 6); // Enable SError, IRQ and FIQ
@@ -168,8 +178,10 @@ void print_regs(u64 *regs, int el12)
     u64 sp = ((u64)(regs)) + 256;
 
     in_gl = in_gl12();
+    bool el3 = in_el3();
 
-    u64 spsr = in_gl ? mrs(SYS_IMP_APL_SPSR_GL1) : (el12 ? mrs(SPSR_EL12) : mrs(SPSR_EL1));
+    u64 spsr = in_gl ? mrs(SYS_IMP_APL_SPSR_GL1)
+                     : (el12 ? mrs(SPSR_EL12) : (el3 ? mrs(SPSR_EL3) : mrs(SPSR_EL1)));
 
     printf("Exception taken from %s\n", get_exception_source(spsr));
     printf("Running in %s\n", get_exception_level());
@@ -184,9 +196,12 @@ void print_regs(u64 *regs, int el12)
     printf("x24-x27: %016lx %016lx %016lx %016lx\n", regs[24], regs[25], regs[26], regs[27]);
     printf("x28-x30: %016lx %016lx %016lx\n", regs[28], regs[29], regs[30]);
 
-    u64 elr = in_gl ? mrs(SYS_IMP_APL_ELR_GL1) : (el12 ? mrs(ELR_EL12) : mrs(ELR_EL1));
+    u64 elr = in_gl ? mrs(SYS_IMP_APL_ELR_GL1)
-    u64 esr = in_gl ? mrs(SYS_IMP_APL_ESR_GL1) : (el12 ? mrs(ESR_EL12) : mrs(ESR_EL1));
+                    : (el12 ? mrs(ELR_EL12) : (el3 ? mrs(ELR_EL3) : mrs(ELR_EL1)));
-    u64 far = in_gl ? mrs(SYS_IMP_APL_FAR_GL1) : (el12 ? mrs(FAR_EL12) : mrs(FAR_EL1));
+    u64 esr = in_gl ? mrs(SYS_IMP_APL_ESR_GL1)
+                    : (el12 ? mrs(ESR_EL12) : (el3 ? mrs(ESR_EL3) : mrs(ESR_EL1)));
+    u64 far = in_gl ? mrs(SYS_IMP_APL_FAR_GL1)
+                    : (el12 ? mrs(FAR_EL12) : (el3 ? mrs(FAR_EL3) : mrs(FAR_EL1)));
 
     printf("PC:       0x%lx (rel: 0x%lx)\n", elr, elr - (u64)_base);
     printf("SP:       0x%lx\n", sp);
@@ -221,10 +236,11 @@ void exc_sync(u64 *regs)
     u32 insn;
     int el12 = 0;
     bool in_gl = in_gl12();
+    bool el3 = in_el3();
 
-    u64 spsr = in_gl ? mrs(SYS_IMP_APL_SPSR_GL1) : mrs(SPSR_EL1);
+    u64 spsr = in_gl ? mrs(SYS_IMP_APL_SPSR_GL1) : (el3 ? mrs(SPSR_EL3) : mrs(SPSR_EL1));
-    u64 esr = in_gl ? mrs(SYS_IMP_APL_ESR_GL1) : mrs(ESR_EL1);
+    u64 esr = in_gl ? mrs(SYS_IMP_APL_ESR_GL1) : (el3 ? mrs(ESR_EL3) : mrs(ESR_EL1));
-    u64 elr = in_gl ? mrs(SYS_IMP_APL_ELR_GL1) : mrs(ELR_EL1);
+    u64 elr = in_gl ? mrs(SYS_IMP_APL_ELR_GL1) : (el3 ? mrs(ELR_EL3) : mrs(ELR_EL1));
 
     if ((spsr & 0xf) == 0 && ((esr >> 26) & 0x3f) == 0x3c) {
         // On clean EL0 return, let the normal exception return
@@ -256,6 +272,14 @@ void exc_sync(u64 *regs)
                 printf("Unknown HVC: 0x%x\n", imm);
                 break;
         }
+    } else if (in_el3() && ((esr >> 26) & 0x3f) == 0x17) {
+        // Monitor call
+        u32 imm = mrs(ESR_EL3) & 0xffff;
+        switch (imm) {
+            default:
+                printf("Unknown SMC: 0x%x\n", imm);
+                break;
+        }
     } else {
         if (!(exc_guard & GUARD_SILENT))
             printf("Exception: SYNC\n");
@@ -297,6 +321,8 @@ void exc_sync(u64 *regs)
         printf("Recovering from exception (ELR=0x%lx)\n", elr);
     if (in_gl)
         msr(SYS_IMP_APL_ELR_GL1, elr);
+    if (el3)
+        msr(ELR_EL3, elr);
     else
         msr(ELR_EL1, elr);
 

src/start.S

@@ -96,11 +96,27 @@ _start:
     mov w0, '\n'
     bl debug_putc
 
+    mrs x8, CurrentEL
+    cmp x8, #0xc
+    beq start_el3
+
     mov x0, x19
     mov x1, x20
     bl _start_c
     b .
 
+.globl exception_initialize
+start_el3:
+    bl exception_initialize
+    adrp x8, _stack_bot_el3
+    mov sp, x8
+    mov x0, x19
+    mov x1, x20
+    msr tpidr_el3, xzr
+    adrp x6, _stack_bot
+    adr x7, _start_c
+    b el3_eret_to_el1
+
 .globl exc_unk
 .type exc_unk, @function
 exc_unk:
@@ -148,6 +164,22 @@ cpu_reset:
     bl _cpu_reset_c
     b .
 
+el3_eret_to_el1:
+    adr x8, _vectors_start
+    msr sctlr_el1, xzr
+
+    mrs x8, scr_el3
+    orr x8, x8, #(1<<10) // EL1 execution state is AArch64
+    orr x8, x8, #(1<<0)  // EL1 is non-secure
+    msr scr_el3, x8
+    msr elr_el3, x7 // Set EL1 entry point
+    mov x8, #0x5 // EL1h
+    msr spsr_el3, x8
+
+    msr sp_el1, x6 // Set EL1 stack pointer
+
+    eret
+
 .globl debug_putc
 .type debug_putc, @function
 debug_putc:

src/startup.c

@@ -115,9 +115,13 @@ void _cpu_reset_c(void *stack)
     printf("  MPIDR: 0x%lx\n", mrs(MPIDR_EL1));
     const char *type = init_cpu();
     printf("  CPU: %s\n", type);
+    printf("  Running in EL%lu\n\n", mrs(CurrentEL) >> 2);
 
     exception_initialize();
 
+    if (in_el3())
+        return;
+
     if (!is_boot_cpu())
         smp_secondary_entry();
     else

src/utils.h

@@ -342,6 +342,16 @@ static inline int in_el2(void)
     return (mrs(CurrentEL) >> 2) == 2;
 }
 
+static inline int in_el3(void)
+{
+    return (mrs(CurrentEL) >> 2) == 3;
+}
+
+static inline int has_el3(void)
+{
+    return !!(mrs(ID_AA64PFR0_EL1) & 0xf000);
+}
+
 extern int boot_cpu_idx;
 extern u64 boot_cpu_mpidr;
 static inline int is_boot_cpu(void)
@@ -438,7 +448,7 @@ struct vector_args {
 
 extern u32 board_id, chip_id;
 extern bool is_mac;
-extern bool cpufeat_actlr_el2;
+extern bool cpufeat_actlr_el2, cpufeat_fast_ipi;
 
 extern struct vector_args next_stage;