libsupport_zynq/abort: restart core1 main on core1 IRQ#0.

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");
+}

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 {}