|
| 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::RefCell; |
| 14 | +use core::sync::atomic::{AtomicBool, AtomicU32, Ordering}; |
| 15 | + |
| 16 | +// pull in our start-up code |
| 17 | +use mps3_an536 as _; |
| 18 | + |
| 19 | +use semihosting::println; |
| 20 | + |
| 21 | +static CORE1_BOOTED: AtomicBool = AtomicBool::new(false); |
| 22 | + |
| 23 | +static SHARED_VARIABLE: AtomicU32 = AtomicU32::new(0); |
| 24 | + |
| 25 | +static SHARED_VARIABLE_2: critical_section::Mutex<RefCell<u32>> = |
| 26 | + critical_section::Mutex::new(RefCell::new(0)); |
| 27 | + |
| 28 | +/// How long core 0 waits for core 1 |
| 29 | +const CORE0_WILL_WAIT: usize = 1_000_000; |
| 30 | + |
| 31 | +/// How many CAS loops to run? |
| 32 | +const CAS_LOOPS: u32 = 1000; |
| 33 | + |
| 34 | +/// How many CS Mutex loops to run? |
| 35 | +const CS_MUTEX_LOOPS: u32 = 1000; |
| 36 | + |
| 37 | +/// The entry-point to the Rust application. |
| 38 | +/// |
| 39 | +/// It is called by the start-up code in `cortex-m-rt`. |
| 40 | +#[no_mangle] |
| 41 | +pub extern "C" fn kmain() { |
| 42 | + let fpga_led = 0xE020_2000 as *mut u32; |
| 43 | + unsafe { |
| 44 | + // Activate second core by writing to FPGA LEDs. |
| 45 | + // We needed a shared register that wasn't in RAM, and this will do. |
| 46 | + // TODO: We could use the GICv3 to send an interrupt instead? |
| 47 | + fpga_led.write_volatile(1); |
| 48 | + } |
| 49 | + |
| 50 | + // wait some time for core 1 to start |
| 51 | + for counter in 0..=CORE0_WILL_WAIT { |
| 52 | + if CORE1_BOOTED.load(Ordering::SeqCst) { |
| 53 | + break; |
| 54 | + } |
| 55 | + if counter == CORE0_WILL_WAIT { |
| 56 | + println!("CPU 1 is missing?!"); |
| 57 | + |
| 58 | + semihosting::process::exit(0); |
| 59 | + } |
| 60 | + } |
| 61 | + |
| 62 | + for _ in 0..CAS_LOOPS { |
| 63 | + SHARED_VARIABLE.fetch_add(1, Ordering::Relaxed); |
| 64 | + } |
| 65 | + |
| 66 | + for _ in 0..CS_MUTEX_LOOPS { |
| 67 | + critical_section::with(|cs| { |
| 68 | + let mut value_ref = SHARED_VARIABLE_2.borrow_ref_mut(cs); |
| 69 | + *value_ref += 1; |
| 70 | + }) |
| 71 | + } |
| 72 | + |
| 73 | + // let the other core finish |
| 74 | + for _ in 0..CORE0_WILL_WAIT { |
| 75 | + cortex_ar::asm::nop(); |
| 76 | + } |
| 77 | + |
| 78 | + let total_a = SHARED_VARIABLE.load(Ordering::Relaxed); |
| 79 | + if total_a == CAS_LOOPS * 2 { |
| 80 | + println!("CAS test passed"); |
| 81 | + } else { |
| 82 | + println!("CAS test failed, got {} not 2000", total_a); |
| 83 | + } |
| 84 | + |
| 85 | + let total_b = critical_section::with(|cs| { |
| 86 | + let value_ref = SHARED_VARIABLE_2.borrow_ref(cs); |
| 87 | + *value_ref |
| 88 | + }); |
| 89 | + |
| 90 | + if total_b == CS_MUTEX_LOOPS * 2 { |
| 91 | + println!("CS Mutex test passed"); |
| 92 | + } else { |
| 93 | + println!("CS Mutex test failed, got {} not 2000", total_b); |
| 94 | + } |
| 95 | + |
| 96 | + semihosting::process::exit(0); |
| 97 | +} |
| 98 | + |
| 99 | +/// The entry-point to the Rust application. |
| 100 | +/// |
| 101 | +/// It is called by the start-up code below, on Core 1. |
| 102 | +#[no_mangle] |
| 103 | +pub extern "C" fn kmain2() { |
| 104 | + CORE1_BOOTED.store(true, Ordering::SeqCst); |
| 105 | + |
| 106 | + for _ in 0..CAS_LOOPS { |
| 107 | + SHARED_VARIABLE.fetch_add(1, Ordering::Relaxed); |
| 108 | + } |
| 109 | + |
| 110 | + for _ in 0..CS_MUTEX_LOOPS { |
| 111 | + critical_section::with(|cs| { |
| 112 | + let mut value_ref = SHARED_VARIABLE_2.borrow_ref_mut(cs); |
| 113 | + *value_ref += 1; |
| 114 | + }) |
| 115 | + } |
| 116 | + |
| 117 | + loop { |
| 118 | + core::hint::spin_loop(); |
| 119 | + } |
| 120 | +} |
| 121 | + |
| 122 | +// Start-up code for multi-core Armv8-R, as implemented on the MPS3-AN536. |
| 123 | +// |
| 124 | +// We boot into EL2, set up a stack pointer, init .data on .bss on core0, and |
| 125 | +// run `kmain` in EL1 on all cores. |
| 126 | +#[cfg(arm_architecture = "v8-r")] |
| 127 | +core::arch::global_asm!( |
| 128 | + r#" |
| 129 | + .section .text.startup |
| 130 | + .align 4 |
| 131 | +
|
| 132 | + .global _start |
| 133 | + .global core1_released |
| 134 | + .type _start, %function |
| 135 | + _start: |
| 136 | + // Read MPIDR into R0 |
| 137 | + mrc p15, 0, r0, c0, c0, 5 |
| 138 | + ands r0, r0, 0xFF |
| 139 | + bne core1 |
| 140 | + core0: |
| 141 | + ldr pc, =_default_start |
| 142 | + core1: |
| 143 | + ldr r0, =0xE0202000 |
| 144 | + mov r1, #0 |
| 145 | + core1_spin: |
| 146 | + wfe |
| 147 | + // spin until any LED is on |
| 148 | + ldr r2, [r0] |
| 149 | + cmp r1, r2 |
| 150 | + beq core1_spin |
| 151 | + core1_released: |
| 152 | + // now any LED is on, we know all the global variables are initialised |
| 153 | + // First we must exit EL2... |
| 154 | + // Set the HVBAR (for EL2) to _vector_table |
| 155 | + ldr r0, =_vector_table |
| 156 | + mcr p15, 4, r0, c12, c0, 0 |
| 157 | + // Configure HACTLR to let us enter EL1 |
| 158 | + mrc p15, 4, r0, c1, c0, 1 |
| 159 | + mov r1, {hactlr_bits} |
| 160 | + orr r0, r0, r1 |
| 161 | + mcr p15, 4, r0, c1, c0, 1 |
| 162 | + // Program the SPSR - enter system mode (0x1F) in Arm mode with IRQ, FIQ masked |
| 163 | + mov r0, {sys_mode} |
| 164 | + msr spsr_hyp, r0 |
| 165 | + adr r0, 1f |
| 166 | + msr elr_hyp, r0 |
| 167 | + dsb |
| 168 | + isb |
| 169 | + eret |
| 170 | + 1: |
| 171 | + // Set the VBAR (for EL1) to _vector_table. NB: This isn't required on |
| 172 | + // Armv7-R because that only supports 'low' (default) or 'high'. |
| 173 | + ldr r0, =_vector_table |
| 174 | + mcr p15, 0, r0, c12, c0, 0 |
| 175 | + ldr r0, =_core1_stack_top |
| 176 | + // set up our stacks using that stack pointer |
| 177 | + bl _stack_setup |
| 178 | + bl kmain2 |
| 179 | + .size _start, . - _start |
| 180 | + "#, |
| 181 | + hactlr_bits = const { |
| 182 | + cortex_ar::register::Hactlr::new_with_raw_value(0) |
| 183 | + .with_cpuactlr(true) |
| 184 | + .with_cdbgdci(true) |
| 185 | + .with_flashifregionr(true) |
| 186 | + .with_periphpregionr(true) |
| 187 | + .with_qosr(true) |
| 188 | + .with_bustimeoutr(true) |
| 189 | + .with_intmonr(true) |
| 190 | + .with_err(true) |
| 191 | + .with_testr1(true) |
| 192 | + .raw_value() |
| 193 | + }, |
| 194 | + sys_mode = const { |
| 195 | + cortex_ar::register::Cpsr::new_with_raw_value(0) |
| 196 | + .with_mode(cortex_ar::register::cpsr::ProcessorMode::Sys) |
| 197 | + .with_i(true) |
| 198 | + .with_f(true) |
| 199 | + .raw_value() |
| 200 | + } |
| 201 | +); |
0 commit comments