From 9b8abfc5d70c9adb6d815e6d89abb50c62d8c776 Mon Sep 17 00:00:00 2001
From: Jeremy Soller <jackpot51@gmail.com>
Date: Sat, 20 Aug 2022 13:48:13 -0600
Subject: [PATCH] Copy exception handlers from early_init

---
 src/arch/aarch64/mod.rs     |   5 +-
 src/arch/aarch64/start.rs   |  12 ++++
 src/arch/aarch64/vectors.rs | 128 ++++++++++++++++++++++++++++++++++++
 3 files changed, 144 insertions(+), 1 deletion(-)
 create mode 100644 src/arch/aarch64/vectors.rs

diff --git a/src/arch/aarch64/mod.rs b/src/arch/aarch64/mod.rs
index 397865c..1fd6289 100644
--- a/src/arch/aarch64/mod.rs
+++ b/src/arch/aarch64/mod.rs
@@ -27,7 +27,10 @@ pub mod start;
 /// Stop function
 pub mod stop;
 
+// Interrupt vectors
+pub mod vectors;
+
 /// Early init support
 pub mod init;
 
-pub use ::rmm::AArch64Arch as CurrentRmmArch;
\ No newline at end of file
+pub use ::rmm::AArch64Arch as CurrentRmmArch;
diff --git a/src/arch/aarch64/start.rs b/src/arch/aarch64/start.rs
index 9a187ad..102cf98 100644
--- a/src/arch/aarch64/start.rs
+++ b/src/arch/aarch64/start.rs
@@ -121,6 +121,18 @@ pub unsafe extern fn kstart(args_ptr: *const KernelArgs) -> ! {
         info!("Bootstrap: {:X}:{:X}", args.bootstrap_base, args.bootstrap_base + args.bootstrap_size);
         info!("Bootstrap entry point: {:X}", args.bootstrap_entry);
 
+        // Setup interrupt handlers
+        extern "C" {
+            fn exception_vector_base();
+        }
+        core::arch::asm!(
+            "
+            ldr x0, =exception_vector_base
+            msr vbar_el1, x0
+            ",
+            out("x0") _,
+        );
+
         /* NOT USED WITH UEFI
         device_tree::fill_memory_map(crate::PHYS_OFFSET + dtb_base, dtb_size);
 
diff --git a/src/arch/aarch64/vectors.rs b/src/arch/aarch64/vectors.rs
new file mode 100644
index 0000000..340cbdf
--- /dev/null
+++ b/src/arch/aarch64/vectors.rs
@@ -0,0 +1,128 @@
+core::arch::global_asm!(
+"
+    //  Exception vector stubs
+    //
+    //  The hex values in x18 are to aid debugging
+    //  Unhandled exceptions spin in a wfi loop for the moment
+    //  This can be macro-ified
+
+.globl exception_vector_base
+
+    .align 11
+exception_vector_base:
+
+    // Synchronous
+    .align 7
+__vec_00:
+    mov     x18, #0xb0b0
+    b       synchronous_exception_at_el1_with_sp0
+    b       __vec_00
+
+    // IRQ
+    .align 7
+__vec_01:
+    mov     x18, #0xb0b1
+    b       irq_at_el1
+    b       __vec_01
+
+    // FIQ
+    .align 7
+__vec_02:
+    mov     x18, #0xb0b2
+    b       unhandled_exception
+    b       __vec_02
+
+    // SError
+    .align 7
+__vec_03:
+    mov     x18, #0xb0b3
+    b       unhandled_exception
+    b       __vec_03
+
+    // Synchronous
+    .align 7
+__vec_04:
+    mov     x18, #0xb0b4
+    b       synchronous_exception_at_el1_with_spx
+    b       __vec_04
+
+    // IRQ
+    .align 7
+__vec_05:
+    mov     x18, #0xb0b5
+    b       irq_at_el1
+    b       __vec_05
+
+    // FIQ
+    .align 7
+__vec_06:
+    mov     x18, #0xb0b6
+    b       unhandled_exception
+    b       __vec_06
+
+    // SError
+    .align 7
+__vec_07:
+    mov     x18, #0xb0b7
+    b       unhandled_exception
+    b       __vec_07
+
+    // Synchronous
+    .align 7
+__vec_08:
+    mov     x18, #0xb0b8
+    b       synchronous_exception_at_el0
+    b       __vec_08
+
+    // IRQ
+    .align 7
+__vec_09:
+    mov     x18, #0xb0b9
+    b       irq_at_el0
+    b       __vec_09
+
+    // FIQ
+    .align 7
+__vec_10:
+    mov     x18, #0xb0ba
+    b       unhandled_exception
+    b       __vec_10
+
+    // SError
+    .align 7
+__vec_11:
+    mov     x18, #0xb0bb
+    b       unhandled_exception
+    b       __vec_11
+
+    // Synchronous
+    .align 7
+__vec_12:
+    mov     x18, #0xb0bc
+    b       unhandled_exception
+    b       __vec_12
+
+    // IRQ
+    .align 7
+__vec_13:
+    mov     x18, #0xb0bd
+    b       unhandled_exception
+    b       __vec_13
+
+    // FIQ
+    .align 7
+__vec_14:
+    mov     x18, #0xb0be
+    b       unhandled_exception
+    b       __vec_14
+
+    // SError
+    .align 7
+__vec_15:
+    mov     x18, #0xb0bf
+    b       unhandled_exception
+    b       __vec_15
+    
+    .align 7
+exception_vector_end:
+");