diff --git a/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.S b/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.S index 8f119ef914..4e3f9c511d 100644 --- a/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.S +++ b/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.S @@ -23,14 +23,21 @@ Lstart: bne Lstart ret +.global return_from_el2 +.type return_from_el2, @function +return_from_el2: + adr x0, jump_to_os_start + msr elr_el2, x0 + eret + .global return_from_el3 .type return_from_el3, @function return_from_el3: - adr x0, start_in_el1 + adr x0, jump_to_os_start msr elr_el3, x0 eret -start_in_el1: +jump_to_os_start: // Let stack start before .text for now. // 512 kiB (0x80000) of stack are probably not sufficient, especially once we give the other cores some stack too, // but for now it's ok. diff --git a/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.h b/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.h index 5ac68d2399..33bb2b0861 100644 --- a/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.h +++ b/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.h @@ -12,4 +12,5 @@ extern "C" uint8_t get_current_exception_level(); extern "C" void wait_cycles(int n); // CPU initialization functions +extern "C" [[noreturn]] void return_from_el2(); extern "C" [[noreturn]] void return_from_el3(); diff --git a/Kernel/Prekernel/Arch/aarch64/AarchRegisters.h b/Kernel/Prekernel/Arch/aarch64/AarchRegisters.h index 49271b2ed6..80859fb54d 100644 --- a/Kernel/Prekernel/Arch/aarch64/AarchRegisters.h +++ b/Kernel/Prekernel/Arch/aarch64/AarchRegisters.h @@ -147,6 +147,40 @@ struct Aarch64_SCR_EL3 { }; static_assert(sizeof(Aarch64_SCR_EL3) == 8); +struct Aarch64_SPSR_EL2 { + enum Mode : uint16_t { + EL0t = 0b0000, + EL1t = 0b0100, + EL1h = 0b0101, + EL2t = 0b1000, + EL2h = 0b1001 + }; + + Mode M : 4; + int M_4 : 1 = 0; + int _reserved5 : 1 = 0; + int F : 1; + int I : 1; + int A : 1; + int D : 1; + int BTYPE : 2; + int SSBS : 1; + int _reserved13 : 7 = 0; + int IL : 1; + int SS : 1; + int PAN : 1; + int UA0 : 1; + int DIT : 1; + int TCO : 1; + int _reserved26 : 2 = 0; + int V : 1; + int C : 1; + int Z : 1; + int N : 1; + int _reserved32 : 32 = 0; +}; +static_assert(sizeof(Aarch64_SPSR_EL2) == 8); + struct Aarch64_SPSR_EL3 { enum Mode : uint16_t { EL0t = 0b0000, diff --git a/Kernel/Prekernel/Arch/aarch64/init.cpp b/Kernel/Prekernel/Arch/aarch64/init.cpp index 0e03ac80c1..da0a1250e9 100644 --- a/Kernel/Prekernel/Arch/aarch64/init.cpp +++ b/Kernel/Prekernel/Arch/aarch64/init.cpp @@ -18,7 +18,9 @@ extern "C" [[noreturn]] void os_start(); static void set_up_el1_mode(); static void set_up_el2_mode(); static void set_up_el3_mode(); -[[noreturn]] static void switch_to_el1(); +static void print_current_exception_level(const char* msg); +[[noreturn]] static void jump_to_os_start_from_el2(); +[[noreturn]] static void jump_to_os_start_from_el3(); extern "C" [[noreturn]] void init() { @@ -33,11 +35,43 @@ extern "C" [[noreturn]] void init() uart.print_num(firmware_version); uart.print_str("\r\n"); - set_up_el3_mode(); + print_current_exception_level("CPU started in:"); + set_up_el2_mode(); set_up_el1_mode(); - switch_to_el1(); + auto current_exception_level = get_current_exception_level(); + switch (current_exception_level) { + case 2: + jump_to_os_start_from_el2(); + break; + case 3: + set_up_el3_mode(); + jump_to_os_start_from_el3(); + break; + default: + uart.print_str("FATAL: CPU booted in unsupported exception mode!\r\n"); + halt(); + } +} + +extern "C" [[noreturn]] void os_start() +{ + auto& uart = Prekernel::UART::the(); + + print_current_exception_level("CPU switched to:"); + + auto& timer = Prekernel::Timer::the(); + u64 start_musec = 0; + for (;;) { + u64 now_musec; + while ((now_musec = timer.microseconds_since_boot()) - start_musec < 1'000'000) + ; + start_musec = now_musec; + uart.print_str("Timer: "); + uart.print_num(now_musec); + uart.print_str("\r\n"); + } } // FIXME: Share this with the Intel Prekernel. @@ -128,7 +162,27 @@ static void set_up_el3_mode() asm("msr scr_el3, %[value]" ::[value] "r"(secure_configuration_register_el3)); } -[[noreturn]] static void switch_to_el1() +[[noreturn]] static void jump_to_os_start_from_el2() +{ + // Processor state to set when returned from this function (in new EL1 world) + Kernel::Aarch64_SPSR_EL2 saved_program_status_register_el2 = {}; + + // Mask (disable) all interrupts + saved_program_status_register_el2.A = 1; + saved_program_status_register_el2.I = 1; + saved_program_status_register_el2.F = 1; + + // Indicate EL1 as exception origin mode (so we go back there) + saved_program_status_register_el2.M = Kernel::Aarch64_SPSR_EL2::Mode::EL1h; + + // Set the register + asm("msr spsr_el2, %[value]" ::[value] "r"(saved_program_status_register_el2)); + + // This will jump into os_start() + return_from_el2(); +} + +[[noreturn]] static void jump_to_os_start_from_el3() { // Processor state to set when returned from this function (in new EL1 world) Kernel::Aarch64_SPSR_EL3 saved_program_status_register_el3 = {}; @@ -144,28 +198,17 @@ static void set_up_el3_mode() // Set the register asm("msr spsr_el3, %[value]" ::[value] "r"(saved_program_status_register_el3)); - // This will jump into os_start() below, but in EL1 + // This will jump into os_start() below return_from_el3(); } -extern "C" [[noreturn]] void os_start() +static void print_current_exception_level(const char* msg) { auto& uart = Prekernel::UART::the(); auto exception_level = get_current_exception_level(); - uart.print_str("Current CPU exception level: EL"); + uart.print_str(msg); + uart.print_str(" EL"); uart.print_num(exception_level); uart.print_str("\r\n"); - - auto& timer = Prekernel::Timer::the(); - u64 start_musec = 0; - for (;;) { - u64 now_musec; - while ((now_musec = timer.microseconds_since_boot()) - start_musec < 1'000'000) - ; - start_musec = now_musec; - uart.print_str("Timer: "); - uart.print_num(now_musec); - uart.print_str("\r\n"); - } }