embassy-rp: Add multicore support

6 files changed, 475 insertions, 2 deletions

diff --git a/embassy-rp/Cargo.toml b/embassy-rp/Cargo.toml
index 284d458c6..07cd1bc1b 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 000000000..ce284c856
--- /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<const N: usize>(core::marker::PhantomData<()>)
+where
+    Spinlock<N>: SpinlockValid;
+
+impl<const N: usize> Spinlock<N>
+where
+    Spinlock<N>: 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<Self> {
+        // 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<const N: usize> Drop for Spinlock<N>
+where
+    Spinlock<N>: 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 d21b5f7b0..9a2fb7fc3 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 000000000..dd3b70a67
--- /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<const SIZE: usize> {
+    /// Memory to be used for the stack
+    pub mem: [usize; SIZE],
+}
+
+impl<const SIZE: usize> Stack<SIZE> {
+    /// Construct a stack of length SIZE, initialized to 0
+    pub const fn new() -> Stack<SIZE> {
+        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<F>(&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<F: FnOnce() -> bad::Never>(
+                    _: u64,
+                    _: u64,
+                    entry: &mut ManuallyDrop<F>,
+                    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<F>>().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::<F> 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 = <F as HasOutput>::Output;
+
+    pub trait HasOutput {
+        type Output;
+    }
+
+    impl<O> HasOutput for fn() -> O {
+        type Output = O;
+    }
+
+    type F = fn() -> !;
+}
diff --git a/examples/rp/Cargo.toml b/examples/rp/Cargo.toml
index 60a8ba94d..bd624a329 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 000000000..46d3cd17c
--- /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<Executor> = StaticCell::new();
+static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
+static CHANNEL: Channel<CriticalSectionRawMutex, LedState, 1> = 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