multicore (#89)

mvp multicore implementation

Co-authored-by: Jan Niehusmann <jan@gondor.com>

rp2040-hal/Cargo.toml

@@ -45,4 +45,4 @@ pio-proc = { git = "https://github.com/rp-rs/pio-rs.git", branch = "main" }
 [features]
 rt = ["rp2040-pac/rt"]
 embassy-traits = ["embassy_traits", "futures"]
-
+alloc = []

rp2040-hal/src/lib.rs

@@ -21,6 +21,7 @@ pub mod clocks;
 pub mod dma;
 pub mod gpio;
 pub mod i2c;
+pub mod multicore;
 pub mod pio;
 pub mod pll;
 pub mod prelude;

rp2040-hal/src/multicore.rs

@@ -0,0 +1,211 @@
+//! Multicore support
+// See [Chapter ?? Section ??](https://datasheets.raspberrypi.org/rp2040/rp2040_datasheet.pdf) for more details
+
+use crate::pac;
+
+#[cfg(feature = "alloc")]
+extern crate alloc;
+
+/// Errors for multicore operations.
+#[derive(Debug)]
+pub enum Error {
+    /// Operation is invalid on this core.
+    InvalidCore,
+    /// Core was unresposive to commands.
+    Unresponsive,
+}
+
+// We pass data to cores via the stack, so we read
+// the data off the stack and into parameters that
+// rust can read here. Ideally this would be a
+// #[naked] function but that is not stable yet.
+static MULTICORE_TRAMPOLINE: [u16; 2] = [
+    0xbd03, // pop {r0, r1, pc} - call wrapper (pc) with r0 and r1
+    0x46c0, // nop - pad this out to 32 bits long
+];
+static mut CORE1_STACK: [usize; 4096] = [0; 4096];
+
+#[inline(always)]
+fn install_stack_guard(stack_bottom: *mut usize) {
+    let core = unsafe { pac::CorePeripherals::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<'p> {
+    cores: [Core<'p>; 2],
+}
+
+impl<'p> Multicore<'p> {
+    /// Create a new |Multicore| instance.
+    pub fn new(psm: &'p mut pac::PSM, ppb: &'p mut pac::PPB, sio: &'p mut crate::sio::Sio) -> Self {
+        Self {
+            cores: [
+                Core { inner: None },
+                Core {
+                    inner: Some((psm, ppb, sio)),
+                },
+            ],
+        }
+    }
+
+    /// Get the available |Core| instances.
+    pub fn cores(&mut self) -> &'p mut [Core] {
+        &mut self.cores
+    }
+}
+
+/// A handle for controlling a logical core.
+pub struct Core<'p> {
+    inner: Option<(&'p mut pac::PSM, &'p mut pac::PPB, &'p mut crate::sio::Sio)>,
+}
+
+impl<'p> Core<'p> {
+    /// Get the id of this core.
+    pub fn id(&self) -> u8 {
+        match self.inner {
+            None => 0,
+            Some(..) => 1,
+        }
+    }
+
+    fn inner_spawn(&mut self, wrapper: *mut (), entry: *mut ()) -> Result<(), Error> {
+        if let Some((psm, ppb, sio)) = self.inner.as_mut() {
+            // Reset the core
+            psm.frce_off.modify(|_, w| w.proc1().set_bit());
+            while !psm.frce_off.read().proc1().bit_is_set() {
+                cortex_m::asm::nop();
+            }
+            psm.frce_off.modify(|_, w| w.proc1().clear_bit());
+
+            // Set up the stack
+            let core1_stack = unsafe { &mut CORE1_STACK };
+            let mut stack_ptr = unsafe { core1_stack.as_mut_ptr().add(core1_stack.len()) };
+
+            let mut push = |v: usize| unsafe {
+                stack_ptr = stack_ptr.sub(1);
+                stack_ptr.write(v);
+            };
+
+            push(wrapper as usize);
+            push(core1_stack.as_mut_ptr() as usize);
+            push(entry as usize);
+
+            let vector_table = ppb.vtor.read().bits();
+
+            // 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,
+                MULTICORE_TRAMPOLINE.as_ptr() as usize + 1,
+            ];
+
+            let mut seq = 0;
+            let mut fails = 0;
+            loop {
+                let cmd = cmd_seq[seq] as u32;
+                if cmd == 0 {
+                    sio.fifo.drain();
+                    cortex_m::asm::sev();
+                }
+                sio.fifo.write_blocking(cmd);
+                let response = sio.fifo.read_blocking();
+                if cmd == response {
+                    seq += 1;
+                } else {
+                    seq = 0;
+                    fails += 1;
+                    if fails > 16 {
+                        return Err(Error::Unresponsive);
+                    }
+                }
+                if seq >= cmd_seq.len() {
+                    break;
+                }
+            }
+
+            Ok(())
+        } else {
+            Err(Error::InvalidCore)
+        }
+    }
+
+    /// Spawn a function on this core.
+    #[cfg(not(feature = "alloc"))]
+    pub fn spawn(&mut self, entry: fn() -> !) -> Result<(), Error> {
+        #[allow(improper_ctypes_definitions)]
+        extern "C" fn core1_no_alloc(entry: fn() -> !, stack_bottom: *mut usize) -> ! {
+            core1_setup(stack_bottom);
+            entry();
+        }
+
+        self.inner_spawn(core1_no_alloc as _, entry as _)
+    }
+
+    /// Spawn a function on this core.
+    #[cfg(feature = "alloc")]
+    pub fn spawn<F>(&mut self, entry: F) -> Result<(), Error>
+    where
+        F: FnOnce() -> bad::Never,
+        F: Send + 'static,
+    {
+        use alloc::boxed::Box;
+
+        let main: Box<dyn FnOnce() -> bad::Never> = Box::new(move || entry());
+        let p = Box::into_raw(Box::new(main));
+
+        extern "C" fn core1_alloc(entry: *mut (), stack_bottom: *mut usize) -> ! {
+            core1_setup(stack_bottom);
+            let main = unsafe { Box::from_raw(entry as *mut Box<dyn FnOnce() -> bad::Never>) };
+            main();
+        }
+
+        self.inner_spawn(core1_alloc as _, p as _)
+    }
+}
+
+// https://github.com/nvzqz/bad-rs/blob/master/src/never.rs
+#[cfg(feature = "alloc")]
+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() -> !;
+}