2020-08-04 10:17:19 +08:00
|
|
|
use libboard_zynq::{gic, mpcore, println, stdio};
|
|
|
|
use libcortex_a9::{
|
2021-01-28 12:56:54 +08:00
|
|
|
asm, interrupt_handler,
|
|
|
|
regs::MPIDR,
|
2020-08-04 14:14:25 +08:00
|
|
|
spin_lock_yield, notify_spin_lock
|
2020-08-04 10:17:19 +08:00
|
|
|
};
|
2021-01-28 12:56:54 +08:00
|
|
|
use libregister::RegisterR;
|
2020-08-04 10:17:19 +08:00
|
|
|
use core::sync::atomic::{AtomicBool, Ordering};
|
|
|
|
|
|
|
|
extern "C" {
|
|
|
|
static mut __stack1_start: u32;
|
|
|
|
fn main_core1() -> !;
|
|
|
|
}
|
|
|
|
|
|
|
|
static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
|
|
|
|
|
2021-01-28 12:56:54 +08:00
|
|
|
interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
|
2020-08-04 10:17:19 +08:00
|
|
|
if MPIDR.read().cpu_id() == 1 {
|
2020-08-18 01:17:15 +08:00
|
|
|
let mpcore = mpcore::RegisterBlock::mpcore();
|
|
|
|
let mut gic = gic::InterruptController::gic(mpcore);
|
2020-08-04 10:17:19 +08:00
|
|
|
let id = gic.get_interrupt_id();
|
|
|
|
if id.0 == 0 {
|
|
|
|
gic.end_interrupt(id);
|
|
|
|
asm::exit_irq();
|
2021-01-28 12:56:54 +08:00
|
|
|
asm!("b core1_restart");
|
2020-08-04 10:17:19 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
stdio::drop_uart();
|
|
|
|
println!("IRQ");
|
|
|
|
loop {}
|
2021-01-28 12:56:54 +08:00
|
|
|
});
|
|
|
|
|
|
|
|
// This is actually not an interrupt handler, just use the macro for convenience.
|
|
|
|
// This function would be called in normal mode (instead of interrupt mode), the outer naked
|
|
|
|
// function wrapper is to tell libunwind to stop when it reaches here.
|
|
|
|
interrupt_handler!(core1_restart, core1_restart_impl, __stack0_start, __stack1_start, {
|
|
|
|
asm::enable_irq();
|
|
|
|
CORE1_RESTART.store(false, Ordering::Relaxed);
|
|
|
|
notify_spin_lock();
|
|
|
|
main_core1();
|
|
|
|
});
|
2020-08-04 10:17:19 +08:00
|
|
|
|
|
|
|
pub fn restart_core1() {
|
2020-08-18 01:17:15 +08:00
|
|
|
let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());
|
2020-08-04 10:17:19 +08:00
|
|
|
CORE1_RESTART.store(true, Ordering::Relaxed);
|
|
|
|
interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core1.into());
|
|
|
|
while CORE1_RESTART.load(Ordering::Relaxed) {
|
2020-08-04 14:14:25 +08:00
|
|
|
spin_lock_yield();
|
2020-08-04 10:17:19 +08:00
|
|
|
}
|
|
|
|
}
|