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

This commit is contained in:
pca006132 2020-08-03 11:23:41 +08:00 committed by Gitea
parent c1f61b5673
commit 8f0a6bd5ea
2 changed files with 30 additions and 3 deletions

View File

@ -39,3 +39,12 @@ pub fn isb() {
pub unsafe fn enable_irq() { pub unsafe fn enable_irq() {
llvm_asm!("cpsie i":::: "volatile"); llvm_asm!("cpsie i":::: "volatile");
} }
/// Exiting IRQ
#[inline]
pub unsafe fn exit_irq() {
llvm_asm!("
mrs r0, SPSR
msr CPSR, r0
" ::: "r0");
}

View File

@ -1,6 +1,12 @@
use libregister::RegisterR; use libregister::{RegisterR, RegisterW};
use libcortex_a9::regs::{DFSR, MPIDR}; use libcortex_a9::regs::{DFSR, MPIDR, SP};
use libboard_zynq::{println, stdio}; 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"] #[link_section = ".text.boot"]
#[no_mangle] #[no_mangle]
@ -54,6 +60,18 @@ pub unsafe extern "C" fn ReservedException() {
#[no_mangle] #[no_mangle]
#[naked] #[naked]
pub unsafe extern "C" fn IRQ() { 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(); stdio::drop_uart();
println!("IRQ"); println!("IRQ");
loop {} loop {}