take 2

Cargo.toml

@@ -15,7 +15,7 @@ version = "0.3.2"
 [dependencies]
 cortex-m = "0.4.0"
 cortex-m-rtfm-macros = { path = "macros", version = "0.3.1" }
-heapless = "0.2.6"
+heapless = "0.2.7"
 rtfm-core = "0.2.0"
 untagged-option = "0.1.1"
 

examples/tq.rs

@@ -10,8 +10,10 @@ extern crate cortex_m_rtfm as rtfm;
 extern crate panic_abort;
 extern crate stm32f103xx;
 
+use core::cmp;
+
 use cortex_m::peripheral::syst::SystClkSource;
-use cortex_m::peripheral::{DWT, ITM, SCB};
+use cortex_m::peripheral::{DWT, ITM};
 use rtfm::ll::{Consumer, FreeList, Message, Node, Payload, Producer, RingBuffer, Slot, TimerQueue};
 use rtfm::{app, Resource, Threshold};
 use stm32f103xx::Interrupt;
@@ -29,23 +31,21 @@ app! {
 
         /* a */
         // payloads w/ after
-        static AN0: Node<u32> = Node::new();
+        static AN: [Node<i32>; 2] = [Node::new(), Node::new()];
-        static AN1: Node<u32> = Node::new();
+        static AFL: FreeList<i32> = FreeList::new();
-        static AFL: FreeList<u32> = FreeList::new();
 
-        // payloads w/o after
+        static AQ: RingBuffer<(u32, i32), [(u32, i32); ACAP + 1], u8> = RingBuffer::u8();
-        static AQ: RingBuffer<u32, [u32; ACAP + 1]> = RingBuffer::new();
+        static AQC: Consumer<'static, (u32, i32), [(u32, i32); ACAP + 1], u8>;
-        static AQC: Consumer<'static, u32, [u32; ACAP + 1]>;
+        static AQP: Producer<'static, (u32, i32), [(u32, i32); ACAP + 1], u8>;
-        static AQP: Producer<'static, u32, [u32; ACAP + 1]>;
 
         /* exti0 */
-        static Q1: RingBuffer<Task1, [Task1; ACAP + 1]> = RingBuffer::new();
+        static Q1: RingBuffer<Task1, [Task1; ACAP + 1], u8> = RingBuffer::u8();
-        static Q1C: Consumer<'static, Task1, [Task1; ACAP + 1]>;
+        static Q1C: Consumer<'static, Task1, [Task1; ACAP + 1], u8>;
-        static Q1P: Producer<'static, Task1, [Task1; ACAP + 1]>;
+        static Q1P: Producer<'static, Task1, [Task1; ACAP + 1], u8>;
     },
 
     init: {
-        resources: [AN0, AN1, Q1, AQ],
+        resources: [AN, Q1, AQ],
     },
 
     tasks: {
@@ -68,7 +68,7 @@ app! {
         SYS_TICK: {
             path: sys_tick,
             resources: [TQ, AQP, Q1P, AFL],
-            priority: 1,
+            priority: 2,
         },
     },
 }
@@ -80,11 +80,13 @@ pub fn init(mut p: ::init::Peripherals, r: init::Resources) -> init::LateResourc
     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();
+    p.core.SYST.enable_counter();
+    p.core.SYST.disable_interrupt();
 
     // populate the free list
-    r.AFL.push(Slot::new(r.AN0));
+    for n in r.AN {
-    r.AFL.push(Slot::new(r.AN1));
+        r.AFL.push(Slot::new(n));
+    }
 
     let (aqp, aqc) = r.AQ.split();
     let (q1p, q1c) = r.Q1.split();
@@ -105,12 +107,13 @@ pub fn idle() -> ! {
     }
 }
 
-fn a(_t: &mut Threshold, payload: u32) {
+fn a(_t: &mut Threshold, bl: u32, payload: i32) {
-    let bl = DWT::get_cycle_count();
+    let now = DWT::get_cycle_count();
     unsafe {
         iprintln!(
             &mut (*ITM::ptr()).stim[0],
-            "a(bl={}, payload={})",
+            "a(now={}, bl={}, payload={})",
+            now,
             bl,
             payload
         )
@@ -126,7 +129,6 @@ fn exti1(t: &mut Threshold, r: EXTI1::Resources) {
     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 */
@@ -134,8 +136,8 @@ 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!());
+                let (bl, payload) = r.AQC.dequeue().unwrap();
-                a(&mut unsafe { Threshold::new(1) }, payload);
+                a(&mut unsafe { Threshold::new(1) }, bl, payload);
             }
         }
     }
@@ -150,54 +152,72 @@ fn sys_tick(t: &mut Threshold, r: SYS_TICK::Resources) {
         mut TQ,
     } = r;
 
-    TQ.claim_mut(t, |tq, t| {
+    enum State<T> {
-        tq.syst.disable_counter();
+        Message(Message<T>),
+        Baseline(u32),
+        Done,
+    }
 
-        if let Some(m) = tq.queue.pop() {
+    loop {
+        let state = TQ.claim_mut(t, |tq, _| {
+            if let Some(m) = tq.queue.peek().cloned() {
+                if (DWT::get_cycle_count() as i32).wrapping_sub(m.baseline as i32) >= 0 {
+                    // message ready
+                    tq.queue.pop();
+                    State::Message(m)
+                } else {
+                    // set timeout
+                    State::Baseline(m.baseline)
+                }
+            } else {
+                // empty queue
+                tq.syst.disable_interrupt();
+                State::Done
+            }
+        });
+
+        match state {
+            State::Message(m) => {
                 match m.task {
                     Task::a => {
                         // read payload
-                    let (payload, slot) = unsafe { Payload::<u32>::from(m.payload) }.read();
+                        let (payload, slot) = unsafe { Payload::<i32>::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() {
+                        // enqueue a new `a` task
-                // set up a new interrupt
+                        AQP.claim_mut(t, |aqp, t| {
-                let now = DWT::get_cycle_count();
+                            aqp.enqueue_unchecked((m.baseline, payload));
-
+                            Q1P.claim_mut(t, |q1p, _| {
-                if let Some(timeout) = tq.baseline.wrapping_add(m.deadline).checked_sub(now) {
+                                q1p.enqueue_unchecked(Task1::a);
-                    // TODO deal with the 24-bit limit
+                                rtfm::set_pending(Interrupt::EXTI0);
-                    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!()
-        }
                             });
+                        });
+                    }
+                }
+            }
+            State::Baseline(bl) => {
+                const MAX: u32 = 0x00ffffff;
+
+                let diff = (bl as i32).wrapping_sub(DWT::get_cycle_count() as i32);
+
+                if diff < 0 {
+                    // message became ready
+                    continue;
+                } else {
+                    TQ.claim_mut(t, |tq, _| {
+                        tq.syst.set_reload(cmp::min(MAX, diff as u32));
+                        tq.syst.clear_current();
+                    });
+                    return;
+                }
+            }
+            State::Done => {
+                return;
+            }
+        }
+    }
 }
 
 // Tasks dispatched at a priority of 1
@@ -215,12 +235,16 @@ pub enum Task {
 }
 
 mod a {
+    use cortex_m::peripheral::SCB;
+
+    use rtfm::ll::Message;
     use rtfm::{Resource, Threshold};
     use Task;
 
     #[allow(non_snake_case)]
     pub struct Async {
-        bl: u32,
+        // inherited baseline
+        baseline: u32,
         TQ: ::EXTI1::TQ,
         AFL: ::EXTI1::AFL,
     }
@@ -228,17 +252,38 @@ mod a {
     impl Async {
         #[allow(non_snake_case)]
         pub fn new(bl: u32, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL) -> Self {
-            Async { bl, TQ, AFL }
+            Async {
+                baseline: bl,
+                TQ,
+                AFL,
+            }
         }
 
-        pub fn a(&mut self, t: &mut Threshold, after: u32, payload: u32) -> Result<(), u32> {
+        pub fn a(&mut self, t: &mut Threshold, after: u32, payload: i32) -> Result<(), i32> {
             if let Some(slot) = self.AFL.claim_mut(t, |afl, _| afl.pop()) {
-                let bl = self.bl;
+                let baseline = self.baseline;
-                self.TQ
+                self.TQ.claim_mut(t, |tq, _| {
-                    .claim_mut(t, |tq, _| tq.insert(bl, after, Task::a, payload, slot))
+                    if tq.queue.capacity() == tq.queue.len() {
-                    .map_err(|(p, slot)| {
+                        // full
-                        self.AFL.claim_mut(t, |afl, _| afl.push(slot));
+                        Err(payload)
-                        p
+                    } else {
+                        let bl = baseline.wrapping_add(after);
+                        if tq.queue
+                            .peek()
+                            .map(|head| (bl as i32).wrapping_sub(head.baseline as i32) < 0)
+                            .unwrap_or(true)
+                        {
+                            tq.syst.enable_interrupt();
+                            // Set SYST pending
+                            unsafe { (*SCB::ptr()).icsr.write(1 << 26) }
+                        }
+
+                        tq.queue
+                            .push(Message::new(bl, Task::a, slot.write(payload)))
+                            .ok();
+
+                        Ok(())
+                    }
                 })
             } else {
                 Err(payload)

src/ll.rs

@@ -2,55 +2,16 @@ use core::cmp::Ordering;
 use core::marker::Unsize;
 use core::ptr;
 
-use cortex_m::peripheral::{DWT, SCB, SYST};
+use cortex_m::peripheral::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>,
 }
 
@@ -60,52 +21,49 @@ where
 {
     pub fn new(syst: SYST) -> Self {
         TimerQueue {
-            baseline: 0,
-            queue: BinaryHeap::new(),
             syst,
+            queue: BinaryHeap::new(),
+        }
     }
 }
 
-    pub fn insert<P>(
+#[derive(Clone, Copy)]
-        &mut self,
+pub struct Message<T> {
-        bl: u32,
+    pub baseline: u32,
-        after: u32,
+    pub task: T,
-        task: T,
+    pub payload: usize,
-        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);
+impl<T> Message<T> {
-
+    pub fn new<P>(bl: u32, task: T, payload: Payload<P>) -> Self {
-            if self.queue.peek().map(|m| dl < m.deadline).unwrap_or(true) {
+        Message {
-                // the new message is the most urgent; set a new timeout
+            baseline: bl,
-                let now = DWT::get_cycle_count();
+            task,
-
+            payload: payload.erase(),
-                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
+impl<T> PartialEq for Message<T> {
-                .push(Message::new(dl, task, slot.write(payload)))
+    fn eq(&self, other: &Self) -> bool {
-                .unwrap_or_else(|_| unreachable!());
+        self.baseline.eq(&other.baseline)
-
-            Ok(())
     }
 }
+
+impl<T> Eq for Message<T> {}
+
+impl<T> PartialOrd for Message<T> {
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+impl<T> Ord for Message<T> {
+    fn cmp(&self, other: &Self) -> Ordering {
+        (self.baseline as i32)
+            .wrapping_sub(other.baseline as i32)
+            .cmp(&0)
+    }
 }
 
 pub struct Node<T>