|
| 1 | +//! Multi-core hello-world for Arm Cortex-R |
| 2 | +//! |
| 3 | +//! Runs code on two cores, checking that atomic fetch_add works. |
| 4 | +//! |
| 5 | +//! Abuses the FPGA LED register as a place to record whether Core 0 has |
| 6 | +//! started. |
| 7 | +//! |
| 8 | +//! Run with `cargo run --bin smp_test --target=armv8r-none-eabihf -- -smp 2`. |
| 9 | +
|
| 10 | +#![no_std] |
| 11 | +#![no_main] |
| 12 | + |
| 13 | +use core::cell::UnsafeCell; |
| 14 | +use core::sync::atomic::{AtomicU32, Ordering}; |
| 15 | + |
| 16 | +// pull in our start-up code |
| 17 | +use mps3_an536 as _; |
| 18 | + |
| 19 | +use semihosting::println; |
| 20 | + |
| 21 | +#[repr(align(16))] |
| 22 | +struct Stack<const LEN_BYTES: usize> { |
| 23 | + contents: UnsafeCell<[u8; LEN_BYTES]>, |
| 24 | +} |
| 25 | + |
| 26 | +impl<const LEN_BYTES: usize> Stack<LEN_BYTES> { |
| 27 | + const fn new() -> Self { |
| 28 | + Self { |
| 29 | + contents: UnsafeCell::new([0u8; LEN_BYTES]), |
| 30 | + } |
| 31 | + } |
| 32 | + |
| 33 | + fn stack_top(&self) -> usize { |
| 34 | + let stack_start = self.contents.get() as usize; |
| 35 | + stack_start + LEN_BYTES |
| 36 | + } |
| 37 | +} |
| 38 | + |
| 39 | +unsafe impl<const LEN_BYTES: usize> Sync for Stack<LEN_BYTES> {} |
| 40 | + |
| 41 | +static CORE1_STACK: Stack<65536> = Stack::new(); |
| 42 | + |
| 43 | +static SHARED_VARIABLE: AtomicU32 = AtomicU32::new(0); |
| 44 | + |
| 45 | +/// The entry-point to the Rust application. |
| 46 | +/// |
| 47 | +/// It is called by the start-up code in `cortex-m-rt`. |
| 48 | +#[no_mangle] |
| 49 | +pub extern "C" fn kmain() { |
| 50 | + let fpga_led = 0xE020_2000 as *mut u32; |
| 51 | + extern "C" { |
| 52 | + static mut _core1_stack_pointer: usize; |
| 53 | + } |
| 54 | + unsafe { |
| 55 | + let p = &raw mut _core1_stack_pointer; |
| 56 | + p.write(CORE1_STACK.stack_top()); |
| 57 | + } |
| 58 | + unsafe { |
| 59 | + // Activate second core by writing to FPGA LEDs. |
| 60 | + // We needed a shared register that wasn't in RAM, and this will do. |
| 61 | + fpga_led.write_volatile(1); |
| 62 | + } |
| 63 | + |
| 64 | + // wait some time for core 1 to start |
| 65 | + for counter in 0..1000 { |
| 66 | + if SHARED_VARIABLE.load(Ordering::SeqCst) != 0 { |
| 67 | + break; |
| 68 | + } |
| 69 | + if counter == 999 { |
| 70 | + println!("CPU 1 is missing?!"); |
| 71 | + |
| 72 | + semihosting::process::exit(0); |
| 73 | + } |
| 74 | + } |
| 75 | + |
| 76 | + for _ in 0..1000 { |
| 77 | + SHARED_VARIABLE.fetch_add(1, Ordering::Relaxed); |
| 78 | + } |
| 79 | + |
| 80 | + println!( |
| 81 | + "Total is {} (is it 2001?)", |
| 82 | + SHARED_VARIABLE.load(Ordering::Relaxed) |
| 83 | + ); |
| 84 | + |
| 85 | + semihosting::process::exit(0); |
| 86 | +} |
| 87 | + |
| 88 | +/// The entry-point to the Rust application. |
| 89 | +/// |
| 90 | +/// It is called by the start-up code below, on Core 1. |
| 91 | +#[no_mangle] |
| 92 | +pub extern "C" fn kmain2() { |
| 93 | + SHARED_VARIABLE.store(1, Ordering::SeqCst); |
| 94 | + for _ in 0..1000 { |
| 95 | + SHARED_VARIABLE.fetch_add(1, Ordering::Relaxed); |
| 96 | + } |
| 97 | + loop { |
| 98 | + core::hint::spin_loop(); |
| 99 | + } |
| 100 | +} |
| 101 | + |
| 102 | +// Start-up code for multi-core Armv8-R, as implemented on the MPS3-AN536. |
| 103 | +// |
| 104 | +// We boot into EL2, set up a stack pointer, init .data on .bss on core0, and |
| 105 | +// run `kmain` in EL1 on all cores. |
| 106 | +#[cfg(arm_architecture = "v8-r")] |
| 107 | +core::arch::global_asm!( |
| 108 | + r#" |
| 109 | + .section .bss |
| 110 | + .align 4 |
| 111 | + _core1_stack_pointer: |
| 112 | + .word 0 |
| 113 | +
|
| 114 | + .section .text.startup |
| 115 | + .align 4 |
| 116 | +
|
| 117 | + .global _start |
| 118 | + .global core1_released |
| 119 | + .type _start, %function |
| 120 | + _start: |
| 121 | + // Read MPIDR into R0 |
| 122 | + mrc p15, 0, r0, c0, c0, 5 |
| 123 | + ands r0, r0, 0xFF |
| 124 | + bne core1 |
| 125 | + core0: |
| 126 | + ldr pc, =_default_start |
| 127 | + core1: |
| 128 | + ldr r0, =0xE0202000 |
| 129 | + mov r1, #0 |
| 130 | + core1_spin: |
| 131 | + wfe |
| 132 | + // spin until an LED0 is on |
| 133 | + ldr r2, [r0] |
| 134 | + cmp r1, r2 |
| 135 | + beq core1_spin |
| 136 | + core1_released: |
| 137 | + // now an LED is on, we assume _core1_stack_pointer contains our stack pointer |
| 138 | + // First we must exit EL2... |
| 139 | + // Set the HVBAR (for EL2) to _vector_table |
| 140 | + ldr r0, =_vector_table |
| 141 | + mcr p15, 4, r0, c12, c0, 0 |
| 142 | + // Configure HACTLR to let us enter EL1 |
| 143 | + mrc p15, 4, r0, c1, c0, 1 |
| 144 | + mov r1, {hactlr_bits} |
| 145 | + orr r0, r0, r1 |
| 146 | + mcr p15, 4, r0, c1, c0, 1 |
| 147 | + // Program the SPSR - enter system mode (0x1F) in Arm mode with IRQ, FIQ masked |
| 148 | + mov r0, {sys_mode} |
| 149 | + msr spsr_hyp, r0 |
| 150 | + adr r0, 1f |
| 151 | + msr elr_hyp, r0 |
| 152 | + dsb |
| 153 | + isb |
| 154 | + eret |
| 155 | + 1: |
| 156 | + // Set the VBAR (for EL1) to _vector_table. NB: This isn't required on |
| 157 | + // Armv7-R because that only supports 'low' (default) or 'high'. |
| 158 | + ldr r0, =_vector_table |
| 159 | + mcr p15, 0, r0, c12, c0, 0 |
| 160 | + ldr r0, =_core1_stack_pointer |
| 161 | + ldr r0, [r0] |
| 162 | + // set up our stacks using that stack pointer |
| 163 | + bl _stack_setup |
| 164 | + bl kmain2 |
| 165 | + .size _start, . - _start |
| 166 | + "#, |
| 167 | + hactlr_bits = const { |
| 168 | + cortex_ar::register::Hactlr::new_with_raw_value(0) |
| 169 | + .with_cpuactlr(true) |
| 170 | + .with_cdbgdci(true) |
| 171 | + .with_flashifregionr(true) |
| 172 | + .with_periphpregionr(true) |
| 173 | + .with_qosr(true) |
| 174 | + .with_bustimeoutr(true) |
| 175 | + .with_intmonr(true) |
| 176 | + .with_err(true) |
| 177 | + .with_testr1(true) |
| 178 | + .raw_value() |
| 179 | + }, |
| 180 | + sys_mode = const { |
| 181 | + cortex_ar::register::Cpsr::new_with_raw_value(0) |
| 182 | + .with_mode(cortex_ar::register::cpsr::ProcessorMode::Sys) |
| 183 | + .with_i(true) |
| 184 | + .with_f(true) |
| 185 | + .raw_value() |
| 186 | + } |
| 187 | +); |
0 commit comments