diff --git a/cortex-a-rt/src/lib.rs b/cortex-a-rt/src/lib.rs
index a59b3bd..488697f 100644
--- a/cortex-a-rt/src/lib.rs
+++ b/cortex-a-rt/src/lib.rs
@@ -56,6 +56,24 @@
 //! `__sbss` and `__ebss` is zeroed, and the memory between `__sdata` and
 //! `__edata` is initialised with the data found at `__sidata`.
 //!
+//! The stacks look like:
+//!
+//! ```text
+//! +------------------+ <----_stack_top
+//! |     UND Stack    | } _und_stack_size bytes
+//! +------------------+
+//! |     SVC Stack    | } _svc_stack_size bytes
+//! +------------------+
+//! |     ABT Stack    | } _abt_stack_size bytes
+//! +------------------+
+//! |     IRQ Stack    | } _irq_stack_size bytes
+//! +------------------+
+//! |     FIQ Stack    | } _fiq_stack_size bytes
+//! +------------------+
+//! |     SYS Stack    | } No specific size
+//! +------------------+
+//! ```
+//!
 //! ### C-Compatible Functions
 //!
 //! * `kmain` - the `extern "C"` entry point to your application.
diff --git a/cortex-ar/Cargo.toml b/cortex-ar/Cargo.toml
index b2ed0b1..91e52a0 100644
--- a/cortex-ar/Cargo.toml
+++ b/cortex-ar/Cargo.toml
@@ -28,7 +28,7 @@ version = "0.1.0"
 arbitrary-int = "1.3.0"
 bitbybit = "1.3.3"
 num_enum = { version = "0.7", default-features = false }
-critical-section = {version = "1.2.0", features = ["restore-state-bool"], optional = true}
+critical-section = {version = "1.2.0", features = ["restore-state-u8"], optional = true}
 defmt = {version = "0.3", optional = true}
 
 [build-dependencies]
@@ -38,5 +38,8 @@ arm-targets = {version = "0.1.0", path = "../arm-targets"}
 # Adds a critical-section implementation that only disables interrupts.
 # This is not sound on multi-core systems because interrupts are per-core.
 critical-section-single-core = ["critical-section"]
+# Adds a critical-section implementation that disables interrupts and does
+# a CAS spinlock.
+critical-section-multi-core = ["critical-section"]
 # Adds defmt::Format implementation for the register types
 defmt = ["dep:defmt"]
diff --git a/cortex-ar/src/asm.rs b/cortex-ar/src/asm.rs
index 590c8da..58184ce 100644
--- a/cortex-ar/src/asm.rs
+++ b/cortex-ar/src/asm.rs
@@ -56,3 +56,15 @@ pub fn sev() {
         core::arch::asm!("sev");
     }
 }
+
+/// Which core are we?
+///
+/// Return the bottom 24-bits of the MPIDR
+#[inline]
+pub fn core_id() -> u32 {
+    let r: u32;
+    unsafe {
+        core::arch::asm!("MRC p15, 0, {}, c0, c0, 5", out(reg) r, options(nomem, nostack, preserves_flags));
+    }
+    return r & 0x00FF_FFFF;
+}
diff --git a/cortex-ar/src/critical_section.rs b/cortex-ar/src/critical_section.rs
index 86e1440..08094b2 100644
--- a/cortex-ar/src/critical_section.rs
+++ b/cortex-ar/src/critical_section.rs
@@ -1,30 +1,147 @@
-//! Code that implements the `critical-section` traits on Cortex-R or Cortex-A when only a single
-//! core is used.
+//! Code that implements the `critical-section` traits on Cortex-R or Cortex-A
 //!
-//! Only valid if you have a single core.
+//! We have single-core and multi-core versions. Select with the
+//! `critical-section-single-core` and `critical-section-multi-core` features.
 
-use core::sync::atomic;
+#[cfg(feature = "critical-section-single-core")]
+mod single_core {
+    struct SingleCoreCriticalSection;
 
-struct SingleCoreCriticalSection;
-critical_section::set_impl!(SingleCoreCriticalSection);
+    critical_section::set_impl!(SingleCoreCriticalSection);
 
-unsafe impl critical_section::Impl for SingleCoreCriticalSection {
-    unsafe fn acquire() -> critical_section::RawRestoreState {
-        // the i bit means "masked"
-        let was_active = !crate::register::Cpsr::read().i();
-        crate::interrupt::disable();
-        atomic::compiler_fence(atomic::Ordering::SeqCst);
-        was_active
+    /// Indicates the critical section was entered with interrupts on
+    pub const INT_ON: u8 = 0;
+
+    /// Indicates the critical section was entered with interrupts off
+    pub const INT_OFF: u8 = 1;
+
+    #[cfg(feature = "critical-section-single-core")]
+    unsafe impl critical_section::Impl for SingleCoreCriticalSection {
+        unsafe fn acquire() -> critical_section::RawRestoreState {
+            use core::sync::atomic;
+            // the i bit means "masked"
+            let was_active = !crate::register::Cpsr::read().i();
+            crate::interrupt::disable();
+            atomic::compiler_fence(atomic::Ordering::SeqCst);
+            if was_active {
+                INT_ON
+            } else {
+                INT_OFF
+            }
+        }
+
+        unsafe fn release(was_active: critical_section::RawRestoreState) {
+            use core::sync::atomic;
+            // Only re-enable interrupts if they were enabled before the critical section.
+            if was_active == INT_ON {
+                atomic::compiler_fence(atomic::Ordering::SeqCst);
+                // Safety: This is OK because we're releasing a lock that was
+                // entered with interrupts enabled
+                unsafe {
+                    crate::interrupt::enable();
+                }
+            }
+        }
     }
+}
+
+#[cfg(feature = "critical-section-multi-core")]
+mod multi_core {
+    struct MultiCoreCriticalSection;
+
+    critical_section::set_impl!(MultiCoreCriticalSection);
+
+    /// The default value for our spin-lock
+    pub const UNLOCKED: u32 = 0xFFFF_FFFF;
+
+    /// Indicates the critical section was entered with interrupts on, and the spin-lock unlocked
+    pub const INT_ON_UNLOCKED: u8 = 0;
+
+    /// Indicates the critical section was entered with interrupts off, and the spin-lock locked (by us)
+    pub const INT_OFF_LOCKED: u8 = 1;
+
+    /// Indicates the critical section was entered with interrupts off, and the spin-lock unlocked
+    pub const INT_OFF_UNLOCKED: u8 = 2;
+
+    pub static CORE_SPIN_LOCK: core::sync::atomic::AtomicU32 =
+        core::sync::atomic::AtomicU32::new(UNLOCKED);
+    unsafe impl critical_section::Impl for MultiCoreCriticalSection {
+        unsafe fn acquire() -> critical_section::RawRestoreState {
+            use core::sync::atomic;
+
+            // the i bit means "masked"
+            let was_active = !crate::register::Cpsr::read().i();
+            crate::interrupt::disable();
+
+            let core_id = crate::asm::core_id();
+
+            let locked_already = loop {
+                match CORE_SPIN_LOCK.compare_exchange(
+                    UNLOCKED,
+                    core_id,
+                    atomic::Ordering::Acquire,
+                    atomic::Ordering::Relaxed,
+                ) {
+                    Ok(_) => {
+                        // we got the lock
+                        break false;
+                    }
+                    Err(n) if n == core_id => {
+                        // we already held the lock
+                        break true;
+                    }
+                    Err(_) => {
+                        // someone else holds the lock
+                        core::hint::spin_loop();
+                    }
+                }
+            };
+
+            atomic::compiler_fence(atomic::Ordering::SeqCst);
+
+            match (was_active, locked_already) {
+                (true, true) => {
+                    panic!("Invalid CS state?!");
+                }
+                (true, false) => {
+                    // we need to turn interrupts on, and release the lock
+                    INT_ON_UNLOCKED
+                }
+                (false, false) => {
+                    // we need release the lock
+                    INT_OFF_UNLOCKED
+                }
+                (false, true) => {
+                    // we need to do nothing
+                    INT_OFF_LOCKED
+                }
+            }
+        }
+
+        unsafe fn release(was_active: critical_section::RawRestoreState) {
+            use core::sync::atomic;
 
-    unsafe fn release(was_active: critical_section::RawRestoreState) {
-        // Only re-enable interrupts if they were enabled before the critical section.
-        if was_active {
             atomic::compiler_fence(atomic::Ordering::SeqCst);
-            // Safety: This is OK because we're releasing a lock that was
-            // entered with interrupts enabled
-            unsafe {
-                crate::interrupt::enable();
+            match was_active {
+                INT_OFF_LOCKED => {
+                    // do nothing
+                }
+                INT_OFF_UNLOCKED => {
+                    // the spin-lock was unlocked before, so unlock it
+                    CORE_SPIN_LOCK.store(UNLOCKED, atomic::Ordering::Release);
+                }
+                INT_ON_UNLOCKED => {
+                    // the spin-lock was unlocked before, so unlock it
+                    CORE_SPIN_LOCK.store(UNLOCKED, atomic::Ordering::Release);
+                    // Safety: This is OK because we're releasing a lock that was
+                    // entered with interrupts enabled
+                    unsafe {
+                        crate::interrupt::enable();
+                    }
+                }
+                _ => {
+                    unreachable!()
+                }
             }
         }
     }
diff --git a/cortex-ar/src/lib.rs b/cortex-ar/src/lib.rs
index d76dd98..ed25e57 100644
--- a/cortex-ar/src/lib.rs
+++ b/cortex-ar/src/lib.rs
@@ -2,7 +2,6 @@
 
 #![no_std]
 
-#[cfg(feature = "critical-section-single-core")]
 mod critical_section;
 
 pub mod asm;
diff --git a/cortex-r-rt/link.x b/cortex-r-rt/link.x
index 309d1bf..e2da1f0 100644
--- a/cortex-r-rt/link.x
+++ b/cortex-r-rt/link.x
@@ -1,7 +1,8 @@
 /*
 Basic Cortex-R linker script.
 
-You must supply a file called `memory.x` which defines the memory regions 'CODE' and 'DATA'.
+You must supply a file called `memory.x` which defines the memory regions
+'VECTORS', 'CODE' and 'DATA'.
 
 The stack pointer(s) will be (near) the top of the DATA region by default.
 
@@ -10,13 +11,17 @@ Based upon the linker script from https://github.com/rust-embedded/cortex-m
 
 INCLUDE memory.x
 
-ENTRY(_vector_table);
+ENTRY(_start);
 EXTERN(_vector_table);
+EXTERN(_start);
 
 SECTIONS {
-    .text : {
+    .vector_table ORIGIN(VECTORS) : {
         /* The vector table must come first */
         *(.vector_table)
+    } > VECTORS
+
+    .text : {
         /* Now the rest of the code */
         *(.text .text*)
     } > CODE
@@ -76,6 +81,7 @@ remainder is our system mode stack.
 You must keep _stack_top and the stack sizes aligned to eight byte boundaries.
 */
 PROVIDE(_stack_top = ORIGIN(DATA) + LENGTH(DATA));
+PROVIDE(_hyp_stack_size = 0x400);
 PROVIDE(_und_stack_size = 0x400);
 PROVIDE(_svc_stack_size = 0x400);
 PROVIDE(_abt_stack_size = 0x400);
diff --git a/cortex-r-rt/src/lib.rs b/cortex-r-rt/src/lib.rs
index 75b5f34..d82f7fc 100644
--- a/cortex-r-rt/src/lib.rs
+++ b/cortex-r-rt/src/lib.rs
@@ -53,6 +53,26 @@
 //! `__sbss` and `__ebss` is zeroed, and the memory between `__sdata` and
 //! `__edata` is initialised with the data found at `__sidata`.
 //!
+//! The stacks look like:
+//!
+//! ```text
+//! +------------------+ <----_stack_top
+//! |     HYP Stack    | } _hyp_stack_size bytes (Armv8-R only)
+//! +------------------+
+//! |     UND Stack    | } _und_stack_size bytes
+//! +------------------+
+//! |     SVC Stack    | } _svc_stack_size bytes
+//! +------------------+
+//! |     ABT Stack    | } _abt_stack_size bytes
+//! +------------------+
+//! |     IRQ Stack    | } _irq_stack_size bytes
+//! +------------------+
+//! |     FIQ Stack    | } _fiq_stack_size bytes
+//! +------------------+
+//! |     SYS Stack    | } No specific size
+//! +------------------+
+//! ```
+//!
 //! ### C-Compatible Functions
 //!
 //! * `kmain` - the `extern "C"` entry point to your application.
@@ -213,6 +233,9 @@
 //!   `_irq_handler`
 //! * `_asm_default_fiq_handler` - an FIQ handler that just spins
 //! * `_default_handler` - a C compatible function that spins forever.
+//! * `_init_segments` - initialises `.bss` and `.data`
+//! * `_stack_setup` - initialises UND, SVC, ABT, IRQ, FIQ and SYS stacks from
+//!   the address given in `r0`
 //!
 //! The assembly language trampolines are required because Armv7-R (and Armv8-R)
 //! processors do not save a great deal of state on entry to an exception
@@ -573,12 +596,17 @@ core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3-d16
-    
-    .section .text.el1_start
-    .type _el1_start, %function
-    _el1_start:
-        // Set up stacks.
-        ldr     r0, =_stack_top
+
+    // Configure a stack for every mode. Leaves you in sys mode.
+    //
+    // Pass in stack top in r0.
+    .section .text._stack_setup
+    .global _stack_setup
+    .type _stack_setup, %function
+    _stack_setup:
+        // Save LR from whatever mode we're currently in
+        mov     r2, lr
+        // (we might not be in the same mode when we return).
         // Set stack pointer (right after) and mask interrupts for for UND mode (Mode 0x1B)
         msr     cpsr, {und_mode}
         mov     sp, r0
@@ -607,13 +635,19 @@ core::arch::global_asm!(
         // Set stack pointer (right after) and mask interrupts for for System mode (Mode 0x1F)
         msr     cpsr, {sys_mode}
         mov     sp, r0
-        // Clear the Thumb Exception bit because we're in Arm mode
-        mrc     p15, 0, r0, c1, c0, 0
-        bic     r0, #{te_bit}
-        mcr     p15, 0, r0, c1, c0, 0
-    "#,
-    fpu_enable!(),
-    r#"
+        // Clear the Thumb Exception bit because all our targets are currently
+        // for Arm (A32) mode
+        mrc     p15, 0, r1, c1, c0, 0
+        bic     r1, #{te_bit}
+        mcr     p15, 0, r1, c1, c0, 0
+        bx      r2
+    .size _stack_setup, . - _stack_setup
+
+    // Initialises stacks, .data and .bss
+    .section .text._init_segments
+    .global _init_segments
+    .type _init_segments, %function
+    _init_segments:
         // Initialise .bss
         ldr     r0, =__sbss
         ldr     r1, =__ebss
@@ -635,11 +669,8 @@ core::arch::global_asm!(
         stm     r0!, {{r3}}
         b       0b
     1:
-        // Jump to application
-        bl      kmain
-        // In case the application returns, loop forever
-        b       .
-    .size _el1_start, . - _el1_start
+        bx      lr
+    .size _init_segments, . - _init_segments
     "#,
     und_mode = const {
         Cpsr::new_with_raw_value(0)
@@ -703,7 +734,18 @@ core::arch::global_asm!(
     .global _default_start
     .type _default_start, %function
     _default_start:
-        ldr     pc, =_el1_start
+        // Set up stacks.
+        ldr     r0, =_stack_top
+        bl      _stack_setup
+        // Init .data and .bss
+        bl      _init_segments
+        "#,
+    fpu_enable!(),
+    r#"
+        // Jump to application
+        bl      kmain
+        // In case the application returns, loop forever
+        b       .
     .size _default_start, . - _default_start
     "#
 );
@@ -731,30 +773,42 @@ core::arch::global_asm!(
         cmp     r0, {cpsr_mode_hyp}
         bne     1f
         // Set stack pointer
-        ldr     sp, =_stack_top
+        ldr     r0, =_stack_top
+        mov     sp, r0
+        ldr     r1, =_hyp_stack_size
+        sub     r0, r0, r1
         // Set the HVBAR (for EL2) to _vector_table
-        ldr     r0, =_vector_table
-        mcr     p15, 4, r0, c12, c0, 0
+        ldr     r1, =_vector_table
+        mcr     p15, 4, r1, c12, c0, 0
         // Configure HACTLR to let us enter EL1
-        mrc     p15, 4, r0, c1, c0, 1
-        mov     r1, {hactlr_bits}
-        orr     r0, r0, r1
-        mcr     p15, 4, r0, c1, c0, 1
+        mrc     p15, 4, r1, c1, c0, 1
+        mov     r2, {hactlr_bits}
+        orr     r1, r1, r2
+        mcr     p15, 4, r1, c1, c0, 1
         // Program the SPSR - enter system mode (0x1F) in Arm mode with IRQ, FIQ masked
-        mov		r0, {sys_mode}
-        msr		spsr_hyp, r0
-        adr		r0, 1f
-        msr		elr_hyp, r0
+        mov		r1, {sys_mode}
+        msr		spsr_hyp, r1
+        adr		r1, 1f
+        msr		elr_hyp, r1
         dsb
         isb
         eret
     1:
+        // Set up stacks. r0 points to the bottom of the hyp stack.
+        bl      _stack_setup
         // Set the VBAR (for EL1) to _vector_table. NB: This isn't required on
         // Armv7-R because that only supports 'low' (default) or 'high'.
         ldr     r0, =_vector_table
         mcr     p15, 0, r0, c12, c0, 0
-        // go do the rest of the EL1 init
-        ldr     pc, =_el1_start
+        // Init .data and .bss
+        bl      _init_segments
+        "#,
+        fpu_enable!(),
+        r#"
+        // Jump to application
+        bl      kmain
+        // In case the application returns, loop forever
+        b       .
     .size _default_start, . - _default_start
     "#,
     cpsr_mode_hyp = const ProcessorMode::Hyp as u8,
diff --git a/examples/mps3-an536/Cargo.toml b/examples/mps3-an536/Cargo.toml
index 99a9d15..a93ea8e 100644
--- a/examples/mps3-an536/Cargo.toml
+++ b/examples/mps3-an536/Cargo.toml
@@ -12,10 +12,11 @@ rust-version = "1.82"
 version = "0.1.0"
 
 [dependencies]
-cortex-ar = { path = "../../cortex-ar", features = ["critical-section-single-core"] }
+cortex-ar = { path = "../../cortex-ar", features = ["critical-section-multi-core"] }
 cortex-r-rt = { path = "../../cortex-r-rt" }
 semihosting = { version = "0.1.18", features = ["stdio"] }
 arm-gic = { git = "https://github.com/google/arm-gic.git", rev = "46a8fc1720f5c28fccf4dfb5953b88dab7012e9c", optional = true }
+critical-section = "1.2.0"
 
 [build-dependencies]
 arm-targets = {version = "0.1.0", path = "../../arm-targets"}
diff --git a/examples/mps3-an536/memory.x b/examples/mps3-an536/memory.x
index 95e1c36..6ac99e2 100644
--- a/examples/mps3-an536/memory.x
+++ b/examples/mps3-an536/memory.x
@@ -9,5 +9,6 @@ MEMORY {
     DDR  : ORIGIN = 0x20000000, LENGTH = 128M
 }
 
+REGION_ALIAS("VECTORS", QSPI);
 REGION_ALIAS("CODE", QSPI);
 REGION_ALIAS("DATA", DDR);
diff --git a/examples/mps3-an536/reference/smp_test-armv8r-none-eabihf.out b/examples/mps3-an536/reference/smp_test-armv8r-none-eabihf.out
new file mode 100644
index 0000000..7fe30b8
--- /dev/null
+++ b/examples/mps3-an536/reference/smp_test-armv8r-none-eabihf.out
@@ -0,0 +1 @@
+CPU 1 is missing?!
diff --git a/examples/mps3-an536/reference/smp_test-armv8r-none-eabihf_smp2.out b/examples/mps3-an536/reference/smp_test-armv8r-none-eabihf_smp2.out
new file mode 100644
index 0000000..08c8c79
--- /dev/null
+++ b/examples/mps3-an536/reference/smp_test-armv8r-none-eabihf_smp2.out
@@ -0,0 +1,2 @@
+CAS test passed
+CS Mutex test passed
diff --git a/examples/mps3-an536/src/bin/smp_test.rs b/examples/mps3-an536/src/bin/smp_test.rs
new file mode 100644
index 0000000..c7f8a76
--- /dev/null
+++ b/examples/mps3-an536/src/bin/smp_test.rs
@@ -0,0 +1,235 @@
+//! Multi-core hello-world for Arm Cortex-R
+//!
+//! Runs code on two cores, checking that atomic fetch_add works.
+//!
+//! Abuses the FPGA LED register as a place to record whether Core 0 has
+//! started.
+//!
+//! Run with `cargo run --bin smp_test --target=armv8r-none-eabihf -- -smp 2`.
+
+#![no_std]
+#![no_main]
+
+use core::cell::{RefCell, UnsafeCell};
+use core::sync::atomic::{AtomicBool, AtomicU32, Ordering};
+
+// pull in our start-up code
+use mps3_an536 as _;
+
+use semihosting::println;
+
+#[repr(align(16))]
+struct Stack<const LEN_BYTES: usize> {
+    contents: UnsafeCell<[u8; LEN_BYTES]>,
+}
+
+impl<const LEN_BYTES: usize> Stack<LEN_BYTES> {
+    const fn new() -> Self {
+        Self {
+            contents: UnsafeCell::new([0u8; LEN_BYTES]),
+        }
+    }
+
+    fn stack_top(&self) -> usize {
+        let stack_start = self.contents.get() as usize;
+        stack_start + LEN_BYTES
+    }
+}
+
+unsafe impl<const LEN_BYTES: usize> Sync for Stack<LEN_BYTES> {}
+
+static CORE1_STACK: Stack<65536> = Stack::new();
+
+static CORE1_BOOTED: AtomicBool = AtomicBool::new(false);
+
+static SHARED_VARIABLE: AtomicU32 = AtomicU32::new(0);
+
+static SHARED_VARIABLE_2: critical_section::Mutex<RefCell<u32>> =
+    critical_section::Mutex::new(RefCell::new(0));
+
+/// How long core 0 waits for core 1
+const CORE0_WILL_WAIT: usize = 1_000_000;
+
+/// How many CAS loops to run?
+const CAS_LOOPS: u32 = 1000;
+
+/// How many CS Mutex loops to run?
+const CS_MUTEX_LOOPS: u32 = 1000;
+
+/// The entry-point to the Rust application.
+///
+/// It is called by the start-up code in `cortex-m-rt`.
+#[no_mangle]
+pub extern "C" fn kmain() {
+    let fpga_led = 0xE020_2000 as *mut u32;
+    extern "C" {
+        static mut _core1_stack_pointer: usize;
+    }
+    unsafe {
+        let p = &raw mut _core1_stack_pointer;
+        p.write(CORE1_STACK.stack_top());
+    }
+    unsafe {
+        // Activate second core by writing to FPGA LEDs.
+        // We needed a shared register that wasn't in RAM, and this will do.
+        fpga_led.write_volatile(1);
+    }
+
+    // wait some time for core 1 to start
+    for counter in 0..=CORE0_WILL_WAIT {
+        if CORE1_BOOTED.load(Ordering::SeqCst) {
+            break;
+        }
+        if counter == CORE0_WILL_WAIT {
+            println!("CPU 1 is missing?!");
+
+            semihosting::process::exit(0);
+        }
+    }
+
+    for _ in 0..CAS_LOOPS {
+        SHARED_VARIABLE.fetch_add(1, Ordering::Relaxed);
+    }
+
+    for _ in 0..CS_MUTEX_LOOPS {
+        critical_section::with(|cs| {
+            let mut value_ref = SHARED_VARIABLE_2.borrow_ref_mut(cs);
+            *value_ref += 1;
+        })
+    }
+
+    // let the other core finish
+    for _ in 0..CORE0_WILL_WAIT {
+        cortex_ar::asm::nop();
+    }
+
+    let total_a = SHARED_VARIABLE.load(Ordering::Relaxed);
+    if total_a == CAS_LOOPS * 2 {
+        println!("CAS test passed");
+    } else {
+        println!("CAS test failed, got {} not 2000", total_a);
+    }
+
+    let total_b = critical_section::with(|cs| {
+        let value_ref = SHARED_VARIABLE_2.borrow_ref(cs);
+        *value_ref
+    });
+
+    if total_b == CS_MUTEX_LOOPS * 2 {
+        println!("CS Mutex test passed");
+    } else {
+        println!("CS Mutex test failed, got {} not 2000", total_b);
+    }
+
+    semihosting::process::exit(0);
+}
+
+/// The entry-point to the Rust application.
+///
+/// It is called by the start-up code below, on Core 1.
+#[no_mangle]
+pub extern "C" fn kmain2() {
+    CORE1_BOOTED.store(true, Ordering::SeqCst);
+
+    for _ in 0..CAS_LOOPS {
+        SHARED_VARIABLE.fetch_add(1, Ordering::Relaxed);
+    }
+
+    for _ in 0..CS_MUTEX_LOOPS {
+        critical_section::with(|cs| {
+            let mut value_ref = SHARED_VARIABLE_2.borrow_ref_mut(cs);
+            *value_ref += 1;
+        })
+    }
+
+    loop {
+        core::hint::spin_loop();
+    }
+}
+
+// Start-up code for multi-core Armv8-R, as implemented on the MPS3-AN536.
+//
+// We boot into EL2, set up a stack pointer, init .data on .bss on core0, and
+// run `kmain` in EL1 on all cores.
+#[cfg(arm_architecture = "v8-r")]
+core::arch::global_asm!(
+    r#"
+    .section .bss
+    .align 4
+    _core1_stack_pointer:
+        .word 0
+
+    .section .text.startup
+    .align 4
+
+    .global _start
+    .global core1_released
+    .type _start, %function
+    _start:
+        // Read MPIDR into R0
+        mrc     p15, 0, r0, c0, c0, 5
+        ands    r0, r0, 0xFF
+        bne     core1
+    core0:
+        ldr     pc, =_default_start
+    core1:
+        ldr     r0, =0xE0202000
+        mov     r1, #0
+    core1_spin:
+        wfe
+        // spin until an LED0 is on
+        ldr     r2, [r0]  
+        cmp     r1, r2
+        beq     core1_spin
+    core1_released:
+        // now an LED is on, we assume _core1_stack_pointer contains our stack pointer
+        // First we must exit EL2...
+        // Set the HVBAR (for EL2) to _vector_table
+        ldr     r0, =_vector_table
+        mcr     p15, 4, r0, c12, c0, 0
+        // Configure HACTLR to let us enter EL1
+        mrc     p15, 4, r0, c1, c0, 1
+        mov     r1, {hactlr_bits}
+        orr     r0, r0, r1
+        mcr     p15, 4, r0, c1, c0, 1
+        // Program the SPSR - enter system mode (0x1F) in Arm mode with IRQ, FIQ masked
+        mov		r0, {sys_mode}
+        msr		spsr_hyp, r0
+        adr		r0, 1f
+        msr		elr_hyp, r0
+        dsb
+        isb
+        eret
+    1:
+        // Set the VBAR (for EL1) to _vector_table. NB: This isn't required on
+        // Armv7-R because that only supports 'low' (default) or 'high'.
+        ldr     r0, =_vector_table
+        mcr     p15, 0, r0, c12, c0, 0
+        ldr     r0, =_core1_stack_pointer
+        ldr     r0, [r0]
+        // set up our stacks using that stack pointer
+        bl      _stack_setup
+        bl      kmain2
+    .size _start, . - _start
+    "#,
+    hactlr_bits = const {
+        cortex_ar::register::Hactlr::new_with_raw_value(0)
+            .with_cpuactlr(true)
+            .with_cdbgdci(true)
+            .with_flashifregionr(true)
+            .with_periphpregionr(true)
+            .with_qosr(true)
+            .with_bustimeoutr(true)
+            .with_intmonr(true)
+            .with_err(true)
+            .with_testr1(true)
+            .raw_value()
+    },
+    sys_mode = const {
+        cortex_ar::register::Cpsr::new_with_raw_value(0)
+            .with_mode(cortex_ar::register::cpsr::ProcessorMode::Sys)
+            .with_i(true)
+            .with_f(true)
+            .raw_value()
+    }
+);
diff --git a/examples/versatileab/memory.x b/examples/versatileab/memory.x
index aced48a..9b7e5e6 100644
--- a/examples/versatileab/memory.x
+++ b/examples/versatileab/memory.x
@@ -8,5 +8,6 @@ MEMORY {
     SDRAM : ORIGIN = 0, LENGTH = 128M
 }
 
+REGION_ALIAS("VECTORS", SDRAM);
 REGION_ALIAS("CODE", SDRAM);
 REGION_ALIAS("DATA", SDRAM);
diff --git a/tests.sh b/tests.sh
index 19d4c13..5b037c7 100755
--- a/tests.sh
+++ b/tests.sh
@@ -67,6 +67,8 @@ if qemu-system-arm --version | grep "version 9"; then
         cargo +nightly run ${mps3_an536_cargo} --target=armv8r-none-eabihf --bin $binary --features=gic -Zbuild-std=core | tee ./target/$binary-armv8r-none-eabihf.out
         my_diff ./examples/mps3-an536/reference/$binary-armv8r-none-eabihf.out ./target/$binary-armv8r-none-eabihf.out || fail $binary "armv8r-none-eabihf"
     done
+    cargo +nightly run ${mps3_an536_cargo} --target=armv8r-none-eabihf --bin smp_test --features=gic -Zbuild-std=core -- -smp 2 | tee ./target/smp_test-armv8r-none-eabihf_smp2.out
+    my_diff ./examples/mps3-an536/reference/smp_test-armv8r-none-eabihf_smp2.out ./target/smp_test-armv8r-none-eabihf_smp2.out || fail smp_test "armv8r-none-eabihf"
 fi
 
 if [ "$FAILURE" == "1" ]; then