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