diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs index 2cd99f45..c6442c56 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 3703fe81..2dfa215b 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 { /// Memory to be used for the stack - pub mem: [usize; SIZE], + pub mem: [u8; SIZE], } impl Stack { /// Construct a stack of length SIZE, initialized to 0 pub const fn new() -> Stack { - 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(&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 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); +/// Spawn a function on this core +pub fn spawn_core1(_core1: CORE1, stack: &'static mut Stack, 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 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); - 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(); + 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)); - } - - // 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(()) + 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)); + } + + let mem = unsafe { core::slice::from_raw_parts_mut(stack.mem.as_mut_ptr() as *mut usize, stack.mem.len() / 4) }; + + // Set up the stack + let mut stack_ptr = unsafe { mem.as_mut_ptr().add(mem.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(mem.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 + panic!("CORE1 not responding"); + } + } + 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 53941da6..376b2b61 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 00000000..da78e887 --- /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 = StaticCell::new(); +static EXECUTOR1: StaticCell = StaticCell::new(); +static CHANNEL0: Channel = Channel::new(); +static CHANNEL1: Channel = 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; +}