timer queue

.cargo/config

@@ -29,3 +29,6 @@ rustflags = [
   "-C", "linker=arm-none-eabi-ld",
   "-Z", "linker-flavor=ld",
 ]
+
+[build]
+target = "thumbv7m-none-eabi"

.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

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"]

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<Task, [Message<Task>; 2]>;
+
+        /* a */
+        // payloads w/ after
+        static AN0: Node<u32> = Node::new();
+        static AN1: Node<u32> = Node::new();
+        static AFL: FreeList<u32> = FreeList::new();
+
+        // payloads w/o after
+        static AQ: RingBuffer<u32, [u32; ACAP + 1]> = RingBuffer::new();
+        static AQC: Consumer<'static, u32, [u32; ACAP + 1]>;
+        static AQP: Producer<'static, u32, [u32; ACAP + 1]>;
+
+        /* exti0 */
+        static Q1: RingBuffer<Task1, [Task1; ACAP + 1]> = 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::<u32>::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)
+            }
+        }
+    }
+}

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
 ///

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<T> {
+    // relative to the TimerQueue baseline
+    pub deadline: u32,
+    pub task: T,
+    pub payload: usize,
+}
+
+impl<T> Message<T> {
+    fn new<P>(dl: u32, task: T, payload: Payload<P>) -> Self {
+        Message {
+            deadline: dl,
+            task,
+            payload: payload.erase(),
+        }
+    }
+}
+
+impl<T> PartialEq for Message<T> {
+    fn eq(&self, other: &Self) -> bool {
+        self.deadline.eq(&other.deadline)
+    }
+}
+
+impl<T> Eq for Message<T> {}
+
+impl<T> PartialOrd for Message<T> {
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        self.deadline.partial_cmp(&other.deadline)
+    }
+}
+
+impl<T> Ord for Message<T> {
+    fn cmp(&self, other: &Self) -> Ordering {
+        self.deadline.cmp(&other.deadline)
+    }
+}
+
+pub struct TimerQueue<T, A>
+where
+    A: Unsize<[Message<T>]>,
+{
+    pub syst: SYST,
+    pub baseline: u32,
+    pub queue: BinaryHeap<Message<T>, A, Min>,
+}
+
+impl<T, A> TimerQueue<T, A>
+where
+    A: Unsize<[Message<T>]>,
+{
+    pub fn new(syst: SYST) -> Self {
+        TimerQueue {
+            baseline: 0,
+            queue: BinaryHeap::new(),
+            syst,
+        }
+    }
+
+    pub fn insert<P>(
+        &mut self,
+        bl: u32,
+        after: u32,
+        task: T,
+        payload: P,
+        slot: Slot<P>,
+    ) -> Result<(), (P, Slot<P>)> {
+        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<T>
+where
+    T: 'static,
+{
+    data: UntaggedOption<T>,
+    next: Option<&'static mut Node<T>>,
+}
+
+impl<T> Node<T> {
+    pub const fn new() -> Self {
+        Node {
+            data: UntaggedOption::none(),
+            next: None,
+        }
+    }
+}
+
+pub struct Payload<T>
+where
+    T: 'static,
+{
+    node: &'static mut Node<T>,
+}
+
+impl<T> Payload<T> {
+    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<T>) {
+        unsafe {
+            let payload = ptr::read(&self.node.data.some);
+
+            (payload, Slot::new(self.node))
+        }
+    }
+}
+
+pub struct Slot<T>
+where
+    T: 'static,
+{
+    node: &'static mut Node<T>,
+}
+
+impl<T> Slot<T> {
+    pub fn new(node: &'static mut Node<T>) -> Self {
+        Slot { node }
+    }
+
+    pub fn write(self, data: T) -> Payload<T> {
+        unsafe {
+            ptr::write(&mut self.node.data.some, data);
+            Payload { node: self.node }
+        }
+    }
+}
+
+pub struct FreeList<T>
+where
+    T: 'static,
+{
+    head: Option<Slot<T>>,
+}
+
+impl<T> FreeList<T> {
+    pub const fn new() -> Self {
+        FreeList { head: None }
+    }
+
+    pub fn is_empty(&self) -> bool {
+        self.head.is_none()
+    }
+
+    pub fn pop(&mut self) -> Option<Slot<T>> {
+        self.head.take().map(|head| {
+            self.head = head.node.next.take().map(Slot::new);
+            head
+        })
+    }
+
+    pub fn push(&mut self, free: Slot<T>) {
+        free.node.next = self.head.take().map(|slot| slot.node);
+        self.head = Some(Slot::new(free.node));
+    }
+}