Kernel/aarch64: Execute kernel with SP_EL1 instead of SP_EL0

Until now the kernel was always executing with SP_EL0, as this made the
initial dropping to EL1 a bit easier. This commit changes this behaviour
to use the corresponding SP_ELx for each exception level.

To make sure that the execution of the C++ code can continue, the
current stack pointer is copied into the corresponding SP_ELx just
before dropping an exception level.

Kernel/Arch/aarch64/ASM_wrapper.h

@@ -75,7 +75,11 @@ inline void load_el1_vector_table(void* vector_table)
 
 inline void enter_el2_from_el3()
 {
-    asm volatile("    adr x0, entered_el2\n"
+    // NOTE: This also copies the current stack pointer into SP_EL2, as
+    //       the processor is set up to use SP_EL2 when jumping into EL2.
+    asm volatile("    mov x0, sp\n"
+                 "    msr sp_el2, x0\n"
+                 "    adr x0, entered_el2\n"
                  "    msr elr_el3, x0\n"
                  "    eret\n"
                  "entered_el2:" ::
@@ -84,7 +88,11 @@ inline void enter_el2_from_el3()
 
 inline void enter_el1_from_el2()
 {
-    asm volatile("    adr x0, entered_el1\n"
+    // NOTE: This also copies the current stack pointer into SP_EL1, as
+    //       the processor is set up to use SP_EL1 when jumping into EL1.
+    asm volatile("    mov x0, sp\n"
+                 "    msr sp_el1, x0\n"
+                 "    adr x0, entered_el1\n"
                  "    msr elr_el2, x0\n"
                  "    eret\n"
                  "entered_el1:" ::

Kernel/Arch/aarch64/Exceptions.cpp

@@ -34,7 +34,7 @@ static void drop_el3_to_el2()
     saved_program_status_register_el3.D = 1;
 
     // Indicate EL1 as exception origin mode (so we go back there)
-    saved_program_status_register_el3.M = Aarch64::SPSR_EL3::Mode::EL2t;
+    saved_program_status_register_el3.M = Aarch64::SPSR_EL3::Mode::EL2h;
 
     // Set the register
     Aarch64::SPSR_EL3::write(saved_program_status_register_el3);
@@ -49,10 +49,6 @@ static void drop_el2_to_el1()
     hypervisor_configuration_register_el2.RW = 1; // EL1 to use 64-bit mode
     Aarch64::HCR_EL2::write(hypervisor_configuration_register_el2);
 
-    // Set up initial exception stack
-    // FIXME: Define in linker script
-    Aarch64::Asm::set_sp_el1(0x40000);
-
     Aarch64::SPSR_EL2 saved_program_status_register_el2 = {};
 
     // Mask (disable) all interrupts
@@ -61,7 +57,7 @@ static void drop_el2_to_el1()
     saved_program_status_register_el2.F = 1;
 
     // Indicate EL1 as exception origin mode (so we go back there)
-    saved_program_status_register_el2.M = Aarch64::SPSR_EL2::Mode::EL1t;
+    saved_program_status_register_el2.M = Aarch64::SPSR_EL2::Mode::EL1h;
 
     Aarch64::SPSR_EL2::write(saved_program_status_register_el2);
     Aarch64::Asm::enter_el1_from_el2();

Kernel/Arch/aarch64/Processor.cpp

@@ -252,9 +252,9 @@ FlatPtr Processor::init_context(Thread& thread, bool leave_crit)
     saved_program_status_register_el1.I = 0;
     saved_program_status_register_el1.F = 0;
 
-    // Set exception origin mode to EL1t, so when the context is restored, we'll be executing in EL1 with SP_EL0
+    // Set exception origin mode to EL1h, so when the context is restored, we'll be executing in EL1 with SP_EL1
     // FIXME: This must be EL0t when aarch64 supports userspace applications.
-    saved_program_status_register_el1.M = Aarch64::SPSR_EL1::Mode::EL1t;
+    saved_program_status_register_el1.M = Aarch64::SPSR_EL1::Mode::EL1h;
     memcpy(&eretframe.spsr_el1, &saved_program_status_register_el1, sizeof(u64));
 
     // Push a TrapFrame onto the stack

Kernel/Arch/aarch64/boot.S

@@ -18,7 +18,6 @@ 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.
-  msr SPSel, #0   //Use the same SP as we descend into EL1
   adrp x14, start
   add x14, x14, :lo12:start
   mov sp, x14

Kernel/Arch/aarch64/vector_table.S

@@ -6,7 +6,7 @@
 
 .section .text.vector_table
 
-#define TRAP_FRAME_SIZE     272
+#define REGISTER_STATE_SIZE 272
 #define SPSR_EL1_SLOT       (31 * 8)
 #define ELR_EL1_SLOT        (32 * 8)
 #define TPIDR_EL0_SLOT      (33 * 8)
@@ -34,7 +34,7 @@
 //
 .macro save_current_context
     // Allocate stack space for Trap Frame
-    sub sp, sp, #TRAP_FRAME_SIZE
+    sub sp, sp, #REGISTER_STATE_SIZE
 
     stp x0, x1,     [sp, #(0 * 0)]
     stp x2, x3,     [sp, #(2 * 8)]
@@ -60,10 +60,12 @@
     str x0, [sp, #ELR_EL1_SLOT]
     mrs x0, tpidr_el0
     str x0, [sp, #TPIDR_EL0_SLOT]
+    mrs x0, sp_el0
+    str x0, [sp, #SP_EL0_SLOT]
 
     // Set up TrapFrame struct on the stack
-    sub sp, sp, #16
     mov x0, sp
+    sub sp, sp, #16
     str x0, [sp, #(1 * 8)]
     str xzr, [sp, #(0 * 0)]
 
@@ -83,6 +85,8 @@
     msr elr_el1, x0
     ldr x0, [sp, #TPIDR_EL0_SLOT]
     msr tpidr_el0, x0
+    ldr x0, [sp, #SP_EL0_SLOT]
+    msr sp_el0, x0
 
     ldp x0, x1,     [sp, #(0 * 0)]
     ldp x2, x3,     [sp, #(2 * 8)]
@@ -101,7 +105,7 @@
     ldp x28, x29,   [sp, #(28 * 8)]
     ldr x30,        [sp, #(30 * 8)]
 
-    add sp, sp, #TRAP_FRAME_SIZE
+    add sp, sp, #REGISTER_STATE_SIZE
 .endm
 
 .global vector_table_el1
@@ -143,7 +147,7 @@ synchronous_current_elsp_elx:
 
 irq_current_elsp_elx:
     save_current_context
-    bl exception_common
+    bl handle_interrupt
     restore_previous_context
     eret
 
@@ -166,10 +170,6 @@ synchronous_current_elsp_el0:
     eret
 
 irq_current_elsp_el0:
-    // An IRQ will always switch the stack pointer to SP_EL1, however we want to use SP_EL0, so switch
-    // to SP_EL0. This means that the stack of the currently executing thread is used as the irq stack.
-    msr SPSel, #0
-
     save_current_context
     bl handle_interrupt
     restore_previous_context