NaxdyOrg/embassy
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Pause CORE1 execution during flash operations

Browse source
This commit is contained in:
kalkyl 2022-12-13 04:02:28 +01:00
parent 96d6c7243b
commit eb1d2e1295
2 changed files with 223 additions and 149 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

29
embassy-rp/src/flash.rs
View file

@ -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(())
}
}

167
embassy-rp/src/multicore.rs
View file

@ -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 id: CoreId,
pub struct Core0 {}
/// 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 {
/// Spawn a function on this core.
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,
{
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();
}
}
}