From eb8282a571daf3c1775cb984e543f27fd8c440d4 Mon Sep 17 00:00:00 2001 From: Jorge Aparicio Date: Thu, 19 Apr 2018 18:38:12 +0200 Subject: [PATCH] timer queue --- .cargo/config | 3 + .gdbinit | 12 +++ Cargo.toml | 8 +- examples/tq.rs | 248 +++++++++++++++++++++++++++++++++++++++++++++++++ src/lib.rs | 9 +- src/ll.rs | 202 ++++++++++++++++++++++++++++++++++++++++ 6 files changed, 476 insertions(+), 6 deletions(-) create mode 100644 examples/tq.rs create mode 100644 src/ll.rs diff --git a/.cargo/config b/.cargo/config index 36061ee450..b16330a6d5 100644 --- a/.cargo/config +++ b/.cargo/config @@ -29,3 +29,6 @@ rustflags = [ "-C", "linker=arm-none-eabi-ld", "-Z", "linker-flavor=ld", ] + +[build] +target = "thumbv7m-none-eabi" \ No newline at end of file diff --git a/.gdbinit b/.gdbinit index 7c72d4fb1f..67abcb10e3 100644 --- a/.gdbinit +++ b/.gdbinit @@ -2,5 +2,17 @@ target remote :3333 monitor arm semihosting enable +# # send captured ITM to the file itm.fifo +# # (the microcontroller SWO pin must be connected to the programmer SWO pin) +# # 8000000 must match the core clock frequency +# monitor tpiu config internal itm.fifo uart off 8000000 + +# OR: make the microcontroller SWO pin output compatible with UART (8N1) +# 2000000 is the frequency of the SWO pin +monitor tpiu config external uart off 8000000 2000000 + +# enable ITM port 0 +monitor itm port 0 on + load step diff --git a/Cargo.toml b/Cargo.toml index fdb1d18d65..82704bff98 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,19 +15,19 @@ version = "0.3.2" [dependencies] cortex-m = "0.4.0" cortex-m-rtfm-macros = { path = "macros", version = "0.3.1" } +heapless = "0.2.6" rtfm-core = "0.2.0" untagged-option = "0.1.1" [target.'cfg(target_arch = "x86_64")'.dev-dependencies] compiletest_rs = "0.3.5" -[dev-dependencies.cortex-m-rt] -features = ["abort-on-panic"] -version = "0.3.9" +[dev-dependencies] +panic-abort = "0.1.1" [dev-dependencies.stm32f103xx] features = ["rt"] -version = "0.8.0" +version = "0.9.0" [features] cm7-r0p1 = ["cortex-m/cm7-r0p1"] diff --git a/examples/tq.rs b/examples/tq.rs new file mode 100644 index 0000000000..07cfcbd796 --- /dev/null +++ b/examples/tq.rs @@ -0,0 +1,248 @@ +// #![deny(unsafe_code)] +// #![deny(warnings)] +#![allow(dead_code)] +#![feature(proc_macro)] +#![no_std] + +#[macro_use] +extern crate cortex_m; +extern crate cortex_m_rtfm as rtfm; +extern crate panic_abort; +extern crate stm32f103xx; + +use cortex_m::peripheral::syst::SystClkSource; +use cortex_m::peripheral::{DWT, ITM, SCB}; +use rtfm::ll::{Consumer, FreeList, Message, Node, Payload, Producer, RingBuffer, Slot, TimerQueue}; +use rtfm::{app, Resource, Threshold}; +use stm32f103xx::Interrupt; + +const ACAP: usize = 2; + +const MS: u32 = 8_000; + +app! { + device: stm32f103xx, + + resources: { + /* timer queue */ + static TQ: TimerQueue; 2]>; + + /* a */ + // payloads w/ after + static AN0: Node = Node::new(); + static AN1: Node = Node::new(); + static AFL: FreeList = FreeList::new(); + + // payloads w/o after + static AQ: RingBuffer = RingBuffer::new(); + static AQC: Consumer<'static, u32, [u32; ACAP + 1]>; + static AQP: Producer<'static, u32, [u32; ACAP + 1]>; + + /* exti0 */ + static Q1: RingBuffer = RingBuffer::new(); + static Q1C: Consumer<'static, Task1, [Task1; ACAP + 1]>; + static Q1P: Producer<'static, Task1, [Task1; ACAP + 1]>; + }, + + init: { + resources: [AN0, AN1, Q1, AQ], + }, + + tasks: { + EXTI1: { + path: exti1, + resources: [TQ, AFL], + priority: 1, + + // async: [a], + }, + + // dispatch interrupt + EXTI0: { + path: exti0, + resources: [AQC, Q1C], + priority: 1, + }, + + // timer queue + SYS_TICK: { + path: sys_tick, + resources: [TQ, AQP, Q1P, AFL], + priority: 1, + }, + }, +} + +pub fn init(mut p: ::init::Peripherals, r: init::Resources) -> init::LateResources { + // .. + + /* executed after `init` end */ + p.core.DWT.enable_cycle_counter(); + unsafe { p.core.DWT.cyccnt.write(0) }; + p.core.SYST.set_clock_source(SystClkSource::Core); + p.core.SYST.enable_interrupt(); + + // populate the free list + r.AFL.push(Slot::new(r.AN0)); + r.AFL.push(Slot::new(r.AN1)); + + let (aqp, aqc) = r.AQ.split(); + let (q1p, q1c) = r.Q1.split(); + init::LateResources { + TQ: TimerQueue::new(p.core.SYST), + AQC: aqc, + AQP: aqp, + Q1C: q1c, + Q1P: q1p, + } +} + +pub fn idle() -> ! { + rtfm::set_pending(Interrupt::EXTI1); + + loop { + rtfm::wfi() + } +} + +fn a(_t: &mut Threshold, payload: u32) { + let bl = DWT::get_cycle_count(); + unsafe { + iprintln!( + &mut (*ITM::ptr()).stim[0], + "a(bl={}, payload={})", + bl, + payload + ) + } +} + +fn exti1(t: &mut Threshold, r: EXTI1::Resources) { + /* expansion */ + let bl = DWT::get_cycle_count(); + let mut async = a::Async::new(bl, r.TQ, r.AFL); + /* end of expansion */ + + unsafe { iprintln!(&mut (*ITM::ptr()).stim[0], "EXTI0(bl={})", bl) } + async.a(t, 100 * MS, 0).unwrap(); + async.a(t, 50 * MS, 1).unwrap(); + // rtfm::bkpt(); +} + +/* auto generated */ +fn exti0(_t: &mut Threshold, mut r: EXTI0::Resources) { + while let Some(task) = r.Q1C.dequeue() { + match task { + Task1::a => { + let payload = r.AQC.dequeue().unwrap_or_else(|| unreachable!()); + a(&mut unsafe { Threshold::new(1) }, payload); + } + } + } +} + +fn sys_tick(t: &mut Threshold, r: SYS_TICK::Resources) { + #[allow(non_snake_case)] + let SYS_TICK::Resources { + mut AFL, + mut AQP, + mut Q1P, + mut TQ, + } = r; + + TQ.claim_mut(t, |tq, t| { + tq.syst.disable_counter(); + + if let Some(m) = tq.queue.pop() { + match m.task { + Task::a => { + // read payload + let (payload, slot) = unsafe { Payload::::from(m.payload) }.read(); + + // enqueue a new `a` task + AQP.claim_mut(t, |aqp, t| { + aqp.enqueue(payload).ok().unwrap(); + Q1P.claim_mut(t, |q1p, _| { + q1p.enqueue(Task1::a).ok().unwrap_or_else(|| unreachable!()); + rtfm::set_pending(Interrupt::EXTI0); + }); + }); + + // return free slot to the free list + AFL.claim_mut(t, |afl, _| afl.push(slot)); + } + } + + if let Some(m) = tq.queue.peek().cloned() { + // set up a new interrupt + let now = DWT::get_cycle_count(); + + if let Some(timeout) = tq.baseline.wrapping_add(m.deadline).checked_sub(now) { + // TODO deal with the 24-bit limit + tq.syst.set_reload(timeout); + tq.syst.clear_current(); + tq.syst.enable_counter(); + + // update the timer queue baseline + tq.baseline = now; + tq.queue.iter_mut().for_each(|m| m.deadline -= timeout); + } else { + // next message already expired, pend immediately + // NOTE(unsafe) atomic write to a stateless (from the programmer PoV) register + unsafe { (*SCB::ptr()).icsr.write(1 << 26) } + } + } else { + // no message left to process + } + } else { + unreachable!() + } + }); +} + +// Tasks dispatched at a priority of 1 +#[allow(non_camel_case_types)] +#[derive(Clone, Copy)] +pub enum Task1 { + a, +} + +// All tasks +#[allow(non_camel_case_types)] +#[derive(Clone, Copy)] +pub enum Task { + a, +} + +mod a { + use rtfm::{Resource, Threshold}; + use Task; + + #[allow(non_snake_case)] + pub struct Async { + bl: u32, + TQ: ::EXTI1::TQ, + AFL: ::EXTI1::AFL, + } + + impl Async { + #[allow(non_snake_case)] + pub fn new(bl: u32, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL) -> Self { + Async { bl, TQ, AFL } + } + + pub fn a(&mut self, t: &mut Threshold, after: u32, payload: u32) -> Result<(), u32> { + if let Some(slot) = self.AFL.claim_mut(t, |afl, _| afl.pop()) { + let bl = self.bl; + self.TQ + .claim_mut(t, |tq, _| tq.insert(bl, after, Task::a, payload, slot)) + .map_err(|(p, slot)| { + self.AFL.claim_mut(t, |afl, _| afl.push(slot)); + p + }) + } else { + Err(payload) + } + } + } +} diff --git a/src/lib.rs b/src/lib.rs index 8e5884cab8..9c98b23fa0 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -77,13 +77,16 @@ //! > A description of the RTFM task and resource model. [PDF][rtfm] //! //! [rtfm]: http://www.diva-portal.org/smash/get/diva2:1005680/FULLTEXT01.pdf -#![deny(missing_docs)] -#![deny(warnings)] +// #![deny(missing_docs)] +// #![deny(warnings)] +#![feature(const_fn)] #![feature(proc_macro)] +#![feature(unsize)] #![no_std] extern crate cortex_m; extern crate cortex_m_rtfm_macros; +extern crate heapless; extern crate rtfm_core; extern crate untagged_option; @@ -101,6 +104,8 @@ use cortex_m::peripheral::NVIC; use cortex_m::register::basepri; pub mod examples; +#[doc(hidden)] +pub mod ll; /// Executes the closure `f` in a preemption free context /// diff --git a/src/ll.rs b/src/ll.rs new file mode 100644 index 0000000000..42873a160f --- /dev/null +++ b/src/ll.rs @@ -0,0 +1,202 @@ +use core::cmp::Ordering; +use core::marker::Unsize; +use core::ptr; + +use cortex_m::peripheral::{DWT, SCB, SYST}; +use heapless::binary_heap::{BinaryHeap, Min}; +pub use heapless::ring_buffer::{Consumer, Producer, RingBuffer}; +use untagged_option::UntaggedOption; + +#[derive(Clone, Copy)] +pub struct Message { + // relative to the TimerQueue baseline + pub deadline: u32, + pub task: T, + pub payload: usize, +} + +impl Message { + fn new

(dl: u32, task: T, payload: Payload

) -> Self { + Message { + deadline: dl, + task, + payload: payload.erase(), + } + } +} + +impl PartialEq for Message { + fn eq(&self, other: &Self) -> bool { + self.deadline.eq(&other.deadline) + } +} + +impl Eq for Message {} + +impl PartialOrd for Message { + fn partial_cmp(&self, other: &Self) -> Option { + self.deadline.partial_cmp(&other.deadline) + } +} + +impl Ord for Message { + fn cmp(&self, other: &Self) -> Ordering { + self.deadline.cmp(&other.deadline) + } +} + +pub struct TimerQueue +where + A: Unsize<[Message]>, +{ + pub syst: SYST, + pub baseline: u32, + pub queue: BinaryHeap, A, Min>, +} + +impl TimerQueue +where + A: Unsize<[Message]>, +{ + pub fn new(syst: SYST) -> Self { + TimerQueue { + baseline: 0, + queue: BinaryHeap::new(), + syst, + } + } + + pub fn insert

( + &mut self, + bl: u32, + after: u32, + task: T, + payload: P, + slot: Slot

, + ) -> Result<(), (P, Slot

)> { + if self.queue.len() == self.queue.capacity() { + Err((payload, slot)) + } else { + if self.queue.is_empty() { + self.baseline = bl; + } + + let dl = bl.wrapping_add(after).wrapping_sub(self.baseline); + + if self.queue.peek().map(|m| dl < m.deadline).unwrap_or(true) { + // the new message is the most urgent; set a new timeout + let now = DWT::get_cycle_count(); + + if let Some(timeout) = dl.wrapping_add(self.baseline).checked_sub(now) { + self.syst.disable_counter(); + self.syst.set_reload(timeout); + self.syst.clear_current(); + self.syst.enable_counter(); + } else { + // message already expired, pend immediately + // NOTE(unsafe) atomic write to a stateless (from the programmer PoV) register + unsafe { (*SCB::ptr()).icsr.write(1 << 26) } + } + } + + self.queue + .push(Message::new(dl, task, slot.write(payload))) + .unwrap_or_else(|_| unreachable!()); + + Ok(()) + } + } +} + +pub struct Node +where + T: 'static, +{ + data: UntaggedOption, + next: Option<&'static mut Node>, +} + +impl Node { + pub const fn new() -> Self { + Node { + data: UntaggedOption::none(), + next: None, + } + } +} + +pub struct Payload +where + T: 'static, +{ + node: &'static mut Node, +} + +impl Payload { + pub unsafe fn from(ptr: usize) -> Self { + Payload { + node: &mut *(ptr as *mut _), + } + } + + pub fn erase(self) -> usize { + self.node as *mut _ as usize + } + + pub fn read(self) -> (T, Slot) { + unsafe { + let payload = ptr::read(&self.node.data.some); + + (payload, Slot::new(self.node)) + } + } +} + +pub struct Slot +where + T: 'static, +{ + node: &'static mut Node, +} + +impl Slot { + pub fn new(node: &'static mut Node) -> Self { + Slot { node } + } + + pub fn write(self, data: T) -> Payload { + unsafe { + ptr::write(&mut self.node.data.some, data); + Payload { node: self.node } + } + } +} + +pub struct FreeList +where + T: 'static, +{ + head: Option>, +} + +impl FreeList { + pub const fn new() -> Self { + FreeList { head: None } + } + + pub fn is_empty(&self) -> bool { + self.head.is_none() + } + + pub fn pop(&mut self) -> Option> { + self.head.take().map(|head| { + self.head = head.node.next.take().map(Slot::new); + head + }) + } + + pub fn push(&mut self, free: Slot) { + free.node.next = self.head.take().map(|slot| slot.node); + self.head = Some(Slot::new(free.node)); + } +}