4 files changed, 152 insertions, 139 deletions

diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs
index 2cd99f456..c6442c56e 100644
--- a/embassy-rp/src/lib.rs
+++ b/embassy-rp/src/lib.rs
@@ -106,6 +106,8 @@ embassy_hal_common::peripherals! {
     FLASH,
 
     ADC,
+
+    CORE1,
 }
 
 #[link_section = ".boot2"]
diff --git a/embassy-rp/src/multicore.rs b/embassy-rp/src/multicore.rs
index 3703fe819..2dfa215b5 100644
--- a/embassy-rp/src/multicore.rs
+++ b/embassy-rp/src/multicore.rs
@@ -36,20 +36,13 @@ use core::sync::atomic::{compiler_fence, Ordering};
 use atomic_polyfill::AtomicBool;
 
 use crate::interrupt::{Interrupt, InterruptExt};
+use crate::peripherals::CORE1;
 use crate::{interrupt, pac};
 
 const PAUSE_TOKEN: u32 = 0xDEADBEEF;
 const RESUME_TOKEN: u32 = !0xDEADBEEF;
 static IS_CORE1_INIT: AtomicBool = AtomicBool::new(false);
 
-/// Errors for multicore operations.
-#[derive(Debug)]
-#[cfg_attr(feature = "defmt", derive(defmt::Format))]
-pub enum Error {
-    /// Core was unresponsive to commands.
-    Unresponsive,
-}
-
 #[inline(always)]
 fn install_stack_guard(stack_bottom: *mut usize) {
     let core = unsafe { cortex_m::Peripherals::steal() };
@@ -81,44 +74,20 @@ fn core1_setup(stack_bottom: *mut usize) {
     install_stack_guard(stack_bottom);
 }
 
-/// MultiCore execution management.
-pub struct MultiCore {
-    pub cores: (Core0, Core1),
-}
-
-/// Data type for a properly aligned stack of N 32-bit (usize) words
+/// Data type for a properly aligned stack of N bytes
 #[repr(C, align(32))]
 pub struct Stack<const SIZE: usize> {
     /// Memory to be used for the stack
-    pub mem: [usize; SIZE],
+    pub mem: [u8; 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] }
+        Stack { mem: [0_u8; SIZE] }
     }
 }
 
-impl MultiCore {
-    /// Create a new |MultiCore| instance.
-    pub fn new() -> Self {
-        Self {
-            cores: (Core0 {}, Core1 {}),
-        }
-    }
-
-    /// Get the available |Core| instances.
-    pub fn cores(&mut self) -> &mut (Core0, Core1) {
-        &mut self.cores
-    }
-}
-
-/// A handle for controlling a logical core.
-pub struct Core0 {}
-/// A handle for controlling a logical core.
-pub struct Core1 {}
-
 #[interrupt]
 #[link_section = ".data.ram_func"]
 unsafe fn SIO_IRQ_PROC1() {
@@ -143,117 +112,113 @@ unsafe fn SIO_IRQ_PROC1() {
     }
 }
 
-impl Core1 {
-    /// 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,
-    {
-        // 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);
-
-            IS_CORE1_INIT.store(true, Ordering::Release);
-            // Enable fifo interrupt on CORE1 for `pause` functionality.
-            let irq = unsafe { interrupt::SIO_IRQ_PROC1::steal() };
-            irq.enable();
-
-            entry()
-        }
+/// Spawn a function on this core
+pub fn spawn_core1<F, const SIZE: usize>(_core1: CORE1, stack: &'static mut Stack<SIZE>, entry: F)
+where
+    F: FnOnce() -> bad::Never + Send + 'static,
+{
+    // 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);
+
+        IS_CORE1_INIT.store(true, Ordering::Release);
+        // Enable fifo interrupt on CORE1 for `pause` functionality.
+        let irq = unsafe { interrupt::SIO_IRQ_PROC1::steal() };
+        irq.enable();
+
+        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));
+    // 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()) };
+    let mem = unsafe { core::slice::from_raw_parts_mut(stack.mem.as_mut_ptr() as *mut usize, stack.mem.len() / 4) };
 
-        // We don't want to drop this, since it's getting moved to the other core.
-        let mut entry = ManuallyDrop::new(entry);
+    // Set up the stack
+    let mut stack_ptr = unsafe { mem.as_mut_ptr().add(mem.len()) };
 
-        // 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());
+    // We don't want to drop this, since it's getting moved to the other core.
+    let mut entry = ManuallyDrop::new(entry);
 
-            // Push `entry`.
-            stack_ptr = stack_ptr.sub(1);
-            stack_ptr.cast::<&mut ManuallyDrop<F>>().write(&mut 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(mem.as_mut_ptr());
 
-        // 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;
+        // 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
+                panic!("CORE1 not responding");
             }
         }
-
-        // Wait until the other core has copied `entry` before returning.
-        fifo_read();
-
-        Ok(())
+        if seq >= cmd_seq.len() {
+            break;
+        }
     }
+
+    // Wait until the other core has copied `entry` before returning.
+    fifo_read();
 }
 
 /// Pause execution on CORE1.
diff --git a/examples/rp/src/bin/multicore.rs b/examples/rp/src/bin/multicore.rs
index 53941da60..376b2b61e 100644
--- a/examples/rp/src/bin/multicore.rs
+++ b/examples/rp/src/bin/multicore.rs
@@ -6,7 +6,7 @@ use defmt::*;
 use embassy_executor::Executor;
 use embassy_executor::_export::StaticCell;
 use embassy_rp::gpio::{Level, Output};
-use embassy_rp::multicore::{MultiCore, Stack};
+use embassy_rp::multicore::{spawn_core1, Stack};
 use embassy_rp::peripherals::PIN_25;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::channel::Channel;
@@ -28,8 +28,7 @@ fn main() -> ! {
     let p = embassy_rp::init(Default::default());
     let led = Output::new(p.PIN_25, Level::Low);
 
-    let mut mc = MultiCore::new();
-    let _ = mc.cores.1.spawn(unsafe { &mut CORE1_STACK.mem }, move || {
+    spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
         let executor1 = EXECUTOR1.init(Executor::new());
         executor1.run(|spawner| unwrap!(spawner.spawn(core1_task(led))));
     });
diff --git a/tests/rp/src/bin/multicore.rs b/tests/rp/src/bin/multicore.rs
new file mode 100644
index 000000000..da78e887a
--- /dev/null
+++ b/tests/rp/src/bin/multicore.rs
@@ -0,0 +1,47 @@
+#![no_std]
+#![no_main]
+#![feature(type_alias_impl_trait)]
+
+use defmt::{info, unwrap};
+use embassy_executor::Executor;
+use embassy_executor::_export::StaticCell;
+use embassy_rp::multicore::{spawn_core1, Stack};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::channel::Channel;
+use {defmt_rtt as _, panic_probe as _};
+
+static mut CORE1_STACK: Stack<1024> = Stack::new();
+static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
+static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
+static CHANNEL0: Channel<CriticalSectionRawMutex, bool, 1> = Channel::new();
+static CHANNEL1: Channel<CriticalSectionRawMutex, bool, 1> = Channel::new();
+
+#[cortex_m_rt::entry]
+fn main() -> ! {
+    let p = embassy_rp::init(Default::default());
+    spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
+        let executor1 = EXECUTOR1.init(Executor::new());
+        executor1.run(|spawner| unwrap!(spawner.spawn(core1_task())));
+    });
+    let executor0 = EXECUTOR0.init(Executor::new());
+    executor0.run(|spawner| unwrap!(spawner.spawn(core0_task())));
+}
+
+#[embassy_executor::task]
+async fn core0_task() {
+    info!("CORE0 is running");
+    let ping = true;
+    CHANNEL0.send(ping).await;
+    let pong = CHANNEL1.recv().await;
+    assert_eq!(ping, pong);
+
+    info!("Test OK");
+    cortex_m::asm::bkpt();
+}
+
+#[embassy_executor::task]
+async fn core1_task() {
+    info!("CORE1 is running");
+    let ping = CHANNEL0.recv().await;
+    CHANNEL1.send(ping).await;
+}