diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index 60d2812d..08b56d10 100644
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -231,7 +231,6 @@ jobs:
           - test-qemu-v7r
           - test-qemu-v7a
           - test-qemu-v8r
-          - test-qemu-v8r-smp
           - test-qemu-v8r-el2
     steps:
       - name: Checkout
diff --git a/.gitignore b/.gitignore
index e58f52da..df3528dc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,8 +1,6 @@
 target
 examples/mps3-an536/target
 examples/mps3-an536/target-d32
-examples/mps3-an536-smp/target
-examples/mps3-an536-smp/target-d32
 examples/mps3-an536-el2/target
 examples/mps3-an536-el2/target-d32
 examples/versatileab/target
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 1c0f63d0..85074989 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -10,7 +10,6 @@
         "./arm-targets/Cargo.toml",
         "examples/versatileab/Cargo.toml",
         "examples/mps3-an536/Cargo.toml",
-        "examples/mps3-an536-smp/Cargo.toml",
-        "examples/mps3-an536-el2/Cargo.toml"
+        "examples/mps3-an536-el2/Cargo.toml",
     ]
 }
diff --git a/Cargo.toml b/Cargo.toml
index 61c8cd0c..3543151c 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -3,7 +3,6 @@ exclude = [
   "arm-targets",
   "examples/versatileab",
   "examples/mps3-an536",
-  "examples/mps3-an536-smp",
   "examples/mps3-an536-el2",
   "examples/c-code",
   "examples/xilinx-zynq-a9",
diff --git a/aarch32-rt/link.x b/aarch32-rt/link.x
index 309e3bca..fd038217 100644
--- a/aarch32-rt/link.x
+++ b/aarch32-rt/link.x
@@ -254,6 +254,9 @@ PROVIDE(_sys_stack_size = 16K);
 /* Default to one CPU core (i.e. one copy of each stack) */
 PROVIDE(_num_cores      = 1);
 
+/* Our default main function for any non-primary cores */
+PROVIDE(kmain_secondary = _default_kmain_secondary);
+
 /* Default stack alignment. You can over-align if you want to set up MPU regions for the stacks */
 PROVIDE(_stack_alignment = 8);
 
@@ -274,9 +277,9 @@ PROVIDE(_asm_svc_handler            = _asm_default_svc_handler);
 PROVIDE(_asm_hvc_handler            = _asm_default_hvc_handler);
 PROVIDE(_asm_prefetch_abort_handler = _asm_default_prefetch_abort_handler);
 PROVIDE(_asm_data_abort_handler     = _asm_default_data_abort_handler);
-/* TODO: Hyp handler goes here */
 PROVIDE(_asm_irq_handler            = _asm_default_irq_handler);
 PROVIDE(_asm_fiq_handler            = _asm_default_fiq_handler);
+PROVIDE(_asm_secondary_core_park    = _asm_default_secondary_core_park);
 
 /* Weak aliases for C default handlers */
 PROVIDE(_undefined_handler      = _default_handler);
@@ -284,7 +287,6 @@ PROVIDE(_svc_handler            = _default_handler);
 PROVIDE(_hvc_handler            = _default_handler);
 PROVIDE(_prefetch_abort_handler = _default_handler);
 PROVIDE(_data_abort_handler     = _default_handler);
-/* TODO: Hyp handler goes here */
 PROVIDE(_irq_handler            = _default_handler);
 /* NB: There is no default C-language FIQ handler */
 
diff --git a/aarch32-rt/src/arch_v4/abort.rs b/aarch32-rt/src/arch_v4/abort.rs
index c2caa8f1..14033f22 100644
--- a/aarch32-rt/src/arch_v4/abort.rs
+++ b/aarch32-rt/src/arch_v4/abort.rs
@@ -1,13 +1,17 @@
 //! Data and Prefetch Abort handlers for Armv4 to Armv6
 
+// # _asm_default_data_abort_handler
+//
+// Called from the vector table when we have an undefined exception.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _data_abort_handler(addr: usize);`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
-
-    // Called from the vector table when we have an undefined exception.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _data_abort_handler(addr: usize);`
     .pushsection .text._asm_default_data_abort_handler
     .arm
     .global _asm_default_data_abort_handler
@@ -40,14 +44,18 @@ core::arch::global_asm!(
     "#
 );
 
+// # _asm_default_prefetch_abort_handler
+//
+// Called from the vector table when we have a prefetch abort.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _prefetch_abort_handler(addr: usize);`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
-
-    // Called from the vector table when we have a prefetch abort.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _prefetch_abort_handler(addr: usize);`
     .pushsection .text._asm_default_prefetch_abort_handler
     .arm
     .global _asm_default_prefetch_abort_handler
diff --git a/aarch32-rt/src/arch_v4/boot.rs b/aarch32-rt/src/arch_v4/boot.rs
index 901130c2..9ee1f80d 100644
--- a/aarch32-rt/src/arch_v4/boot.rs
+++ b/aarch32-rt/src/arch_v4/boot.rs
@@ -1,50 +1,27 @@
-//! Start-up code for CPUs that always boot into EL1
+//! Start-up code for ARMv4 - ARMv6 CPUs that always boot into EL1
 
+// # _default_start
+//
+// Reset function for ARMv4 to ARMv6
+//
+// Initialises global memory and sets up stacks. Only supports one CPU core.
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
-
     .pushsection .text.default_start
     .arm
     .global _default_start
     .type _default_start, %function
     _default_start:
-        // Init .data and .bss
-        bl      _init_segments
-        // Set up stacks.
+        // Init .data and .bss on primary core
+        bl      _asm_init_segments
+        // Do standard core init - only one core supported
         mov     r0, #0
-        bl      _stack_setup_preallocated
-    "#,
-    #[cfg(any(target_abi = "eabihf", feature = "eabi-fpu"))]
-    r#"
-        // Allow VFP coprocessor access
-        mrc     p15, 0, r0, c1, c0, 2
-        orr     r0, r0, #0xF00000
-        mcr     p15, 0, r0, c1, c0, 2
-        // Enable VFP
-        mov     r0, #0x40000000
-        vmsr    fpexc, r0
-    "#,
-    r#"
-        // Zero all registers before calling kmain
-        mov     r0, 0
-        mov     r1, 0
-        mov     r2, 0
-        mov     r3, 0
-        mov     r4, 0
-        mov     r5, 0
-        mov     r6, 0
-        mov     r7, 0
-        mov     r8, 0
-        mov     r9, 0
-        mov     r10, 0
-        mov     r11, 0
-        mov     r12, 0
-        // Jump to application
-        bl      kmain
-        // In case the application returns, loop forever
-        b       .
+        b       _asm_core_start
     .size _default_start, . - _default_start
     .popsection
     "#
diff --git a/aarch32-rt/src/arch_v4/hvc.rs b/aarch32-rt/src/arch_v4/hvc.rs
index 3c674911..39e605b8 100644
--- a/aarch32-rt/src/arch_v4/hvc.rs
+++ b/aarch32-rt/src/arch_v4/hvc.rs
@@ -1,12 +1,13 @@
 //! Dummy hypervisor handler for architectures without HYP mode
 
+// # _asm_default_hvc_handler
+//
+// Never called but makes the linker happy
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
-
-    // Never called but makes the linker happy
     .pushsection .text._asm_default_hvc_handler
     .arm
     .global _asm_default_hvc_handler
diff --git a/aarch32-rt/src/arch_v4/interrupt.rs b/aarch32-rt/src/arch_v4/interrupt.rs
index a1f73783..117da939 100644
--- a/aarch32-rt/src/arch_v4/interrupt.rs
+++ b/aarch32-rt/src/arch_v4/interrupt.rs
@@ -2,29 +2,33 @@
 
 use crate::{Cpsr, ProcessorMode};
 
+// # _asm_default_irq_handler
+//
+// Called from the vector table when we have an interrupt. Saves state and
+// calls a C-compatible handler like `extern "C" fn _irq_handler();` in
+// system mode (or SVC mode if the `svc-stack-interrupt` feature is
+// enabled).
+//
+// We call the C-compatible handler in a different mode because when when an
+// IRQ occurs, the PC is copied to LR_irq immediately. If the C code was
+// running in IRQ mode, then it will be using LR_irq for normal LR things
+// (because that's the LR register when you are in IRQ mode). Instantly
+// trashing the LR register of running code is bad. So, by switching to SYS
+// mode (or SVC mode) we ensure that LR_irq is always unused at the point an
+// IRQ occurs.
+//
+// See [ARM Cortex-R Series (Armv7-R) Programmer's Guide] for more details.
+//
+// [ARM Cortex-R Series (Armv7-R) Programmer's Guide]:
+//     https://developer.arm.com/documentation/den0042/0100/Exceptions-and-Interrupts/External-interrupt-requests/Nested-interrupt-handling
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
-
-    // Called from the vector table when we have an interrupt. Saves state and
-    // calls a C-compatible handler like `extern "C" fn _irq_handler();` in
-    // system mode (or SVC mode if the `svc-stack-interrupt` feature is
-    // enabled).
-    //
-    // We call the C-compatible handler in a different mode because when when an
-    // IRQ occurs, the PC is copied to LR_irq immediately. If the C code was
-    // running in IRQ mode, then it will be using LR_irq for normal LR things
-    // (because that's the LR register when you are in IRQ mode). Instantly
-    // trashing the LR register of running code is bad. So, by switching to SYS
-    // mode (or SVC mode) we ensure that LR_irq is always unused at the point an
-    // IRQ occurs.
-    //
-    // See [ARM Cortex-R Series (Armv7-R) Programmer's Guide] for more details.
-    //
-    // [ARM Cortex-R Series (Armv7-R) Programmer's Guide]:
-    //     https://developer.arm.com/documentation/den0042/0100/Exceptions-and-Interrupts/External-interrupt-requests/Nested-interrupt-handling
     .pushsection .text._asm_default_irq_handler
     .arm
     .global _asm_default_irq_handler
diff --git a/aarch32-rt/src/arch_v4/svc.rs b/aarch32-rt/src/arch_v4/svc.rs
index 531803bf..456cce14 100644
--- a/aarch32-rt/src/arch_v4/svc.rs
+++ b/aarch32-rt/src/arch_v4/svc.rs
@@ -1,14 +1,18 @@
 //! SVC handler for Armv4 to Armv6
 
+// # _asm_default_svc_handler
+//
+// Called from the vector table when we have an software interrupt.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _svc_handler(arg: u32, frame: &Frame) -> u32;`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
-
-    // Called from the vector table when we have an software interrupt.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _svc_handler(arg: u32, frame: &Frame) -> u32;`
     .pushsection .text._asm_default_svc_handler
     .arm
     .global _asm_default_svc_handler
diff --git a/aarch32-rt/src/arch_v4/undefined.rs b/aarch32-rt/src/arch_v4/undefined.rs
index 2d655ea3..26879299 100644
--- a/aarch32-rt/src/arch_v4/undefined.rs
+++ b/aarch32-rt/src/arch_v4/undefined.rs
@@ -1,16 +1,20 @@
 //! Undefined handler for Armv4 to Armv6
 
+// # _asm_default_undefined_handler
+//
+// Called from the vector table when we have an undefined exception.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _undefined_handler(addr: usize) -> usize;`
+// or
+// `extern "C" fn _undefined_handler(addr: usize) -> !;`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
-        
-    // Called from the vector table when we have an undefined exception.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _undefined_handler(addr: usize) -> usize;`
-    // or
-    // `extern "C" fn _undefined_handler(addr: usize) -> !;`
     .pushsection .text._asm_default_undefined_handler
     .arm
     .global _asm_default_undefined_handler
diff --git a/aarch32-rt/src/arch_v7/abort.rs b/aarch32-rt/src/arch_v7/abort.rs
index 81875981..7f92a731 100644
--- a/aarch32-rt/src/arch_v7/abort.rs
+++ b/aarch32-rt/src/arch_v7/abort.rs
@@ -1,13 +1,17 @@
 //! Data and Prefetch Abort handlers for Armv7 and higher
 
+// # _asm_default_data_abort_handler
+//
+// Called from the vector table when we have an undefined exception.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _data_abort_handler(addr: usize);`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-
-    // Called from the vector table when we have an undefined exception.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _data_abort_handler(addr: usize);`
     .pushsection .text._asm_default_data_abort_handler
     .arm
     .global _asm_default_data_abort_handler
@@ -39,14 +43,18 @@ core::arch::global_asm!(
     abt_mode = const crate::ProcessorMode::Abt as u8,
 );
 
+// # _asm_default_prefetch_abort_handler
+//
+// Called from the vector table when we have a prefetch abort.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _prefetch_abort_handler(addr: usize);`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-  
-    // Called from the vector table when we have a prefetch abort.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _prefetch_abort_handler(addr: usize);`
     .pushsection .text._asm_default_prefetch_abort_handler
     .arm
     .global _asm_default_prefetch_abort_handler
diff --git a/aarch32-rt/src/arch_v7/boot_from_el1.rs b/aarch32-rt/src/arch_v7/boot_from_el1.rs
index b98a5526..eae9431b 100644
--- a/aarch32-rt/src/arch_v7/boot_from_el1.rs
+++ b/aarch32-rt/src/arch_v7/boot_from_el1.rs
@@ -1,55 +1,47 @@
-//! Start-up code for CPUs that always boot into EL1
+//! Boot code for Armv7-R
 
+// # _default_start
+//
+// Reset function for ARMv7-R
+//
+// Core 0 will initialise global memory and call `_asm_core_start`. Other cores
+// call `_asm_secondary_core_park` and then `_asm_core_start`.
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
-    // Work around https://github.com/rust-lang/rust/issues/127269
-    .fpu vfp2
-
-    .pushsection .text.default_start
+    .pushsection .text._default_start
     .arm
     .global _default_start
     .type _default_start, %function
     _default_start:
-        // Init .data and .bss
-        bl      _init_segments
-        // Set up stacks.
-        mov     r0, #0
-        bl      _stack_setup_preallocated
-        // Clear Thumb Exception bit
-        mrc     p15, 0, r0, c1, c0, 0
-        bic     r0, #0x40000000
-        mcr     p15, 0, r0, c1, c0, 0
-    "#,
-    #[cfg(any(target_abi = "eabihf", feature = "eabi-fpu"))]
-    r#"
-        // Allow VFP coprocessor access
-        mrc     p15, 0, r0, c1, c0, 2
-        orr     r0, r0, #0xF00000
-        mcr     p15, 0, r0, c1, c0, 2
-        // Enable VFP
-        mov     r0, #0x40000000
-        vmsr    fpexc, r0
-    "#,
-    r#"
-        // Zero all registers before calling kmain
-        mov     r0, 0
-        mov     r1, 0
-        mov     r2, 0
-        mov     r3, 0
-        mov     r4, 0
-        mov     r5, 0
-        mov     r6, 0
-        mov     r7, 0
-        mov     r8, 0
-        mov     r9, 0
-        mov     r10, 0
-        mov     r11, 0
-        mov     r12, 0
-        // Jump to application
-        bl      kmain
-        // In case the application returns, loop forever
-        b       .
+        // Read MPIDR into R0
+        mrc     p15, 0, r0, c0, c0, 5
+        // Check if core ID (bottom 8 bits) is zero
+        ands    r0, r0, 0xFF
+        bne     1f
+        // Primary core (core 0) can do normal start-up
+        mov     r4, r0
+        bl      _asm_init_segments
+        mov     r0, r4
+        b       _asm_core_start
+    1:
+        // Secondary core needs to spin until some magic flag is set
+        mov     r4, r0
+        bl      _asm_secondary_core_park
+        mov     r0, r4
+        b       _asm_core_start
     .size _default_start, . - _default_start
     .popsection
     "#
 );
+
+#[unsafe(naked)]
+#[unsafe(no_mangle)]
+extern "C" fn _asm_default_secondary_core_park() {
+    core::arch::naked_asm!(
+        // just spin
+        "b       ."
+    )
+}
diff --git a/aarch32-rt/src/arch_v7/boot_from_el2.rs b/aarch32-rt/src/arch_v7/boot_from_el2.rs
index e72c384c..5aa39caa 100644
--- a/aarch32-rt/src/arch_v7/boot_from_el2.rs
+++ b/aarch32-rt/src/arch_v7/boot_from_el2.rs
@@ -1,25 +1,49 @@
-//! Start-up code for CPUs that *might* boot into EL2 but that we want in EL1.
+//! Boot code for Armv7-A and Armv8-R
 
+#[cfg(any(
+    arm_architecture = "v7-a",
+    all(arm_architecture = "v8-r", not(feature = "el2-mode")),
+))]
 use aarch32_cpu::register::{cpsr::ProcessorMode, Cpsr, Hactlr};
 
+// # _default_start
+//
+// Reset function for ARMv7-A and ARMv8-R
+//
+// Cores will first set the HYP stack and then leave EL2 and enter EL1.
+// Core 0 will initialise global memory and call `_asm_core_start`. Other cores
+// call `_asm_secondary_core_park` and then `_asm_core_start`.
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
+#[cfg(any(
+    arm_architecture = "v7-a",
+    all(arm_architecture = "v8-r", not(feature = "el2-mode")),
+))]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
     .cpu cortex-r52
-
     .pushsection .text.default_start
     .arm
     .global _default_start
     .type _default_start, %function
     _default_start:
+        // Read MPIDR into R0
+        mrc     p15, 0, r0, c0, c0, 5
+        // Core ID in bottom 8 bits
+        and     r0, r0, 0xFF
         // Are we in EL2? If not, skip the EL2 setup portion
-        mrs     r0, cpsr
-        and     r0, r0, 0x1F
-        cmp     r0, {cpsr_mode_hyp}
+        mrs     r1, cpsr
+        and     r1, r1, 0x1F
+        cmp     r1, {cpsr_mode_hyp}
         bne     1f
-        // Set stack pointer
-        ldr     sp, =_hyp_stack_high_end
+        // Set up the Hyp stack for this core
+        ldr	    sp, =_hyp_stack_high_end
+        ldr	    r1, =_hyp_stack_size
+        muls    r1, r1, r0
+        subs    sp, sp, r1
         // Set the HVBAR (for EL2) to _vector_table
         ldr     r1, =_vector_table
         mcr     p15, 4, r1, c12, c0, 0
@@ -37,49 +61,23 @@ core::arch::global_asm!(
         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
-        // Init .data and .bss
-        bl      _init_segments
-        // Set up stacks.
-        mov     r0, #0
-        bl      _stack_setup_preallocated
-        // Clear Thumb Exception bit
-        mrc     p15, 0, r0, c1, c0, 0
-        bic     r0, #0x40000000
-        mcr     p15, 0, r0, c1, c0, 0
-    "#,
-    #[cfg(any(target_abi = "eabihf", feature = "eabi-fpu"))]
-    r#"
-        // Allow VFP coprocessor access
-        mrc     p15, 0, r0, c1, c0, 2
-        orr     r0, r0, #0xF00000
-        mcr     p15, 0, r0, c1, c0, 2
-        // Enable VFP
-        mov     r0, #0x40000000
-        vmsr    fpexc, r0
-    "#,
-    r#"
-        // Zero all registers before calling kmain
-        mov     r0, 0
-        mov     r1, 0
-        mov     r2, 0
-        mov     r3, 0
-        mov     r4, 0
-        mov     r5, 0
-        mov     r6, 0
-        mov     r7, 0
-        mov     r8, 0
-        mov     r9, 0
-        mov     r10, 0
-        mov     r11, 0
-        mov     r12, 0
-        // Jump to application
-        bl      kmain
-        // In case the application returns, loop forever
-        b       .
+        // Set the VBAR (for EL1) to _vector_table.
+        ldr     r1, =_vector_table
+        mcr     p15, 0, r1, c12, c0, 0
+        // Check if core ID is zero
+        cmp     r0, 0
+        bne     2f
+        // Primary core (core 0) can do normal start-up
+        mov     r4, r0
+        bl      _asm_init_segments
+        mov     r0, r4
+        b       _asm_core_start
+    2:
+        // Secondary core needs to spin until some magic flag is set
+        mov     r4, r0
+        bl      _asm_secondary_core_park
+        mov     r0, r4
+        b       _asm_core_start
     .size _default_start, . - _default_start
     .popsection
     "#,
@@ -105,3 +103,12 @@ core::arch::global_asm!(
             .raw_value()
     }
 );
+
+#[unsafe(naked)]
+#[unsafe(no_mangle)]
+extern "C" fn _asm_default_secondary_core_park() {
+    core::arch::naked_asm!(
+        // just spin
+        "b       ."
+    )
+}
diff --git a/aarch32-rt/src/arch_v7/hvc.rs b/aarch32-rt/src/arch_v7/hvc.rs
index 4c160c78..83528323 100644
--- a/aarch32-rt/src/arch_v7/hvc.rs
+++ b/aarch32-rt/src/arch_v7/hvc.rs
@@ -1,17 +1,18 @@
 //! HVC handler for Armv7 and higher
 
+// # _asm_default_hvc_handler
+//
+// Called from the vector table when we have an hypervisor call.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _hvc_handler(hsr: u32, frame: &Frame) -> u32;`
 #[cfg(target_arch = "arm")]
 #[cfg(arm_architecture = "v8-r")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-
     .pushsection .text._asm_default_hvc_handler
-
-    // Called from the vector table when we have an hypervisor call.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _hvc_handler(hsr: u32, frame: &Frame) -> u32;`
+    .arm
     .global _asm_default_hvc_handler
     .type _asm_default_hvc_handler, %function
     _asm_default_hvc_handler:
@@ -37,15 +38,15 @@ core::arch::global_asm!(
     "#,
 );
 
+// # _asm_default_hvc_handler
+//
+// Never called but makes the linker happy
 #[cfg(target_arch = "arm")]
 #[cfg(not(arm_architecture = "v8-r"))]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
-
-
-    // Never called but makes the linker happy
     .pushsection .text._asm_default_hvc_handler
     .arm
     .global _asm_default_hvc_handler
diff --git a/aarch32-rt/src/arch_v7/interrupt.rs b/aarch32-rt/src/arch_v7/interrupt.rs
index 3493eded..322bf330 100644
--- a/aarch32-rt/src/arch_v7/interrupt.rs
+++ b/aarch32-rt/src/arch_v7/interrupt.rs
@@ -1,32 +1,36 @@
 //! IRQ handler for for Armv7 and higher
 
+// # _asm_default_irq_handler
+//
+// Called from the vector table when we have an interrupt. Saves state and
+// calls a C-compatible handler like `extern "C" fn _irq_handler();` in
+// system mode (or SVC mode if the `svc-stack-interrupt` feature is
+// enabled).
+//
+// We call the C-compatible handler in a different mode because when when an
+// IRQ occurs, the PC is copied to LR_irq immediately. If the C code was
+// running in IRQ mode, then it will be using LR_irq for normal LR things
+// (because that's the LR register when you are in IRQ mode). Instantly
+// trashing the LR register of running code is bad. So, by switching to SYS
+// mode (or SVC mode) we ensure that LR_irq is always unused at the point an
+// IRQ occurs.
+//
+// Because this is ARMv7, we can save state (SPSR_irq and LR_irq) straight
+// to another mode's stack, meaning that we never actually push anything to
+// the IRQ stack. You can therefore run with an IRQ stack size of zero.
+//
+// See [ARM Cortex-R Series (Armv7-R) Programmer's Guide] for more details.
+//
+// [ARM Cortex-R Series (Armv7-R) Programmer's Guide]:
+//     https://developer.arm.com/documentation/den0042/0100/Exceptions-and-Interrupts/External-interrupt-requests/Nested-interrupt-handling
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-
-    // Called from the vector table when we have an interrupt. Saves state and
-    // calls a C-compatible handler like `extern "C" fn _irq_handler();` in
-    // system mode (or SVC mode if the `svc-stack-interrupt` feature is
-    // enabled).
-    //
-    // We call the C-compatible handler in a different mode because when when an
-    // IRQ occurs, the PC is copied to LR_irq immediately. If the C code was
-    // running in IRQ mode, then it will be using LR_irq for normal LR things
-    // (because that's the LR register when you are in IRQ mode). Instantly
-    // trashing the LR register of running code is bad. So, by switching to SYS
-    // mode (or SVC mode) we ensure that LR_irq is always unused at the point an
-    // IRQ occurs.
-    //
-    // Because this is ARMv7, we can save state (SPSR_irq and LR_irq) straight
-    // to another mode's stack, meaning that we never actually push anything to
-    // the IRQ stack. You can therefore run with an IRQ stack size of zero.
-    //
-    // See [ARM Cortex-R Series (Armv7-R) Programmer's Guide] for more details.
-    //
-    // [ARM Cortex-R Series (Armv7-R) Programmer's Guide]:
-    //     https://developer.arm.com/documentation/den0042/0100/Exceptions-and-Interrupts/External-interrupt-requests/Nested-interrupt-handling
     .pushsection .text._asm_default_irq_handler
     .arm
     .global _asm_default_irq_handler
diff --git a/aarch32-rt/src/arch_v7/svc.rs b/aarch32-rt/src/arch_v7/svc.rs
index ea863225..d743237f 100644
--- a/aarch32-rt/src/arch_v7/svc.rs
+++ b/aarch32-rt/src/arch_v7/svc.rs
@@ -1,14 +1,19 @@
 //! SVC handler for Armv7 and higher
 
+// # _asm_default_svc_handler
+//
+// Called from the vector table when we have an software interrupt.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _svc_handler(arg: u32, frame: &Frame) -> u32;`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
         
-    // Called from the vector table when we have an software interrupt.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _svc_handler(arg: u32, frame: &Frame) -> u32;`
     .pushsection .text._asm_default_svc_handler
     .arm
     .global _asm_default_svc_handler
diff --git a/aarch32-rt/src/arch_v7/undefined.rs b/aarch32-rt/src/arch_v7/undefined.rs
index 9067a1bf..72543d39 100644
--- a/aarch32-rt/src/arch_v7/undefined.rs
+++ b/aarch32-rt/src/arch_v7/undefined.rs
@@ -1,16 +1,21 @@
 //! Undefined handler for Armv7 and higher
 
+// # _asm_default_undefined_handler
+//
+// Called from the vector table when we have an undefined exception.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _undefined_handler(addr: usize) -> usize;`
+// or
+// `extern "C" fn _undefined_handler(addr: usize) -> !;`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
 
-    // Called from the vector table when we have an undefined exception.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _undefined_handler(addr: usize) -> usize;`
-    // or
-    // `extern "C" fn _undefined_handler(addr: usize) -> !;`
     .pushsection .text._asm_default_undefined_handler
     .arm
     .global _asm_default_undefined_handler
diff --git a/aarch32-rt/src/arch_v8_hyp/abort.rs b/aarch32-rt/src/arch_v8_hyp/abort.rs
index 62ddf2f3..d69ceef8 100644
--- a/aarch32-rt/src/arch_v8_hyp/abort.rs
+++ b/aarch32-rt/src/arch_v8_hyp/abort.rs
@@ -1,15 +1,19 @@
 //! Data and Prefetch Abort handlers for Armv8-R at EL2
 
+// # _asm_default_data_abort_handler
+//
+// Called from the vector table when we have an undefined exception.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _data_abort_handler(addr: usize);`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-
-    .section .text._asm_default_data_abort_handler
-
-    // Called from the vector table when we have an undefined exception.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _data_abort_handler(addr: usize);`
+    .pushsection .text._asm_default_data_abort_handler
+    .arm
     .global _asm_default_data_abort_handler
     .type _asm_default_data_abort_handler, %function
     _asm_default_data_abort_handler:
@@ -33,19 +37,24 @@ core::arch::global_asm!(
         pop     {{ r0-r3, r12, lr }}      // Pop state that C function didn't save to undo (1)
         eret                              // Return from the asm handler
     .size _asm_default_data_abort_handler, . - _asm_default_data_abort_handler
+    .popsection
     "#,
 );
 
+// # _asm_default_prefetch_abort_handler
+//
+// Called from the vector table when we have an undefined exception.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _prefetch_abort_handler(addr: usize);`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-
-    .section .text._asm_default_prefetch_abort_handler
-
-    // Called from the vector table when we have an undefined exception.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _prefetch_abort_handler(addr: usize);`
+    .pushsection .text._asm_default_prefetch_abort_handler
+    .arm
     .global _asm_default_prefetch_abort_handler
     .type _asm_default_prefetch_abort_handler, %function
     _asm_default_prefetch_abort_handler:
@@ -69,5 +78,6 @@ core::arch::global_asm!(
         pop     {{ r0-r3, r12, lr }}      // Pop state that C function didn't save to undo (1)
         eret                              // Return from the asm handler
     .size _asm_default_prefetch_abort_handler, . - _asm_default_prefetch_abort_handler
+    .popsection
     "#,
 );
diff --git a/aarch32-rt/src/arch_v8_hyp/boot.rs b/aarch32-rt/src/arch_v8_hyp/boot.rs
index b756f31e..61515082 100644
--- a/aarch32-rt/src/arch_v8_hyp/boot.rs
+++ b/aarch32-rt/src/arch_v8_hyp/boot.rs
@@ -1,40 +1,61 @@
 //! Start-up code for Armv8-R to stay in EL2.
-//!
-//! We boot into EL2, set up a HYP stack pointer, and run `kmain` in EL2.
 
+// # _default_start
+//
+// We boot into EL2, set up a HYP stack pointer, and run `kmain` in EL2 on the
+// primary core, and `kmain_secondary` in EL2 on any secondary cores.
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-
-    .section .text.default_start
+    .pushsection .text.default_start
+    .arm
     .global _default_start
     .type _default_start, %function
     _default_start:
-        // Init .data and .bss
-        bl      _init_segments
+        // Read MPIDR into R0
+        mrc     p15, 0, r0, c0, c0, 5
+        // Check if core ID (bottom 8 bits) is zero
+        ands    r0, r0, 0xFF
+        bne     1f
+        // Primary core (core 0) can do normal start-up
+        mov     r4, r0
+        bl      _asm_init_segments
+        mov     r0, r4
+        b       2f
+    1:
+        // Secondary core needs to spin until some magic flag is set
+        mov     r4, r0
+        bl      _asm_secondary_core_park
+        mov     r0, r4
+    2:
         // Set stack pointer
-        ldr     sp, =_hyp_stack_high_end
+        ldr	    sp, =_hyp_stack_high_end
+        ldr	    r1, =_hyp_stack_size
+        muls    r1, r1, r0
+        subs    sp, sp, r1
         // Set the HVBAR (for EL2) to _vector_table
         ldr     r1, =_vector_table
         mcr     p15, 4, r1, c12, c0, 0
         // Mask IRQ and FIQ
-        mrs     r0, CPSR
-        orr     r0, {irq_fiq}
-        msr     CPSR, r0
+        mrs     r1, CPSR
+        orr     r1, {irq_fiq}
+        msr     CPSR, r1
         // Clear Thumb Exception bit
-        mrc     p15, 0, r0, c1, c0, 0
-        bic     r0, #0x40000000
-        mcr     p15, 0, r0, c1, c0, 0
+        mrc     p15, 0, r1, c1, c0, 0
+        bic     r1, #0x40000000
+        mcr     p15, 0, r1, c1, c0, 0
         // Allow VFP coprocessor access
-        mrc     p15, 0, r0, c1, c0, 2
-        orr     r0, r0, #0xF00000
-        mcr     p15, 0, r0, c1, c0, 2
+        mrc     p15, 0, r1, c1, c0, 2
+        orr     r1, r1, #0xF00000
+        mcr     p15, 0, r1, c1, c0, 2
         // Enable VFP
-        mov     r0, #0x40000000
-        vmsr    fpexc, r0
+        mov     r1, #0x40000000
+        vmsr    fpexc, r1
         // Zero all registers before calling kmain
-        mov     r0, 0
         mov     r1, 0
         mov     r2, 0
         mov     r3, 0
@@ -47,11 +68,28 @@ core::arch::global_asm!(
         mov     r10, 0
         mov     r11, 0
         mov     r12, 0
-        // Jump to application
+        cmp     r0, 0
+        bne     3f
+        // Jump to application with primary core
         bl      kmain
         // In case the application returns, loop forever
         b       .
+    3:
+        // Jump to application with secondary core
+        bl      kmain_secondary
+        // In case the application returns, loop forever
+        b       .
     .size _default_start, . - _default_start
+    .popsection
     "#,
     irq_fiq = const aarch32_cpu::register::Cpsr::new_with_raw_value(0).with_i(true).with_f(true).raw_value()
 );
+
+#[unsafe(naked)]
+#[unsafe(no_mangle)]
+extern "C" fn _asm_default_secondary_core_park() {
+    core::arch::naked_asm!(
+        // just spin
+        "b       ."
+    )
+}
diff --git a/aarch32-rt/src/arch_v8_hyp/hvc.rs b/aarch32-rt/src/arch_v8_hyp/hvc.rs
index 3abfa280..79b63023 100644
--- a/aarch32-rt/src/arch_v8_hyp/hvc.rs
+++ b/aarch32-rt/src/arch_v8_hyp/hvc.rs
@@ -1,16 +1,18 @@
 //! HVC handler for Armv8-R at EL2
 
+// # _asm_default_hvc_handler
+//
+// Called from the vector table when we have an hypervisor call.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _hvc_handler(hsr: u32, frame: &Frame) -> u32;`
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
 
-    .section .text._asm_default_hvc_handler
-
-    // Called from the vector table when we have an hypervisor call.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _hvc_handler(hsr: u32, frame: &Frame) -> u32;`
+    .pushsection .text._asm_default_hvc_handler
+    .arm
     .global _asm_default_hvc_handler
     .type _asm_default_hvc_handler, %function
     _asm_default_hvc_handler:
@@ -41,5 +43,6 @@ core::arch::global_asm!(
         pop     {{ r12, lr }}             // Pop R12 and LR from stack
         eret                              // Return from the asm handler
     .size _asm_default_hvc_handler, . - _asm_default_hvc_handler
+    .popsection
     "#,
 );
diff --git a/aarch32-rt/src/arch_v8_hyp/interrupt.rs b/aarch32-rt/src/arch_v8_hyp/interrupt.rs
index 0aff8722..9b2c860d 100644
--- a/aarch32-rt/src/arch_v8_hyp/interrupt.rs
+++ b/aarch32-rt/src/arch_v8_hyp/interrupt.rs
@@ -1,16 +1,20 @@
 //! IRQ handler for Armv8-R at EL2
 
+// # _asm_default_irq_handler
+//
+// Called from the vector table when we have an interrupt.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _irq_handler();`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-
-    .section .text._asm_default_irq_handler
-
-    // Called from the vector table when we have an interrupt.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _irq_handler();`
+    .pushsection .text._asm_default_irq_handler
+    .arm
     .global _asm_default_irq_handler
     .type _asm_default_irq_handler, %function
     _asm_default_irq_handler:
@@ -34,5 +38,6 @@ core::arch::global_asm!(
         pop     {{ r0-r3, r12, lr }}      // Pop preserved registers (1)
         eret                              // Return from the asm handler
     .size _asm_default_irq_handler, . - _asm_default_irq_handler
+    .popsection
     "#,
 );
diff --git a/aarch32-rt/src/arch_v8_hyp/svc.rs b/aarch32-rt/src/arch_v8_hyp/svc.rs
index 56a5f184..08a26710 100644
--- a/aarch32-rt/src/arch_v8_hyp/svc.rs
+++ b/aarch32-rt/src/arch_v8_hyp/svc.rs
@@ -1,21 +1,26 @@
 //! SVC handler for Armv8-R at EL2
 
+// # _asm_default_svc_handler
+//
+// Called from the vector table when we have an hypervisor call from Hyp
+// mode (which seems to end up in this SVC handler).
+//
+// Saves state and calls a C-compatible handler like `extern "C" fn
+// _hvc_handler(hsr: u32, frame: &Frame) -> u32;`
+//
+// NOTE: We call '_hvc_handler' rather than '_svc_handler', because we are
+// passing the Hypervisor Syndrome Register contents, rather trying to parse
+// the HVC instruction.
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
 
-    // Called from the vector table when we have an hypervisor call from Hyp
-    // mode (which seems to end up in this SVC handler).
-    //
-    // Saves state and calls a C-compatible handler like `extern "C" fn
-    // _hvc_handler(hsr: u32, frame: &Frame) -> u32;`
-    //
-    // NOTE: We call '_hvc_handler' rather than '_svc_handler', because we are
-    // passing the Hypervisor Syndrome Register contents, rather trying to parse
-    // the HVC instruction.
-    .section .text._asm_default_svc_handler
+    .pushsection .text._asm_default_svc_handler
     .arm
     .global _asm_default_svc_handler
     .type _asm_default_svc_handler, %function
@@ -47,5 +52,6 @@ core::arch::global_asm!(
         pop     {{ r12, lr }}             // Pop R12 and LR from stack to undo (1)
         eret                              // Return from the asm handler
     .size _asm_default_svc_handler, . - _asm_default_svc_handler
+    .popsection
     "#,
 );
diff --git a/aarch32-rt/src/arch_v8_hyp/undefined.rs b/aarch32-rt/src/arch_v8_hyp/undefined.rs
index 3e49e990..b577c878 100644
--- a/aarch32-rt/src/arch_v8_hyp/undefined.rs
+++ b/aarch32-rt/src/arch_v8_hyp/undefined.rs
@@ -1,17 +1,22 @@
 //! Undefined handler for Armv8-R at EL2
 
+// # _asm_default_undefined_handler
+//
+// Called from the vector table when we have an undefined exception.
+// Saves state and calls a C-compatible handler like
+// `extern "C" fn _undefined_handler(addr: usize) -> usize;`
+// or
+// `extern "C" fn _undefined_handler(addr: usize) -> !;`
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp3
-
-    // Called from the vector table when we have an undefined exception.
-    // Saves state and calls a C-compatible handler like
-    // `extern "C" fn _undefined_handler(addr: usize) -> usize;`
-    // or
-    // `extern "C" fn _undefined_handler(addr: usize) -> !;`
-    .section .text._asm_default_undefined_handler
+    .pushsection .text._asm_default_undefined_handler
+    .arm
     .global _asm_default_undefined_handler
     .type _asm_default_undefined_handler, %function
     _asm_default_undefined_handler:
@@ -35,5 +40,6 @@ core::arch::global_asm!(
         pop     {{ r0-r3, r12, lr }}      // Pop preserved registers (1)
         eret                              // Return from the asm handler
     .size _asm_default_undefined_handler, . - _asm_default_undefined_handler
+    .popsection
     "#,
 );
diff --git a/aarch32-rt/src/lib.rs b/aarch32-rt/src/lib.rs
index 3393de34..1b2dc163 100644
--- a/aarch32-rt/src/lib.rs
+++ b/aarch32-rt/src/lib.rs
@@ -49,7 +49,7 @@
 //! ## Constants
 //!
 //! * `_num_cores` - the number of CPU core (and hence the number of copies of
-//!   each stack). Must be > 0.
+//!   each stack). Must be > 0. Defaults to 1.
 //! * `__sbss` - the start of zero-initialised data in RAM. Must be 4-byte
 //!   aligned.
 //! * `__ebss` - the end of zero-initialised data in RAM. Must be 4-byte
@@ -499,6 +499,56 @@
 //!   `_asm_default_fiq_handler` but you can override it. The provided default
 //!   just spins forever.
 //!
+//! ## SMP Support
+//!
+//! This library supports SMP operation on ARMv7-A, ARMv7-R and ARMv8-R.
+//!
+//! To enable SMP support, add `PROVIDE(_num_cores = N)` to your linker script
+//! (e.g. your `memory.x` file). This will cause space for 'N' copies of each
+//! stack to be reserved so that each core gets its own stack (see the section
+//! on 'Stacks', above).
+//!
+//! You must also write a function called `_asm_secondary_core_park` (which must
+//! be written in assembly, and not Rust, because it is executed before stacks
+//! and global memory are initialised). On start-up the bottom eight bits of
+//! MPIDR register are taken as the Core ID. On Core ID 0, normal start-up will
+//! occur. For non-zero Core IDs (so-called *secondary cores*), the cores call
+//! the `_asm_secondary_core_park` function, passing the core ID in `r0`. This
+//! function (which defaults to an infinite loop) should put the running core to
+//! sleep and cause it to wait for some sort of signal from Core 0. This allows
+//! Core 0 to complete the initialisation of global memory (`.data`, `.bss`,
+//! etc) before the secondary cores run (and those cores must not re-initialise
+//! global memory). After the cores have left the park routine and completed
+//! their local initialisation (i.e. set their stack pointers to their unique
+//! stacks), they execute the function `kmain_secondary` (recall that Core 0
+//! executes a function called `kmain`).
+//!
+//! In our example for the MPS3-AN536, we have the secondary core wait on a
+//! hardware register in one of the peripherals, because it has a known value at
+//! reset.
+//!
+//! ```rust,ignore
+//! #[unsafe(naked)]
+//! #[unsafe(no_mangle)]
+//! pub unsafe extern "C" fn _asm_secondary_core_park() {
+//!     core::arch::naked_asm!(
+//!         r#"
+//!         // Some hardware register
+//!         ldr     r0, =0xE020_2000
+//!     1:
+//!         // Wait until Core 0 does a 'sev'
+//!         wfe
+//!         // Spin until register is non-zero.
+//!         ldr     r1, [r0]  
+//!         cmp     r1, 0
+//!         beq     1b
+//!         // return to start-up
+//!         bx      lr
+//!     "#,
+//!     );
+//! }
+//! ```
+//!
 //! ## Outputs
 //!
 //! This library produces global symbols called:
@@ -517,11 +567,15 @@
 //! * `_asm_default_irq_handler` - assembly language trampoline that calls
 //!   `_irq_handler`
 //! * `_asm_default_fiq_handler` - an FIQ handler that just spins
+//! * `_asm_default_secondary_core_park` - spins secondary cores forever
 //! * `_default_handler` - a C compatible function that spins forever.
-//! * `_init_segments` - initialises `.bss` and `.data` and zeroes the stacks
-//! * `_stack_setup_preallocated` - initialises UND, SVC, ABT, IRQ, FIQ and SYS
+//! * `_asm_init_segments` - initialises `.bss` and `.data` and zeroes the
+//!   stacks
+//! * `_asm_core_start` - sets up stacks, enables FPU (if required), and jumps
+//!   to `kmain` or `kmain_secondary`. Takes the Core ID in `r0`.
+//! * `_asm_stack_setup_preallocated` - initialises UND, SVC, ABT, IRQ, FIQ and SYS
 //!   stacks from the `.stacks` section defined in link.x, based on
-//!   _xxx_stack_size values, and the core number given in `r0`
+//!   `_xxx_stack_size` values. Takes the Core ID in `r0`.
 //! * `_xxx_stack_high_end` and `_xxx_stack_low_end` where the former is the top
 //!   and the latter the bottom of the stack for each mode (`und`, `svc`, `abt`,
 //!   `irq`, `fiq`, `sys`)
@@ -722,21 +776,99 @@ core::arch::global_asm!(
     "#
 );
 
-// Shared library routines for all architectures
+// # _asm_core_start
+//
+// The _asm_core_start function takes the core number in r0. It sets
+// up the stack pointers, the FPU (if required), and jumps to kmain.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
+    .pushsection .text._asm_core_start
+    .global _asm_core_start
+    .type _asm_core_start, %function
+    _asm_core_start:
+        // Keep our core number for later
+        mov     r12, r0
+        // Set up stacks (core number in r0)
+        bl      _asm_stack_setup_preallocated
+    "#,
+    #[cfg(armv6_or_higher)]
+    r#"
+        // Clear Thumb Exception bit
+        mrc     p15, 0, r0, c1, c0, 0
+        bic     r0, #0x40000000
+        mcr     p15, 0, r0, c1, c0, 0
+    "#,
+    #[cfg(any(target_abi = "eabihf", feature = "eabi-fpu"))]
+    r#"
+        // Allow VFP coprocessor access
+        mrc     p15, 0, r0, c1, c0, 2
+        orr     r0, r0, #0xF00000
+        mcr     p15, 0, r0, c1, c0, 2
+        // Enable VFP
+        mov     r0, #0x40000000
+        vmsr    fpexc, r0
+    "#,
+    r#"
+        // Zero all registers before calling kmain (except r0)
+        mov     r1, 0
+        mov     r2, 0
+        mov     r3, 0
+        mov     r4, 0
+        mov     r5, 0
+        mov     r6, 0
+        mov     r7, 0
+        mov     r8, 0
+        mov     r9, 0
+        mov     r10, 0
+        mov     r11, 0
+        // Check if this is the primary core
+        mov     r0, r12
+        mov     r12, 0
+        cmp     r0, 0
+        bne     1f
+        // Jump to application with primary core
+        bl      kmain
+        // In case the application returns, loop forever
+        b       .
+    1:
+        // Jump to application with secondary core
+        bl      kmain_secondary
+        // In case the application returns, loop forever
+        b       .
+    .size _asm_core_start, . - _asm_core_start
+    .popsection
+    "#
+);
 
-    // Configure a stack for every mode. Leaves you in sys mode.
-    //
-    // Pass the core number in r0
-    .pushsection .text._stack_setup_preallocated
-    .global _stack_setup_preallocated
-    .arm
-    .type _stack_setup_preallocated, %function
-    _stack_setup_preallocated:
+/// Spins secondary cores.
+#[unsafe(no_mangle)]
+#[cfg(target_arch = "arm")]
+pub extern "C" fn _default_kmain_secondary() {
+    #[cfg(armv7_or_higher)]
+    loop {
+        aarch32_cpu::asm::wfe();
+    }
+    #[cfg(not(armv7_or_higher))]
+    loop {
+        core::hint::spin_loop();
+    }
+}
+
+// # _asm_stack_setup_preallocated
+//
+// Configure a stack for every mode. Leaves you in sys mode.
+//
+// Pass the core number in r0
+#[cfg(target_arch = "arm")]
+core::arch::global_asm!(
+    r#"
+    .pushsection .text._asm_stack_setup_preallocated
+    .global _asm_stack_setup_preallocated
+    .type _asm_stack_setup_preallocated, %function
+    _asm_stack_setup_preallocated:
         // Save LR from whatever mode we're currently in
         mov     r3, lr
         // (we might not be in the same mode when we return).
@@ -778,49 +910,7 @@ core::arch::global_asm!(
         subs    sp, r2, r1
         // return to caller
         bx      r3
-    .size _stack_setup_preallocated, . - _stack_setup_preallocated
-    .popsection
-
-    // Initialises stacks, .data and .bss
-    .pushsection .text._init_segments
-    .arm
-    .global _init_segments
-    .type _init_segments, %function
-    _init_segments:
-        // Zero .bss
-        ldr     r0, =__sbss
-        ldr     r1, =__ebss
-        mov     r2, 0
-    0:
-        cmp     r1, r0
-        beq     1f
-        stm     r0!, {{r2}}
-        b       0b
-    1:
-        // Zero the stacks
-        ldr     r0, =_stacks_low_end
-        ldr     r1, =_stacks_high_end
-        mov     r2, 0
-    0:
-        cmp     r1, r0
-        beq     1f
-        stm     r0!, {{r2}}
-        b       0b
-    1:
-        // Initialise .data
-        ldr     r0, =__sdata
-        ldr     r1, =__edata
-        ldr     r2, =__sidata
-    0:
-        cmp     r1, r0
-        beq     1f
-        ldm     r2!, {{r3}}
-        stm     r0!, {{r3}}
-        b       0b
-    1:
-    	// return to caller
-        bx      lr
-    .size _init_segments, . - _init_segments
+    .size _asm_stack_setup_preallocated, . - _asm_stack_setup_preallocated
     .popsection
     "#,
     und_mode = const {
@@ -867,18 +957,73 @@ core::arch::global_asm!(
     },
 );
 
+// # _asm_init_segments
+//
+// Initialises stacks, .data and .bss
+#[cfg(target_arch = "arm")]
+core::arch::global_asm!(
+    r#"
+    // Work around https://github.com/rust-lang/rust/issues/127269
+    .fpu vfp2
+
+    .pushsection .text._asm_init_segments
+    .global _asm_init_segments
+    .type _asm_init_segments, %function
+    _asm_init_segments:
+        // Zero .bss
+        ldr     r0, =__sbss
+        ldr     r1, =__ebss
+        mov     r2, 0
+    0:
+        cmp     r1, r0
+        beq     1f
+        stm     r0!, {{r2}}
+        b       0b
+    1:
+        // Zero the stacks
+        ldr     r0, =_stacks_low_end
+        ldr     r1, =_stacks_high_end
+        mov     r2, 0
+    0:
+        cmp     r1, r0
+        beq     1f
+        stm     r0!, {{r2}}
+        b       0b
+    1:
+        // Initialise .data
+        ldr     r0, =__sdata
+        ldr     r1, =__edata
+        ldr     r2, =__sidata
+    0:
+        cmp     r1, r0
+        beq     1f
+        ldm     r2!, {{r3}}
+        stm     r0!, {{r3}}
+        b       0b
+    1:
+    	// return to caller
+        bx      lr
+    .size _asm_init_segments, . - _asm_init_segments
+    .popsection
+    "#,
+);
+
+// # _asm_default_fiq_handler
+//
 // Default asm FIQ exception handler (it's just a spin-loop)
 //
 // We end up here if a FIQ fires and the weak 'PROVIDE' in the link.x
 // file hasn't been over-ridden.
 //
 // Cannot be a Rust/C function because it can only touch registers R8 to R12, SP and LR
+//
+// This function must produce A32 machine code, because it's called by the Vector Table
+// with a raw PC load and the Vector Table is always in A32 machine code.
 #[cfg(target_arch = "arm")]
 core::arch::global_asm!(
     r#"
     .pushsection .text._asm_default_fiq_handler
-
-    // Our default FIQ handler
+    .arm
     .global _asm_default_fiq_handler
     .type _asm_default_fiq_handler, %function
     _asm_default_fiq_handler:
diff --git a/arm-targets/README.md b/arm-targets/README.md
index 7b4bd308..989d643f 100644
--- a/arm-targets/README.md
+++ b/arm-targets/README.md
@@ -19,12 +19,12 @@ cargo:rustc-cfg=arm_architecture="v7-r"
 cargo:rustc-check-cfg=cfg(arm_architecture, values("v6-m", "v7-m", "v7e-m", "v8-m.base", "v8-m.main", "v7-r", "v8-r", "v7-a", "v8-a"))
 cargo:rustc-cfg=arm_isa="A32"
 cargo:rustc-check-cfg=cfg(arm_isa, values("A64", "A32", "T32"))
-cargo:rustc-check-cfg=cfg(arm_v4t_or_higher)
-cargo:rustc-check-cfg=cfg(arm_v5te_or_higher)
-cargo:rustc-check-cfg=cfg(arm_v6_or_higher)
-cargo:rustc-check-cfg=cfg(arm_v7_or_higher)
-cargo:rustc-check-cfg=cfg(arm_v7_or_lower)
-cargo:rustc-check-cfg=cfg(arm_v8_or_lower)
+cargo:rustc-check-cfg=cfg(armv4t_or_higher)
+cargo:rustc-check-cfg=cfg(armv5te_or_higher)
+cargo:rustc-check-cfg=cfg(armv6_or_higher)
+cargo:rustc-check-cfg=cfg(armv7_or_higher)
+cargo:rustc-check-cfg=cfg(armv7_or_lower)
+cargo:rustc-check-cfg=cfg(armv8_or_lower)
 ```
 
 This allows you to write Rust code in your firmware like:
diff --git a/examples/mps3-an536-el2/memory.x b/examples/mps3-an536-el2/memory.x
index 00158077..89a0be65 100644
--- a/examples/mps3-an536-el2/memory.x
+++ b/examples/mps3-an536-el2/memory.x
@@ -15,30 +15,12 @@ REGION_ALIAS("CODE", QSPI);
 REGION_ALIAS("DATA", BRAM);
 REGION_ALIAS("STACKS", BRAM);
 
-SECTIONS {
-    /* ### Interrupt Handler Entries
-     *
-     * The IRQ handler walks this section to find registered
-     * interrupt handlers
-     */
-    .irq_entries : ALIGN(4)
-    {
-        /* We put this in the header */
-        __irq_entries_start = .;
-        /* Here are the entries */
-        KEEP(*(.irq_entries));
-        /* Keep this block a nice round size */
-        . = ALIGN(4);
-        /* We put this in the header */
-        __irq_entries_end = .;
-    } > CODE
-} INSERT AFTER .text;
-
-
 PROVIDE(_hyp_stack_size = 16K);
-PROVIDE(_und_stack_size = 16K);
-PROVIDE(_svc_stack_size = 16K);
-PROVIDE(_abt_stack_size = 16K);
-PROVIDE(_irq_stack_size = 64);
-PROVIDE(_fiq_stack_size = 64);
-PROVIDE(_sys_stack_size = 16K);
+PROVIDE(_und_stack_size = 8);
+PROVIDE(_svc_stack_size = 8);
+PROVIDE(_abt_stack_size = 8);
+PROVIDE(_irq_stack_size = 8);
+PROVIDE(_fiq_stack_size = 8);
+PROVIDE(_sys_stack_size = 8);
+
+PROVIDE(_num_cores = 2);
\ No newline at end of file
diff --git a/examples/mps3-an536-smp/.cargo/config.toml b/examples/mps3-an536-smp/.cargo/config.toml
deleted file mode 100644
index b3ab14b1..00000000
--- a/examples/mps3-an536-smp/.cargo/config.toml
+++ /dev/null
@@ -1,10 +0,0 @@
-[target.armv8r-none-eabihf]
-# Note, this requires QEMU 9 or higher
-runner = "qemu-system-arm -machine mps3-an536 -cpu cortex-r52 -semihosting -nographic -audio none -smp 2 -kernel"
-
-[target.thumbv8r-none-eabihf]
-# Note, this requires QEMU 9 or higher
-runner = "qemu-system-arm -machine mps3-an536 -cpu cortex-r52 -semihosting -nographic -audio none -smp 2 -kernel"
-
-[build]
-target = "armv8r-none-eabihf"
diff --git a/examples/mps3-an536-smp/Cargo.toml b/examples/mps3-an536-smp/Cargo.toml
deleted file mode 100644
index e7d97c45..00000000
--- a/examples/mps3-an536-smp/Cargo.toml
+++ /dev/null
@@ -1,32 +0,0 @@
-[package]
-authors = [
-	"Jonathan Pallant <jonathan.pallant@ferrous-systems.com>",
-	"The Embedded Devices Working Group Arm Team <arm@teams.rust-embedded.org>"
-]
-default-run = "smp-test"
-description = "Examples for SMP MPS3-AN536 device (2x Arm Cortex-R52)"
-edition = "2024"
-homepage = "https://github.com/rust-embedded/aarch32"
-license = "MIT OR Apache-2.0"
-name = "mps3-an536-smp"
-publish = false
-readme = "README.md"
-repository = "https://github.com/rust-embedded/aarch32.git"
-version = "0.0.0"
-
-[dependencies]
-aarch32-cpu = { path = "../../aarch32-cpu", features = ["critical-section-multi-core"] }
-aarch32-rt = { path = "../../aarch32-rt" }
-arm-gic = "0.8.1"
-critical-section = "1.2.0"
-heapless = "0.9.1"
-libm = "0.2.15"
-semihosting = { version = "0.1.18", features = ["stdio"] }
-
-[build-dependencies]
-arm-targets = { version = "0.4.3", path = "../../arm-targets" }
-
-[features]
-eabi-fpu = ["aarch32-rt/eabi-fpu"]
-fpu-d32 = ["aarch32-rt/fpu-d32"]
-svc-stack-interrupt = ["aarch32-rt/svc-stack-interrupt"]
diff --git a/examples/mps3-an536-smp/README.md b/examples/mps3-an536-smp/README.md
deleted file mode 100644
index d43a504e..00000000
--- a/examples/mps3-an536-smp/README.md
+++ /dev/null
@@ -1,106 +0,0 @@
-# Examples for Arm MPS3-AN536
-
-This package contains example binaries for the Arm MPS3-AN536 evaluation system,
-featuring one or two Arm Cortex-R52 processor cores. This crate is tested on the
-following targets:
-
-- `armv8r-none-eabihf` - ARMv8-R AArch32, hard-float, Arm mode
-- `thumbv8r-none-eabihf` - ARMv8-R AArch32, hard-float, Thumb mode
-
-The repo-level [`.cargo/config.toml`] will ensure the code runs on the
-appropriate QEMU configuration.
-
-As of Rust 1.92, `armv8r-none-eabihf` is a Tier 2 target and so any stable
-release from 1.92 or newer should work for that target. However,
-`thumbv8r-none-eabihf` is still a Tier 3 target, which means Nightly Rust is
-required. This folder contains a [`rust-toolchain.toml`] which pins us to a
-specific release of nightly that is known to work.
-
-We have only tested this crate on `qemu-system-arm` emulating the Arm
-MPS3-AN536, not the real thing.
-
-[`.cargo/config.toml`]: ../../.cargo/config.toml
-[`rust-toolchain.toml`]: ./rust-toolchain.toml
-
-## Running
-
-Run these examples as follows:
-
-```console
-$ cargo run
-    Finished `dev` profile [unoptimized + debuginfo] target(s) in 0.09s
-     Running `qemu-system-arm -machine mps3-an536 -cpu cortex-r52 -semihosting -nographic -audio none -smp 2 -kernel target/armv8r-none-eabihf/debug/smp-test`
-I am core 0 - Mpidr(80000000)
-I am core 1 - Mpidr(80000001)
-CAS test passed
-CS Mutex test passed
-Stack usage report:
-SYS0 Stack =   2680 used of  16384 bytes (016%) @ 0x1006bf80..0x1006ff80
-FIQ0 Stack =      0 used of     64 bytes (000%) @ 0x1006ff80..0x1006ffc0
-IRQ0 Stack =      0 used of     64 bytes (000%) @ 0x1006ffc0..0x10070000
-ABT0 Stack =      0 used of  16384 bytes (000%) @ 0x10070000..0x10074000
-SVC0 Stack =      0 used of  16384 bytes (000%) @ 0x10074000..0x10078000
-UND0 Stack =      0 used of  16384 bytes (000%) @ 0x10078000..0x1007c000
-HYP0 Stack =      0 used of  16384 bytes (000%) @ 0x1007c000..0x10080000
-SYS1 Stack =    680 used of  16384 bytes (004%) @ 0x10000018..0x10004018
-FIQ1 Stack =      0 used of     64 bytes (000%) @ 0x10004018..0x10004058
-IRQ1 Stack =      0 used of     64 bytes (000%) @ 0x10004058..0x10004098
-ABT1 Stack =      0 used of  16384 bytes (000%) @ 0x10004098..0x10008098
-SVC1 Stack =      0 used of  16384 bytes (000%) @ 0x10008098..0x1000c098
-UND1 Stack =      0 used of  16384 bytes (000%) @ 0x1000c098..0x10010098
-HYP1 Stack =      0 used of  16384 bytes (000%) @ 0x10010098..0x10014098
-$ cargo run --target thumbv8r-none-eabihf -Zbuild-std=core
-    Finished `dev` profile [unoptimized + debuginfo] target(s) in 0.08s
-     Running `qemu-system-arm -machine mps3-an536 -cpu cortex-r52 -semihosting -nographic -audio none -smp 2 -kernel target/thumbv8r-none-eabihf/debug/smp-test`
-I am core 0 - Mpidr(80000000)
-I am core 1 - Mpidr(80000001)
-CAS test passed
-CS Mutex test passed
-Stack usage report:
-SYS0 Stack =   4840 used of  16384 bytes (029%) @ 0x1006bf80..0x1006ff80
-FIQ0 Stack =      0 used of     64 bytes (000%) @ 0x1006ff80..0x1006ffc0
-IRQ0 Stack =      0 used of     64 bytes (000%) @ 0x1006ffc0..0x10070000
-ABT0 Stack =      0 used of  16384 bytes (000%) @ 0x10070000..0x10074000
-SVC0 Stack =      0 used of  16384 bytes (000%) @ 0x10074000..0x10078000
-UND0 Stack =      0 used of  16384 bytes (000%) @ 0x10078000..0x1007c000
-HYP0 Stack =      0 used of  16384 bytes (000%) @ 0x1007c000..0x10080000
-SYS1 Stack =   1568 used of  16384 bytes (009%) @ 0x10000018..0x10004018
-FIQ1 Stack =      0 used of     64 bytes (000%) @ 0x10004018..0x10004058
-IRQ1 Stack =      0 used of     64 bytes (000%) @ 0x10004058..0x10004098
-ABT1 Stack =      0 used of  16384 bytes (000%) @ 0x10004098..0x10008098
-SVC1 Stack =      0 used of  16384 bytes (000%) @ 0x10008098..0x1000c098
-UND1 Stack =      0 used of  16384 bytes (000%) @ 0x1000c098..0x10010098
-HYP1 Stack =      0 used of  16384 bytes (000%) @ 0x10010098..0x10014098
-```
-
-## Debugging
-
-You can start a GDB server by adding `-- -s -S` to the end of the `cargo run`
-command, and the connect with GDB as follows:
-
-```console
-$ cargo run --bin hello -- -s -S
-# QEMU runs and hangs waiting for a connection. In another terminal run:
-$ arm-none-eabi-gdb -x commands.gdb target/armv8r-none-eabihf/debug/hello
-# GDB will start and connect to QEMU's GDB server. The commands.gdb file sets up some useful defaults.
-```
-
-## Minimum Supported Rust Version (MSRV)
-
-These examples are guaranteed to compile on the version of Rust given in the
-[`rust-toolchain.toml`] file. These examples are not version controlled and we
-may change the MSRV at any time.
-
-## Licence
-
-- Copyright (c) Ferrous Systems
-- Copyright (c) The Rust Embedded Devices Working Group developers
-
-Licensed under either [MIT](../LICENSE-MIT) or [Apache-2.0](../LICENSE-APACHE) at
-your option.
-
-## Contribution
-
-Unless you explicitly state otherwise, any contribution intentionally submitted
-for inclusion in the work by you shall be licensed as above, without any
-additional terms or conditions.
diff --git a/examples/mps3-an536-smp/build.rs b/examples/mps3-an536-smp/build.rs
deleted file mode 100644
index 0ae57894..00000000
--- a/examples/mps3-an536-smp/build.rs
+++ /dev/null
@@ -1,26 +0,0 @@
-//! # Build script for the MPS3-AN536 Examples
-//!
-//! This script only executes when using `cargo` to build the project.
-//!
-//! Copyright (c) Ferrous Systems, 2025
-
-use std::io::Write;
-
-fn main() {
-    arm_targets::process();
-    write("memory.x", include_bytes!("memory.x"));
-    // Use the aarch32-rt linker script
-    println!("cargo:rustc-link-arg=-Tlink.x");
-}
-
-fn write(file: &str, contents: &[u8]) {
-    // Put linker file in our output directory and ensure it's on the
-    // linker search path.
-    let out = &std::path::PathBuf::from(std::env::var_os("OUT_DIR").unwrap());
-    std::fs::File::create(out.join(file))
-        .unwrap()
-        .write_all(contents)
-        .unwrap();
-    println!("cargo:rustc-link-search={}", out.display());
-    println!("cargo:rerun-if-changed={}", file);
-}
diff --git a/examples/mps3-an536-smp/commands.gdb b/examples/mps3-an536-smp/commands.gdb
deleted file mode 100644
index 0634363b..00000000
--- a/examples/mps3-an536-smp/commands.gdb
+++ /dev/null
@@ -1,13 +0,0 @@
-target extended-remote :1234
-break kmain
-break _asm_undefined_handler
-break _asm_svc_handler
-break _asm_prefetch_abort_handler
-break _asm_data_abort_handler
-break _asm_irq_handler
-break _asm_fiq_handler
-layout asm
-layout regs
-set logging file ./target/debug.log
-set logging enabled on
-stepi
diff --git a/examples/mps3-an536-smp/memory.x b/examples/mps3-an536-smp/memory.x
deleted file mode 100644
index c4aa51bb..00000000
--- a/examples/mps3-an536-smp/memory.x
+++ /dev/null
@@ -1,19 +0,0 @@
-/*
-Memory configuration for the MPS3-AN536 machine.
-
-See https://github.com/qemu/qemu/blob/master/hw/arm/mps3r.c
-*/
-
-MEMORY {
-    QSPI : ORIGIN = 0x08000000, LENGTH = 8M
-    BRAM : ORIGIN = 0x10000000, LENGTH = 512K
-    DDR  : ORIGIN = 0x20000000, LENGTH = 1536M
-}
-
-REGION_ALIAS("VECTORS", QSPI);
-REGION_ALIAS("CODE", QSPI);
-REGION_ALIAS("DATA", BRAM);
-REGION_ALIAS("STACKS", BRAM);
-
-PROVIDE(_num_cores = 2);
-PROVIDE(kmain2 = default_kmain2);
diff --git a/examples/mps3-an536-smp/src/lib.rs b/examples/mps3-an536-smp/src/lib.rs
deleted file mode 100644
index d89d09ea..00000000
--- a/examples/mps3-an536-smp/src/lib.rs
+++ /dev/null
@@ -1,308 +0,0 @@
-//! Common code for all examples
-//!
-//! ## Interrupt Map
-//!
-//! | Interrupt ID | Description                  |
-//! |--------------|------------------------------|
-//! | `EXTPPI0[0]` | UART 0 Receive Interrupt    |
-//! | `EXTPPI0[1]` | UART 0 Transmit Interrupt   |
-//! | `EXTPPI0[2]` | UART 0 Combined Interrupt   |
-//! | `EXTPPI0[3]` | UART 0 Overflow             |
-//! | `EXTPPI1[0]` | UART 1 Receive Interrupt    |
-//! | `EXTPPI1[1]` | UART 1 Transmit Interrupt   |
-//! | `EXTPPI1[2]` | UART 1 Combined Interrupt   |
-//! | `EXTPPI1[3]` | UART 1 Overflow             |
-//! | `SP[0]`      | WDG                         |
-//! | `SP[1]`      | DualTimer 1                 |
-//! | `SP[2]`      | DualTimer 2                 |
-//! | `SP[3]`      | DualTimer Combined          |
-//! | `SP[4]`      | RTC                         |
-//! | `SP[5]`      | UART 2 Receive Interrupt    |
-//! | `SP[6]`      | UART 2 Transmit Interrupt   |
-//! | `SP[7]`      | UART 3 Receive Interrupt    |
-//! | `SP[8]`      | UART 3 Transmit Interrupt   |
-//! | `SP[9]`      | UART 4 Receive Interrupt    |
-//! | `SP[10]`     | UART 4 Transmit Interrupt   |
-//! | `SP[11]`     | UART 5 Receive Interrupt    |
-//! | `SP[12]`     | UART 5 Transmit Interrupt   |
-//! | `SP[13]`     | UART 2 Combined Interrupt   |
-//! | `SP[14]`     | UART 3 Combined Interrupt   |
-//! | `SP[15]`     | UART 4 Combined Interrupt   |
-//! | `SP[16]`     | UART 5 Combined Interrupt   |
-//! | `SP[17]`     | UART Overflow (2, 3, 4 & 5) |
-//! | `SP[18]`     | Ethernet                    |
-//! | `SP[19]`     | USB                         |
-//! | `SP[20]`     | FPGA Audio I2S              |
-//! | `SP[21]`     | Touch Screen                |
-//! | `SP[22]`     | SPI ADC                     |
-//! | `SP[23]`     | SPI Shield 0                |
-//! | `SP[24]`     | SPI Shield 1                |
-//! | `SP[25]`     | HDCLCD Interrupt            |
-//! | `SP[26]`     | GPIO 0 Combined Interrupt   |
-//! | `SP[27]`     | GPIO 1 Combined Interrupt   |
-//! | `SP[28]`     | GPIO 2 Combined Interrupt   |
-//! | `SP[29]`     | GPIO 3 Combined Interrupt   |
-//! | `SP[30..=45]`| GPIO 0.x Interrupt          |
-//! | `SP[46..=61]`| GPIO 1.x Interrupt          |
-//! | `SP[62..=77]`| GPIO 2.x Interrupt          |
-//! | `SP[78..=93]`| GPIO 3.x Interrupt          |
-//!
-//! * Interrupt ID `SP[x]` are shared across cores
-//! * Interrupt ID `EXTPPI0[x]` is only available on Core 0
-//! * Interrupt ID `EXTPPI1[x]` is only available on Core 1
-
-#![no_std]
-
-use aarch32_cpu::register::{Cpsr, Hactlr, cpsr::ProcessorMode};
-
-use core::sync::atomic::{AtomicBool, Ordering};
-
-/// The PPI for the virutal timer, according to the Cortex-R52 Technical Reference Manual,
-/// Table 10-3: PPI assignments.
-///
-/// This corresponds to Interrupt ID 27.
-pub const VIRTUAL_TIMER_PPI: arm_gic::IntId = arm_gic::IntId::ppi(11);
-
-#[cfg(not(arm_architecture = "v8-r"))]
-compile_error!("This example is only compatible to the ARMv8-R architecture");
-
-static WANT_PANIC: AtomicBool = AtomicBool::new(false);
-
-/// Called when the application raises an unrecoverable `panic!`.
-///
-/// Prints the panic to the console and then exits QEMU using a semihosting
-/// breakpoint.
-#[panic_handler]
-#[cfg(target_os = "none")]
-fn panic(info: &core::panic::PanicInfo) -> ! {
-    semihosting::println!("PANIC: {:#?}", info);
-    if WANT_PANIC.load(Ordering::Relaxed) {
-        exit(0);
-    } else {
-        exit(1);
-    }
-}
-
-/// Set the panic function as no longer returning a failure code via semihosting
-pub fn want_panic() {
-    WANT_PANIC.store(true, Ordering::Relaxed);
-}
-
-/// Exit from QEMU with code
-pub fn exit(code: i32) -> ! {
-    stack_dump();
-    semihosting::process::exit(code)
-}
-
-/// Print stack using to semihosting output for each stack
-///
-/// Produces output like:
-///
-/// ```text
-/// Stack usage report:
-/// UND1 Stack =      0 used of  16384 bytes (000%) @ 0x10057f00..0x1005bf00
-/// UND0 Stack =      0 used of  16384 bytes (000%) @ 0x1005bf00..0x1005ff00
-/// SVC1 Stack =      0 used of  16384 bytes (000%) @ 0x1005ff00..0x10063f00
-/// SVC0 Stack =      0 used of  16384 bytes (000%) @ 0x10063f00..0x10067f00
-/// ABT1 Stack =      0 used of  16384 bytes (000%) @ 0x10067f00..0x1006bf00
-/// ABT0 Stack =      0 used of  16384 bytes (000%) @ 0x1006bf00..0x1006ff00
-/// HYP1 Stack =      0 used of  16384 bytes (000%) @ 0x1006ff00..0x10073f00
-/// HYP0 Stack =      0 used of  16384 bytes (000%) @ 0x10073f00..0x10077f00
-/// IRQ1 Stack =      0 used of     64 bytes (000%) @ 0x10077f00..0x10077f40
-/// IRQ0 Stack =      0 used of     64 bytes (000%) @ 0x10077f40..0x10077f80
-/// FIQ1 Stack =      0 used of     64 bytes (000%) @ 0x10077f80..0x10077fc0
-/// FIQ0 Stack =      0 used of     64 bytes (000%) @ 0x10077fc0..0x10078000
-/// SYS1 Stack =    808 used of  16384 bytes (004%) @ 0x10078000..0x1007c000
-/// SYS0 Stack =   1432 used of  16384 bytes (008%) @ 0x1007c000..0x10080000
-/// ```
-fn stack_dump() {
-    use aarch32_cpu::stacks::stack_used_bytes;
-    use aarch32_rt::stacks::Stack;
-
-    semihosting::eprintln!("Stack usage report:");
-
-    unsafe {
-        for stack in Stack::iter() {
-            for core in (0..Stack::num_cores()).rev() {
-                let core_range = stack.range(core).unwrap();
-                let (total, used) = stack_used_bytes(core_range.clone());
-                let percent = used * 100 / total;
-                // Send to stderr, so it doesn't mix with expected output on stdout
-                semihosting::eprintln!(
-                    "{}{} Stack = {:6} used of {:6} bytes ({:03}%) @ {:08x?}",
-                    stack,
-                    core,
-                    used,
-                    total,
-                    percent,
-                    core_range
-                );
-            }
-        }
-    }
-}
-
-/// Create the ARM GIC driver
-///
-/// # Safety
-///
-/// Only call this function once, from Core 0.
-pub unsafe fn make_gic() -> arm_gic::gicv3::GicV3<'static> {
-    /// Offset from PERIPHBASE for GIC Distributor
-    const GICD_BASE_OFFSET: usize = 0x0000_0000usize;
-
-    /// Offset from PERIPHBASE for the first GIC Redistributor
-    const GICR_BASE_OFFSET: usize = 0x0010_0000usize;
-
-    // Get the GIC address by reading CBAR
-    let periphbase = aarch32_cpu::register::ImpCbar::read().periphbase();
-    semihosting::println!("Found PERIPHBASE {:010p}", periphbase);
-    let gicd_base = periphbase.wrapping_byte_add(GICD_BASE_OFFSET);
-    let gicr_base = periphbase.wrapping_byte_add(GICR_BASE_OFFSET);
-
-    // Initialise the GIC.
-    semihosting::println!(
-        "Creating GIC driver @ {:010p} / {:010p}",
-        gicd_base,
-        gicr_base
-    );
-    // SAFETY: `gicd_base` points to the valid GICD MMIO region as obtained from the
-    // hardware CBAR register. This pointer is used exclusively by this GIC instance.
-    let gicd = unsafe {
-        arm_gic::UniqueMmioPointer::new(core::ptr::NonNull::new(gicd_base.cast()).unwrap())
-    };
-    let gicr_base = core::ptr::NonNull::new(gicr_base.cast()).unwrap();
-    // SAFETY: The GICD and GICR base addresses point to valid GICv3 MMIO regions as
-    // obtained from the hardware CBAR register. This function is only called once
-    // (via Board::new()'s atomic guard), ensuring exclusive ownership of the GIC.
-    let mut gic = unsafe { arm_gic::gicv3::GicV3::new(gicd, gicr_base, 2, false) };
-    semihosting::println!("Calling git.setup(0)");
-    gic.setup(0);
-    arm_gic::gicv3::GicCpuInterface::set_priority_mask(0xFF);
-    gic
-}
-
-/// Release core1 from spin loop
-pub fn start_core1() {
-    let fpga_led = 0xE020_2000 as *mut u32;
-    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);
-    }
-}
-
-/// 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.
-///
-/// # Safety
-///
-/// This function should not be called manually. It should only be called on reset
-/// from the reset vector.
-#[unsafe(naked)]
-#[unsafe(no_mangle)]
-#[unsafe(link_section = ".text.startup")]
-#[instruction_set(arm::a32)]
-#[cfg(arm_architecture = "v8-r")]
-pub unsafe extern "C" fn _start() {
-    core::arch::naked_asm!(
-        r#"
-        // Read MPIDR into R0
-        mrc     p15, 0, r0, c0, c0, 5
-        ands    r0, r0, 0xFF
-        bne     core1
-    core0:
-        ldr     pc, =_default_start
-    core1:
-        // LED GPIO register base address
-        ldr     r0, =0xE0202000
-        mov     r1, #0
-    core1_spin:
-        wfe
-        // spin until an LED0 is on. We use the LED because unlike RAM this register resets to a known value.
-        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:
-        // Allow VFP coprocessor access
-        mrc     p15, 0, r0, c1, c0, 2
-        orr     r0, r0, #0xF00000
-        mcr     p15, 0, r0, c1, c0, 2
-        // Enable VFP
-        mov     r0, #0x40000000
-        vmsr    fpexc, r0
-        // 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
-        // set up our stacks - also switches to SYS mode
-        movs    r0, #1
-        bl      _stack_setup_preallocated
-        // Zero all registers before calling kmain2
-        mov     r0, 0
-        mov     r1, 0
-        mov     r2, 0
-        mov     r3, 0
-        mov     r4, 0
-        mov     r5, 0
-        mov     r6, 0
-        mov     r7, 0
-        mov     r8, 0
-        mov     r9, 0
-        mov     r10, 0
-        mov     r11, 0
-        mov     r12, 0
-        // call our kmain2 for core 1
-        bl      kmain2
-    "#,
-        hactlr_bits = const {
-            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 {
-            Cpsr::new_with_raw_value(0)
-                .with_mode(ProcessorMode::Sys)
-                .with_i(true)
-                .with_f(true)
-                .raw_value()
-        },
-    )
-}
-
-/// What a second core does when no `kmain2` is supplied.
-#[unsafe(no_mangle)]
-pub extern "C" fn default_kmain2() {
-    loop {
-        aarch32_cpu::asm::wfe();
-    }
-}
diff --git a/examples/mps3-an536/.cargo/config.toml b/examples/mps3-an536/.cargo/config.toml
index 6960eb34..218af892 100644
--- a/examples/mps3-an536/.cargo/config.toml
+++ b/examples/mps3-an536/.cargo/config.toml
@@ -1,10 +1,10 @@
 [target.armv8r-none-eabihf]
 # Note, this requires QEMU 9 or higher
-runner = "qemu-system-arm -machine mps3-an536 -cpu cortex-r52 -semihosting -nographic -audio none -kernel"
+runner = "qemu-system-arm -machine mps3-an536 -cpu cortex-r52 -semihosting -nographic -audio none -smp 2 -kernel"
 
 [target.thumbv8r-none-eabihf]
 # Note, this requires QEMU 9 or higher
-runner = "qemu-system-arm -machine mps3-an536 -cpu cortex-r52 -semihosting -nographic -audio none -kernel"
+runner = "qemu-system-arm -machine mps3-an536 -cpu cortex-r52 -semihosting -nographic -audio none -smp 2 -kernel"
 
 [build]
 target = "armv8r-none-eabihf"
\ No newline at end of file
diff --git a/examples/mps3-an536/memory.x b/examples/mps3-an536/memory.x
index 78257ebb..aefc73c5 100644
--- a/examples/mps3-an536/memory.x
+++ b/examples/mps3-an536/memory.x
@@ -44,3 +44,5 @@ PROVIDE(_sys_stack_size = 16K);
 PROVIDE(_stack_alignment = 64);
 PROVIDE(_inter_stack_padding = 64);
 PROVIDE(_region_alignment = 64K);
+
+PROVIDE(_num_cores = 2);
diff --git a/examples/mps3-an536/reference/el2_hello-armv8r-none-eabihf.out b/examples/mps3-an536/reference/el2_hello-armv8r-none-eabihf.out
deleted file mode 100644
index bf0ed132..00000000
--- a/examples/mps3-an536/reference/el2_hello-armv8r-none-eabihf.out
+++ /dev/null
@@ -1,27 +0,0 @@
-Hello, this is semihosting! x = 1.000, y = 2.000
-Region 0: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 1: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 2: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 3: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 4: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 5: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 6: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 7: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 8: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 9: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 10: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 11: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 12: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 13: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 14: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 15: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-PANIC: PanicInfo {
-    message: I am an example panic,
-    location: Location {
-        file: "src/bin/el2_hello.rs",
-        line: 28,
-        column: 5,
-    },
-    can_unwind: true,
-    force_no_backtrace: false,
-}
diff --git a/examples/mps3-an536/reference/el2_hello-thumbv8r-none-eabihf.out b/examples/mps3-an536/reference/el2_hello-thumbv8r-none-eabihf.out
deleted file mode 100644
index bf0ed132..00000000
--- a/examples/mps3-an536/reference/el2_hello-thumbv8r-none-eabihf.out
+++ /dev/null
@@ -1,27 +0,0 @@
-Hello, this is semihosting! x = 1.000, y = 2.000
-Region 0: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 1: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 2: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 3: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 4: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 5: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 6: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 7: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 8: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 9: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 10: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 11: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 12: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 13: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 14: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-Region 15: El2Region { range: 0x0..=0x3f, shareability: NonShareable, access: ReadWriteNoEL10, no_exec: false, mair: 0, enable: false }
-PANIC: PanicInfo {
-    message: I am an example panic,
-    location: Location {
-        file: "src/bin/el2_hello.rs",
-        line: 28,
-        column: 5,
-    },
-    can_unwind: true,
-    force_no_backtrace: false,
-}
diff --git a/examples/mps3-an536-smp/reference/gic-armv8r-none-eabihf.out b/examples/mps3-an536/reference/gic-smp-armv8r-none-eabihf.out
similarity index 100%
rename from examples/mps3-an536-smp/reference/gic-armv8r-none-eabihf.out
rename to examples/mps3-an536/reference/gic-smp-armv8r-none-eabihf.out
diff --git a/examples/mps3-an536-smp/reference/gic-thumbv8r-none-eabihf.out b/examples/mps3-an536/reference/gic-smp-thumbv8r-none-eabihf.out
similarity index 100%
rename from examples/mps3-an536-smp/reference/gic-thumbv8r-none-eabihf.out
rename to examples/mps3-an536/reference/gic-smp-thumbv8r-none-eabihf.out
diff --git a/examples/mps3-an536/reference/mpu_setup-armv8r-none-eabihf.out b/examples/mps3-an536/reference/mpu_setup-armv8r-none-eabihf.out
index 7c873eaf..4f56b461 100644
--- a/examples/mps3-an536/reference/mpu_setup-armv8r-none-eabihf.out
+++ b/examples/mps3-an536/reference/mpu_setup-armv8r-none-eabihf.out
@@ -1,9 +1,9 @@
-          UND @ 0x1006be00..=0x1006fdff
-          SVC @ 0x1006fe40..=0x10073e3f
-          ABT @ 0x10073e80..=0x10077e7f
-          HYP @ 0x10077ec0..=0x1007bebf
-          IRQ @ 0x1007bf00..=0x1007bf3f
-          FIQ @ 0x1007bf80..=0x1007bfbf
+          UND @ 0x1005bd80..=0x1005fd7f
+          SVC @ 0x10063dc0..=0x10067dbf
+          ABT @ 0x1006be00..=0x1006fdff
+          HYP @ 0x10073e40..=0x10077e3f
+          IRQ @ 0x10077ec0..=0x10077eff
+          FIQ @ 0x10077f80..=0x10077fbf
           SYS @ 0x1007c000..=0x1007ffff
 .vector_table @ 0x08000000..=0x0800ffff
         .text @ 0x08010000..=0x0801ffff
@@ -11,12 +11,12 @@
         .data @ 0x10000000..=0x0fffffff
          .bss @ 0x10000000..=0x1000ffff
       .uninit @ 0x10010000..=0x1000ffff
-Region 00: El1Region { range: 0x1006be00..=0x1006fdff, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 01: El1Region { range: 0x1006fe40..=0x10073e3f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 02: El1Region { range: 0x10073e80..=0x10077e7f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 03: El1Region { range: 0x10077ec0..=0x1007bebf, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 04: El1Region { range: 0x1007bf00..=0x1007bf3f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 05: El1Region { range: 0x1007bf80..=0x1007bfbf, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 00: El1Region { range: 0x1005bd80..=0x1005fd7f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 01: El1Region { range: 0x10063dc0..=0x10067dbf, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 02: El1Region { range: 0x1006be00..=0x1006fdff, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 03: El1Region { range: 0x10073e40..=0x10077e3f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 04: El1Region { range: 0x10077ec0..=0x10077eff, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 05: El1Region { range: 0x10077f80..=0x10077fbf, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
 Region 06: El1Region { range: 0x1007c000..=0x1007ffff, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
 Region 07: El1Region { range: 0x08000000..=0x0800ffff, shareability: NonShareable, access: ReadOnly, no_exec: false     , mair: 0000000000, enable: true       }
 Region 08: El1Region { range: 0x08010000..=0x0801ffff, shareability: NonShareable, access: ReadOnly, no_exec: false     , mair: 0000000000, enable: true       }
diff --git a/examples/mps3-an536/reference/mpu_setup-thumbv8r-none-eabihf.out b/examples/mps3-an536/reference/mpu_setup-thumbv8r-none-eabihf.out
index 7c873eaf..4f56b461 100644
--- a/examples/mps3-an536/reference/mpu_setup-thumbv8r-none-eabihf.out
+++ b/examples/mps3-an536/reference/mpu_setup-thumbv8r-none-eabihf.out
@@ -1,9 +1,9 @@
-          UND @ 0x1006be00..=0x1006fdff
-          SVC @ 0x1006fe40..=0x10073e3f
-          ABT @ 0x10073e80..=0x10077e7f
-          HYP @ 0x10077ec0..=0x1007bebf
-          IRQ @ 0x1007bf00..=0x1007bf3f
-          FIQ @ 0x1007bf80..=0x1007bfbf
+          UND @ 0x1005bd80..=0x1005fd7f
+          SVC @ 0x10063dc0..=0x10067dbf
+          ABT @ 0x1006be00..=0x1006fdff
+          HYP @ 0x10073e40..=0x10077e3f
+          IRQ @ 0x10077ec0..=0x10077eff
+          FIQ @ 0x10077f80..=0x10077fbf
           SYS @ 0x1007c000..=0x1007ffff
 .vector_table @ 0x08000000..=0x0800ffff
         .text @ 0x08010000..=0x0801ffff
@@ -11,12 +11,12 @@
         .data @ 0x10000000..=0x0fffffff
          .bss @ 0x10000000..=0x1000ffff
       .uninit @ 0x10010000..=0x1000ffff
-Region 00: El1Region { range: 0x1006be00..=0x1006fdff, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 01: El1Region { range: 0x1006fe40..=0x10073e3f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 02: El1Region { range: 0x10073e80..=0x10077e7f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 03: El1Region { range: 0x10077ec0..=0x1007bebf, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 04: El1Region { range: 0x1007bf00..=0x1007bf3f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
-Region 05: El1Region { range: 0x1007bf80..=0x1007bfbf, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 00: El1Region { range: 0x1005bd80..=0x1005fd7f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 01: El1Region { range: 0x10063dc0..=0x10067dbf, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 02: El1Region { range: 0x1006be00..=0x1006fdff, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 03: El1Region { range: 0x10073e40..=0x10077e3f, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 04: El1Region { range: 0x10077ec0..=0x10077eff, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
+Region 05: El1Region { range: 0x10077f80..=0x10077fbf, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
 Region 06: El1Region { range: 0x1007c000..=0x1007ffff, shareability: NonShareable, access: ReadWrite, no_exec: true      , mair: 0000000001, enable: true       }
 Region 07: El1Region { range: 0x08000000..=0x0800ffff, shareability: NonShareable, access: ReadOnly, no_exec: false     , mair: 0000000000, enable: true       }
 Region 08: El1Region { range: 0x08010000..=0x0801ffff, shareability: NonShareable, access: ReadOnly, no_exec: false     , mair: 0000000000, enable: true       }
diff --git a/examples/mps3-an536-smp/reference/smp-test-armv8r-none-eabihf.out b/examples/mps3-an536/reference/smp-test-armv8r-none-eabihf.out
similarity index 100%
rename from examples/mps3-an536-smp/reference/smp-test-armv8r-none-eabihf.out
rename to examples/mps3-an536/reference/smp-test-armv8r-none-eabihf.out
diff --git a/examples/mps3-an536-smp/reference/smp-test-thumbv8r-none-eabihf.out b/examples/mps3-an536/reference/smp-test-thumbv8r-none-eabihf.out
similarity index 100%
rename from examples/mps3-an536-smp/reference/smp-test-thumbv8r-none-eabihf.out
rename to examples/mps3-an536/reference/smp-test-thumbv8r-none-eabihf.out
diff --git a/examples/mps3-an536/src/bin/el2_hello.rs b/examples/mps3-an536/src/bin/el2_hello.rs
deleted file mode 100644
index 0de1b457..00000000
--- a/examples/mps3-an536/src/bin/el2_hello.rs
+++ /dev/null
@@ -1,99 +0,0 @@
-//! Semihosting hello-world for Arm Cortex-R52 running in EL2 (Hyp Mode)
-
-#![no_std]
-#![no_main]
-
-use aarch32_cpu::register::Hactlr;
-use aarch32_rt::entry;
-use mps3_an536 as _;
-use semihosting::println;
-
-/// The entry-point to the Rust application.
-///
-/// It is called by the start-up code at the bottom of this file.
-#[entry]
-fn main() -> ! {
-    let x = 1.0f64;
-    let y = x * 2.0;
-    println!("Hello, this is semihosting! x = {:0.3}, y = {:0.3}", x, y);
-
-    let mut mpu = unsafe { aarch32_cpu::pmsav8::El2Mpu::new() };
-    for idx in 0..mpu.num_regions() {
-        if let Some(region) = mpu.get_region(idx) {
-            println!("Region {}: {:?}", idx, region);
-        }
-    }
-
-    mps3_an536::want_panic();
-    panic!("I am an example panic");
-}
-
-/// Provide a custom `_start` function that sets us up in EL2 mode, with a
-/// stack.
-///
-/// Unlike the default routine, it does not initialise any other stacks, or
-/// switch to EL1 mode.
-///
-/// # Safety
-///
-/// This function should not be called manually. It should only be called on reset
-/// from the reset vector.
-#[unsafe(naked)]
-#[unsafe(no_mangle)]
-#[instruction_set(arm::t32)]
-pub unsafe extern "C" fn _start() {
-    core::arch::naked_asm!(
-        r#"
-        // Set stack pointer
-        ldr     sp, =_hyp_stack_high_end
-        // Set the HVBAR (for EL2) to _vector_table
-        ldr     r1, =_vector_table
-        mcr     p15, 4, r1, c12, c0, 0
-        // Configure HACTLR to let us enter EL1
-        mrc     p15, 4, r1, c1, c0, 1
-        mov     r2, {hactlr_bits}
-        orr     r1, r1, r2
-        mcr     p15, 4, r1, c1, c0, 1
-        // Init .data and .bss
-        bl      _init_segments
-        // Allow VFP coprocessor access
-        mrc     p15, 0, r0, c1, c0, 2
-        orr     r0, r0, #0xF00000
-        mcr     p15, 0, r0, c1, c0, 2
-        // Enable VFP
-        mov     r0, #0x40000000
-        vmsr    fpexc, r0
-        // Zero all registers before calling kmain
-        mov     r0, 0
-        mov     r1, 0
-        mov     r2, 0
-        mov     r3, 0
-        mov     r4, 0
-        mov     r5, 0
-        mov     r6, 0
-        mov     r7, 0
-        mov     r8, 0
-        mov     r9, 0
-        mov     r10, 0
-        mov     r11, 0
-        mov     r12, 0
-        // Jump to application
-        bl      kmain
-        // In case the application returns, loop forever
-        b       .
-        "#,
-        hactlr_bits = const {
-            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()
-        },
-    );
-}
diff --git a/examples/mps3-an536-smp/src/bin/gic.rs b/examples/mps3-an536/src/bin/gic-smp.rs
similarity index 94%
rename from examples/mps3-an536-smp/src/bin/gic.rs
rename to examples/mps3-an536/src/bin/gic-smp.rs
index 090b5e6c..af54a2fd 100644
--- a/examples/mps3-an536-smp/src/bin/gic.rs
+++ b/examples/mps3-an536/src/bin/gic-smp.rs
@@ -38,7 +38,7 @@ fn main() -> ! {
         aarch32_cpu::register::Mpidr::read()
     );
 
-    let mut gic = unsafe { mps3_an536_smp::make_gic() };
+    let mut gic = unsafe { mps3_an536::make_gic() };
 
     // Configure two Software Generated Interrupts for Core 0
     println!("Configure SGI on both cores...");
@@ -61,7 +61,7 @@ fn main() -> ! {
         aarch32_cpu::interrupt::enable();
     }
 
-    mps3_an536_smp::start_core1();
+    mps3_an536::start_core1();
 
     // wait some time for core 1 to start
     for counter in 0..=CORE0_WILL_WAIT {
@@ -71,7 +71,7 @@ fn main() -> ! {
         if counter == CORE0_WILL_WAIT {
             println!("CPU 1 is missing?!");
 
-            mps3_an536_smp::exit(0);
+            mps3_an536::exit(0);
         }
     }
 
@@ -98,14 +98,14 @@ fn main() -> ! {
         }
     }
 
-    mps3_an536_smp::exit(0);
+    mps3_an536::exit(0);
 }
 
 /// The entry-point to the Rust application.
 ///
-/// It is called by the start-up code below, on Core 1.
+/// It is called by the start-up code on Core 1.
 #[unsafe(no_mangle)]
-pub extern "C" fn kmain2() {
+pub extern "C" fn kmain_secondary() {
     critical_section::with(|cs| {
         println!(
             "I am core 1 - {:08x?}",
diff --git a/examples/mps3-an536/src/bin/mpu_setup.rs b/examples/mps3-an536/src/bin/mpu_setup.rs
index 42d5881e..6584106a 100644
--- a/examples/mps3-an536/src/bin/mpu_setup.rs
+++ b/examples/mps3-an536/src/bin/mpu_setup.rs
@@ -55,6 +55,7 @@ static MEM_ATTRS: [MemAttr; 8] = [
 #[entry]
 fn main() -> ! {
     let mut mpu = unsafe { El1Mpu::new() };
+    mps3_an536::MPU_ENABLED.store(true, core::sync::atomic::Ordering::Relaxed);
 
     mpu.set_attributes(&MEM_ATTRS);
 
diff --git a/examples/mps3-an536-smp/src/bin/smp-test.rs b/examples/mps3-an536/src/bin/smp-test.rs
similarity index 93%
rename from examples/mps3-an536-smp/src/bin/smp-test.rs
rename to examples/mps3-an536/src/bin/smp-test.rs
index 9fb75435..d559a439 100644
--- a/examples/mps3-an536-smp/src/bin/smp-test.rs
+++ b/examples/mps3-an536/src/bin/smp-test.rs
@@ -37,7 +37,7 @@ fn main() -> ! {
         aarch32_cpu::register::Mpidr::read()
     );
 
-    mps3_an536_smp::start_core1();
+    mps3_an536::start_core1();
 
     // wait some time for core 1 to start
     for counter in 0..=CORE0_WILL_WAIT {
@@ -47,7 +47,7 @@ fn main() -> ! {
         if counter == CORE0_WILL_WAIT {
             println!("CPU 1 is missing?!");
 
-            mps3_an536_smp::exit(0);
+            mps3_an536::exit(0);
         }
     }
 
@@ -88,14 +88,14 @@ fn main() -> ! {
         code = 1;
     }
 
-    mps3_an536_smp::exit(code);
+    mps3_an536::exit(code);
 }
 
 /// The entry-point to the Rust application.
 ///
-/// It is called by the start-up code below, on Core 1.
+/// It is called by the start-up code on Core 1.
 #[unsafe(no_mangle)]
-pub extern "C" fn kmain2() {
+pub extern "C" fn kmain_secondary() {
     println!(
         "I am core 1 - {:08x?}",
         aarch32_cpu::register::Mpidr::read()
diff --git a/examples/mps3-an536/src/lib.rs b/examples/mps3-an536/src/lib.rs
index a5e5396e..2d7bf4c3 100644
--- a/examples/mps3-an536/src/lib.rs
+++ b/examples/mps3-an536/src/lib.rs
@@ -72,6 +72,9 @@ compile_error!("This example is only compatible to the ARMv8-R architecture");
 
 static WANT_PANIC: AtomicBool = AtomicBool::new(false);
 
+/// Set this if you've turned the MPU on. We won't walk the other core's stacks.
+pub static MPU_ENABLED: AtomicBool = AtomicBool::new(false);
+
 /// Track if we're already in the exit routine.
 ///
 /// Stops us doing infinite recursion if we panic whilst doing the stack reporting.
@@ -127,7 +130,12 @@ fn stack_dump() {
 
     unsafe {
         for stack in Stack::iter() {
-            for core in (0..Stack::num_cores()).rev() {
+            let bound = if MPU_ENABLED.load(Ordering::Relaxed) {
+                1
+            } else {
+                Stack::num_cores()
+            };
+            for core in (0..bound).rev() {
                 let core_range = stack.range(core).unwrap();
                 let (total, used) = stack_used_bytes(core_range.clone());
                 let percent = used * 100 / total;
@@ -215,7 +223,7 @@ impl Board {
 /// # Safety
 ///
 /// Only call this function once.
-unsafe fn make_gic() -> arm_gic::gicv3::GicV3<'static> {
+pub unsafe fn make_gic() -> arm_gic::gicv3::GicV3<'static> {
     /// Offset from PERIPHBASE for GIC Distributor
     const GICD_BASE_OFFSET: usize = 0x0000_0000usize;
 
@@ -243,9 +251,50 @@ unsafe fn make_gic() -> arm_gic::gicv3::GicV3<'static> {
     // SAFETY: The GICD and GICR base addresses point to valid GICv3 MMIO regions as
     // obtained from the hardware CBAR register. This function is only called once
     // (via Board::new()'s atomic guard), ensuring exclusive ownership of the GIC.
-    let mut gic = unsafe { arm_gic::gicv3::GicV3::new(gicd, gicr_base, 1, false) };
+    let mut gic = unsafe { arm_gic::gicv3::GicV3::new(gicd, gicr_base, 2, false) };
     semihosting::println!("Calling git.setup(0)");
     gic.setup(0);
     arm_gic::gicv3::GicCpuInterface::set_priority_mask(0x80);
     gic
 }
+
+const FPGA_LED: u32 = 0xE020_2000;
+
+/// Release core1 from spin loop
+pub fn start_core1() {
+    unsafe {
+        core::arch::asm!(
+            // Activate second core by writing to FPGA LEDs.
+            // We needed a shared register that wasn't in RAM, and this will do.
+            //
+            // STL is an ARMv8 Store-Release, to ensure any loads/stores before
+            // this point are completed first
+            "stl     {value}, [{addr}]",
+            // send an event to wake the other core out of WFE
+            "sev",
+            value = in(reg) 1,
+            addr = in(reg) FPGA_LED
+        );
+    }
+}
+
+/// Park function for secondary cores
+///
+/// We sleep the cores with a `WFE` and check a register in the FPGA to see if
+/// it is time to boot.
+#[unsafe(naked)]
+#[unsafe(no_mangle)]
+pub extern "C" fn _asm_secondary_core_park() {
+    core::arch::naked_asm!(
+        r#"
+        ldr     r0, ={fpga_led}   // LED GPIO register base address
+    1:
+        wfe                       // Wait for Event
+        lda     r1, [r0]          // Load-Acquire the value we wait on
+        cmp     r1, 0             // Is it zero?
+        beq     1b                // If so, loop and try again
+        bx      lr                // Else, return to start-up
+    "#,
+    fpga_led = const FPGA_LED
+    );
+}
diff --git a/justfile b/justfile
index 3d9a6502..12114f08 100644
--- a/justfile
+++ b/justfile
@@ -28,8 +28,6 @@ clean:
 	rm -rf examples/versatileab/target-d32
 	cd examples/mps3-an536 && cargo clean
 	rm -rf examples/mps3-an536/target-d32
-	cd examples/mps3-an536-smp && cargo clean
-	rm -rf examples/mps3-an536-smp/target-d32
 	cd examples/xilinx-zynq-a9 && cargo clean
 	rm -rf examples/xilinx-zynq-a9/target-d32
 
@@ -110,13 +108,11 @@ build-versatileab-tier2 target:
 # Builds the MPS3-AN536 examples, building core from source
 build-mps3-tier3 target:
 	cd examples/mps3-an536 && cargo build --target={{target}} -Zbuild-std=core {{verbose}}
-	cd examples/mps3-an536-smp && cargo build --target={{target}} -Zbuild-std=core {{verbose}}
 	cd examples/mps3-an536-el2 && cargo build --target={{target}} -Zbuild-std=core {{verbose}}
 
 # Builds the MPS3-AN536 examples, assuming core has been prebuilt
 build-mps3-tier2 target:
 	cd examples/mps3-an536 && cargo build --target={{target}} {{verbose}}
-	cd examples/mps3-an536-smp && cargo build --target={{target}} {{verbose}}
 	cd examples/mps3-an536-el2 && cargo build --target={{target}} {{verbose}}
 
 # Builds the Xilinx Zynq-A9 examples, assuming core has been prebuilt
@@ -174,7 +170,6 @@ fmt:
 	# The cross-compiled examples	cargo fmt
 	cd examples/versatileab && cargo fmt {{verbose}}
 	cd examples/mps3-an536 && cargo fmt {{verbose}}
-	cd examples/mps3-an536-smp && cargo fmt {{verbose}}
 	cd examples/mps3-an536-el2 && cargo fmt {{verbose}}
 	cd examples/xilinx-zynq-a9 && cargo fmt {{verbose}}
 
@@ -187,7 +182,6 @@ fmt-check:
 	# The cross-compiled examples	cargo fmt
 	cd examples/versatileab && cargo fmt --check {{verbose}}
 	cd examples/mps3-an536 && cargo fmt --check {{verbose}}
-	cd examples/mps3-an536-smp && cargo fmt --check {{verbose}}
 	cd examples/mps3-an536-el2 && cargo fmt --check {{verbose}}
 	cd examples/xilinx-zynq-a9 && cargo fmt --check {{verbose}}
 
@@ -224,7 +218,6 @@ clippy-arm-targets:
 clippy-examples:
 	cd examples/versatileab && cargo clippy --target=armv7r-none-eabi {{verbose}}
 	cd examples/mps3-an536 && cargo clippy --target=armv8r-none-eabihf {{verbose}}
-	cd examples/mps3-an536-smp && cargo clippy --target=armv8r-none-eabihf {{verbose}}
 	cd examples/mps3-an536-el2 && cargo clippy --target=armv8r-none-eabihf {{verbose}}
 	cd examples/xilinx-zynq-a9 && cargo clippy --target=armv7a-none-eabi {{verbose}}
 
@@ -256,7 +249,7 @@ test-cargo:
 	cd arm-targets && cargo test {{verbose}}
 
 # Run the integration tests in QEMU
-test-qemu: test-qemu-v4t test-qemu-v5te test-qemu-v6 test-qemu-v7a test-qemu-v7a-zynq test-qemu-v7r test-qemu-v8r test-qemu-v8r-smp test-qemu-v8r-el2
+test-qemu: test-qemu-v4t test-qemu-v5te test-qemu-v6 test-qemu-v7a test-qemu-v7a-zynq test-qemu-v7r test-qemu-v8r test-qemu-v8r-el2
 
 test-qemu-v4t:
 	#!/bin/bash
@@ -335,17 +328,6 @@ test-qemu-v8r:
 	RUSTFLAGS=-Ctarget-cpu=cortex-r52 ./tests.sh examples/mps3-an536 thumbv8r-none-eabihf --features=fpu-d32 --target-dir=target-d32 {{verbose}} --release || FAIL=1
 	if [ "${FAIL}" == "1" ]; then exit 1; fi
 
-test-qemu-v8r-smp:
-	#!/bin/bash
-	FAIL=0
-	./tests.sh examples/mps3-an536-smp armv8r-none-eabihf {{verbose}} --release || FAIL=1
-	./tests.sh examples/mps3-an536-smp thumbv8r-none-eabihf {{verbose}} --release || FAIL=1
-	./tests.sh examples/mps3-an536-smp armv8r-none-eabihf {{verbose}} --features=svc-stack-interrupt --release || FAIL=1
-	./tests.sh examples/mps3-an536-smp thumbv8r-none-eabihf {{verbose}} --features=svc-stack-interrupt --release || FAIL=1
-	RUSTFLAGS=-Ctarget-cpu=cortex-r52 ./tests.sh examples/mps3-an536-smp armv8r-none-eabihf --features=fpu-d32 --target-dir=target-d32 {{verbose}} --release || FAIL=1
-	RUSTFLAGS=-Ctarget-cpu=cortex-r52 ./tests.sh examples/mps3-an536-smp thumbv8r-none-eabihf --features=fpu-d32 --target-dir=target-d32 {{verbose}} --release || FAIL=1
-	if [ "${FAIL}" == "1" ]; then exit 1; fi
-
 test-qemu-v8r-el2:
 	#!/bin/bash
 	FAIL=0