srenblad
/
zynq-rs
forked from M-Labs/zynq-rs
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
experiments/src/main.rs
View File

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

17
libcortex_a9/src/lib.rs
View File

@ -36,7 +36,9 @@ pub fn notify_spin_lock() {
} }
#[macro_export] #[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. /// - `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. /// - `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. /// - `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` is the body of the actual interrupt handler, should be a normal unsafe rust function
/// body. /// body.
/// ///
/// Note that the interrupt handler would use the same stack as normal programs by default, so /// Note that the interrupt handler would use the same stack as normal programs by default.
/// interrupt handlers should not return to normal program or it may corrupt the stack.
macro_rules! interrupt_handler { macro_rules! interrupt_handler {
($name:ident, $name2:ident, $stack0:ident, $stack1:ident, $body:block) => { ($name:ident, $name2:ident, $stack0:ident, $stack1:ident, $body:block) => {
#[link_section = ".text.boot"] #[link_section = ".text.boot"]
@ -54,19 +55,27 @@ macro_rules! interrupt_handler {
pub unsafe extern "C" fn $name() -> ! { pub unsafe extern "C" fn $name() -> ! {
asm!( asm!(
// setup SP, depending on CPU 0 or 1 // 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", "mrc p15, #0, r0, c0, c0, #5",
concat!("movw r1, :lower16:", stringify!($stack0)), concat!("movw r1, :lower16:", stringify!($stack0)),
concat!("movt r1, :upper16:", stringify!($stack0)), concat!("movt r1, :upper16:", stringify!($stack0)),
"tst r0, #3", "tst r0, #3",
concat!("movwne r1, :lower16:", stringify!($stack1)), concat!("movwne r1, :lower16:", stringify!($stack1)),
concat!("movtne r1, :upper16:", stringify!($stack1)), concat!("movtne r1, :upper16:", stringify!($stack1)),
"mov r0, sp",
"mov sp, r1", "mov sp, r1",
"push {{r0, r1}}", // 2 registers are pushed to maintain 8 byte stack alignment
concat!("bl ", stringify!($name2)), concat!("bl ", stringify!($name2)),
"pop {{r0, r1}}",
"mov sp, r0",
"ldmfd sp!, {{r0-r12, pc}}^", // caret ^ : copy SPSR to the CPSR
options(noreturn) options(noreturn)
); );
} }
#[no_mangle] #[no_mangle]
pub unsafe extern "C" fn $name2() -> ! $body pub unsafe extern "C" fn $name2() $body
}; };
} }