Software interrupts runtime binding (#1398)

* Split software interrupts

* Make swint0 unavailable on a multi_core target when using thread-executor

* Clarify why embassy_multiprio example needs two executors

* Make interrupt-executor require a SoftwareInterrupt<n>

* Improve code

* CHANGELOG.md

* Don't use `#[interrupt]` in thread-executor

* More docs

* Typo fixed
This commit is contained in:
Björn Quentin 2024-04-05 14:33:21 +02:00 committed by GitHub
parent d26b1bd504
commit b3bc28efef
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
11 changed files with 349 additions and 333 deletions

View File

@ -55,6 +55,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
- SYSTIMER and TIMG instances can now be created in async or blocking mode (#1348)
- Runtime ISR binding for TWAI (#1384)
- Runtime ISR binding for assist_debug (#1395)
- Runtime ISR binding for software interrupts, software interrupts are split now, interrupt-executor takes the software interrupt to use, interrupt-executor is easier to use (#1398)
### Removed

View File

@ -1,128 +1,96 @@
//! Interrupt-mode executor.
use core::{cell::UnsafeCell, marker::PhantomData, mem::MaybeUninit};
use core::{cell::UnsafeCell, mem::MaybeUninit};
use embassy_executor::{raw, SendSpawner};
#[cfg(any(esp32c6, esp32h2))]
use peripherals::INTPRI as SystemPeripheral;
#[cfg(not(any(esp32c6, esp32h2)))]
use peripherals::SYSTEM as SystemPeripheral;
use portable_atomic::{AtomicUsize, Ordering};
use crate::{get_core, interrupt, peripherals};
static FROM_CPU_IRQ_USED: AtomicUsize = AtomicUsize::new(0);
pub trait SwPendableInterrupt {
fn enable(priority: interrupt::Priority);
fn number() -> usize;
fn pend();
fn clear();
}
macro_rules! from_cpu {
($irq:literal) => {
paste::paste! {
pub struct [<FromCpu $irq>];
impl [<FromCpu $irq>] {
fn set_bit(value: bool) {
let system = unsafe { &*SystemPeripheral::PTR };
system
.[<cpu_intr_from_cpu_ $irq>]()
.write(|w| w.[<cpu_intr_from_cpu_ $irq>]().bit(value));
}
}
impl SwPendableInterrupt for [<FromCpu $irq>] {
fn enable(priority: interrupt::Priority) {
let mask = 1 << $irq;
// We don't allow using the same interrupt for multiple executors.
if FROM_CPU_IRQ_USED.fetch_or(mask, Ordering::SeqCst) & mask != 0 {
panic!("FROM_CPU_{} is already used by a different executor.", $irq);
}
unwrap!(interrupt::enable(peripherals::Interrupt::[<FROM_CPU_INTR $irq>], priority));
}
fn number() -> usize {
$irq
}
fn pend() {
Self::set_bit(true);
}
fn clear() {
Self::set_bit(false);
}
}
}
use crate::{
get_core,
interrupt::{self, InterruptHandler},
system::SoftwareInterrupt,
};
}
// from_cpu!(0); // reserve 0 for thread mode & multi-core
from_cpu!(1);
from_cpu!(2);
from_cpu!(3);
static mut EXECUTORS: [CallbackContext; 4] = [
CallbackContext::new(),
CallbackContext::new(),
CallbackContext::new(),
CallbackContext::new(),
];
/// Interrupt mode executor.
///
/// This executor runs tasks in interrupt mode. The interrupt handler is set up
/// to poll tasks, and when a task is woken the interrupt is pended from
/// software.
///
/// # Interrupt requirements
///
/// You must write the interrupt handler yourself, and make it call
/// [`Self::on_interrupt()`]
///
/// ```rust,ignore
/// #[interrupt]
/// fn FROM_CPU_INTR1() {
/// unsafe { INT_EXECUTOR.on_interrupt() }
/// }
/// ```
pub struct InterruptExecutor<SWI>
where
SWI: SwPendableInterrupt,
{
pub struct InterruptExecutor<const SWI: u8> {
core: AtomicUsize,
executor: UnsafeCell<MaybeUninit<raw::Executor>>,
_interrupt: PhantomData<SWI>,
interrupt: SoftwareInterrupt<SWI>,
}
unsafe impl<SWI: SwPendableInterrupt> Send for InterruptExecutor<SWI> {}
unsafe impl<SWI: SwPendableInterrupt> Sync for InterruptExecutor<SWI> {}
unsafe impl<const SWI: u8> Send for InterruptExecutor<SWI> {}
unsafe impl<const SWI: u8> Sync for InterruptExecutor<SWI> {}
impl<SWI> InterruptExecutor<SWI>
where
SWI: SwPendableInterrupt,
{
struct CallbackContext {
raw_executor: UnsafeCell<*mut raw::Executor>,
}
impl CallbackContext {
const fn new() -> Self {
Self {
raw_executor: UnsafeCell::new(core::ptr::null_mut()),
}
}
fn get(&self) -> *mut raw::Executor {
unsafe { (*self.raw_executor.get()) as *mut raw::Executor }
}
fn set(&self, executor: *mut raw::Executor) {
unsafe {
self.raw_executor.get().write(executor);
}
}
}
fn handle_interrupt<const NUM: u8>() {
let mut swi = unsafe { SoftwareInterrupt::<NUM>::steal() };
swi.reset();
unsafe {
let executor = EXECUTORS[NUM as usize].get().as_mut().unwrap();
executor.poll();
}
}
extern "C" fn swi_handler0() {
handle_interrupt::<0>();
}
extern "C" fn swi_handler1() {
handle_interrupt::<1>();
}
extern "C" fn swi_handler2() {
handle_interrupt::<2>();
}
extern "C" fn swi_handler3() {
handle_interrupt::<3>();
}
impl<const SWI: u8> InterruptExecutor<SWI> {
/// Create a new `InterruptExecutor`.
/// This takes the software interrupt to be used internally.
#[inline]
pub const fn new() -> Self {
pub const fn new(interrupt: SoftwareInterrupt<SWI>) -> Self {
Self {
core: AtomicUsize::new(usize::MAX),
executor: UnsafeCell::new(MaybeUninit::uninit()),
_interrupt: PhantomData,
interrupt,
}
}
/// Executor interrupt callback.
///
/// # Safety
///
/// You MUST call this from the interrupt handler, and from nowhere else.
// TODO: it would be pretty sweet if we could register our own interrupt handler
// when vectoring is enabled. The user shouldn't need to provide the handler for
// us.
pub unsafe fn on_interrupt(&'static self) {
SWI::clear();
let executor = unsafe { (*self.executor.get()).assume_init_ref() };
executor.poll();
}
/// Start the executor at the given priority level.
///
/// This initializes the executor, enables the interrupt, and returns.
@ -135,18 +103,7 @@ where
///
/// To obtain a [`Spawner`] for this executor, use
/// [`Spawner::for_current_executor()`] from a task running in it.
///
/// # Interrupt requirements
///
/// You must write the interrupt handler yourself, and make it call
/// [`Self::on_interrupt()`]
///
/// This method already enables (unmasks) the interrupt, you must NOT do it
/// yourself.
///
/// [`Spawner`]: embassy_executor::Spawner
/// [`Spawner::for_current_executor()`]: embassy_executor::Spawner::for_current_executor()
pub fn start(&'static self, priority: interrupt::Priority) -> SendSpawner {
pub fn start(&'static mut self, priority: interrupt::Priority) -> SendSpawner {
if self
.core
.compare_exchange(
@ -163,10 +120,21 @@ where
unsafe {
(*self.executor.get())
.as_mut_ptr()
.write(raw::Executor::new(SWI::number() as *mut ()))
.write(raw::Executor::new(SWI as *mut ()));
EXECUTORS[SWI as usize].set((*self.executor.get()).as_mut_ptr());
}
SWI::enable(priority);
let swi_handler = match SWI {
0 => swi_handler0,
1 => swi_handler1,
2 => swi_handler2,
3 => swi_handler3,
_ => unreachable!(),
};
self.interrupt
.set_interrupt_handler(InterruptHandler::new(swi_handler, priority));
let executor = unsafe { (*self.executor.get()).assume_init_ref() };
executor.spawner().make_send()

View File

@ -16,6 +16,9 @@ pub use interrupt::*;
))]
#[export_name = "__pender"]
fn __pender(context: *mut ()) {
#[cfg(feature = "embassy-executor-interrupt")]
use crate::system::SoftwareInterrupt;
let context = (context as usize).to_le_bytes();
cfg_if::cfg_if! {
@ -23,9 +26,12 @@ fn __pender(context: *mut ()) {
match context[0] {
#[cfg(feature = "embassy-executor-thread")]
0 => thread::pend_thread_mode(context[1] as usize),
1 => FromCpu1::pend(),
2 => FromCpu2::pend(),
3 => FromCpu3::pend(),
#[cfg(not(feature = "embassy-executor-thread"))]
0 => unsafe { SoftwareInterrupt::<0>::steal().raise() },
1 => unsafe { SoftwareInterrupt::<1>::steal().raise() },
2 => unsafe { SoftwareInterrupt::<2>::steal().raise() },
3 => unsafe { SoftwareInterrupt::<3>::steal().raise() },
_ => {}
}
} else {

View File

@ -3,13 +3,12 @@ use core::marker::PhantomData;
use embassy_executor::{raw, Spawner};
use portable_atomic::{AtomicBool, Ordering};
use crate::{get_core, prelude::interrupt};
#[cfg(multi_core)]
use crate::{
interrupt,
peripherals::{self, SYSTEM},
};
use procmacros::handler;
use crate::get_core;
#[cfg(multi_core)]
use crate::peripherals::SYSTEM;
/// global atomic used to keep track of whether there is work to do since sev()
/// is not available on either Xtensa or RISC-V
@ -18,10 +17,9 @@ static SIGNAL_WORK_THREAD_MODE: [AtomicBool; 1] = [AtomicBool::new(false)];
#[cfg(multi_core)]
static SIGNAL_WORK_THREAD_MODE: [AtomicBool; 2] = [AtomicBool::new(false), AtomicBool::new(false)];
#[interrupt]
fn FROM_CPU_INTR0() {
#[cfg(multi_core)]
{
#[handler]
fn software0_interrupt() {
// This interrupt is fired when the thread-mode executor's core needs to be
// woken. It doesn't matter which core handles this interrupt first, the
// point is just to wake up the core that is currently executing
@ -31,7 +29,6 @@ fn FROM_CPU_INTR0() {
.cpu_intr_from_cpu_0()
.write(|w| w.cpu_intr_from_cpu_0().bit(false));
}
}
pub(crate) fn pend_thread_mode(core: usize) {
// Signal that there is work to be done.
@ -60,12 +57,15 @@ pub struct Executor {
impl Executor {
/// Create a new Executor.
///
/// On multi_core systems this will use software-interrupt 0 which isn't
/// available for anything else.
pub fn new() -> Self {
#[cfg(multi_core)]
unwrap!(interrupt::enable(
peripherals::Interrupt::FROM_CPU_INTR0,
interrupt::Priority::Priority1,
));
unsafe {
crate::system::SoftwareInterrupt::<0>::steal()
.set_interrupt_handler(software0_interrupt)
}
Self {
inner: raw::Executor::new(usize::from_le_bytes([0, get_core() as u8, 0, 0]) as *mut ()),
@ -107,6 +107,7 @@ impl Executor {
}
}
#[doc(hidden)]
#[cfg(xtensa)]
pub fn wait_impl(cpu: usize) {
// Manual critical section implementation that only masks interrupts handlers.
@ -138,6 +139,7 @@ impl Executor {
}
}
#[doc(hidden)]
#[cfg(riscv)]
pub fn wait_impl(cpu: usize) {
// we do not care about race conditions between the load and store operations,

View File

@ -46,7 +46,7 @@ use core::{
/// dedicated struct is memory efficiency:
///
/// Peripheral singletons are typically either zero-sized (for concrete
/// peripehrals like `PA9` or `Spi4`) or very small (for example `AnyPin` which
/// peripherals like `PA9` or `Spi4`) or very small (for example `AnyPin` which
/// is 1 byte). However `&mut T` is always 4 bytes for 32-bit targets, even if T
/// is zero-sized. PeripheralRef stores a copy of `T` instead, so it's the same
/// size.

View File

@ -7,12 +7,11 @@
//! configure chip clocks, and control radio peripherals.
//!
//! ### Software Interrupts
//! The `SoftwareInterrupt` enum represents the available software interrupt
//! sources.
//! The `SoftwareInterruptControl` struct gives access to the available software
//! interrupts.
//!
//! The SoftwareInterruptControl struct allows raising or resetting software
//! interrupts using the `raise()` and `reset()` methods. The behavior of these
//! methods depends on the specific chip variant.
//! The `SoftwareInterrupt` struct allows raising or resetting software
//! interrupts using the `raise()` and `reset()` methods.
//!
//! ### Peripheral Clock Control
//! The `PeripheralClockControl` struct controls the enablement of peripheral
@ -28,14 +27,7 @@
//! let clocks = ClockControl::boot_defaults(system.clock_control).freeze();
//! ```
use crate::{peripheral::PeripheralRef, peripherals::SYSTEM};
pub enum SoftwareInterrupt {
SoftwareInterrupt0,
SoftwareInterrupt1,
SoftwareInterrupt2,
SoftwareInterrupt3,
}
use crate::{interrupt::InterruptHandler, peripheral::PeripheralRef, peripherals::SYSTEM};
/// Peripherals which can be enabled via `PeripheralClockControl`
pub enum Peripheral {
@ -107,68 +99,141 @@ pub enum Peripheral {
LcdCam,
}
pub struct SoftwareInterruptControl {
_private: (),
/// A software interrupt can be triggered by software.
#[non_exhaustive]
pub struct SoftwareInterrupt<const NUM: u8> {}
impl<const NUM: u8> SoftwareInterrupt<NUM> {
/// Sets the interrupt handler for this software-interrupt
// TODO interrupts missing in PAC or named wrong for P4
#[cfg(not(esp32p4))]
pub fn set_interrupt_handler(&mut self, handler: InterruptHandler) {
let interrupt = match NUM {
0 => crate::peripherals::Interrupt::FROM_CPU_INTR0,
1 => crate::peripherals::Interrupt::FROM_CPU_INTR1,
2 => crate::peripherals::Interrupt::FROM_CPU_INTR2,
3 => crate::peripherals::Interrupt::FROM_CPU_INTR3,
_ => unreachable!(),
};
unsafe {
crate::interrupt::bind_interrupt(interrupt, handler.handler());
crate::interrupt::enable(interrupt, handler.priority()).unwrap();
}
}
impl SoftwareInterruptControl {
pub fn raise(&mut self, interrupt: SoftwareInterrupt) {
/// Trigger this software-interrupt
pub fn raise(&mut self) {
#[cfg(not(any(esp32c6, esp32h2)))]
let system = unsafe { &*SYSTEM::PTR };
#[cfg(any(esp32c6, esp32h2))]
let system = unsafe { &*crate::peripherals::INTPRI::PTR };
match interrupt {
SoftwareInterrupt::SoftwareInterrupt0 => {
match NUM {
0 => {
system
.cpu_intr_from_cpu_0()
.write(|w| w.cpu_intr_from_cpu_0().set_bit());
}
SoftwareInterrupt::SoftwareInterrupt1 => {
1 => {
system
.cpu_intr_from_cpu_1()
.write(|w| w.cpu_intr_from_cpu_1().set_bit());
}
SoftwareInterrupt::SoftwareInterrupt2 => {
2 => {
system
.cpu_intr_from_cpu_2()
.write(|w| w.cpu_intr_from_cpu_2().set_bit());
}
SoftwareInterrupt::SoftwareInterrupt3 => {
3 => {
system
.cpu_intr_from_cpu_3()
.write(|w| w.cpu_intr_from_cpu_3().set_bit());
}
_ => unreachable!(),
}
}
pub fn reset(&mut self, interrupt: SoftwareInterrupt) {
/// Resets this software-interrupt
pub fn reset(&mut self) {
#[cfg(not(any(esp32c6, esp32h2)))]
let system = unsafe { &*SYSTEM::PTR };
#[cfg(any(esp32c6, esp32h2))]
let system = unsafe { &*crate::peripherals::INTPRI::PTR };
match interrupt {
SoftwareInterrupt::SoftwareInterrupt0 => {
match NUM {
0 => {
system
.cpu_intr_from_cpu_0()
.write(|w| w.cpu_intr_from_cpu_0().clear_bit());
}
SoftwareInterrupt::SoftwareInterrupt1 => {
1 => {
system
.cpu_intr_from_cpu_1()
.write(|w| w.cpu_intr_from_cpu_1().clear_bit());
}
SoftwareInterrupt::SoftwareInterrupt2 => {
2 => {
system
.cpu_intr_from_cpu_2()
.write(|w| w.cpu_intr_from_cpu_2().clear_bit());
}
SoftwareInterrupt::SoftwareInterrupt3 => {
3 => {
system
.cpu_intr_from_cpu_3()
.write(|w| w.cpu_intr_from_cpu_3().clear_bit());
}
_ => unreachable!(),
}
}
/// Unsafely create an instance of this peripheral out of thin air.
///
/// # Safety
///
/// You must ensure that you're only using one instance of this type at a
/// time.
#[inline]
pub unsafe fn steal() -> Self {
Self {}
}
}
impl<const NUM: u8> crate::peripheral::Peripheral for SoftwareInterrupt<NUM> {
type P = SoftwareInterrupt<NUM>;
#[inline]
unsafe fn clone_unchecked(&mut self) -> Self::P {
Self::steal()
}
}
impl<const NUM: u8> crate::private::Sealed for SoftwareInterrupt<NUM> {}
/// This gives access to the available software interrupts.
///
/// Please note: Software interrupt 0 is not available when using the
/// `embassy-executor-thread` feature
#[non_exhaustive]
pub struct SoftwareInterruptControl {
#[cfg(not(all(feature = "embassy-executor-thread", multi_core)))]
pub software_interrupt0: SoftwareInterrupt<0>,
pub software_interrupt1: SoftwareInterrupt<1>,
pub software_interrupt2: SoftwareInterrupt<2>,
pub software_interrupt3: SoftwareInterrupt<3>,
}
impl SoftwareInterruptControl {
fn new_internal() -> Self {
// the thread-executor uses SW-INT0 when used on a multi-core system
// we cannot easily require `software_interrupt0` there since it's created
// before `main` via proc-macro
SoftwareInterruptControl {
#[cfg(not(all(feature = "embassy-executor-thread", multi_core)))]
software_interrupt0: SoftwareInterrupt {},
software_interrupt1: SoftwareInterrupt {},
software_interrupt2: SoftwareInterrupt {},
software_interrupt3: SoftwareInterrupt {},
}
}
}
@ -1025,6 +1090,7 @@ pub struct CpuControl {
_private: (),
}
/// Enumeration of the available radio peripherals for this chip.
pub enum RadioPeripherals {
#[cfg(phy)]
Phy,
@ -1036,6 +1102,7 @@ pub enum RadioPeripherals {
Ieee802154,
}
/// Functionality of clocks controlling the radio peripherals.
pub struct RadioClockControl {
_private: (),
}
@ -1087,7 +1154,7 @@ impl<'d, T: crate::peripheral::Peripheral<P = SYSTEM> + 'd> SystemExt<'d> for T
clock_control: SystemClockControl { _private: () },
cpu_control: CpuControl { _private: () },
radio_clock_control: RadioClockControl { _private: () },
software_interrupt_control: SoftwareInterruptControl { _private: () },
software_interrupt_control: SoftwareInterruptControl::new_internal(),
}
}
}

View File

@ -11,10 +11,10 @@ use esp_hal::{
interrupt::{self, CpuInterrupt, Priority},
peripherals::{Interrupt, Peripherals},
prelude::*,
system::{SoftwareInterrupt, SoftwareInterruptControl},
system::SoftwareInterrupt,
};
static SWINT: Mutex<RefCell<Option<SoftwareInterruptControl>>> = Mutex::new(RefCell::new(None));
static SWINT0: Mutex<RefCell<Option<SoftwareInterrupt<0>>>> = Mutex::new(RefCell::new(None));
#[entry]
fn main() -> ! {
@ -31,7 +31,11 @@ fn main() -> ! {
let system = peripherals.SYSTEM.split();
let sw_int = system.software_interrupt_control;
critical_section::with(|cs| SWINT.borrow_ref_mut(cs).replace(sw_int));
critical_section::with(|cs| {
SWINT0
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt0)
});
interrupt::enable_direct(
Interrupt::FROM_CPU_INTR0,
Priority::Priority3,
@ -67,14 +71,10 @@ fn main() -> ! {
}
#[no_mangle]
fn cpu_int_20_handler() {
fn interrupt20() {
unsafe { asm!("csrrwi x0, 0x7e1, 0 #disable timer") }
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt0);
SWINT0.borrow_ref_mut(cs).as_mut().unwrap().reset();
});
let mut perf_counter: u32 = 0;

View File

@ -18,10 +18,7 @@ use esp_backtrace as _;
use esp_hal::{
clock::ClockControl,
cpu_control::{CpuControl, Stack},
embassy::{
self,
executor::{FromCpu1, FromCpu2, InterruptExecutor},
},
embassy::{self, executor::InterruptExecutor},
get_core,
gpio::{GpioPin, Output, PushPull, IO},
interrupt::Priority,
@ -34,19 +31,6 @@ use static_cell::make_static;
static mut APP_CORE_STACK: Stack<8192> = Stack::new();
static INT_EXECUTOR_CORE_0: InterruptExecutor<FromCpu1> = InterruptExecutor::new();
static INT_EXECUTOR_CORE_1: InterruptExecutor<FromCpu2> = InterruptExecutor::new();
#[interrupt]
fn FROM_CPU_INTR1() {
unsafe { INT_EXECUTOR_CORE_0.on_interrupt() }
}
#[interrupt]
fn FROM_CPU_INTR2() {
unsafe { INT_EXECUTOR_CORE_1.on_interrupt() }
}
/// Waits for a message that contains a duration, then flashes a led for that
/// duration of time.
#[embassy_executor::task]
@ -101,8 +85,13 @@ fn main() -> ! {
let led_ctrl_signal = &*make_static!(Signal::new());
let led = io.pins.gpio0.into_push_pull_output();
let executor_core1 =
InterruptExecutor::new(system.software_interrupt_control.software_interrupt1);
let executor_core1 = make_static!(executor_core1);
let cpu1_fnctn = move || {
let spawner = INT_EXECUTOR_CORE_1.start(Priority::Priority1);
let spawner = executor_core1.start(Priority::Priority1);
spawner.spawn(control_led(led, led_ctrl_signal)).ok();
@ -113,7 +102,11 @@ fn main() -> ! {
.start_app_core(unsafe { &mut *addr_of_mut!(APP_CORE_STACK) }, cpu1_fnctn)
.unwrap();
let spawner = INT_EXECUTOR_CORE_0.start(Priority::Priority1);
let executor_core0 =
InterruptExecutor::new(system.software_interrupt_control.software_interrupt0);
let executor_core0 = make_static!(executor_core0);
let spawner = executor_core0.start(Priority::Priority1);
spawner.spawn(enable_disable_led(led_ctrl_signal)).ok();
// Just loop to show that the main thread does not need to poll the executor.

View File

@ -11,8 +11,11 @@
//! demonstrates that this task will continue to run even while the low
//! priority blocking task is running.
// HINT: At first it looks a bit suspicious that we need *two* executor features enabled here but that's because we are really using
// both together. The thread-executor is created by the `#[main]` macro and is used to spawn `low_prio_async` and `low_prio_blocking`.
// The interrupt-executor is created in `main` and is used to spawn `high_prio`.
//% CHIPS: esp32 esp32c2 esp32c3 esp32c6 esp32h2 esp32s2 esp32s3
// FIXME: We should not need *two* executor features enabled here...
//% FEATURES: embassy embassy-executor-interrupt embassy-executor-thread embassy-time-timg0 embassy-generic-timers
#![no_std]
@ -24,23 +27,14 @@ use embassy_time::{Duration, Instant, Ticker, Timer};
use esp_backtrace as _;
use esp_hal::{
clock::ClockControl,
embassy::{
self,
executor::{FromCpu1, InterruptExecutor},
},
embassy::{self, executor::InterruptExecutor},
interrupt::Priority,
peripherals::Peripherals,
prelude::*,
timer::TimerGroup,
};
use esp_println::println;
static INT_EXECUTOR_0: InterruptExecutor<FromCpu1> = InterruptExecutor::new();
#[interrupt]
fn FROM_CPU_INTR1() {
unsafe { INT_EXECUTOR_0.on_interrupt() }
}
use static_cell::make_static;
/// Periodically print something.
#[embassy_executor::task]
@ -79,6 +73,7 @@ async fn low_prio_async() {
#[main]
async fn main(low_prio_spawner: Spawner) {
esp_println::logger::init_logger_from_env();
println!("Init!");
let peripherals = Peripherals::take();
let system = peripherals.SYSTEM.split();
@ -87,7 +82,10 @@ async fn main(low_prio_spawner: Spawner) {
let timg0 = TimerGroup::new_async(peripherals.TIMG0, &clocks);
embassy::init(&clocks, timg0);
let spawner = INT_EXECUTOR_0.start(Priority::Priority2);
let executor = InterruptExecutor::new(system.software_interrupt_control.software_interrupt2);
let executor = make_static!(executor);
let spawner = executor.start(Priority::Priority3);
spawner.must_spawn(high_prio());
println!("Spawning low-priority tasks");

View File

@ -13,27 +13,48 @@ use core::cell::RefCell;
use critical_section::Mutex;
use esp_backtrace as _;
use esp_hal::{
interrupt::{self, Priority},
peripherals::{Interrupt, Peripherals},
prelude::*,
system::{SoftwareInterrupt, SoftwareInterruptControl},
};
use esp_hal::{peripherals::Peripherals, prelude::*, system::SoftwareInterrupt};
static SWINT: Mutex<RefCell<Option<SoftwareInterruptControl>>> = Mutex::new(RefCell::new(None));
static SWINT0: Mutex<RefCell<Option<SoftwareInterrupt<0>>>> = Mutex::new(RefCell::new(None));
static SWINT1: Mutex<RefCell<Option<SoftwareInterrupt<1>>>> = Mutex::new(RefCell::new(None));
static SWINT2: Mutex<RefCell<Option<SoftwareInterrupt<2>>>> = Mutex::new(RefCell::new(None));
static SWINT3: Mutex<RefCell<Option<SoftwareInterrupt<3>>>> = Mutex::new(RefCell::new(None));
#[entry]
fn main() -> ! {
let peripherals = Peripherals::take();
let system = peripherals.SYSTEM.split();
let sw_int = system.software_interrupt_control;
let mut sw_int = system.software_interrupt_control;
critical_section::with(|cs| SWINT.borrow_ref_mut(cs).replace(sw_int));
critical_section::with(|cs| {
sw_int
.software_interrupt0
.set_interrupt_handler(swint0_handler);
SWINT0
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt0);
interrupt::enable(Interrupt::FROM_CPU_INTR0, Priority::Priority1).unwrap();
interrupt::enable(Interrupt::FROM_CPU_INTR1, Priority::Priority2).unwrap();
interrupt::enable(Interrupt::FROM_CPU_INTR2, Priority::Priority2).unwrap();
interrupt::enable(Interrupt::FROM_CPU_INTR3, Priority::Priority3).unwrap();
sw_int
.software_interrupt1
.set_interrupt_handler(swint1_handler);
SWINT1
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt1);
sw_int
.software_interrupt2
.set_interrupt_handler(swint2_handler);
SWINT2
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt2);
sw_int
.software_interrupt3
.set_interrupt_handler(swint3_handler);
SWINT3
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt3);
});
// Raise mid priority interrupt.
//
@ -42,77 +63,44 @@ fn main() -> ! {
// exiting the handler Once the handler is exited we expect to see same
// priority and low priority interrupts served in that order.
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.raise(SoftwareInterrupt::SoftwareInterrupt1);
SWINT1.borrow_ref_mut(cs).as_mut().unwrap().raise();
});
loop {}
}
#[interrupt]
fn FROM_CPU_INTR0() {
#[handler(priority = esp_hal::interrupt::Priority::Priority1)]
fn swint0_handler() {
esp_println::println!("SW interrupt0");
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt0);
SWINT0.borrow_ref_mut(cs).as_mut().unwrap().reset();
});
}
#[interrupt]
fn FROM_CPU_INTR1() {
#[handler(priority = esp_hal::interrupt::Priority::Priority2)]
fn swint1_handler() {
esp_println::println!("SW interrupt1 entry");
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt1);
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.raise(SoftwareInterrupt::SoftwareInterrupt2); // raise interrupt at same priority
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.raise(SoftwareInterrupt::SoftwareInterrupt3); // raise interrupt at higher priority
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.raise(SoftwareInterrupt::SoftwareInterrupt0); // raise interrupt at
// lower priority
SWINT1.borrow_ref_mut(cs).as_mut().unwrap().reset();
SWINT2.borrow_ref_mut(cs).as_mut().unwrap().raise(); // raise interrupt at same priority
SWINT3.borrow_ref_mut(cs).as_mut().unwrap().raise(); // raise interrupt at higher priority
SWINT0.borrow_ref_mut(cs).as_mut().unwrap().raise(); // raise interrupt at lower priority
});
esp_println::println!("SW interrupt1 exit");
}
#[interrupt]
fn FROM_CPU_INTR2() {
#[handler(priority = esp_hal::interrupt::Priority::Priority2)]
fn swint2_handler() {
esp_println::println!("SW interrupt2");
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt2);
SWINT2.borrow_ref_mut(cs).as_mut().unwrap().reset();
});
}
#[interrupt]
fn FROM_CPU_INTR3() {
#[handler(priority = esp_hal::interrupt::Priority::Priority3)]
fn swint3_handler() {
esp_println::println!("SW interrupt3");
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt3);
SWINT3.borrow_ref_mut(cs).as_mut().unwrap().reset();
});
}

View File

@ -16,13 +16,15 @@ use esp_backtrace as _;
use esp_hal::{
clock::ClockControl,
delay::Delay,
interrupt::{self, Priority},
peripherals::{Interrupt, Peripherals},
peripherals::Peripherals,
prelude::*,
system::{SoftwareInterrupt, SoftwareInterruptControl},
system::SoftwareInterrupt,
};
static SWINT: Mutex<RefCell<Option<SoftwareInterruptControl>>> = Mutex::new(RefCell::new(None));
static SWINT0: Mutex<RefCell<Option<SoftwareInterrupt<0>>>> = Mutex::new(RefCell::new(None));
static SWINT1: Mutex<RefCell<Option<SoftwareInterrupt<1>>>> = Mutex::new(RefCell::new(None));
static SWINT2: Mutex<RefCell<Option<SoftwareInterrupt<2>>>> = Mutex::new(RefCell::new(None));
static SWINT3: Mutex<RefCell<Option<SoftwareInterrupt<3>>>> = Mutex::new(RefCell::new(None));
#[entry]
fn main() -> ! {
@ -30,13 +32,36 @@ fn main() -> ! {
let system = peripherals.SYSTEM.split();
let clocks = ClockControl::boot_defaults(system.clock_control).freeze();
let sw_int = system.software_interrupt_control;
critical_section::with(|cs| SWINT.borrow_ref_mut(cs).replace(sw_int));
let mut sw_int = system.software_interrupt_control;
critical_section::with(|cs| {
sw_int
.software_interrupt0
.set_interrupt_handler(swint0_handler);
SWINT0
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt0);
interrupt::enable(Interrupt::FROM_CPU_INTR0, Priority::Priority3).unwrap();
interrupt::enable(Interrupt::FROM_CPU_INTR1, Priority::Priority3).unwrap();
interrupt::enable(Interrupt::FROM_CPU_INTR2, Priority::Priority3).unwrap();
interrupt::enable(Interrupt::FROM_CPU_INTR3, Priority::Priority3).unwrap();
sw_int
.software_interrupt1
.set_interrupt_handler(swint1_handler);
SWINT1
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt1);
sw_int
.software_interrupt2
.set_interrupt_handler(swint2_handler);
SWINT2
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt2);
sw_int
.software_interrupt3
.set_interrupt_handler(swint3_handler);
SWINT3
.borrow_ref_mut(cs)
.replace(sw_int.software_interrupt3);
});
let delay = Delay::new(&clocks);
let mut counter = 0;
@ -45,33 +70,17 @@ fn main() -> ! {
delay.delay_millis(500);
match counter {
0 => critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.raise(SoftwareInterrupt::SoftwareInterrupt0);
SWINT0.borrow_ref_mut(cs).as_mut().unwrap().raise();
}),
1 => critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.raise(SoftwareInterrupt::SoftwareInterrupt1);
SWINT1.borrow_ref_mut(cs).as_mut().unwrap().raise();
}),
2 => critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.raise(SoftwareInterrupt::SoftwareInterrupt2);
SWINT2.borrow_ref_mut(cs).as_mut().unwrap().raise();
}),
3 => {
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.raise(SoftwareInterrupt::SoftwareInterrupt3);
SWINT3.borrow_ref_mut(cs).as_mut().unwrap().raise();
});
counter = -1
}
@ -81,50 +90,34 @@ fn main() -> ! {
}
}
#[interrupt]
fn FROM_CPU_INTR0() {
#[handler]
fn swint0_handler() {
esp_println::println!("SW interrupt0");
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt0);
SWINT0.borrow_ref_mut(cs).as_mut().unwrap().reset();
});
}
#[interrupt]
fn FROM_CPU_INTR1() {
#[handler]
fn swint1_handler() {
esp_println::println!("SW interrupt1");
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt1);
SWINT1.borrow_ref_mut(cs).as_mut().unwrap().reset();
});
}
#[interrupt]
fn FROM_CPU_INTR2() {
#[handler]
fn swint2_handler() {
esp_println::println!("SW interrupt2");
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt2);
SWINT2.borrow_ref_mut(cs).as_mut().unwrap().reset();
});
}
#[interrupt]
fn FROM_CPU_INTR3() {
#[handler]
fn swint3_handler() {
esp_println::println!("SW interrupt3");
critical_section::with(|cs| {
SWINT
.borrow_ref_mut(cs)
.as_mut()
.unwrap()
.reset(SoftwareInterrupt::SoftwareInterrupt3);
SWINT3.borrow_ref_mut(cs).as_mut().unwrap().reset();
});
}