From 7c707882a1dc9e4cda867294feee002884bd1106 Mon Sep 17 00:00:00 2001 From: Kaspar Schleiser Date: Sun, 6 Jul 2025 18:00:31 +0200 Subject: [PATCH] embassy-rp: allow injection of FIFO handler Add `fifo-handler` feature for injecting a custom handler of FIFO messages. Also enable FIFO queue in core1->core0 direction. Signed-off-by: Kaspar Schleiser embassy_rp: fix unsafe call to extern fn --- embassy-rp/CHANGELOG.md | 1 + embassy-rp/Cargo.toml | 5 ++ embassy-rp/src/multicore.rs | 107 +++++++++++++++++++++++++----------- 3 files changed, 80 insertions(+), 33 deletions(-) diff --git a/embassy-rp/CHANGELOG.md b/embassy-rp/CHANGELOG.md index d1265ffc41..aa2bd23557 100644 --- a/embassy-rp/CHANGELOG.md +++ b/embassy-rp/CHANGELOG.md @@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Add PIO I2S input - Add PIO onewire parasite power strong pullup - add `wait_for_alarm` and `alarm_scheduled` methods to rtc module ([#4216](https://github.com/embassy-rs/embassy/pull/4216)) +- allow injection of FIFO handler ## 0.8.0 - 2025-08-26 diff --git a/embassy-rp/Cargo.toml b/embassy-rp/Cargo.toml index 101914a361..4aa35a620b 100644 --- a/embassy-rp/Cargo.toml +++ b/embassy-rp/Cargo.toml @@ -146,6 +146,11 @@ _test = [] ## program and other details. binary-info = ["rt", "dep:rp-binary-info", "rp-binary-info?/binary-info"] +## Inject function for handling FIFO messages. +## If this feature is enabled, a foreign function +## `fn handle_fifo(u32) -> bool` must be implemented. +fifo-handler = [] + [dependencies] embassy-sync = { version = "0.7.2", path = "../embassy-sync" } embassy-time-driver = { version = "0.2.1", path = "../embassy-time-driver", optional = true } diff --git a/embassy-rp/src/multicore.rs b/embassy-rp/src/multicore.rs index adedc98ad5..f0f46b8c4c 100644 --- a/embassy-rp/src/multicore.rs +++ b/embassy-rp/src/multicore.rs @@ -108,54 +108,63 @@ impl Stack { } } -#[cfg(all(feature = "rt", feature = "rp2040"))] -#[interrupt] +#[cfg(feature = "fifo-handler")] +extern "Rust" { + fn handle_fifo_token(token: u32) -> bool; +} + +#[cfg(feature = "rt")] #[link_section = ".data.ram_func"] -unsafe fn SIO_IRQ_PROC1() { +#[inline] +unsafe fn sio_handler() { 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 { + let token = fifo_read_wfe(); + + // Forward to user handler. + #[cfg(feature = "fifo-handler")] + if handle_fifo_token(token) { + continue; + } + + // Pause this core's execution and disable interrupts + if token == PAUSE_TOKEN { cortex_m::interrupt::disable(); - // Signal to CORE0 that execution is paused + // Signal to other that execution is paused fifo_write(PAUSE_TOKEN); - // Wait for `resume` signal from CORE0 + // Wait for `resume` signal from other core while fifo_read_wfe() != RESUME_TOKEN { cortex_m::asm::nop(); } cortex_m::interrupt::enable(); - // Signal to CORE0 that execution is resumed + // Signal to other core that execution is resumed fifo_write(RESUME_TOKEN); } } } +#[cfg(all(feature = "rt", feature = "rp2040"))] +#[interrupt] +#[link_section = ".data.ram_func"] +unsafe fn SIO_IRQ_PROC0() { + sio_handler(); +} + +#[cfg(all(feature = "rt", feature = "rp2040"))] +#[interrupt] +#[link_section = ".data.ram_func"] +unsafe fn SIO_IRQ_PROC1() { + sio_handler(); +} + #[cfg(all(feature = "rt", feature = "_rp235x"))] #[interrupt] #[link_section = ".data.ram_func"] unsafe fn SIO_IRQ_FIFO() { - 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); - } - } + sio_handler(); } /// Spawn a function on this core @@ -286,23 +295,55 @@ where // Wait until the other core has copied `entry` before returning. fifo_read(); + + // Enable FIFO interrupts from core1. + #[cfg(feature = "rp2040")] + unsafe { + interrupt::SIO_IRQ_PROC0.enable() + }; + #[cfg(feature = "_rp235x")] + unsafe { + interrupt::SIO_IRQ_FIFO.enable() + }; } /// 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 {} + cortex_m::interrupt::free(|_| { + fifo_write(PAUSE_TOKEN); + // Wait for CORE1 to signal it has paused execution. + loop { + let token = fifo_read(); + if token == PAUSE_TOKEN { + break; + } + #[cfg(feature = "fifo-handler")] + unsafe { + handle_fifo_token(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 {} + cortex_m::interrupt::free(|_| { + fifo_write(RESUME_TOKEN); + // Wait for CORE1 to signal it has resumed execution. + loop { + let token = fifo_read(); + if token == RESUME_TOKEN { + break; + } + #[cfg(feature = "fifo-handler")] + unsafe { + handle_fifo_token(token); + } + } + }) } }