diff --git a/embassy-rp/Cargo.toml b/embassy-rp/Cargo.toml index 284d458c..07cd1bc1 100644 --- a/embassy-rp/Cargo.toml +++ b/embassy-rp/Cargo.toml @@ -50,7 +50,7 @@ nb = "1.0.0" cfg-if = "1.0.0" cortex-m-rt = ">=0.6.15,<0.8" cortex-m = "0.7.6" -critical-section = "1.1" +critical-section = { version = "1.1", features = ["restore-state-u8"] } futures = { version = "0.3.17", default-features = false, features = ["async-await"] } chrono = { version = "0.4", default-features = false, optional = true } embedded-io = { version = "0.4.0", features = ["async"], optional = true } diff --git a/embassy-rp/src/critical_section_impl.rs b/embassy-rp/src/critical_section_impl.rs new file mode 100644 index 00000000..ce284c85 --- /dev/null +++ b/embassy-rp/src/critical_section_impl.rs @@ -0,0 +1,142 @@ +use core::sync::atomic::{AtomicU8, Ordering}; + +use crate::pac; + +struct RpSpinlockCs; +critical_section::set_impl!(RpSpinlockCs); + +/// Marker value to indicate no-one has the lock. +/// +/// Initialising `LOCK_OWNER` to 0 means cheaper static initialisation so it's the best choice +const LOCK_UNOWNED: u8 = 0; + +/// Indicates which core owns the lock so that we can call critical_section recursively. +/// +/// 0 = no one has the lock, 1 = core0 has the lock, 2 = core1 has the lock +static LOCK_OWNER: AtomicU8 = AtomicU8::new(LOCK_UNOWNED); + +/// Marker value to indicate that we already owned the lock when we started the `critical_section`. +/// +/// Since we can't take the spinlock when we already have it, we need some other way to keep track of `critical_section` ownership. +/// `critical_section` provides a token for communicating between `acquire` and `release` so we use that. +/// If we're the outermost call to `critical_section` we use the values 0 and 1 to indicate we should release the spinlock and set the interrupts back to disabled and enabled, respectively. +/// The value 2 indicates that we aren't the outermost call, and should not release the spinlock or re-enable interrupts in `release` +const LOCK_ALREADY_OWNED: u8 = 2; + +unsafe impl critical_section::Impl for RpSpinlockCs { + unsafe fn acquire() -> u8 { + RpSpinlockCs::acquire() + } + + unsafe fn release(token: u8) { + RpSpinlockCs::release(token); + } +} + +impl RpSpinlockCs { + unsafe fn acquire() -> u8 { + // Store the initial interrupt state and current core id in stack variables + let interrupts_active = cortex_m::register::primask::read().is_active(); + // We reserved 0 as our `LOCK_UNOWNED` value, so add 1 to core_id so we get 1 for core0, 2 for core1. + let core = pac::SIO.cpuid().read() as u8 + 1; + // Do we already own the spinlock? + if LOCK_OWNER.load(Ordering::Acquire) == core { + // We already own the lock, so we must have called acquire within a critical_section. + // Return the magic inner-loop value so that we know not to re-enable interrupts in release() + LOCK_ALREADY_OWNED + } else { + // Spin until we get the lock + loop { + // Need to disable interrupts to ensure that we will not deadlock + // if an interrupt enters critical_section::Impl after we acquire the lock + cortex_m::interrupt::disable(); + // Ensure the compiler doesn't re-order accesses and violate safety here + core::sync::atomic::compiler_fence(Ordering::SeqCst); + // Read the spinlock reserved for `critical_section` + if let Some(lock) = Spinlock31::try_claim() { + // We just acquired the lock. + // 1. Forget it, so we don't immediately unlock + core::mem::forget(lock); + // 2. Store which core we are so we can tell if we're called recursively + LOCK_OWNER.store(core, Ordering::Relaxed); + break; + } + // We didn't get the lock, enable interrupts if they were enabled before we started + if interrupts_active { + cortex_m::interrupt::enable(); + } + } + // If we broke out of the loop we have just acquired the lock + // As the outermost loop, we want to return the interrupt status to restore later + interrupts_active as _ + } + } + + unsafe fn release(token: u8) { + // Did we already own the lock at the start of the `critical_section`? + if token != LOCK_ALREADY_OWNED { + // No, it wasn't owned at the start of this `critical_section`, so this core no longer owns it. + // Set `LOCK_OWNER` back to `LOCK_UNOWNED` to ensure the next critical section tries to obtain the spinlock instead + LOCK_OWNER.store(LOCK_UNOWNED, Ordering::Relaxed); + // Ensure the compiler doesn't re-order accesses and violate safety here + core::sync::atomic::compiler_fence(Ordering::SeqCst); + // Release the spinlock to allow others to enter critical_section again + Spinlock31::release(); + // Re-enable interrupts if they were enabled when we first called acquire() + // We only do this on the outermost `critical_section` to ensure interrupts stay disabled + // for the whole time that we have the lock + if token != 0 { + cortex_m::interrupt::enable(); + } + } + } +} + +pub struct Spinlock(core::marker::PhantomData<()>) +where + Spinlock: SpinlockValid; + +impl Spinlock +where + Spinlock: SpinlockValid, +{ + /// Try to claim the spinlock. Will return `Some(Self)` if the lock is obtained, and `None` if the lock is + /// already in use somewhere else. + pub fn try_claim() -> Option { + // Safety: We're only reading from this register + unsafe { + let lock = pac::SIO.spinlock(N).read(); + if lock > 0 { + Some(Self(core::marker::PhantomData)) + } else { + None + } + } + } + + /// Clear a locked spin-lock. + /// + /// # Safety + /// + /// Only call this function if you hold the spin-lock. + pub unsafe fn release() { + unsafe { + // Write (any value): release the lock + pac::SIO.spinlock(N).write_value(1); + } + } +} + +impl Drop for Spinlock +where + Spinlock: SpinlockValid, +{ + fn drop(&mut self) { + // This is safe because we own the object, and hence hold the lock. + unsafe { Self::release() } + } +} + +pub(crate) type Spinlock31 = Spinlock<31>; +pub trait SpinlockValid {} +impl SpinlockValid for Spinlock<31> {} diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs index d21b5f7b..9a2fb7fc 100644 --- a/embassy-rp/src/lib.rs +++ b/embassy-rp/src/lib.rs @@ -5,6 +5,7 @@ // This mod MUST go first, so that the others see its macros. pub(crate) mod fmt; +mod critical_section_impl; mod intrinsics; pub mod adc; @@ -23,6 +24,7 @@ pub mod usb; pub mod clocks; pub mod flash; +pub mod multicore; mod reset; // Reexports diff --git a/embassy-rp/src/multicore.rs b/embassy-rp/src/multicore.rs new file mode 100644 index 00000000..dd3b70a6 --- /dev/null +++ b/embassy-rp/src/multicore.rs @@ -0,0 +1,266 @@ +//! Multicore support +//! +//! This module handles setup of the 2nd cpu core on the rp2040, which we refer to as core1. +//! It provides functionality for setting up the stack, and starting core1. +//! +//! The entrypoint for core1 can be any function that never returns, including closures. + +use core::mem::ManuallyDrop; +use core::sync::atomic::{compiler_fence, Ordering}; + +use crate::pac; + +/// Errors for multicore operations. +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum Error { + /// Operation is invalid on this core. + InvalidCore, + /// Core was unresponsive to commands. + Unresponsive, +} + +/// Core ID +#[derive(Debug)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum CoreId { + Core0, + Core1, +} + +#[inline(always)] +fn install_stack_guard(stack_bottom: *mut usize) { + let core = unsafe { cortex_m::Peripherals::steal() }; + + // Trap if MPU is already configured + if core.MPU.ctrl.read() != 0 { + cortex_m::asm::udf(); + } + + // The minimum we can protect is 32 bytes on a 32 byte boundary, so round up which will + // just shorten the valid stack range a tad. + let addr = (stack_bottom as u32 + 31) & !31; + // Mask is 1 bit per 32 bytes of the 256 byte range... clear the bit for the segment we want + let subregion_select = 0xff ^ (1 << ((addr >> 5) & 7)); + unsafe { + core.MPU.ctrl.write(5); // enable mpu with background default map + core.MPU.rbar.write((addr & !0xff) | 0x8); + core.MPU.rasr.write( + 1 // enable region + | (0x7 << 1) // size 2^(7 + 1) = 256 + | (subregion_select << 8) + | 0x10000000, // XN = disable instruction fetch; no other bits means no permissions + ); + } +} + +#[inline(always)] +fn core1_setup(stack_bottom: *mut usize) { + install_stack_guard(stack_bottom); + // TODO: irq priorities +} + +/// Multicore execution management. +pub struct Multicore { + cores: (Core, Core), +} + +/// Data type for a properly aligned stack of N 32-bit (usize) words +#[repr(C, align(32))] +pub struct Stack { + /// Memory to be used for the stack + pub mem: [usize; SIZE], +} + +impl Stack { + /// Construct a stack of length SIZE, initialized to 0 + pub const fn new() -> Stack { + Stack { mem: [0; SIZE] } + } +} + +impl Multicore { + /// Create a new |Multicore| instance. + pub fn new() -> Self { + Self { + cores: (Core { id: CoreId::Core0 }, Core { id: CoreId::Core1 }), + } + } + + /// Get the available |Core| instances. + pub fn cores(&mut self) -> &mut (Core, Core) { + &mut self.cores + } +} + +/// A handle for controlling a logical core. +pub struct Core { + pub id: CoreId, +} + +impl Core { + /// Spawn a function on this core. + pub fn spawn(&mut self, stack: &'static mut [usize], entry: F) -> Result<(), Error> + where + F: FnOnce() -> bad::Never + Send + 'static, + { + fn fifo_write(value: u32) { + unsafe { + let sio = pac::SIO; + // Wait for the FIFO to have some space + while !sio.fifo().st().read().rdy() { + cortex_m::asm::nop(); + } + // Signal that it's safe for core 0 to get rid of the original value now. + sio.fifo().wr().write_value(value); + } + + // Fire off an event to the other core. + // This is required as the other core may be `wfe` (waiting for event) + cortex_m::asm::sev(); + } + + fn fifo_read() -> u32 { + unsafe { + let sio = pac::SIO; + // Keep trying until FIFO has data + loop { + if sio.fifo().st().read().vld() { + return sio.fifo().rd().read(); + } else { + // We expect the sending core to `sev` on write. + cortex_m::asm::wfe(); + } + } + } + } + + fn fifo_drain() { + unsafe { + let sio = pac::SIO; + while sio.fifo().st().read().vld() { + let _ = sio.fifo().rd().read(); + } + } + } + + match self.id { + CoreId::Core1 => { + // The first two ignored `u64` parameters are there to take up all of the registers, + // which means that the rest of the arguments are taken from the stack, + // where we're able to put them from core 0. + extern "C" fn core1_startup bad::Never>( + _: u64, + _: u64, + entry: &mut ManuallyDrop, + stack_bottom: *mut usize, + ) -> ! { + core1_setup(stack_bottom); + let entry = unsafe { ManuallyDrop::take(entry) }; + // Signal that it's safe for core 0 to get rid of the original value now. + fifo_write(1); + entry() + } + + // Reset the core + unsafe { + let psm = pac::PSM; + psm.frce_off().modify(|w| w.set_proc1(true)); + while !psm.frce_off().read().proc1() { + cortex_m::asm::nop(); + } + psm.frce_off().modify(|w| w.set_proc1(false)); + } + + // Set up the stack + let mut stack_ptr = unsafe { stack.as_mut_ptr().add(stack.len()) }; + + // We don't want to drop this, since it's getting moved to the other core. + let mut entry = ManuallyDrop::new(entry); + + // Push the arguments to `core1_startup` onto the stack. + unsafe { + // Push `stack_bottom`. + stack_ptr = stack_ptr.sub(1); + stack_ptr.cast::<*mut usize>().write(stack.as_mut_ptr()); + + // Push `entry`. + stack_ptr = stack_ptr.sub(1); + stack_ptr.cast::<&mut ManuallyDrop>().write(&mut entry); + } + + // Make sure the compiler does not reorder the stack writes after to after the + // below FIFO writes, which would result in them not being seen by the second + // core. + // + // From the compiler perspective, this doesn't guarantee that the second core + // actually sees those writes. However, we know that the RP2040 doesn't have + // memory caches, and writes happen in-order. + compiler_fence(Ordering::Release); + + let p = unsafe { cortex_m::Peripherals::steal() }; + let vector_table = p.SCB.vtor.read(); + + // After reset, core 1 is waiting to receive commands over FIFO. + // This is the sequence to have it jump to some code. + let cmd_seq = [ + 0, + 0, + 1, + vector_table as usize, + stack_ptr as usize, + core1_startup:: as usize, + ]; + + let mut seq = 0; + let mut fails = 0; + loop { + let cmd = cmd_seq[seq] as u32; + if cmd == 0 { + fifo_drain(); + cortex_m::asm::sev(); + } + fifo_write(cmd); + + let response = fifo_read(); + if cmd == response { + seq += 1; + } else { + seq = 0; + fails += 1; + if fails > 16 { + // The second core isn't responding, and isn't going to take the entrypoint, + // so we have to drop it ourselves. + drop(ManuallyDrop::into_inner(entry)); + return Err(Error::Unresponsive); + } + } + if seq >= cmd_seq.len() { + break; + } + } + + // Wait until the other core has copied `entry` before returning. + fifo_read(); + + Ok(()) + } + _ => Err(Error::InvalidCore), + } + } +} + +// https://github.com/nvzqz/bad-rs/blob/master/src/never.rs +mod bad { + pub(crate) type Never = ::Output; + + pub trait HasOutput { + type Output; + } + + impl HasOutput for fn() -> O { + type Output = O; + } + + type F = fn() -> !; +} diff --git a/examples/rp/Cargo.toml b/examples/rp/Cargo.toml index 60a8ba94..bd624a32 100644 --- a/examples/rp/Cargo.toml +++ b/examples/rp/Cargo.toml @@ -18,7 +18,8 @@ embassy-usb-logger = { version = "0.1.0", path = "../../embassy-usb-logger" } defmt = "0.3" defmt-rtt = "0.4" -cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } +#cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } +cortex-m = { version = "0.7.6" } cortex-m-rt = "0.7.0" panic-probe = { version = "0.3", features = ["print-defmt"] } futures = { version = "0.3.17", default-features = false, features = ["async-await", "cfg-target-has-atomic", "unstable"] } diff --git a/examples/rp/src/bin/multicore.rs b/examples/rp/src/bin/multicore.rs new file mode 100644 index 00000000..46d3cd17 --- /dev/null +++ b/examples/rp/src/bin/multicore.rs @@ -0,0 +1,62 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] + +use defmt::*; +use embassy_executor::Executor; +use embassy_executor::_export::StaticCell; +use embassy_rp::gpio::{Level, Output}; +use embassy_rp::peripherals::PIN_25; +use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; +use embassy_sync::channel::Channel; +use embassy_time::{Duration, Timer}; +use embassy_rp::multicore::{Multicore, Stack}; +use {defmt_rtt as _, panic_probe as _}; + +static mut CORE1_STACK: Stack<4096> = Stack::new(); +static EXECUTOR0: StaticCell = StaticCell::new(); +static EXECUTOR1: StaticCell = StaticCell::new(); +static CHANNEL: Channel = Channel::new(); + +enum LedState { + On, + Off, +} + +#[cortex_m_rt::entry] +fn main() -> ! { + let p = embassy_rp::init(Default::default()); + let led = Output::new(p.PIN_25, Level::Low); + + let mut mc = Multicore::new(); + let (_, core1) = mc.cores(); + let _ = core1.spawn(unsafe { &mut CORE1_STACK.mem }, move || { + let executor1 = EXECUTOR1.init(Executor::new()); + executor1.run(|spawner| unwrap!(spawner.spawn(core1_task(led)))); + }); + + let executor0 = EXECUTOR0.init(Executor::new()); + executor0.run(|spawner| unwrap!(spawner.spawn(core0_task()))); +} + +#[embassy_executor::task] +async fn core0_task() { + info!("Hello from core 0"); + loop { + CHANNEL.send(LedState::On).await; + Timer::after(Duration::from_millis(100)).await; + CHANNEL.send(LedState::Off).await; + Timer::after(Duration::from_millis(400)).await; + } +} + +#[embassy_executor::task] +async fn core1_task(mut led: Output<'static, PIN_25>) { + info!("Hello from core 1"); + loop { + match CHANNEL.recv().await { + LedState::On => led.set_high(), + LedState::Off => led.set_low(), + } + } +} \ No newline at end of file