From bbe82186863422f800207ac6bdfbe57b9530a8cc Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sat, 11 Jul 2026 20:11:45 +0100
Subject: [PATCH 01/11] Add SMP support to aarch32-rt

Also means we can collapse the mps3-an536-smp example back into
mps3-an536, because we don't need special start-up routines any more
(the presence of a custom `_default_start` before meant we weren't
testing the supplied `_default_start` routine, so we had to split the
example up).
---
 .github/workflows/build.yml                   |   1 -
 .gitignore                                    |   2 -
 .vscode/settings.json                         |   3 +-
 Cargo.toml                                    |   1 -
 aarch32-rt/link.x                             |   6 +-
 aarch32-rt/src/arch_v4/boot.rs                |  38 +--
 aarch32-rt/src/arch_v7/boot_from_el1.rs       |  69 ++--
 aarch32-rt/src/arch_v7/boot_from_el2.rs       |  96 +++---
 aarch32-rt/src/arch_v8_hyp/boot.rs            |  66 +++-
 aarch32-rt/src/lib.rs                         | 137 +++++++-
 examples/mps3-an536-el2/memory.x              |  34 +-
 examples/mps3-an536-smp/.cargo/config.toml    |  10 -
 examples/mps3-an536-smp/Cargo.toml            |  32 --
 examples/mps3-an536-smp/README.md             | 106 ------
 examples/mps3-an536-smp/build.rs              |  26 --
 examples/mps3-an536-smp/commands.gdb          |  13 -
 examples/mps3-an536-smp/memory.x              |  19 --
 examples/mps3-an536-smp/src/lib.rs            | 308 ------------------
 examples/mps3-an536/.cargo/config.toml        |   4 +-
 examples/mps3-an536/memory.x                  |   2 +
 .../el2_hello-armv8r-none-eabihf.out          |  27 --
 .../el2_hello-thumbv8r-none-eabihf.out        |  27 --
 .../reference/gic-smp-armv8r-none-eabihf.out} |   0
 .../gic-smp-thumbv8r-none-eabihf.out}         |   0
 .../mpu_setup-armv8r-none-eabihf.out          |  24 +-
 .../mpu_setup-thumbv8r-none-eabihf.out        |  24 +-
 .../reference/smp-test-armv8r-none-eabihf.out |   0
 .../smp-test-thumbv8r-none-eabihf.out         |   0
 examples/mps3-an536/src/bin/el2_hello.rs      |  99 ------
 .../gic.rs => mps3-an536/src/bin/gic-smp.rs}  |  12 +-
 .../src/bin/smp-test.rs                       |  10 +-
 examples/mps3-an536/src/lib.rs                |  44 ++-
 justfile                                      |  20 +-
 33 files changed, 352 insertions(+), 908 deletions(-)
 delete mode 100644 examples/mps3-an536-smp/.cargo/config.toml
 delete mode 100644 examples/mps3-an536-smp/Cargo.toml
 delete mode 100644 examples/mps3-an536-smp/README.md
 delete mode 100644 examples/mps3-an536-smp/build.rs
 delete mode 100644 examples/mps3-an536-smp/commands.gdb
 delete mode 100644 examples/mps3-an536-smp/memory.x
 delete mode 100644 examples/mps3-an536-smp/src/lib.rs
 delete mode 100644 examples/mps3-an536/reference/el2_hello-armv8r-none-eabihf.out
 delete mode 100644 examples/mps3-an536/reference/el2_hello-thumbv8r-none-eabihf.out
 rename examples/{mps3-an536-smp/reference/gic-armv8r-none-eabihf.out => mps3-an536/reference/gic-smp-armv8r-none-eabihf.out} (100%)
 rename examples/{mps3-an536-smp/reference/gic-thumbv8r-none-eabihf.out => mps3-an536/reference/gic-smp-thumbv8r-none-eabihf.out} (100%)
 rename examples/{mps3-an536-smp => mps3-an536}/reference/smp-test-armv8r-none-eabihf.out (100%)
 rename examples/{mps3-an536-smp => mps3-an536}/reference/smp-test-thumbv8r-none-eabihf.out (100%)
 delete mode 100644 examples/mps3-an536/src/bin/el2_hello.rs
 rename examples/{mps3-an536-smp/src/bin/gic.rs => mps3-an536/src/bin/gic-smp.rs} (94%)
 rename examples/{mps3-an536-smp => mps3-an536}/src/bin/smp-test.rs (93%)

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/boot.rs b/aarch32-rt/src/arch_v4/boot.rs
index 901130c2..b3fd4d6b 100644
--- a/aarch32-rt/src/arch_v4/boot.rs
+++ b/aarch32-rt/src/arch_v4/boot.rs
@@ -10,41 +10,11 @@ core::arch::global_asm!(
     .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_v7/boot_from_el1.rs b/aarch32-rt/src/arch_v7/boot_from_el1.rs
index b98a5526..033c223e 100644
--- a/aarch32-rt/src/arch_v7/boot_from_el1.rs
+++ b/aarch32-rt/src/arch_v7/boot_from_el1.rs
@@ -1,55 +1,38 @@
-//! Start-up code for CPUs that always boot into EL1
+//! Boot code for Armv7-R
 
 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.
-        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..cc649a25 100644
--- a/aarch32-rt/src/arch_v7/boot_from_el2.rs
+++ b/aarch32-rt/src/arch_v7/boot_from_el2.rs
@@ -1,7 +1,15 @@
-//! Start-up code for CPUs that *might* boot into EL2 but that we want in EL1.
+//! Boot code forArmv7-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};
 
+#[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
@@ -13,13 +21,20 @@ core::arch::global_asm!(
     .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 +52,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 +94,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_v8_hyp/boot.rs b/aarch32-rt/src/arch_v8_hyp/boot.rs
index b756f31e..d8a29b4c 100644
--- a/aarch32-rt/src/arch_v8_hyp/boot.rs
+++ b/aarch32-rt/src/arch_v8_hyp/boot.rs
@@ -1,6 +1,8 @@
 //! 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.
+//!
+//! We do not support SMP at EL2 currently.
 
 core::arch::global_asm!(
     r#"
@@ -11,30 +13,46 @@ core::arch::global_asm!(
     .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 +65,27 @@ 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
     "#,
     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/lib.rs b/aarch32-rt/src/lib.rs
index 3393de34..bf7ddb08 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,58 @@
 //!   `_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)]
+//! #[unsafe(link_section = ".text.startup")]
+//! #[instruction_set(arm::a32)]
+//! pub unsafe extern "C" fn _asm_secondary_core_park() {
+//!     core::arch::naked_asm!(
+//!         r#"
+//!         // Some hardware register
+//!         ldr     r1, =0xE020_2000
+//!     1:
+//!         // Wait until Core 0 does a 'sev'
+//!         wfe
+//!         // Spin until register is non-zero.
+//!         ldr     r2, [r1]  
+//!         cmp     r2, 0
+//!         beq     1b
+//!         // return to start-up
+//!         bx      lr
+//!     "#,
+//!     );
+//! }
+//! ```
+//!
 //! ## Outputs
 //!
 //! This library produces global symbols called:
@@ -517,8 +569,10 @@
 //! * `_asm_default_irq_handler` - assembly language trampoline that calls
 //!   `_irq_handler`
 //! * `_asm_default_fiq_handler` - an FIQ handler that just spins
+//! * `_asm_default_core_park_handler` - spins secondary cores forever
 //! * `_default_handler` - a C compatible function that spins forever.
-//! * `_init_segments` - initialises `.bss` and `.data` and zeroes the stacks
+//! * `_asm_init_segments` - initialises `.bss` and `.data` and zeroes the
+//!   stacks
 //! * `_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`
@@ -729,6 +783,75 @@ core::arch::global_asm!(
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
 
+    // 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.
+    .pushsection .text._asm_core_start
+    .arm
+    .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      _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
+
+    // Default main_secondary function just returns so we end up spinning
+    .pushsection .text._default_kmain_secondary
+    .global _default_kmain_secondary
+    .arm
+    .type _default_kmain_secondary, %function
+    _default_kmain_secondary:
+        bx      lr
+    .size _default_kmain_secondary, . - _default_kmain_secondary
+    .popsection
+
     // Configure a stack for every mode. Leaves you in sys mode.
     //
     // Pass the core number in r0
@@ -782,11 +905,11 @@ core::arch::global_asm!(
     .popsection
 
     // Initialises stacks, .data and .bss
-    .pushsection .text._init_segments
+    .pushsection .text._asm_init_segments
     .arm
-    .global _init_segments
-    .type _init_segments, %function
-    _init_segments:
+    .global _asm_init_segments
+    .type _asm_init_segments, %function
+    _asm_init_segments:
         // Zero .bss
         ldr     r0, =__sbss
         ldr     r1, =__ebss
@@ -820,7 +943,7 @@ core::arch::global_asm!(
     1:
     	// return to caller
         bx      lr
-    .size _init_segments, . - _init_segments
+    .size _asm_init_segments, . - _asm_init_segments
     .popsection
     "#,
     und_mode = const {
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-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..b0f3b6d3 100644
--- a/examples/mps3-an536/src/lib.rs
+++ b/examples/mps3-an536/src/lib.rs
@@ -72,6 +72,7 @@ compile_error!("This example is only compatible to the ARMv8-R architecture");
 
 static WANT_PANIC: 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.
@@ -215,7 +216,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 +244,48 @@ 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() {
+    let fpga_led = FPGA_LED 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);
+        // send an event to wake the other core out of WFE
+        core::arch::asm!("sev");
+    }
+}
+
+/// 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)]
+#[instruction_set(arm::a32)]
+pub extern "C" fn _asm_secondary_core_park() {
+    core::arch::naked_asm!(
+        r#"
+        // LED GPIO register base address
+        ldr     r1, ={fpga_led}
+    1:
+        wfe
+        // Spin until register non-zero (i.e. an LED is switched on)
+        ldr     r2, [r1]  
+        cmp     r2, 0
+        beq     1b
+        // return to start-up
+        bx      lr
+    "#,
+    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

From 746a8a9afea0f4dae178e9907951de2d3ec551c1 Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sat, 11 Jul 2026 19:41:22 +0100
Subject: [PATCH 02/11] Fix the mpu_setup example.

You cannot check the stack contents of Core 1 on Core 0 when the MPU
is enabled. So, now I set a flag and make sure not to do that for that
particular test.
---
 examples/mps3-an536/src/bin/mpu_setup.rs | 1 +
 examples/mps3-an536/src/lib.rs           | 9 ++++++++-
 2 files changed, 9 insertions(+), 1 deletion(-)

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/src/lib.rs b/examples/mps3-an536/src/lib.rs
index b0f3b6d3..b5d3a738 100644
--- a/examples/mps3-an536/src/lib.rs
+++ b/examples/mps3-an536/src/lib.rs
@@ -72,6 +72,8 @@ 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.
 ///
@@ -128,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;

From 2896d2df8720249c78d608d540a3341697e8e921 Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sat, 11 Jul 2026 19:41:52 +0100
Subject: [PATCH 03/11] Fix typos in arm-targets README.

I copied from this example and got confused when it didn't work. Now
it's fixed.
---
 arm-targets/README.md | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

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:

From fe7396a4ef8a99666b292267def54b164b71dbf9 Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sun, 12 Jul 2026 08:44:57 +0100
Subject: [PATCH 04/11] Fix typo in aarch32-rt/src/arch_v7/boot_from_el2.rs

Co-authored-by: 9names <60134748+9names@users.noreply.github.com>
---
 aarch32-rt/src/arch_v7/boot_from_el2.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/aarch32-rt/src/arch_v7/boot_from_el2.rs b/aarch32-rt/src/arch_v7/boot_from_el2.rs
index cc649a25..43ce4a35 100644
--- a/aarch32-rt/src/arch_v7/boot_from_el2.rs
+++ b/aarch32-rt/src/arch_v7/boot_from_el2.rs
@@ -1,4 +1,4 @@
-//! Boot code forArmv7-A and Armv8-R
+//! Boot code for Armv7-A and Armv8-R
 
 #[cfg(any(
     arm_architecture = "v7-a",

From 170d7b332d4227412b2300e64341585e5deee4fe Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sun, 12 Jul 2026 10:59:23 +0100
Subject: [PATCH 05/11] Better doc notes

---
 aarch32-rt/src/arch_v4/boot.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/aarch32-rt/src/arch_v4/boot.rs b/aarch32-rt/src/arch_v4/boot.rs
index b3fd4d6b..7809327c 100644
--- a/aarch32-rt/src/arch_v4/boot.rs
+++ b/aarch32-rt/src/arch_v4/boot.rs
@@ -1,4 +1,4 @@
-//! Start-up code for CPUs that always boot into EL1
+//! Start-up code for ARMv4 - ARMv6 CPUs that always boot into EL1
 
 core::arch::global_asm!(
     r#"

From 69ec8331e1a31a3a0ebd6840a50618edaa9bd0fd Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sun, 12 Jul 2026 11:04:01 +0100
Subject: [PATCH 06/11] Fix comments for EL2 boot

---
 aarch32-rt/src/arch_v8_hyp/boot.rs | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/aarch32-rt/src/arch_v8_hyp/boot.rs b/aarch32-rt/src/arch_v8_hyp/boot.rs
index d8a29b4c..201e4ef8 100644
--- a/aarch32-rt/src/arch_v8_hyp/boot.rs
+++ b/aarch32-rt/src/arch_v8_hyp/boot.rs
@@ -1,8 +1,7 @@
 //! 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.
-//!
-//! We do not support SMP at EL2 currently.
+//! 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.
 
 core::arch::global_asm!(
     r#"

From 7b8d14c46e53d6d54156d88618129f6079a6901f Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sun, 12 Jul 2026 11:36:05 +0100
Subject: [PATCH 07/11] Typo in docs for function name

---
 aarch32-rt/src/lib.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/aarch32-rt/src/lib.rs b/aarch32-rt/src/lib.rs
index bf7ddb08..7db78d2c 100644
--- a/aarch32-rt/src/lib.rs
+++ b/aarch32-rt/src/lib.rs
@@ -569,7 +569,7 @@
 //! * `_asm_default_irq_handler` - assembly language trampoline that calls
 //!   `_irq_handler`
 //! * `_asm_default_fiq_handler` - an FIQ handler that just spins
-//! * `_asm_default_core_park_handler` - spins secondary cores forever
+//! * `_asm_default_secondary_core_park` - spins secondary cores forever
 //! * `_default_handler` - a C compatible function that spins forever.
 //! * `_asm_init_segments` - initialises `.bss` and `.data` and zeroes the
 //!   stacks

From d6bd373d616c91cdf1adbdea1033573a3db8dae1 Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sun, 12 Jul 2026 11:36:28 +0100
Subject: [PATCH 08/11] Secondary core park routine doesn't have to be A32

---
 aarch32-rt/src/lib.rs          | 8 +++-----
 examples/mps3-an536/src/lib.rs | 7 +++----
 2 files changed, 6 insertions(+), 9 deletions(-)

diff --git a/aarch32-rt/src/lib.rs b/aarch32-rt/src/lib.rs
index 7db78d2c..9094b891 100644
--- a/aarch32-rt/src/lib.rs
+++ b/aarch32-rt/src/lib.rs
@@ -530,19 +530,17 @@
 //! ```rust,ignore
 //! #[unsafe(naked)]
 //! #[unsafe(no_mangle)]
-//! #[unsafe(link_section = ".text.startup")]
-//! #[instruction_set(arm::a32)]
 //! pub unsafe extern "C" fn _asm_secondary_core_park() {
 //!     core::arch::naked_asm!(
 //!         r#"
 //!         // Some hardware register
-//!         ldr     r1, =0xE020_2000
+//!         ldr     r0, =0xE020_2000
 //!     1:
 //!         // Wait until Core 0 does a 'sev'
 //!         wfe
 //!         // Spin until register is non-zero.
-//!         ldr     r2, [r1]  
-//!         cmp     r2, 0
+//!         ldr     r1, [r0]  
+//!         cmp     r1, 0
 //!         beq     1b
 //!         // return to start-up
 //!         bx      lr
diff --git a/examples/mps3-an536/src/lib.rs b/examples/mps3-an536/src/lib.rs
index b5d3a738..466f7ae3 100644
--- a/examples/mps3-an536/src/lib.rs
+++ b/examples/mps3-an536/src/lib.rs
@@ -278,17 +278,16 @@ pub fn start_core1() {
 /// it is time to boot.
 #[unsafe(naked)]
 #[unsafe(no_mangle)]
-#[instruction_set(arm::a32)]
 pub extern "C" fn _asm_secondary_core_park() {
     core::arch::naked_asm!(
         r#"
         // LED GPIO register base address
-        ldr     r1, ={fpga_led}
+        ldr     r0, ={fpga_led}
     1:
         wfe
         // Spin until register non-zero (i.e. an LED is switched on)
-        ldr     r2, [r1]  
-        cmp     r2, 0
+        ldr     r1, [r0]  
+        cmp     r1, 0
         beq     1b
         // return to start-up
         bx      lr

From 23e085e1004dc984e1d9ea4ec550853e887d335e Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sun, 12 Jul 2026 11:36:41 +0100
Subject: [PATCH 09/11] Document new function.

---
 aarch32-rt/src/lib.rs | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/aarch32-rt/src/lib.rs b/aarch32-rt/src/lib.rs
index 9094b891..65cc578d 100644
--- a/aarch32-rt/src/lib.rs
+++ b/aarch32-rt/src/lib.rs
@@ -571,9 +571,11 @@
 //! * `_default_handler` - a C compatible function that spins forever.
 //! * `_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`.
 //! * `_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`)

From 74f261bc04edf04b42818ab5c8cb672d4ec244ab Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Sun, 12 Jul 2026 11:54:28 +0100
Subject: [PATCH 10/11] Note which functions must be assembled as A32, and pull
 out function docs for each global_asm function.

The docs are easy to read outside of the assembly string, because they
get syntax highlighted as comments and not text.

Also swaps the last few .sections for .pushsection
---
 aarch32-rt/src/arch_v4/abort.rs         |  24 ++--
 aarch32-rt/src/arch_v4/boot.rs          |   9 +-
 aarch32-rt/src/arch_v4/hvc.rs           |   5 +-
 aarch32-rt/src/arch_v4/interrupt.rs     |  40 +++---
 aarch32-rt/src/arch_v4/svc.rs           |  12 +-
 aarch32-rt/src/arch_v4/undefined.rs     |  16 ++-
 aarch32-rt/src/arch_v7/abort.rs         |  24 ++--
 aarch32-rt/src/arch_v7/boot_from_el1.rs |  11 +-
 aarch32-rt/src/arch_v7/boot_from_el2.rs |  11 +-
 aarch32-rt/src/arch_v7/hvc.rs           |  17 +--
 aarch32-rt/src/arch_v7/interrupt.rs     |  48 +++----
 aarch32-rt/src/arch_v7/svc.rs           |  11 +-
 aarch32-rt/src/arch_v7/undefined.rs     |  15 ++-
 aarch32-rt/src/arch_v8_hyp/abort.rs     |  34 +++--
 aarch32-rt/src/arch_v8_hyp/boot.rs      |  15 ++-
 aarch32-rt/src/arch_v8_hyp/hvc.rs       |  13 +-
 aarch32-rt/src/arch_v8_hyp/interrupt.rs |  17 ++-
 aarch32-rt/src/arch_v8_hyp/svc.rs       |  26 ++--
 aarch32-rt/src/arch_v8_hyp/undefined.rs |  20 +--
 aarch32-rt/src/lib.rs                   | 160 ++++++++++++++----------
 20 files changed, 327 insertions(+), 201 deletions(-)

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 7809327c..9ee1f80d 100644
--- a/aarch32-rt/src/arch_v4/boot.rs
+++ b/aarch32-rt/src/arch_v4/boot.rs
@@ -1,10 +1,17 @@
 //! 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
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 033c223e..eae9431b 100644
--- a/aarch32-rt/src/arch_v7/boot_from_el1.rs
+++ b/aarch32-rt/src/arch_v7/boot_from_el1.rs
@@ -1,8 +1,17 @@
 //! 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#"
-    .pushsection .text.default_start
+    .pushsection .text._default_start
     .arm
     .global _default_start
     .type _default_start, %function
diff --git a/aarch32-rt/src/arch_v7/boot_from_el2.rs b/aarch32-rt/src/arch_v7/boot_from_el2.rs
index 43ce4a35..5aa39caa 100644
--- a/aarch32-rt/src/arch_v7/boot_from_el2.rs
+++ b/aarch32-rt/src/arch_v7/boot_from_el2.rs
@@ -6,6 +6,16 @@
 ))]
 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")),
@@ -15,7 +25,6 @@ core::arch::global_asm!(
     // Work around https://github.com/rust-lang/rust/issues/127269
     .fpu vfp2
     .cpu cortex-r52
-
     .pushsection .text.default_start
     .arm
     .global _default_start
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 201e4ef8..61515082 100644
--- a/aarch32-rt/src/arch_v8_hyp/boot.rs
+++ b/aarch32-rt/src/arch_v8_hyp/boot.rs
@@ -1,14 +1,18 @@
 //! 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 on the
-//! primary core, and `kmain_secondary` in EL2 on any secondary cores.
 
+// # _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:
@@ -76,6 +80,7 @@ core::arch::global_asm!(
         // 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()
 );
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 65cc578d..1b2dc163 100644
--- a/aarch32-rt/src/lib.rs
+++ b/aarch32-rt/src/lib.rs
@@ -573,7 +573,7 @@
 //!   stacks
 //! * `_asm_core_start` - sets up stacks, enables FPU (if required), and jumps
 //!   to `kmain` or `kmain_secondary`. Takes the Core ID in `r0`.
-//! * `_stack_setup_preallocated` - initialises UND, SVC, ABT, IRQ, FIQ and SYS
+//! * `_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. Takes the Core ID in `r0`.
 //! * `_xxx_stack_high_end` and `_xxx_stack_low_end` where the former is the top
@@ -776,24 +776,23 @@ 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
-
-    // 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.
     .pushsection .text._asm_core_start
-    .arm
     .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      _stack_setup_preallocated
+        bl      _asm_stack_setup_preallocated
     "#,
     #[cfg(armv6_or_higher)]
     r#"
@@ -841,25 +840,35 @@ core::arch::global_asm!(
         b       .
     .size _asm_core_start, . - _asm_core_start
     .popsection
+    "#
+);
 
-    // Default main_secondary function just returns so we end up spinning
-    .pushsection .text._default_kmain_secondary
-    .global _default_kmain_secondary
-    .arm
-    .type _default_kmain_secondary, %function
-    _default_kmain_secondary:
-        bx      lr
-    .size _default_kmain_secondary, . - _default_kmain_secondary
-    .popsection
+/// 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();
+    }
+}
 
-    // 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:
+// # _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).
@@ -901,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._asm_init_segments
-    .arm
-    .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
+    .size _asm_stack_setup_preallocated, . - _asm_stack_setup_preallocated
     .popsection
     "#,
     und_mode = const {
@@ -990,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:

From a9f8bfd3d7f413bcc182de332064aa2ae5a11911 Mon Sep 17 00:00:00 2001
From: Jonathan 'theJPster' Pallant <github@thejpster.org.uk>
Date: Fri, 17 Jul 2026 21:34:17 +0100
Subject: [PATCH 11/11] MPS3-AN536 park/unpark functions use atomic load/stores

---
 examples/mps3-an536/src/lib.rs | 33 ++++++++++++++++++---------------
 1 file changed, 18 insertions(+), 15 deletions(-)

diff --git a/examples/mps3-an536/src/lib.rs b/examples/mps3-an536/src/lib.rs
index 466f7ae3..2d7bf4c3 100644
--- a/examples/mps3-an536/src/lib.rs
+++ b/examples/mps3-an536/src/lib.rs
@@ -262,13 +262,19 @@ const FPGA_LED: u32 = 0xE020_2000;
 
 /// Release core1 from spin loop
 pub fn start_core1() {
-    let fpga_led = FPGA_LED 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);
-        // send an event to wake the other core out of WFE
-        core::arch::asm!("sev");
+        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
+        );
     }
 }
 
@@ -281,16 +287,13 @@ pub fn start_core1() {
 pub extern "C" fn _asm_secondary_core_park() {
     core::arch::naked_asm!(
         r#"
-        // LED GPIO register base address
-        ldr     r0, ={fpga_led}
+        ldr     r0, ={fpga_led}   // LED GPIO register base address
     1:
-        wfe
-        // Spin until register non-zero (i.e. an LED is switched on)
-        ldr     r1, [r0]  
-        cmp     r1, 0
-        beq     1b
-        // return to start-up
-        bx      lr
+        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
     );