experiments/link.x

@@ -34,6 +34,20 @@ SECTIONS
         __bss_end = .;
     } > OCM3
 
+    .irq_stack1 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack1_end = .;
+        . += 0x100;
+        __irq_stack1_start = .;
+    } > OCM3
+
+    .irq_stack0 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack0_end = .;
+        . += 0x100;
+        __irq_stack0_start = .;
+    } > OCM3
+
     .stack1 (NOLOAD) : ALIGN(8) {
         __stack1_end = .;
         . += 0x200;

experiments/src/main.rs

@@ -1,6 +1,8 @@
 #![no_std]
 #![no_main]
 #![feature(const_in_array_repeat_expressions)]
+#![feature(naked_functions)]
+#![feature(asm)]
 
 extern crate alloc;
 
@@ -33,7 +35,7 @@ use libcortex_a9::{
     sync_channel,
     regs::{MPIDR, SP},
     spin_lock_yield, notify_spin_lock,
-    asm
+    asm, interrupt_handler
 };
 use libregister::{RegisterR, RegisterW};
 use libsupport_zynq::{
@@ -53,9 +55,7 @@ extern "C" {
 
 static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
 
-#[link_section = ".text.boot"]
-#[no_mangle]
-pub unsafe extern "C" fn IRQ() {
+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);
@@ -73,7 +73,7 @@ pub unsafe extern "C" fn IRQ() {
     stdio::drop_uart();
     println!("IRQ");
     loop {}
-}
+});
 
 pub fn restart_core1() {
     let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore());

libcortex_a9/src/lib.rs

@@ -6,17 +6,17 @@
 extern crate alloc;
 
 pub mod asm;
-pub mod regs;
 pub mod cache;
+mod fpu;
+pub mod l2c;
 pub mod mmu;
 pub mod mutex;
-pub mod sync_channel;
+pub mod regs;
 pub mod semaphore;
-pub mod l2c;
+pub mod sync_channel;
 mod uncached;
-mod fpu;
-pub use uncached::UncachedSlice;
 pub use fpu::enable_fpu;
+pub use uncached::UncachedSlice;
 
 global_asm!(include_str!("exceptions.s"));
 
@@ -35,3 +35,38 @@ pub fn notify_spin_lock() {
     }
 }
 
+#[macro_export]
+/// Interrupt handler, which setup the stack and jump to actual interrupt 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.
+/// - `stack1` is the stack for the interrupt handler when called from core1.
+/// - `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.
+macro_rules! interrupt_handler {
+    ($name:ident, $name2:ident, $stack0:ident, $stack1:ident, $body:block) => {
+        #[link_section = ".text.boot"]
+        #[no_mangle]
+        #[naked]
+        pub unsafe extern "C" fn $name() -> ! {
+            asm!(
+                // setup SP, depending on CPU 0 or 1
+                "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 sp, r1",
+                concat!("bl ", stringify!($name2)),
+                options(noreturn)
+            );
+        }
+
+        #[no_mangle]
+        pub unsafe extern "C" fn $name2() -> ! $body
+    };
+}

libsupport_zynq/src/abort.rs

@@ -1,63 +1,49 @@
 use libregister::RegisterR;
-use libcortex_a9::regs::{DFSR, MPIDR};
+use libcortex_a9::{regs::{DFSR, MPIDR}, interrupt_handler};
 use libboard_zynq::{println, stdio};
 
-#[link_section = ".text.boot"]
-#[no_mangle]
-pub unsafe extern "C" fn UndefinedInstruction() {
+interrupt_handler!(UndefinedInstruction, undefined_instruction, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("UndefinedInstruction");
     loop {}
-}
+});
 
-#[link_section = ".text.boot"]
-#[no_mangle]
-pub unsafe extern "C" fn SoftwareInterrupt() {
+interrupt_handler!(SoftwareInterrupt, software_interrupt, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("SoftwareInterrupt");
     loop {}
-}
+});
 
-#[link_section = ".text.boot"]
-#[no_mangle]
-pub unsafe extern "C" fn PrefetchAbort() {
+interrupt_handler!(PrefetchAbort, prefetch_abort, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("PrefetchAbort");
     loop {}
-}
+});
 
-#[link_section = ".text.boot"]
-#[no_mangle]
-pub unsafe extern "C" fn DataAbort() {
+interrupt_handler!(DataAbort, data_abort, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
 
     println!("DataAbort on core {}", MPIDR.read().cpu_id());
     println!("DFSR: {:03X}", DFSR.read());
 
     loop {}
-}
+});
 
-#[link_section = ".text.boot"]
-#[no_mangle]
-pub unsafe extern "C" fn ReservedException() {
+interrupt_handler!(ReservedException, reserved_exception, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("ReservedException");
     loop {}
-}
+});
 
-#[link_section = ".text.boot"]
-#[no_mangle]
 #[cfg(feature = "dummy_irq_handler")]
-pub unsafe extern "C" fn IRQ() {
+interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("IRQ");
     loop {}
-}
+});
 
-#[link_section = ".text.boot"]
-#[no_mangle]
-pub unsafe extern "C" fn FIQ() {
+interrupt_handler!(FIQ, fiq, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("FIQ");
     loop {}
-}
+});

libsupport_zynq/src/boot.rs

@@ -2,9 +2,9 @@ use r0::zero_bss;
 use core::ptr::write_volatile;
 use libregister::{
     VolatileCell,
-    RegisterR, RegisterW, RegisterRW,
+    RegisterR, RegisterRW,
 };
-use libcortex_a9::{asm, l2c, regs::*, cache, mmu, spin_lock_yield, notify_spin_lock, enable_fpu};
+use libcortex_a9::{asm, l2c, regs::*, cache, mmu, spin_lock_yield, notify_spin_lock, enable_fpu, interrupt_handler};
 use libboard_zynq::{slcr, mpcore};
 
 extern "C" {
@@ -18,24 +18,21 @@ extern "C" {
 
 static mut CORE1_ENABLED: VolatileCell<bool> = VolatileCell::new(false);
 
-#[link_section = ".text.boot"]
-#[no_mangle]
-pub unsafe extern "C" fn Reset() -> ! {
+interrupt_handler!(Reset, reset_irq, __stack0_start, __stack1_start, {
+    // no need to setup stack here, as we already did when entering the handler
     match MPIDR.read().cpu_id() {
         0 => {
-            SP.write(&mut __stack0_start as *mut _ as u32);
             boot_core0();
         }
         1 => {
             while !CORE1_ENABLED.get() {
                 spin_lock_yield();
             }
-            SP.write(&mut __stack1_start as *mut _ as u32);
             boot_core1();
         }
         _ => unreachable!(),
     }
-}
+});
 
 #[inline(never)]
 unsafe extern "C" fn boot_core0() -> ! {

libsupport_zynq/src/lib.rs

@@ -2,6 +2,8 @@
 
 #![feature(alloc_error_handler)]
 #![feature(panic_info_message)]
+#![feature(naked_functions)]
+#![feature(asm)]
 
 pub extern crate alloc;
 pub extern crate compiler_builtins;
@@ -11,3 +13,4 @@ mod abort;
 #[cfg(feature = "panic_handler")]
 mod panic;
 pub mod ram;
+

szl/link.x

@@ -59,6 +59,21 @@ SECTIONS
         __stack0_start = .;
     } > OCM3
 
+    .irq_stack1 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack1_end = .;
+        . += 0x100;
+        __irq_stack1_start = .;
+    } > OCM3
+
+    .irq_stack0 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack0_end = .;
+        . += 0x100;
+        __irq_stack0_start = .;
+    } > OCM3
+
+
     /DISCARD/ :
     {
         /* Unused exception related info that only wastes space */