From be01fbd943e8755a8c6fa4d7a3897d08120af9fe Mon Sep 17 00:00:00 2001
From: pca006132 <jl@m-labs.hk>
Date: Mon, 18 Jan 2021 16:43:49 +0800
Subject: [PATCH] runtime/irq: use assembly for naked IRQ handler

and solves the bug introduced due to stack unwinding...
---
 src/runtime/src/irq.rs  | 24 +++++++++++++++++++++++-
 src/runtime/src/main.rs |  1 +
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/src/runtime/src/irq.rs b/src/runtime/src/irq.rs
index 84b6afea..0b9df77b 100644
--- a/src/runtime/src/irq.rs
+++ b/src/runtime/src/irq.rs
@@ -18,14 +18,36 @@ static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
 #[no_mangle]
 #[naked]
 pub unsafe extern "C" fn IRQ() {
+    asm!(
+        // setup SP, depending on CPU 0 or 1
+        "mrc p15, #0, r0, c0, c0, #5",
+        "movw r1, :lower16:__stack0_start",
+        "movt r1, :upper16:__stack0_start",
+        "tst r0, #3",
+        "movwne r1, :lower16:__stack1_start",
+        "movtne r1, :upper16:__stack1_start",
+        "mov sp, r1",
+        "bl __IRQ",
+        options(noreturn)
+    );
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn __IRQ() {
     if MPIDR.read().cpu_id() == 1 {
         let mpcore = mpcore::RegisterBlock::mpcore();
         let mut gic = gic::InterruptController::gic(mpcore);
         let id = gic.get_interrupt_id();
         if id.0 == 0 {
             gic.end_interrupt(id);
+            // save the SP and set it back after exiting IRQ
+            // exception unwinding expect to unwind from this function, as this is not the entrance
+            // function, maybe to IRQ which cannot further unwind...
+            // if we set the SP to __stack1_start, interesting exceptions would be triggered when
+            // we try to unwind the stack...
+            let v = SP.read();
             asm::exit_irq();
-            SP.write(&mut __stack1_start as *mut _ as u32);
+            SP.write(v);
             asm::enable_irq();
             CORE1_RESTART.store(false, Ordering::Relaxed);
             notify_spin_lock();
diff --git a/src/runtime/src/main.rs b/src/runtime/src/main.rs
index f8b7c6a7..5fa0f4af 100644
--- a/src/runtime/src/main.rs
+++ b/src/runtime/src/main.rs
@@ -7,6 +7,7 @@
 #![feature(const_btree_new)]
 #![feature(const_in_array_repeat_expressions)]
 #![feature(naked_functions)]
+#![feature(asm)]
 
 extern crate alloc;