Pause CORE1 execution during flash operations

embassy-rp/src/flash.rs

@@ -6,6 +6,7 @@ use embedded_storage::nor_flash::{
     ReadNorFlash,
 };
 
+use crate::pac;
 use crate::peripherals::FLASH;
 
 pub const FLASH_BASE: usize = 0x10000000;
@@ -28,6 +29,7 @@ pub enum Error {
     OutOfBounds,
     /// Unaligned operation or using unaligned buffers.
     Unaligned,
+    InvalidCore,
     Other,
 }
 
@@ -46,7 +48,7 @@ impl NorFlashError for Error {
         match self {
             Self::OutOfBounds => NorFlashErrorKind::OutOfBounds,
             Self::Unaligned => NorFlashErrorKind::NotAligned,
-            Self::Other => NorFlashErrorKind::Other,
+            _ => NorFlashErrorKind::Other,
         }
     }
 }
@@ -87,7 +89,7 @@ impl<'d, T: Instance, const FLASH_SIZE: usize> Flash<'d, T, FLASH_SIZE> {
 
         let len = to - from;
 
-        unsafe { self.in_ram(|| ram_helpers::flash_range_erase(from, len, true)) };
+        unsafe { self.in_ram(|| ram_helpers::flash_range_erase(from, len, true))? };
 
         Ok(())
     }
@@ -112,7 +114,7 @@ impl<'d, T: Instance, const FLASH_SIZE: usize> Flash<'d, T, FLASH_SIZE> {
 
             let unaligned_offset = offset as usize - start;
 
-            unsafe { self.in_ram(|| ram_helpers::flash_range_program(unaligned_offset as u32, &pad_buf, true)) }
+            unsafe { self.in_ram(|| ram_helpers::flash_range_program(unaligned_offset as u32, &pad_buf, true))? }
         }
 
         let remaining_len = bytes.len() - start_padding;
@@ -130,12 +132,12 @@ impl<'d, T: Instance, const FLASH_SIZE: usize> Flash<'d, T, FLASH_SIZE> {
             if bytes.as_ptr() as usize >= 0x2000_0000 {
                 let aligned_data = &bytes[start_padding..end_padding];
 
-                unsafe { self.in_ram(|| ram_helpers::flash_range_program(aligned_offset as u32, aligned_data, true)) }
+                unsafe { self.in_ram(|| ram_helpers::flash_range_program(aligned_offset as u32, aligned_data, true))? }
             } else {
                 for chunk in bytes[start_padding..end_padding].chunks_exact(PAGE_SIZE) {
                     let mut ram_buf = [0xFF_u8; PAGE_SIZE];
                     ram_buf.copy_from_slice(chunk);
-                    unsafe { self.in_ram(|| ram_helpers::flash_range_program(aligned_offset as u32, &ram_buf, true)) }
+                    unsafe { self.in_ram(|| ram_helpers::flash_range_program(aligned_offset as u32, &ram_buf, true))? }
                     aligned_offset += PAGE_SIZE;
                 }
             }
@@ -150,7 +152,7 @@ impl<'d, T: Instance, const FLASH_SIZE: usize> Flash<'d, T, FLASH_SIZE> {
 
             let unaligned_offset = end_offset - (PAGE_SIZE - rem_offset);
 
-            unsafe { self.in_ram(|| ram_helpers::flash_range_program(unaligned_offset as u32, &pad_buf, true)) }
+            unsafe { self.in_ram(|| ram_helpers::flash_range_program(unaligned_offset as u32, &pad_buf, true))? }
         }
 
         Ok(())
@@ -159,10 +161,17 @@ impl<'d, T: Instance, const FLASH_SIZE: usize> Flash<'d, T, FLASH_SIZE> {
     /// Make sure to uphold the contract points with rp2040-flash.
     /// - interrupts must be disabled
     /// - DMA must not access flash memory
-    unsafe fn in_ram(&mut self, operation: impl FnOnce()) {
+    unsafe fn in_ram(&mut self, operation: impl FnOnce()) -> Result<(), Error> {
         let dma_status = &mut [false; crate::dma::CHANNEL_COUNT];
 
-        // TODO: Make sure CORE1 is paused during the entire duration of the RAM function
+        // Make sure we're running on CORE0
+        let core_id: u32 = unsafe { pac::SIO.cpuid().read() };
+        if core_id != 0 {
+            return Err(Error::InvalidCore);
+        }
+
+        // Make sure CORE1 is paused during the entire duration of the RAM function
+        crate::multicore::pause_core1();
 
         critical_section::with(|_| {
             // Pause all DMA channels for the duration of the ram operation
@@ -185,6 +194,10 @@ impl<'d, T: Instance, const FLASH_SIZE: usize> Flash<'d, T, FLASH_SIZE> {
                 }
             }
         });
+
+        // Resume CORE1 execution
+        crate::multicore::resume_core1();
+        Ok(())
     }
 }
 

embassy-rp/src/multicore.rs

@@ -11,14 +11,19 @@
 use core::mem::ManuallyDrop;
 use core::sync::atomic::{compiler_fence, Ordering};
 
-use crate::pac;
+use atomic_polyfill::AtomicBool;
+
+use crate::interrupt::{Interrupt, InterruptExt};
+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 {
-    /// Operation is invalid on this core.
-    InvalidCore,
     /// Core was unresponsive to commands.
     Unresponsive,
 }
@@ -64,7 +69,7 @@ fn core1_setup(stack_bottom: *mut usize) {
 
 /// MultiCore execution management.
 pub struct MultiCore {
-    pub cores: (Core, Core),
+    pub cores: (Core0, Core1),
 }
 
 /// Data type for a properly aligned stack of N 32-bit (usize) words
@@ -85,69 +90,51 @@ impl MultiCore {
     /// Create a new |MultiCore| instance.
     pub fn new() -> Self {
         Self {
-            cores: (Core { id: CoreId::Core0 }, Core { id: CoreId::Core1 }),
+            cores: (Core0 {}, Core1 {}),
         }
     }
 
     /// Get the available |Core| instances.
-    pub fn cores(&mut self) -> &mut (Core, Core) {
+    pub fn cores(&mut self) -> &mut (Core0, Core1) {
         &mut self.cores
     }
 }
 
 /// A handle for controlling a logical core.
-pub struct Core {
+pub struct Core0 {}
-    pub id: CoreId,
+/// A handle for controlling a logical core.
+pub struct Core1 {}
+
+#[interrupt]
+#[link_section = ".data.ram_func"]
+unsafe fn SIO_IRQ_PROC1() {
+    let sio = pac::SIO;
+    // Clear IRQ
+    sio.fifo().st().write(|w| w.set_wof(false));
+
+    while sio.fifo().st().read().vld() {
+        // Pause CORE1 execution and disable interrupts
+        if fifo_read_wfe() == PAUSE_TOKEN {
+            cortex_m::interrupt::disable();
+            // Signal to CORE0 that execution is paused
+            fifo_write(PAUSE_TOKEN);
+            // Wait for `resume` signal from CORE0
+            while fifo_read_wfe() != RESUME_TOKEN {
+                cortex_m::asm::nop();
+            }
+            cortex_m::interrupt::enable();
+            // Signal to CORE0 that execution is resumed
+            fifo_write(RESUME_TOKEN);
+        }
+    }
 }
 
-impl Core {
+impl Core1 {
-    /// Spawn a function on this 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.
@@ -161,6 +148,12 @@ impl Core {
             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()
         }
 
@@ -247,7 +240,75 @@ impl Core {
 
         Ok(())
     }
-            _ => Err(Error::InvalidCore),
+}
+
+/// Pause execution on CORE1.
+pub fn pause_core1() {
+    if IS_CORE1_INIT.load(Ordering::Acquire) {
+        fifo_write(PAUSE_TOKEN);
+        // Wait for CORE1 to signal it has paused execution.
+        while fifo_read() != PAUSE_TOKEN {}
+    }
+}
+
+/// Resume CORE1 execution.
+pub fn resume_core1() {
+    if IS_CORE1_INIT.load(Ordering::Acquire) {
+        fifo_write(RESUME_TOKEN);
+        // Wait for CORE1 to signal it has resumed execution.
+        while fifo_read() != RESUME_TOKEN {}
+    }
+}
+
+// Push a value to the inter-core FIFO, block until space is available
+#[inline(always)]
+fn fifo_write(value: u32) {
+    unsafe {
+        let sio = pac::SIO;
+        // Wait for the FIFO to have enough space
+        while !sio.fifo().st().read().rdy() {
+            cortex_m::asm::nop();
+        }
+        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();
+}
+
+// Pop a value from inter-core FIFO, block until available
+#[inline(always)]
+fn fifo_read() -> u32 {
+    unsafe {
+        let sio = pac::SIO;
+        // Wait until FIFO has data
+        while !sio.fifo().st().read().vld() {
+            cortex_m::asm::nop();
+        }
+        sio.fifo().rd().read()
+    }
+}
+
+// Pop a value from inter-core FIFO, `wfe` until available
+#[inline(always)]
+fn fifo_read_wfe() -> u32 {
+    unsafe {
+        let sio = pac::SIO;
+        // Wait until FIFO has data
+        while !sio.fifo().st().read().vld() {
+            cortex_m::asm::wfe();
+        }
+        sio.fifo().rd().read()
+    }
+}
+
+// Drain inter-core FIFO
+#[inline(always)]
+fn fifo_drain() {
+    unsafe {
+        let sio = pac::SIO;
+        while sio.fifo().st().read().vld() {
+            let _ = sio.fifo().rd().read();
         }
     }
 }