Kernel: Switch processor to EL1 immediately after boot on Aarch64

4 files changed, 321 insertions, 16 deletions

diff --git a/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.S b/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.S
index b5eba4784d..8f119ef914 100644
--- a/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.S
+++ b/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.S
@@ -22,3 +22,19 @@ Lstart:
   subs x0, x0, #1
   bne Lstart
   ret
+
+.global return_from_el3
+.type return_from_el3, @function
+return_from_el3:
+  adr x0, start_in_el1
+  msr elr_el3, x0
+  eret
+
+start_in_el1:
+  // 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.
+  ldr x14, =start
+  mov sp, x14
+
+  b os_start
diff --git a/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.h b/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.h
index 55d115e2ac..5ac68d2399 100644
--- a/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.h
+++ b/Kernel/Prekernel/Arch/aarch64/Aarch64_asm_utils.h
@@ -6,5 +6,10 @@
 
 #pragma once
 
+#include "AarchRegisters.h"
+
 extern "C" uint8_t get_current_exception_level();
 extern "C" void wait_cycles(int n);
+
+// CPU initialization functions
+extern "C" [[noreturn]] void return_from_el3();
diff --git a/Kernel/Prekernel/Arch/aarch64/AarchRegisters.h b/Kernel/Prekernel/Arch/aarch64/AarchRegisters.h
new file mode 100644
index 0000000000..49271b2ed6
--- /dev/null
+++ b/Kernel/Prekernel/Arch/aarch64/AarchRegisters.h
@@ -0,0 +1,181 @@
+/*
+ * Copyright (c) 2021, Marcin Undak <mcinek@gmail.com>
+ *
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#pragma once
+
+namespace Kernel {
+struct Aarch64_SCTLR_EL1 {
+    int M : 1;
+    int A : 1;
+    int C : 1;
+    int SA : 1;
+    int SA0 : 1;
+    int CP15BEN : 1;
+    int _reserved6 : 1 = 0;
+    int ITD : 1;
+    int SED : 1;
+    int UMA : 1;
+    int _reserved10 : 1 = 0;
+    int _reserved11 : 1 = 1;
+    int I : 1;
+    int EnDB : 1;
+    int DZE : 1;
+    int UCT : 1;
+    int nTWI : 1;
+    int _reserved17 : 1 = 0;
+    int nTWE : 1;
+    int WXN : 1;
+    int _reserved20 : 1 = 1;
+    int IESB : 1;
+    int _reserved22 : 1 = 1;
+    int SPAN : 1;
+    int E0E : 1;
+    int EE : 1;
+    int UCI : 1;
+    int EnDA : 1;
+    int nTLSMD : 1;
+    int LSMAOE : 1;
+    int EnIB : 1;
+    int EnIA : 1;
+    int _reserved32 : 3 = 0;
+    int BT0 : 1;
+    int BT1 : 1;
+    int ITFSB : 1;
+    int TCF0 : 2;
+    int TCF : 2;
+    int ATA0 : 1;
+    int ATA : 1;
+    int DSSBS : 1;
+    int TWEDEn : 1;
+    int TWEDEL : 4;
+    int _reserved50 : 4 = 0;
+    int EnASR : 1;
+    int EnAS0 : 1;
+    int EnALS : 1;
+    int EPAN : 1;
+    int _reserved58 : 6 = 0;
+};
+static_assert(sizeof(Aarch64_SCTLR_EL1) == 8);
+
+struct Aarch64_HCR_EL2 {
+    int VM : 1;
+    int SWIO : 1;
+    int PTW : 1;
+    int FMO : 1;
+    int IMO : 1;
+    int AMO : 1;
+    int VF : 1;
+    int VI : 1;
+    int VSE : 1;
+    int FB : 1;
+    int BSU : 2;
+    int DC : 1;
+    int TWI : 1;
+    int TWE : 1;
+    int TID0 : 1;
+    int TID1 : 1;
+    int TID2 : 1;
+    int TID3 : 1;
+    int TSC : 1;
+    int TIPDCP : 1;
+    int TACR : 1;
+    int TSW : 1;
+    int TPCF : 1;
+    int TPU : 1;
+    int TTLB : 1;
+    int TVM : 1;
+    int TGE : 1;
+    int TDZ : 1;
+    int HCD : 1;
+    int TRVM : 1;
+    int RW : 1;
+    int CD : 1;
+    int ID : 1;
+    int E2H : 1;
+    int TLOR : 1;
+    int TERR : 1;
+    int MIOCNCE : 1;
+    int _reserved39 : 1 = 0;
+    int APK : 1 = 0;
+    int API : 1 = 0;
+    int NV : 1 = 0;
+    int NV1 : 1 = 0;
+    int AT : 1 = 0;
+    int _reserved45 : 18 = 0;
+};
+static_assert(sizeof(Aarch64_HCR_EL2) == 8);
+
+struct Aarch64_SCR_EL3 {
+    int NS : 1;
+    int IRQ : 1;
+    int FIQ : 1;
+    int EA : 1;
+    int _reserved4 : 1 = 1;
+    int _reserved5 : 1 = 1;
+    int _reserved6 : 1 = 0;
+    int SMD : 1;
+    int HCE : 1;
+    int SIF : 1;
+    int RW : 1;
+    int ST : 1;
+    int TWI : 1;
+    int TWE : 1;
+    int TLOR : 1;
+    int TERR : 1;
+    int APK : 1;
+    int API : 1;
+    int EEL2 : 1;
+    int EASE : 1;
+    int NMEA : 1;
+    int FIEN : 1;
+    int _reserved22 : 3 = 0;
+    int EnSCXT : 1;
+    int ATA : 1;
+    int FGTEn : 1;
+    int ECVEn : 1;
+    int TWEDEn : 1;
+    int TWEDEL : 4;
+    int _reserved34 : 1 = 0;
+    int AMVOFFEN : 1;
+    int EnAS0 : 1;
+    int ADEn : 1;
+    int HXEn : 1;
+    int _reserved39 : 14 = 0;
+};
+static_assert(sizeof(Aarch64_SCR_EL3) == 8);
+
+struct Aarch64_SPSR_EL3 {
+    enum Mode : uint16_t {
+        EL0t = 0b0000,
+        EL1t = 0b0100,
+        EL1h = 0b0101,
+        EL2t = 0b1000,
+        EL2h = 0b1001,
+        EL3t = 0b1100,
+        EL3h = 0b1101
+    };
+
+    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 _reserved10 : 10 = 0;
+    int IL : 1;
+    int SS : 1;
+    int PAN : 1;
+    int UA0 : 1;
+    int _reserved24 : 4 = 0;
+    int V : 1;
+    int C : 1;
+    int Z : 1;
+    int N : 1;
+    int _reserved32 : 32 = 0;
+};
+static_assert(sizeof(Aarch64_SPSR_EL3) == 8);
+}
diff --git a/Kernel/Prekernel/Arch/aarch64/init.cpp b/Kernel/Prekernel/Arch/aarch64/init.cpp
index a478bd98c4..0bfd261fc8 100644
--- a/Kernel/Prekernel/Arch/aarch64/init.cpp
+++ b/Kernel/Prekernel/Arch/aarch64/init.cpp
@@ -12,8 +12,14 @@
 #include <Kernel/Prekernel/Arch/aarch64/UART.h>
 
 extern "C" [[noreturn]] void halt();
-
 extern "C" [[noreturn]] void init();
+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();
+
 extern "C" [[noreturn]] void init()
 {
     auto& uart = Prekernel::UART::the();
@@ -27,22 +33,11 @@ extern "C" [[noreturn]] void init()
     uart.print_num(firmware_version);
     uart.print_str("\r\n");
 
-    auto exception_level = get_current_exception_level();
-    uart.print_str("Current CPU exception level: EL");
-    uart.print_num(exception_level);
-    uart.print_str("\r\n");
+    set_up_el3_mode();
+    set_up_el2_mode();
+    set_up_el1_mode();
 
-    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");
-    }
+    switch_to_el1();
 }
 
 // FIXME: Share this with the Intel Prekernel.
@@ -66,3 +61,111 @@ void __stack_chk_fail()
 {
     halt();
 }
+
+static void set_up_el1_mode()
+{
+    Kernel::Aarch64_SCTLR_EL1 sctlr_el1 = {};
+
+    // Those bits are reserved on ARMv8.0
+    sctlr_el1.LSMAOE = 1;
+    sctlr_el1.nTLSMD = 1;
+    sctlr_el1.SPAN = 1;
+    sctlr_el1.IESB = 1;
+
+    // Don't trap access to CTR_EL0
+    sctlr_el1.UCT = 1;
+
+    // Don't trap WFE instructions
+    sctlr_el1.nTWE = 1;
+
+    // Don't trap WFI instructions
+    sctlr_el1.nTWI = 1;
+
+    // Don't trap DC ZVA instructions
+    sctlr_el1.DZE = 1;
+
+    // Don't trap access to DAIF (debugging) flags of EFLAGS register
+    sctlr_el1.UMA = 1;
+
+    // Enable stack access alignment check for EL0
+    sctlr_el1.SA0 = 1;
+
+    // Enable stack access alignment check for EL1
+    sctlr_el1.SA = 1;
+
+    // Enable memory access alignment check
+    sctlr_el1.A = 1;
+
+    // Set the register
+    asm("msr sctlr_el1, %[value]" ::[value] "r"(sctlr_el1));
+}
+
+static void set_up_el2_mode()
+{
+    Kernel::Aarch64_HCR_EL2 hcr_el2 = {};
+
+    // EL1 to use 64-bit mode
+    hcr_el2.RW = 1;
+
+    // Set the register
+    asm("msr hcr_el2, %[value]" ::[value] "r"(hcr_el2));
+}
+
+static void set_up_el3_mode()
+{
+    Kernel::Aarch64_SCR_EL3 scr_el3 = {};
+
+    // Don't trap access to Counter-timer Physical Secure registers
+    scr_el3.ST = 1;
+
+    // Lower level to use Aarch64
+    scr_el3.RW = 1;
+
+    // Enable Hypervisor instructions at all levels
+    scr_el3.HCE = 1;
+
+    // Set the register
+    asm("msr scr_el3, %[value]" ::[value] "r"(scr_el3));
+}
+
+[[noreturn]] static void switch_to_el1()
+{
+    // Processor state to set when returned from this function (in new EL1 world)
+    Kernel::Aarch64_SPSR_EL3 spsr_el3 = {};
+
+    // Mask (disable) all interrupts
+    spsr_el3.A = 1;
+    spsr_el3.I = 1;
+    spsr_el3.F = 1;
+
+    // Indicate EL1 as exception origin mode (so we go back there)
+    spsr_el3.M = Kernel::Aarch64_SPSR_EL3::Mode::EL1h;
+
+    // Set the register
+    asm("msr spsr_el3, %[value]" ::[value] "r"(spsr_el3));
+
+    // This will jump into os_start() below, but in EL1
+    return_from_el3();
+}
+
+extern "C" [[noreturn]] void os_start()
+{
+    auto& uart = Prekernel::UART::the();
+
+    auto exception_level = get_current_exception_level();
+    uart.print_str("Current CPU exception level: 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");
+    }
+}