diff --git a/m1n1-raw.ld b/m1n1-raw.ld index 56ebfe49..7f88b608 100644 --- a/m1n1-raw.ld +++ b/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; diff --git a/m1n1.ld b/m1n1.ld index 6ac86d7a..5d6d95e8 100644 --- a/m1n1.ld +++ b/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 = .); diff --git a/src/chickens.c b/src/chickens.c index 673b8a5d..02329ef6 100644 --- a/src/chickens.c +++ b/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) { diff --git a/src/exception.c b/src/exception.c index d1f606d3..9b2b3753 100644 --- a/src/exception.c +++ b/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); - msr(SYS_IMP_APL_IPI_SR_EL1, IPI_SR_PENDING); + + if (cpufeat_fast_ipi) { + 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 esr = in_gl ? mrs(SYS_IMP_APL_ESR_GL1) : (el12 ? mrs(ESR_EL12) : mrs(ESR_EL1)); - u64 far = in_gl ? mrs(SYS_IMP_APL_FAR_GL1) : (el12 ? mrs(FAR_EL12) : mrs(FAR_EL1)); + u64 elr = in_gl ? mrs(SYS_IMP_APL_ELR_GL1) + : (el12 ? mrs(ELR_EL12) : (el3 ? mrs(ELR_EL3) : mrs(ELR_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 esr = in_gl ? mrs(SYS_IMP_APL_ESR_GL1) : mrs(ESR_EL1); - u64 elr = in_gl ? mrs(SYS_IMP_APL_ELR_GL1) : mrs(ELR_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) : (el3 ? mrs(ESR_EL3) : mrs(ESR_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); diff --git a/src/start.S b/src/start.S index b0051e6f..fcefb961 100644 --- a/src/start.S +++ b/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: diff --git a/src/startup.c b/src/startup.c index 5473f370..a883bea3 100644 --- a/src/startup.c +++ b/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 diff --git a/src/utils.h b/src/utils.h index c3548784..7e823294 100644 --- a/src/utils.h +++ b/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;