forked from M-Labs/zynq-rs
1
0
Fork 0

libcortex_a9: add interrupt exit support for interrupt_handler macro (#107)

Co-authored-by: morgan <mc@m-labs.hk>
Co-committed-by: morgan <mc@m-labs.hk>
This commit is contained in:
morgan 2023-11-20 12:30:27 +08:00 committed by sb10q
parent be672ab662
commit 24c804e6f0
2 changed files with 40 additions and 17 deletions

View File

@ -56,19 +56,29 @@ extern "C" {
static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
if MPIDR.read().cpu_id() == 1{
let mpcore = mpcore::RegisterBlock::mpcore();
let mut gic = gic::InterruptController::gic(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();
CORE1_RESTART.store(false, Ordering::Relaxed);
notify_spin_lock();
main_core1();
}
let mpcore = mpcore::RegisterBlock::mpcore();
let mut gic = gic::InterruptController::gic(mpcore);
let id = gic.get_interrupt_id();
match MPIDR.read().cpu_id(){
0 => {
if id.0 == 0 {
println!("Interrupting core0...");
gic.end_interrupt(id);
return;
}
},
1 => {
if id.0 == 0 {
gic.end_interrupt(id);
asm::exit_irq();
SP.write(&mut __stack1_start as *mut _ as u32);
asm::enable_irq();
CORE1_RESTART.store(false, Ordering::Relaxed);
notify_spin_lock();
main_core1();
}
},
_ => {}
}
stdio::drop_uart();
println!("IRQ");
@ -134,6 +144,10 @@ pub fn main_core0() {
ddr.memtest();
ram::init_alloc_ddr(&mut ddr);
info!("Send software interrupt to core0");
interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core0.into());
info!("Core0 returned from interrupt");
boot::Core1::start(false);
let core1_req = unsafe { &mut CORE1_REQ.0 };

View File

@ -36,7 +36,9 @@ pub fn notify_spin_lock() {
}
#[macro_export]
/// Interrupt handler, which setup the stack and jump to actual interrupt handler.
/// Interrupt handler, which setup the stack and preserve registers before jumping to actual interrupt handler.
/// Registers r0-r12, PC, SP and CPSR are restored after the actual handler.
///
/// - `name` is the name of the interrupt, should be the same as the one defined in vector table.
/// - `name2` is the name for the actual handler, should be different from name.
/// - `stack0` is the stack for the interrupt handler when called from core0.
@ -44,8 +46,7 @@ pub fn notify_spin_lock() {
/// - `body` is the body of the actual interrupt handler, should be a normal unsafe rust function
/// body.
///
/// Note that the interrupt handler would use the same stack as normal programs by default, so
/// interrupt handlers should not return to normal program or it may corrupt the stack.
/// Note that the interrupt handler would use the same stack as normal programs by default.
macro_rules! interrupt_handler {
($name:ident, $name2:ident, $stack0:ident, $stack1:ident, $body:block) => {
#[link_section = ".text.boot"]
@ -54,19 +55,27 @@ macro_rules! interrupt_handler {
pub unsafe extern "C" fn $name() -> ! {
asm!(
// setup SP, depending on CPU 0 or 1
// and preserve registers
"sub lr, lr, #4",
"stmfd sp!, {{r0-r12, lr}}",
"mrc p15, #0, r0, c0, c0, #5",
concat!("movw r1, :lower16:", stringify!($stack0)),
concat!("movt r1, :upper16:", stringify!($stack0)),
"tst r0, #3",
concat!("movwne r1, :lower16:", stringify!($stack1)),
concat!("movtne r1, :upper16:", stringify!($stack1)),
"mov r0, sp",
"mov sp, r1",
"push {{r0, r1}}", // 2 registers are pushed to maintain 8 byte stack alignment
concat!("bl ", stringify!($name2)),
"pop {{r0, r1}}",
"mov sp, r0",
"ldmfd sp!, {{r0-r12, pc}}^", // caret ^ : copy SPSR to the CPSR
options(noreturn)
);
}
#[no_mangle]
pub unsafe extern "C" fn $name2() -> ! $body
pub unsafe extern "C" fn $name2() $body
};
}