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

This commit is contained in:
pca006132 2020-08-03 11:23:41 +08:00
parent 9dd946c0d6
commit c5fc830ff3
2 changed files with 30 additions and 3 deletions

View File

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

View File

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