diff --git a/Notes.md b/Notes.md new file mode 100644 index 0000000000..522dca2665 --- /dev/null +++ b/Notes.md @@ -0,0 +1,188 @@ +# Notes for lock optimizaiton + +## Idea + +We are reading basepri independently if and only if we are actually changing basepri. + +To dump generated assembly: + +``` shell +> arm-none-eabi-objdump target/thumbv7m-none-eabi/release/examples/lockopt -d > lockopt.asm +``` + +Extend `cortex-m-rtfm/src/export::Priority` with an additional fields to store `init_logic` (priority of the task) and `old_basepri_hw`. The latter field is initially `None` on creation. + +``` Rust +// Newtype over `Cell` that forbids mutation through a shared reference +pub struct Priority { + init_logic: u8, + current_logic: Cell, + #[cfg(armv7m)] + old_basepri_hw: Cell>, +} + +impl Priority { + #[inline(always)] + pub unsafe fn new(value: u8) -> Self { + Priority { + init_logic: value, + current_logic: Cell::new(value), + old_basepri_hw: Cell::new(None), + } + } + + #[inline(always)] + fn set_logic(&self, value: u8) { + self.current_logic.set(value) + } + + #[inline(always)] + fn get_logic(&self) -> u8 { + self.current_logic.get() + } + + #[inline(always)] + fn get_init_logic(&self) -> u8 { + self.init_logic + } + + #[cfg(armv7m)] + #[inline(always)] + fn get_old_basepri_hw(&self) -> Option { + self.old_basepri_hw.get() + } + + #[cfg(armv7m)] + #[inline(always)] + fn set_old_basepri_hw(&self, value: u8) { + self.old_basepri_hw.set(Some(value)); + } +} +``` + +The corresponding `lock` is implemented as follows: + +``` Rust +#[cfg(armv7m)] +#[inline(always)] +pub unsafe fn lock( + ptr: *mut T, + priority: &Priority, + ceiling: u8, + nvic_prio_bits: u8, + f: impl FnOnce(&mut T) -> R, +) -> R { + let current = priority.get_logic(); + + if current < ceiling { + if ceiling == (1 << nvic_prio_bits) { + priority.set_logic(u8::max_value()); + let r = interrupt::free(|_| f(&mut *ptr)); + priority.set_logic(current); + r + } else { + match priority.get_old_basepri_hw() { + None => priority.set_old_basepri_hw(basepri::read()), + _ => (), + }; + priority.set_logic(ceiling); + basepri::write(logical2hw(ceiling, nvic_prio_bits)); + let r = f(&mut *ptr); + if current == priority.get_init_logic() { + basepri::write(priority.get_old_basepri_hw().unwrap()); + } else { + basepri::write(logical2hw(priority.get_logic(), nvic_prio_bits)); + } + priority.set_logic(current); + r + } + } else { + f(&mut *ptr) + } +} +``` + +The highest priority is achieved through an `interrupt_free` and does not at all affect the `BASEPRI`. + +For the normal case, on enter we check if the BASEPRI register has been read, if not we read it and update `priority`. On exit we check if are to restore a logical priority (inside a nested lock) or to restore the BASEPRI (previously read). + +## Safety + +We can safely `unwrap` the `get_old_basepri_hw: Option` as the path leading up to the `unwrap` passes an update to `Some` or was already `Some`. Updating `get_old_basepri_hw` is monotonic, the API offers no way of making `get_old_basepri_hw` into `None` (besides `new`). + +Moreover `new` is the only public function of `Priority`, thus we are exposing nothing dangerous to the user. + +## Implementation + +Implementation mainly regards two files, the `rtfm/src/export.rs` (discussed above) and `macros/src/codegen/hardware_tasks.rs`. For the latter the task dispatcher is updated as follows: + +``` Rust + ... + const_app.push(quote!( + #[allow(non_snake_case)] + #[no_mangle] + #section + #cfg_core + unsafe fn #symbol() { + const PRIORITY: u8 = #priority; + #let_instant + crate::#name( + #locals_new + #name::Context::new(&rtfm::export::Priority::new(PRIORITY) #instant) + ); + } + )); + ... +``` + +Basically we create `Priority` (on stack) and use that to create a `Context`. The beauty is that LLVM is completely optimazing out the data structure (and related code), but taking into account its implications to control flow. Thus, the locks AND initial reading of BASEPRI will be optimized at compile time at Zero cost. + +Overall, using this approach, we don't need a trampoline (`run`). We reduce the overhead by at least two machine instructions (additional reading/writing of BASEPRI) for each interrupt. It also reduces the register preasure (as less information needs to be stored). + + +## Evaluation + +The `examples/lockopt.rs` shows that locks are effectively optimized out. + +``` asm +00000132 : + 132: b510 push {r4, lr} + 134: f000 f893 bl 25e <__basepri_r> + 138: 4604 mov r4, r0 + 13a: 20a0 movs r0, #160 ; 0xa0 + 13c: f000 f892 bl 264 <__basepri_w> + 140: f240 0000 movw r0, #0 + 144: f2c2 0000 movt r0, #8192 ; 0x2000 + 148: 6801 ldr r1, [r0, #0] + 14a: 3101 adds r1, #1 + 14c: 6001 str r1, [r0, #0] + 14e: 4620 mov r0, r4 + 150: e8bd 4010 ldmia.w sp!, {r4, lr} + 154: f000 b886 b.w 264 <__basepri_w> + +00000158 : + 158: f240 0000 movw r0, #0 + 15c: f2c2 0000 movt r0, #8192 ; 0x2000 + 160: 6801 ldr r1, [r0, #0] + 162: 3102 adds r1, #2 + 164: 6001 str r1, [r0, #0] + 166: 4770 bx lr +``` + +GPIOB/C are sharing a resource (C higher prio). Notice, there is no BASEPRI manipulation at all. + +For GPIOB, there is a single read of BASEPRI (stored in `old_basepri_hw`) and just two writes, one for entering critical section, one for exiting. On exit we detect that we are indeed at the initial priority for the task, thus we restore the `old_basepri_hw` instead of a logic priority. + +## Limitations and Drawbacks + +None spotted so far. + +## Observations + +``` shell +> llvm-objdump target/thumbv7m-none-eabi/release/examples/lockopt -d > lockopt.asm + +> cargo objdump --example lockopt --release -- -d > lockopt.asm +``` + +Neither give assembly dump with symbols (very annoying to rely on `arm-none-eabi-objdump` for proper objdumps), maybe just an option is missing? diff --git a/examples/lockopt.rs b/examples/lockopt.rs new file mode 100644 index 0000000000..d01186ad0e --- /dev/null +++ b/examples/lockopt.rs @@ -0,0 +1,67 @@ +//! examples/optlock.rs + +#![deny(unsafe_code)] +#![deny(warnings)] +#![no_main] +#![no_std] + +use cortex_m_semihosting::debug; +use lm3s6965::Interrupt; +use panic_semihosting as _; +use rtfm::Exclusive; + +#[rtfm::app(device = lm3s6965)] +const APP: () = { + struct Resources { + #[init(0)] + shared: u32, + } + + #[init] + fn init(_: init::Context) { + rtfm::pend(Interrupt::GPIOA); + } + + // when omitted priority is assumed to be `1` + #[task(binds = GPIOA, resources = [shared])] + fn gpioa(mut c: gpioa::Context) { + // the lower priority task requires a critical section to access the data + c.resources.shared.lock(|shared| { + // data can only be modified within this critical section (closure) + *shared += 1; + + // GPIOB will *not* run right now due to the critical section + rtfm::pend(Interrupt::GPIOB); + + // hprintln!("B - shared = {}", *shared).unwrap(); + + // GPIOC does not contend for `shared` so it's allowed to run now + rtfm::pend(Interrupt::GPIOC); + }); + + // critical section is over: GPIOB can now start + + debug::exit(debug::EXIT_SUCCESS); + } + + #[task(binds = GPIOB, priority = 2, resources = [shared])] + fn gpiob(mut c: gpiob::Context) { + // higher priority task, with critical section + c.resources.shared.lock(|shared| { + *shared += 1; + }); + } + + #[task(binds = GPIOC, priority = 3, resources = [shared])] + fn gpioc(c: gpioc::Context) { + // highest priority task with critical section + // we wrap resource.shared into an Exclusive + let mut exclusive = Exclusive(c.resources.shared); + // we can access through both lock ... + exclusive.lock(|shared| { + *shared += 1; + }); + // and deref, i.e., non-orthogonal design + *exclusive += 1; + } +}; diff --git a/lockopt.asm b/lockopt.asm new file mode 100644 index 0000000000..dadbd91079 --- /dev/null +++ b/lockopt.asm @@ -0,0 +1,192 @@ + +target/thumbv7m-none-eabi/release/examples/lockopt: file format elf32-littlearm + + +Disassembly of section .text: + +000000f0 : + f0: b510 push {r4, lr} + f2: f000 f8b4 bl 25e <__basepri_r> + f6: 4604 mov r4, r0 + f8: 20a0 movs r0, #160 ; 0xa0 + fa: f000 f8b3 bl 264 <__basepri_w> + fe: f240 0000 movw r0, #0 + 102: f2c2 0000 movt r0, #8192 ; 0x2000 + 106: 6801 ldr r1, [r0, #0] + 108: 3101 adds r1, #1 + 10a: 6001 str r1, [r0, #0] + 10c: f24e 2000 movw r0, #57856 ; 0xe200 + 110: 2102 movs r1, #2 + 112: f2ce 0000 movt r0, #57344 ; 0xe000 + 116: 6001 str r1, [r0, #0] + 118: 2104 movs r1, #4 + 11a: 6001 str r1, [r0, #0] + 11c: 4620 mov r0, r4 + 11e: f000 f8a1 bl 264 <__basepri_w> + 122: 2126 movs r1, #38 ; 0x26 + 124: 2018 movs r0, #24 + 126: f2c0 0102 movt r1, #2 + 12a: e8bd 4010 ldmia.w sp!, {r4, lr} + 12e: f000 b88e b.w 24e <__syscall> + +00000132 : + 132: b510 push {r4, lr} + 134: f000 f893 bl 25e <__basepri_r> + 138: 4604 mov r4, r0 + 13a: 20a0 movs r0, #160 ; 0xa0 + 13c: f000 f892 bl 264 <__basepri_w> + 140: f240 0000 movw r0, #0 + 144: f2c2 0000 movt r0, #8192 ; 0x2000 + 148: 6801 ldr r1, [r0, #0] + 14a: 3101 adds r1, #1 + 14c: 6001 str r1, [r0, #0] + 14e: 4620 mov r0, r4 + 150: e8bd 4010 ldmia.w sp!, {r4, lr} + 154: f000 b886 b.w 264 <__basepri_w> + +00000158 : + 158: f240 0000 movw r0, #0 + 15c: f2c2 0000 movt r0, #8192 ; 0x2000 + 160: 6801 ldr r1, [r0, #0] + 162: 3102 adds r1, #2 + 164: 6001 str r1, [r0, #0] + 166: 4770 bx lr + +00000168
: + 168: f000 f873 bl 252 <__cpsid> + 16c: f24e 1000 movw r0, #57600 ; 0xe100 + 170: f24e 4201 movw r2, #58369 ; 0xe401 + 174: f2ce 0000 movt r0, #57344 ; 0xe000 + 178: 21e0 movs r1, #224 ; 0xe0 + 17a: f880 1300 strb.w r1, [r0, #768] ; 0x300 + 17e: 2101 movs r1, #1 + 180: f2ce 0200 movt r2, #57344 ; 0xe000 + 184: 23c0 movs r3, #192 ; 0xc0 + 186: 6001 str r1, [r0, #0] + 188: 7013 strb r3, [r2, #0] + 18a: 2302 movs r3, #2 + 18c: 6003 str r3, [r0, #0] + 18e: 23a0 movs r3, #160 ; 0xa0 + 190: 7053 strb r3, [r2, #1] + 192: 2204 movs r2, #4 + 194: 6002 str r2, [r0, #0] + 196: f64e 5210 movw r2, #60688 ; 0xed10 + 19a: f2ce 0200 movt r2, #57344 ; 0xe000 + 19e: 6813 ldr r3, [r2, #0] + 1a0: f043 0302 orr.w r3, r3, #2 + 1a4: 6013 str r3, [r2, #0] + 1a6: f8c0 1100 str.w r1, [r0, #256] ; 0x100 + 1aa: f000 f854 bl 256 <__cpsie> + 1ae: f000 f854 bl 25a <__wfi> + 1b2: e7fc b.n 1ae + +000001b4 : + 1b4: f000 f84a bl 24c + 1b8: f240 0004 movw r0, #4 + 1bc: f240 0100 movw r1, #0 + 1c0: f2c2 0000 movt r0, #8192 ; 0x2000 + 1c4: f2c2 0100 movt r1, #8192 ; 0x2000 + 1c8: 4281 cmp r1, r0 + 1ca: d214 bcs.n 1f6 + 1cc: f240 0100 movw r1, #0 + 1d0: 2200 movs r2, #0 + 1d2: f2c2 0100 movt r1, #8192 ; 0x2000 + 1d6: f841 2b04 str.w r2, [r1], #4 + 1da: 4281 cmp r1, r0 + 1dc: bf3c itt cc + 1de: f841 2b04 strcc.w r2, [r1], #4 + 1e2: 4281 cmpcc r1, r0 + 1e4: d207 bcs.n 1f6 + 1e6: f841 2b04 str.w r2, [r1], #4 + 1ea: 4281 cmp r1, r0 + 1ec: d203 bcs.n 1f6 + 1ee: f841 2b04 str.w r2, [r1], #4 + 1f2: 4281 cmp r1, r0 + 1f4: d3ef bcc.n 1d6 + 1f6: f240 0000 movw r0, #0 + 1fa: f240 0100 movw r1, #0 + 1fe: f2c2 0000 movt r0, #8192 ; 0x2000 + 202: f2c2 0100 movt r1, #8192 ; 0x2000 + 206: 4281 cmp r1, r0 + 208: d21c bcs.n 244 + 20a: f240 2180 movw r1, #640 ; 0x280 + 20e: f240 0200 movw r2, #0 + 212: f2c0 0100 movt r1, #0 + 216: f2c2 0200 movt r2, #8192 ; 0x2000 + 21a: 680b ldr r3, [r1, #0] + 21c: f842 3b04 str.w r3, [r2], #4 + 220: 4282 cmp r2, r0 + 222: d20f bcs.n 244 + 224: 684b ldr r3, [r1, #4] + 226: f842 3b04 str.w r3, [r2], #4 + 22a: 4282 cmp r2, r0 + 22c: bf3e ittt cc + 22e: 688b ldrcc r3, [r1, #8] + 230: f842 3b04 strcc.w r3, [r2], #4 + 234: 4282 cmpcc r2, r0 + 236: d205 bcs.n 244 + 238: 68cb ldr r3, [r1, #12] + 23a: 3110 adds r1, #16 + 23c: f842 3b04 str.w r3, [r2], #4 + 240: 4282 cmp r2, r0 + 242: d3ea bcc.n 21a + 244: f7ff ff90 bl 168
+ 248: defe udf #254 ; 0xfe + +0000024a : + 24a: Address 0x000000000000024a is out of bounds. + + +0000024b : + 24b: Address 0x000000000000024b is out of bounds. + + +0000024c : + 24c: Address 0x000000000000024c is out of bounds. + + +0000024d <__pre_init>: + 24d: Address 0x000000000000024d is out of bounds. + + +0000024e <__syscall>: + 24e: beab bkpt 0x00ab + 250: 4770 bx lr + +00000252 <__cpsid>: + 252: b672 cpsid i + 254: 4770 bx lr + +00000256 <__cpsie>: + 256: b662 cpsie i + 258: 4770 bx lr + +0000025a <__wfi>: + 25a: bf30 wfi + 25c: 4770 bx lr + +0000025e <__basepri_r>: + 25e: f3ef 8011 mrs r0, BASEPRI + 262: 4770 bx lr + +00000264 <__basepri_w>: + 264: f380 8811 msr BASEPRI, r0 + 268: 4770 bx lr + +0000026a : + 26a: 4670 mov r0, lr + 26c: 2104 movs r1, #4 + 26e: 4208 tst r0, r1 + 270: d102 bne.n 278 + 272: f3ef 8008 mrs r0, MSP + 276: e002 b.n 27e + 278: f3ef 8009 mrs r0, PSP + 27c: e7ff b.n 27e + +0000027e : + 27e: Address 0x000000000000027e is out of bounds. + + +0000027f : + 27f: Address 0x000000000000027f is out of bounds. + diff --git a/lockopt.expand b/lockopt.expand new file mode 100644 index 0000000000..e74a5b97d3 --- /dev/null +++ b/lockopt.expand @@ -0,0 +1,269 @@ +#![feature(prelude_import)] +//! examples/optlock.rs + +#![deny(unsafe_code)] +#![deny(warnings)] +#![no_main] +#![no_std] +#[prelude_import] +use core::prelude::v1::*; +#[macro_use] +extern crate core; +#[macro_use] +extern crate compiler_builtins; + +use cortex_m_semihosting::debug; +use lm3s6965::Interrupt; +use panic_semihosting as _; +use rtfm::Exclusive; + +#[allow(non_snake_case)] +fn init(_: init::Context) { + rtfm::pend(Interrupt::GPIOA); +} +#[allow(non_snake_case)] +fn gpioa( + // when omitted priority is assumed to be `1` + mut c: gpioa::Context, +) { + use rtfm::Mutex as _; + // the lower priority task requires a critical section to access the data + c.resources.shared.lock( + // data can only be modified within this critical section (closure) + + // GPIOB will *not* run right now due to the critical section + + // hprintln!("B - shared = {}", *shared).unwrap(); + + // GPIOC does not contend for `shared` so it's allowed to run now + |shared| { + *shared += 1; + rtfm::pend(Interrupt::GPIOB); + rtfm::pend(Interrupt::GPIOC); + }, + ); + + // critical section is over: GPIOB can now start + + debug::exit(debug::EXIT_SUCCESS); +} +#[allow(non_snake_case)] +fn gpiob(mut c: gpiob::Context) { + use rtfm::Mutex as _; + // higher priority task, with critical section + c.resources.shared.lock(|shared| { + *shared += 1; + }); +} +#[allow(non_snake_case)] +fn gpioc(c: gpioc::Context) { + use rtfm::Mutex as _; + // highest priority task with critical section + // we wrap resource.shared into an Exclusive + let mut exclusive = Exclusive(c.resources.shared); + // we can access through both lock ... + exclusive.lock(|shared| { + *shared += 1; + }); + // and deref, i.e., non-orthogonal design + *exclusive += 1; +} +#[allow(non_snake_case)] +#[doc = "Initialization function"] +pub mod init { + #[doc = r" Execution context"] + pub struct Context { + #[doc = r" Core (Cortex-M) peripherals"] + pub core: rtfm::export::Peripherals, + } + impl Context { + #[inline(always)] + pub unsafe fn new(core: rtfm::export::Peripherals) -> Self { + Context { core } + } + } +} +mod resources { + use rtfm::export::Priority; + #[allow(non_camel_case_types)] + pub struct shared<'a> { + priority: &'a Priority, + } + impl<'a> shared<'a> { + #[inline(always)] + pub unsafe fn new(priority: &'a Priority) -> Self { + shared { priority } + } + #[inline(always)] + pub unsafe fn priority(&self) -> &Priority { + self.priority + } + } +} +#[allow(non_snake_case)] +#[doc = "Resources `gpioa` has access to"] +pub struct gpioaResources<'a> { + pub shared: resources::shared<'a>, +} +#[allow(non_snake_case)] +#[doc = "Hardware task"] +pub mod gpioa { + #[doc(inline)] + pub use super::gpioaResources as Resources; + #[doc = r" Execution context"] + pub struct Context<'a> { + #[doc = r" Resources this task has access to"] + pub resources: Resources<'a>, + } + impl<'a> Context<'a> { + #[inline(always)] + pub unsafe fn new(priority: &'a rtfm::export::Priority) -> Self { + Context { + resources: Resources::new(priority), + } + } + } +} +#[allow(non_snake_case)] +#[doc = "Resources `gpiob` has access to"] +pub struct gpiobResources<'a> { + pub shared: resources::shared<'a>, +} +#[allow(non_snake_case)] +#[doc = "Hardware task"] +pub mod gpiob { + #[doc(inline)] + pub use super::gpiobResources as Resources; + #[doc = r" Execution context"] + pub struct Context<'a> { + #[doc = r" Resources this task has access to"] + pub resources: Resources<'a>, + } + impl<'a> Context<'a> { + #[inline(always)] + pub unsafe fn new(priority: &'a rtfm::export::Priority) -> Self { + Context { + resources: Resources::new(priority), + } + } + } +} +#[allow(non_snake_case)] +#[doc = "Resources `gpioc` has access to"] +pub struct gpiocResources<'a> { + pub shared: &'a mut u32, +} +#[allow(non_snake_case)] +#[doc = "Hardware task"] +pub mod gpioc { + #[doc(inline)] + pub use super::gpiocResources as Resources; + #[doc = r" Execution context"] + pub struct Context<'a> { + #[doc = r" Resources this task has access to"] + pub resources: Resources<'a>, + } + impl<'a> Context<'a> { + #[inline(always)] + pub unsafe fn new(priority: &'a rtfm::export::Priority) -> Self { + Context { + resources: Resources::new(priority), + } + } + } +} +#[doc = r" Implementation details"] +const APP: () = { + #[doc = r" Always include the device crate which contains the vector table"] + use lm3s6965 as _; + #[allow(non_upper_case_globals)] + static mut shared: u32 = 0; + impl<'a> rtfm::Mutex for resources::shared<'a> { + type T = u32; + #[inline(always)] + fn lock(&mut self, f: impl FnOnce(&mut u32) -> R) -> R { + #[doc = r" Priority ceiling"] + const CEILING: u8 = 3u8; + unsafe { + rtfm::export::lock( + &mut shared, + self.priority(), + CEILING, + lm3s6965::NVIC_PRIO_BITS, + f, + ) + } + } + } + #[allow(non_snake_case)] + #[no_mangle] + unsafe fn GPIOA() { + const PRIORITY: u8 = 1u8; + crate::gpioa(gpioa::Context::new(&rtfm::export::Priority::new(PRIORITY))); + } + impl<'a> gpioaResources<'a> { + #[inline(always)] + unsafe fn new(priority: &'a rtfm::export::Priority) -> Self { + gpioaResources { + shared: resources::shared::new(priority), + } + } + } + #[allow(non_snake_case)] + #[no_mangle] + unsafe fn GPIOB() { + const PRIORITY: u8 = 2u8; + crate::gpiob(gpiob::Context::new(&rtfm::export::Priority::new(PRIORITY))); + } + impl<'a> gpiobResources<'a> { + #[inline(always)] + unsafe fn new(priority: &'a rtfm::export::Priority) -> Self { + gpiobResources { + shared: resources::shared::new(priority), + } + } + } + #[allow(non_snake_case)] + #[no_mangle] + unsafe fn GPIOC() { + const PRIORITY: u8 = 3u8; + crate::gpioc(gpioc::Context::new(&rtfm::export::Priority::new(PRIORITY))); + } + impl<'a> gpiocResources<'a> { + #[inline(always)] + unsafe fn new(priority: &'a rtfm::export::Priority) -> Self { + gpiocResources { + shared: &mut shared, + } + } + } + #[no_mangle] + unsafe extern "C" fn main() -> ! { + rtfm::export::interrupt::disable(); + let mut core: rtfm::export::Peripherals = core::mem::transmute(()); + let _ = [(); ((1 << lm3s6965::NVIC_PRIO_BITS) - 1u8 as usize)]; + core.NVIC.set_priority( + lm3s6965::Interrupt::GPIOA, + rtfm::export::logical2hw(1u8, lm3s6965::NVIC_PRIO_BITS), + ); + rtfm::export::NVIC::unmask(lm3s6965::Interrupt::GPIOA); + let _ = [(); ((1 << lm3s6965::NVIC_PRIO_BITS) - 2u8 as usize)]; + core.NVIC.set_priority( + lm3s6965::Interrupt::GPIOB, + rtfm::export::logical2hw(2u8, lm3s6965::NVIC_PRIO_BITS), + ); + rtfm::export::NVIC::unmask(lm3s6965::Interrupt::GPIOB); + let _ = [(); ((1 << lm3s6965::NVIC_PRIO_BITS) - 3u8 as usize)]; + core.NVIC.set_priority( + lm3s6965::Interrupt::GPIOC, + rtfm::export::logical2hw(3u8, lm3s6965::NVIC_PRIO_BITS), + ); + rtfm::export::NVIC::unmask(lm3s6965::Interrupt::GPIOC); + core.SCB.scr.modify(|r| r | 1 << 1); + let late = init(init::Context::new(core.into())); + rtfm::export::interrupt::enable(); + loop { + rtfm::export::wfi() + } + } +}; diff --git a/macros/src/codegen/hardware_tasks.rs b/macros/src/codegen/hardware_tasks.rs index a9c2a2bdc7..70bb339078 100644 --- a/macros/src/codegen/hardware_tasks.rs +++ b/macros/src/codegen/hardware_tasks.rs @@ -64,15 +64,11 @@ pub fn codegen( #cfg_core unsafe fn #symbol() { const PRIORITY: u8 = #priority; - #let_instant - - rtfm::export::run(PRIORITY, || { - crate::#name( - #locals_new - #name::Context::new(&rtfm::export::Priority::new(PRIORITY) #instant) - ) - }); + crate::#name( + #locals_new + #name::Context::new(&rtfm::export::Priority::new(PRIORITY) #instant) + ); } )); diff --git a/src/export.rs b/src/export.rs index 96c444bf46..89f5ba7a80 100644 --- a/src/export.rs +++ b/src/export.rs @@ -23,32 +23,6 @@ pub type MCRQ = Queue<(T, u8), N, u8, MultiCore>; pub type SCFQ = Queue; pub type SCRQ = Queue<(T, u8), N, u8, SingleCore>; -#[cfg(armv7m)] -#[inline(always)] -pub fn run(priority: u8, f: F) -where - F: FnOnce(), -{ - if priority == 1 { - // if the priority of this interrupt is `1` then BASEPRI can only be `0` - f(); - unsafe { basepri::write(0) } - } else { - let initial = basepri::read(); - f(); - unsafe { basepri::write(initial) } - } -} - -#[cfg(not(armv7m))] -#[inline(always)] -pub fn run(_priority: u8, f: F) -where - F: FnOnce(), -{ - f(); -} - pub struct Barrier { inner: AtomicBool, } @@ -71,26 +45,47 @@ impl Barrier { // Newtype over `Cell` that forbids mutation through a shared reference pub struct Priority { - inner: Cell, + init_logic: u8, + current_logic: Cell, + #[cfg(armv7m)] + old_basepri_hw: Cell>, } impl Priority { #[inline(always)] pub unsafe fn new(value: u8) -> Self { Priority { - inner: Cell::new(value), + init_logic: value, + current_logic: Cell::new(value), + old_basepri_hw: Cell::new(None), } } - // these two methods are used by `lock` (see below) but can't be used from the RTFM application #[inline(always)] - fn set(&self, value: u8) { - self.inner.set(value) + fn set_logic(&self, value: u8) { + self.current_logic.set(value) } #[inline(always)] - fn get(&self) -> u8 { - self.inner.get() + fn get_logic(&self) -> u8 { + self.current_logic.get() + } + + #[inline(always)] + fn get_init_logic(&self) -> u8 { + self.init_logic + } + + #[cfg(armv7m)] + #[inline(always)] + fn get_old_basepri_hw(&self) -> Option { + self.old_basepri_hw.get() + } + + #[cfg(armv7m)] + #[inline(always)] + fn set_old_basepri_hw(&self, value: u8) { + self.old_basepri_hw.set(Some(value)); } } @@ -124,20 +119,28 @@ pub unsafe fn lock( nvic_prio_bits: u8, f: impl FnOnce(&mut T) -> R, ) -> R { - let current = priority.get(); + let current = priority.get_logic(); if current < ceiling { if ceiling == (1 << nvic_prio_bits) { - priority.set(u8::max_value()); + priority.set_logic(u8::max_value()); let r = interrupt::free(|_| f(&mut *ptr)); - priority.set(current); + priority.set_logic(current); r } else { - priority.set(ceiling); + match priority.get_old_basepri_hw() { + None => priority.set_old_basepri_hw(basepri::read()), + _ => (), + }; + priority.set_logic(ceiling); basepri::write(logical2hw(ceiling, nvic_prio_bits)); let r = f(&mut *ptr); - basepri::write(logical2hw(current, nvic_prio_bits)); - priority.set(current); + if current == priority.get_init_logic() { + basepri::write(priority.get_old_basepri_hw().unwrap()); + } else { + basepri::write(logical2hw(priority.get_logic(), nvic_prio_bits)); + } + priority.set_logic(current); r } } else { @@ -157,9 +160,9 @@ pub unsafe fn lock( let current = priority.get(); if current < ceiling { - priority.set(u8::max_value()); + priority.set_logic(u8::max_value()); let r = interrupt::free(|_| f(&mut *ptr)); - priority.set(current); + priority.set_logic(current); r } else { f(&mut *ptr)