Prekernel: Handle synchronous EL1 exceptions in C++ on aarch64

We now have a mechanism to save the current CPU context to the stack,
and then pass that to the C++ common exception handler.

2 files changed, 139 insertions, 7 deletions

diff --git a/Kernel/Prekernel/Arch/aarch64/init.cpp b/Kernel/Prekernel/Arch/aarch64/init.cpp
index e031e57c3c..8c6f6a2add 100644
--- a/Kernel/Prekernel/Arch/aarch64/init.cpp
+++ b/Kernel/Prekernel/Arch/aarch64/init.cpp
@@ -23,8 +23,17 @@ static u32 query_firmware_version();
 
 extern "C" void wait_cycles(int n);
 
+struct TrapFrame {
+    u64 x[31];     // Saved general purpose registers
+    u64 spsr_el1;  // Save Processor Status Register, EL1
+    u64 elr_el1;   // Exception Link Reigster, EL1
+    u64 tpidr_el1; // EL0 thread ID
+    u64 sp_el0;    // EL0 stack pointer
+};
+
 extern "C" [[noreturn]] void halt();
 extern "C" [[noreturn]] void init();
+extern "C" void exception_common(TrapFrame const* const trap_frame);
 
 extern "C" [[noreturn]] void init()
 {
@@ -50,9 +59,6 @@ extern "C" [[noreturn]] void init()
     extern uintptr_t vector_table_el1;
     el1_vector_table_install(&vector_table_el1);
 
-    // Set the register
-    asm("msr sctlr_el1, %[value]" ::[value] "r"(system_control_register_el1));
-
     uart.print_str("Initialize MMU\r\n");
     Prekernel::init_prekernel_page_tables();
 
@@ -91,6 +97,41 @@ void __stack_chk_fail()
     Prekernel::halt();
 }
 
+extern "C" void exception_common(TrapFrame const* const trap_frame)
+{
+    static constexpr bool print_stack_frame = true;
+
+    if constexpr (print_stack_frame) {
+        auto& uart = Prekernel::UART::the();
+
+        uart.print_str("Exception Generated by processor!\n");
+        for (auto reg = 0; reg < 31; reg++) {
+            uart.print_str("x");
+            uart.print_num(reg);
+            uart.print_str(": ");
+            uart.print_hex(trap_frame->x[reg]);
+            uart.print_str("\r\n");
+        }
+
+        // Special registers
+        uart.print_str("spsr_el1: ");
+        uart.print_hex(trap_frame->spsr_el1);
+        uart.print_str("\r\n");
+
+        uart.print_str("elr_el1: ");
+        uart.print_hex(trap_frame->elr_el1);
+        uart.print_str("\r\n");
+
+        uart.print_str("tpidr_el1: ");
+        uart.print_hex(trap_frame->tpidr_el1);
+        uart.print_str("\r\n");
+
+        uart.print_str("sp_el0: ");
+        uart.print_hex(trap_frame->sp_el0);
+        uart.print_str("\r\n");
+    }
+}
+
 class QueryFirmwareVersionMboxMessage : Prekernel::Mailbox::Message {
 public:
     u32 version;
diff --git a/Kernel/Prekernel/Arch/aarch64/vector_table.S b/Kernel/Prekernel/Arch/aarch64/vector_table.S
index 92886818d8..826b38ba56 100644
--- a/Kernel/Prekernel/Arch/aarch64/vector_table.S
+++ b/Kernel/Prekernel/Arch/aarch64/vector_table.S
@@ -6,6 +6,12 @@
 
 .section .text.vector_table
 
+#define TRAP_FRAME_SIZE     272
+#define SPSR_EL1_SLOT       (31 * 8)
+#define ELR_EL1_SLOT        (32 * 8)
+#define TPIDR_EL0_SLOT      (33 * 8)
+#define SP_EL0_SLOT         (34 * 8)
+
 // Vector Table Entry macro. Each entry is aligned at 128 bytes, meaning we have
 // at most that many instructions.
 .macro table_entry label
@@ -19,6 +25,79 @@
     b .
 .endm
 
+.extern exception_common
+
+//
+// Save all register states to the current stack
+// and enter the C++ exception handler
+//
+.macro save_current_context
+    // Allocate stack space for Trap Frame
+    sub sp, sp, #TRAP_FRAME_SIZE
+
+    stp x0, x1,     [sp, #(0 * 0)]
+    stp x2, x3,     [sp, #(2 * 8)]
+    stp x4, x5,     [sp, #(4 * 8)]
+    stp x6, x7,     [sp, #(6 * 8)]
+    stp x8, x9,     [sp, #(8 * 8)]
+    stp x10, x11,   [sp, #(10 * 8)]
+    stp x12, x13,   [sp, #(12 * 8)]
+    stp x14, x15,   [sp, #(14 * 8)]
+    stp x16, x17,   [sp, #(16 * 8)]
+    stp x18, x19,   [sp, #(18 * 8)]
+    stp x20, x21,   [sp, #(20 * 8)]
+    stp x22, x23,   [sp, #(22 * 8)]
+    stp x24, x25,   [sp, #(24 * 8)]
+    stp x26, x27,   [sp, #(26 * 8)]
+    stp x28, x29,   [sp, #(28 * 8)]
+    str x30,        [sp, #(30 * 8)]
+
+    // Let's save some special registers
+    mrs x0, spsr_el1
+    str x0, [sp, #SPSR_EL1_SLOT]
+    mrs x0, elr_el1
+    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]
+
+    // Move stack pointer into first argument register
+    // and jump to the C++ exception handler
+    mov x0, sp
+.endm
+
+.macro restore_previous_context
+    // Restore special registers first
+    ldr x0, [sp, #SPSR_EL1_SLOT]
+    msr spsr_el1, x0
+    ldr x0, [sp, #ELR_EL1_SLOT]
+    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)]
+    ldp x4, x5,     [sp, #(4 * 8)]
+    ldp x6, x7,     [sp, #(6 * 8)]
+    ldp x8, x9,     [sp, #(8 * 8)]
+    ldp x10, x11,   [sp, #(10 * 8)]
+    ldp x12, x13,   [sp, #(12 * 8)]
+    ldp x14, x15,   [sp, #(14 * 8)]
+    ldp x16, x17,   [sp, #(16 * 8)]
+    ldp x18, x19,   [sp, #(18 * 8)]
+    ldp x20, x21,   [sp, #(20 * 8)]
+    ldp x22, x23,   [sp, #(22 * 8)]
+    ldp x24, x25,   [sp, #(24 * 8)]
+    ldp x26, x27,   [sp, #(26 * 8)]
+    ldp x28, x29,   [sp, #(28 * 8)]
+    ldr x30,        [sp, #(30 * 8)]
+
+    add sp, sp, #TRAP_FRAME_SIZE
+.endm
+
 .global vector_table_el1
 .weak vector_table_el1 // Vector table is weak in case someone wants to hook us in C++ land :^)
 .type vector_table_el1, @object
@@ -51,13 +130,25 @@ vector_table_el1:
     unimplemented_entry
 
 synchronous_current_elsp_elx:
-    b .
+    save_current_context
+    bl exception_common
+    restore_previous_context
+    eret
 
 irq_current_elsp_elx:
-    b .
+    save_current_context
+    bl exception_common
+    restore_previous_context
+    eret
 
 fiq_current_elsp_elx:
-    b .
+    save_current_context
+    bl exception_common
+    restore_previous_context
+    eret
 
 system_error_current_elsp_elx:
-    b .
+    save_current_context
+    bl exception_common
+    restore_previous_context
+    eret