From c5fc830ff302097efa2c3614178431bf1a6da73a Mon Sep 17 00:00:00 2001
From: pca006132 <john.lck40@gmail.com>
Date: Mon, 3 Aug 2020 11:23:41 +0800
Subject: [PATCH] libsupport_zynq/abort: restart core1 main on core1 IRQ#0.

---
 libcortex_a9/src/asm.rs      |  9 +++++++++
 libsupport_zynq/src/abort.rs | 24 +++++++++++++++++++++---
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/libcortex_a9/src/asm.rs b/libcortex_a9/src/asm.rs
index b552028..7c365a9 100644
--- a/libcortex_a9/src/asm.rs
+++ b/libcortex_a9/src/asm.rs
@@ -39,3 +39,12 @@ pub fn isb() {
 pub unsafe fn enable_irq() {
     llvm_asm!("cpsie i":::: "volatile");
 }
+
+/// Exiting IRQ
+#[inline]
+pub unsafe fn exit_irq() {
+    llvm_asm!("
+        mrs r0, SPSR
+        msr CPSR, r0
+        " ::: "r0");
+}
diff --git a/libsupport_zynq/src/abort.rs b/libsupport_zynq/src/abort.rs
index 254a53c..be68c79 100644
--- a/libsupport_zynq/src/abort.rs
+++ b/libsupport_zynq/src/abort.rs
@@ -1,6 +1,12 @@
-use libregister::RegisterR;
-use libcortex_a9::regs::{DFSR, MPIDR};
-use libboard_zynq::{println, stdio};
+use libregister::{RegisterR, RegisterW};
+use libcortex_a9::regs::{DFSR, MPIDR, SP};
+use libcortex_a9::asm;
+use libboard_zynq::{println, stdio, gic, mpcore};
+
+extern "C" {
+    fn main_core1();
+    static mut __stack1_start: u32;
+}
 
 #[link_section = ".text.boot"]
 #[no_mangle]
@@ -54,6 +60,18 @@ pub unsafe extern "C" fn ReservedException() {
 #[no_mangle]
 #[naked]
 pub unsafe extern "C" fn IRQ() {
+    if MPIDR.read().cpu_id() == 1{
+        let mpcore = mpcore::RegisterBlock::new();
+        let mut gic = gic::InterruptController::new(mpcore);
+        let id = gic.get_interrupt_id();
+        if id.0 == 0 {
+            gic.end_interrupt(id);
+            asm::exit_irq();
+            SP.write(&mut __stack1_start as *mut _ as u32);
+            asm::enable_irq();
+            main_core1();
+        }
+    }
     stdio::drop_uart();
     println!("IRQ");
     loop {}