Kernel: Discover initial exception level when booting Aarch64

When booting on RPI3 firmware puts CPU in EL2 mode which is
different from QEMU's default EL3.

I've added logic to discover initial mode at boot
and then act accordingly. This results in Serenity corectly
switching to EL1 on target hardware now.

4 files changed, 106 insertions, 21 deletions

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");
-    }
 }