take 3

examples/tq.rs

@@ -13,8 +13,9 @@ extern crate stm32f103xx;
 use core::cmp;
 
 use cortex_m::peripheral::syst::SystClkSource;
-use cortex_m::peripheral::{DWT, ITM};
+use cortex_m::peripheral::ITM;
-use rtfm::ll::{Consumer, FreeList, Message, Node, Payload, Producer, RingBuffer, Slot, TimerQueue};
+use rtfm::ll::{Consumer, FreeList, Instant, Node, Producer, RingBuffer, Slot, TaggedPayload,
+               TimerQueue};
 use rtfm::{app, Resource, Threshold};
 use stm32f103xx::Interrupt;
 
@@ -27,25 +28,22 @@ app! {
 
     resources: {
         /* timer queue */
-        static TQ: TimerQueue<Task, [Message<Task>; 2]>;
+        static TQ: TimerQueue<Task, [TaggedPayload<Task>; 2]>;
 
         /* a */
         // payloads w/ after
         static AN: [Node<i32>; 2] = [Node::new(), Node::new()];
         static AFL: FreeList<i32> = FreeList::new();
 
-        static AQ: RingBuffer<(u32, i32), [(u32, i32); ACAP + 1], u8> = RingBuffer::u8();
-        static AQC: Consumer<'static, (u32, i32), [(u32, i32); ACAP + 1], u8>;
-        static AQP: Producer<'static, (u32, i32), [(u32, i32); ACAP + 1], u8>;
-
         /* exti0 */
-        static Q1: RingBuffer<Task1, [Task1; ACAP + 1], u8> = RingBuffer::u8();
+        static Q1: RingBuffer<TaggedPayload<Task1>, [TaggedPayload<Task1>; ACAP + 1], u8> =
-        static Q1C: Consumer<'static, Task1, [Task1; ACAP + 1], u8>;
+            RingBuffer::u8();
-        static Q1P: Producer<'static, Task1, [Task1; ACAP + 1], u8>;
+        static Q1C: Consumer<'static, TaggedPayload<Task1>, [TaggedPayload<Task1>; ACAP + 1], u8>;
+        static Q1P: Producer<'static, TaggedPayload<Task1>, [TaggedPayload<Task1>; ACAP + 1], u8>;
     },
 
     init: {
-        resources: [AN, Q1, AQ],
+        resources: [AN, Q1],
     },
 
     tasks: {
@@ -60,14 +58,14 @@ app! {
         // dispatch interrupt
         EXTI0: {
             path: exti0,
-            resources: [AQC, Q1C],
+            resources: [Q1C, AFL],
             priority: 1,
         },
 
         // timer queue
         SYS_TICK: {
             path: sys_tick,
-            resources: [TQ, AQP, Q1P, AFL],
+            resources: [TQ, Q1P],
             priority: 2,
         },
     },
@@ -88,12 +86,9 @@ pub fn init(mut p: ::init::Peripherals, r: init::Resources) -> init::LateResourc
         r.AFL.push(Slot::new(n));
     }
 
-    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,
     }
@@ -107,13 +102,12 @@ pub fn idle() -> ! {
     }
 }
 
-fn a(_t: &mut Threshold, bl: u32, payload: i32) {
+fn a(_t: &mut Threshold, bl: Instant, payload: i32) {
-    let now = DWT::get_cycle_count();
     unsafe {
         iprintln!(
             &mut (*ITM::ptr()).stim[0],
-            "a(now={}, bl={}, payload={})",
+            "a(now={:?}, bl={:?}, payload={})",
-            now,
+            Instant::now(),
             bl,
             payload
         )
@@ -122,21 +116,24 @@ fn a(_t: &mut Threshold, bl: u32, payload: i32) {
 
 fn exti1(t: &mut Threshold, r: EXTI1::Resources) {
     /* expansion */
-    let bl = DWT::get_cycle_count();
+    let bl = Instant::now();
     let mut async = a::Async::new(bl, r.TQ, r.AFL);
     /* end of expansion */
 
-    unsafe { iprintln!(&mut (*ITM::ptr()).stim[0], "EXTI0(bl={})", bl) }
+    unsafe { iprintln!(&mut (*ITM::ptr()).stim[0], "EXTI0(bl={:?})", bl) }
     async.a(t, 100 * MS, 0).unwrap();
     async.a(t, 50 * MS, 1).unwrap();
 }
 
 /* auto generated */
-fn exti0(_t: &mut Threshold, mut r: EXTI0::Resources) {
+fn exti0(t: &mut Threshold, mut r: EXTI0::Resources) {
-    while let Some(task) = r.Q1C.dequeue() {
+    while let Some(payload) = r.Q1C.dequeue() {
-        match task {
+        match payload.tag() {
             Task1::a => {
-                let (bl, payload) = r.AQC.dequeue().unwrap();
+                let (bl, payload, slot) = unsafe { payload.coerce() }.read();
+
+                r.AFL.claim_mut(t, |afl, _| afl.push(slot));
+
                 a(&mut unsafe { Threshold::new(1) }, bl, payload);
             }
         }
@@ -145,29 +142,26 @@ fn exti0(_t: &mut Threshold, mut r: EXTI0::Resources) {
 
 fn sys_tick(t: &mut Threshold, r: SYS_TICK::Resources) {
     #[allow(non_snake_case)]
-    let SYS_TICK::Resources {
+    let SYS_TICK::Resources { mut Q1P, mut TQ } = r;
-        mut AFL,
-        mut AQP,
-        mut Q1P,
-        mut TQ,
-    } = r;
 
-    enum State<T> {
+    enum State<T>
-        Message(Message<T>),
+    where
-        Baseline(u32),
+        T: Copy,
+    {
+        Payload(TaggedPayload<T>),
+        Baseline(Instant),
         Done,
     }
 
     loop {
         let state = TQ.claim_mut(t, |tq, _| {
-            if let Some(m) = tq.queue.peek().cloned() {
+            if let Some(bl) = tq.queue.peek().map(|p| p.baseline()) {
-                if (DWT::get_cycle_count() as i32).wrapping_sub(m.baseline as i32) >= 0 {
+                if Instant::now() >= bl {
                     // message ready
-                    tq.queue.pop();
+                    State::Payload(tq.queue.pop().unwrap())
-                    State::Message(m)
                 } else {
-                    // set timeout
+                    // new timeout
-                    State::Baseline(m.baseline)
+                    State::Baseline(bl)
                 }
             } else {
                 // empty queue
@@ -177,30 +171,16 @@ fn sys_tick(t: &mut Threshold, r: SYS_TICK::Resources) {
         });
 
         match state {
-            State::Message(m) => {
+            State::Payload(p) => match p.tag() {
-                match m.task {
                 Task::a => {
-                        // read payload
+                    Q1P.claim_mut(t, |q1p, _| q1p.enqueue_unchecked(p.retag(Task1::a)));
-                        let (payload, slot) = unsafe { Payload::<i32>::from(m.payload) }.read();
-
-                        // return free slot to the free list
-                        AFL.claim_mut(t, |afl, _| afl.push(slot));
-
-                        // enqueue a new `a` task
-                        AQP.claim_mut(t, |aqp, t| {
-                            aqp.enqueue_unchecked((m.baseline, payload));
-                            Q1P.claim_mut(t, |q1p, _| {
-                                q1p.enqueue_unchecked(Task1::a);
                     rtfm::set_pending(Interrupt::EXTI0);
-                            });
-                        });
-                    }
-                }
                 }
+            },
             State::Baseline(bl) => {
                 const MAX: u32 = 0x00ffffff;
 
-                let diff = (bl as i32).wrapping_sub(DWT::get_cycle_count() as i32);
+                let diff = bl - Instant::now();
 
                 if diff < 0 {
                     // message became ready
@@ -237,21 +217,21 @@ pub enum Task {
 mod a {
     use cortex_m::peripheral::SCB;
 
-    use rtfm::ll::Message;
+    use rtfm::ll::Instant;
     use rtfm::{Resource, Threshold};
     use Task;
 
     #[allow(non_snake_case)]
     pub struct Async {
         // inherited baseline
-        baseline: u32,
+        baseline: Instant,
         TQ: ::EXTI1::TQ,
         AFL: ::EXTI1::AFL,
     }
 
     impl Async {
         #[allow(non_snake_case)]
-        pub fn new(bl: u32, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL) -> Self {
+        pub fn new(bl: Instant, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL) -> Self {
             Async {
                 baseline: bl,
                 TQ,
@@ -267,10 +247,10 @@ mod a {
                         // full
                         Err(payload)
                     } else {
-                        let bl = baseline.wrapping_add(after);
+                        let bl = baseline + after;
                         if tq.queue
                             .peek()
-                            .map(|head| (bl as i32).wrapping_sub(head.baseline as i32) < 0)
+                            .map(|head| bl < head.baseline())
                             .unwrap_or(true)
                         {
                             tq.syst.enable_interrupt();
@@ -278,9 +258,7 @@ mod a {
                             unsafe { (*SCB::ptr()).icsr.write(1 << 26) }
                         }
 
-                        tq.queue
+                        tq.queue.push(slot.write(bl, payload).tag(Task::a)).ok();
-                            .push(Message::new(bl, Task::a, slot.write(payload)))
-                            .ok();
 
                         Ok(())
                     }

src/lib.rs

@@ -81,6 +81,7 @@
 // #![deny(warnings)]
 #![feature(const_fn)]
 #![feature(proc_macro)]
+#![feature(untagged_unions)]
 #![feature(unsize)]
 #![no_std]
 

src/ll.rs

@@ -1,111 +1,48 @@
 use core::cmp::Ordering;
 use core::marker::Unsize;
-use core::ptr;
+use core::ops;
+use core::{mem, ptr};
 
-use cortex_m::peripheral::SYST;
+use cortex_m::peripheral::{DWT, SYST};
 use heapless::binary_heap::{BinaryHeap, Min};
 pub use heapless::ring_buffer::{Consumer, Producer, RingBuffer};
-use untagged_option::UntaggedOption;
-
-pub struct TimerQueue<T, A>
-where
-    A: Unsize<[Message<T>]>,
-{
-    pub syst: SYST,
-    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 {
-            syst,
-            queue: BinaryHeap::new(),
-        }
-    }
-}
-
-#[derive(Clone, Copy)]
-pub struct Message<T> {
-    pub baseline: u32,
-    pub task: T,
-    pub payload: usize,
-}
-
-impl<T> Message<T> {
-    pub fn new<P>(bl: u32, task: T, payload: Payload<P>) -> Self {
-        Message {
-            baseline: bl,
-            task,
-            payload: payload.erase(),
-        }
-    }
-}
-
-impl<T> PartialEq for Message<T> {
-    fn eq(&self, other: &Self) -> bool {
-        self.baseline.eq(&other.baseline)
-    }
-}
-
-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)
-    }
-}
 
+#[repr(C)]
 pub struct Node<T>
 where
     T: 'static,
 {
-    data: UntaggedOption<T>,
+    baseline: Instant,
-    next: Option<&'static mut Node<T>>,
+    next: Option<Slot<T>>,
+    payload: T,
+}
+
+impl<T> Eq for Node<T> {}
+
+impl<T> Ord for Node<T> {
+    fn cmp(&self, rhs: &Self) -> Ordering {
+        self.baseline.cmp(&rhs.baseline)
+    }
+}
+
+impl<T> PartialEq for Node<T> {
+    fn eq(&self, rhs: &Self) -> bool {
+        self.baseline.eq(&rhs.baseline)
+    }
+}
+
+impl<T> PartialOrd for Node<T> {
+    fn partial_cmp(&self, rhs: &Self) -> Option<Ordering> {
+        Some(self.cmp(rhs))
+    }
 }
 
 impl<T> Node<T> {
     pub const fn new() -> Self {
         Node {
-            data: UntaggedOption::none(),
+            baseline: Instant(0),
             next: None,
-        }
+            payload: uninitialized(),
-    }
-}
-
-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))
         }
     }
 }
@@ -122,13 +59,12 @@ impl<T> Slot<T> {
         Slot { node }
     }
 
-    pub fn write(self, data: T) -> Payload<T> {
+    pub fn write(self, bl: Instant, data: T) -> Payload<T> {
-        unsafe {
+        self.node.baseline = bl;
-            ptr::write(&mut self.node.data.some, data);
+        unsafe { ptr::write(&mut self.node.payload, data) }
         Payload { node: self.node }
     }
 }
-}
 
 pub struct FreeList<T>
 where
@@ -142,19 +78,184 @@ impl<T> FreeList<T> {
         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);
+            self.head = head.node.next.take();
             head
         })
     }
 
-    pub fn push(&mut self, free: Slot<T>) {
+    pub fn push(&mut self, slot: Slot<T>) {
-        free.node.next = self.head.take().map(|slot| slot.node);
+        slot.node.next = self.head.take();
-        self.head = Some(Slot::new(free.node));
+        self.head = Some(slot);
     }
 }
+
+pub struct Payload<T>
+where
+    T: 'static,
+{
+    node: &'static mut Node<T>,
+}
+
+impl<T> Payload<T> {
+    pub fn read(self) -> (Instant, T, Slot<T>) {
+        let data = unsafe { ptr::read(&self.node.payload) };
+        (self.node.baseline, data, Slot { node: self.node })
+    }
+
+    pub fn tag<A>(self, tag: A) -> TaggedPayload<A>
+    where
+        A: Copy,
+    {
+        TaggedPayload {
+            tag,
+            payload: unsafe { mem::transmute(self) },
+        }
+    }
+}
+
+pub struct TaggedPayload<A>
+where
+    A: Copy,
+{
+    tag: A,
+    payload: Payload<!>,
+}
+
+impl<A> TaggedPayload<A>
+where
+    A: Copy,
+{
+    pub unsafe fn coerce<T>(self) -> Payload<T> {
+        mem::transmute(self.payload)
+    }
+
+    pub fn baseline(&self) -> Instant {
+        self.payload.node.baseline
+    }
+
+    pub fn tag(&self) -> A {
+        self.tag
+    }
+
+    pub fn retag<B>(self, tag: B) -> TaggedPayload<B>
+    where
+        B: Copy,
+    {
+        TaggedPayload {
+            tag,
+            payload: self.payload,
+        }
+    }
+}
+
+impl<T> Eq for TaggedPayload<T>
+where
+    T: Copy,
+{
+}
+
+impl<T> Ord for TaggedPayload<T>
+where
+    T: Copy,
+{
+    fn cmp(&self, rhs: &Self) -> Ordering {
+        self.payload.node.cmp(&rhs.payload.node)
+    }
+}
+
+impl<T> PartialEq for TaggedPayload<T>
+where
+    T: Copy,
+{
+    fn eq(&self, rhs: &Self) -> bool {
+        self.payload.node.eq(&rhs.payload.node)
+    }
+}
+
+impl<T> PartialOrd for TaggedPayload<T>
+where
+    T: Copy,
+{
+    fn partial_cmp(&self, rhs: &Self) -> Option<Ordering> {
+        Some(self.cmp(rhs))
+    }
+}
+
+pub struct TimerQueue<T, A>
+where
+    A: Unsize<[TaggedPayload<T>]>,
+    T: Copy,
+{
+    pub syst: SYST,
+    pub queue: BinaryHeap<TaggedPayload<T>, A, Min>,
+}
+
+impl<T, A> TimerQueue<T, A>
+where
+    A: Unsize<[TaggedPayload<T>]>,
+    T: Copy,
+{
+    pub const fn new(syst: SYST) -> Self {
+        TimerQueue {
+            syst,
+            queue: BinaryHeap::new(),
+        }
+    }
+}
+
+#[derive(Clone, Copy, Debug)]
+pub struct Instant(u32);
+
+impl Instant {
+    pub fn now() -> Self {
+        Instant(DWT::get_cycle_count())
+    }
+}
+
+impl Eq for Instant {}
+
+impl Ord for Instant {
+    fn cmp(&self, rhs: &Self) -> Ordering {
+        (self.0 as i32).wrapping_sub(rhs.0 as i32).cmp(&0)
+    }
+}
+
+impl PartialEq for Instant {
+    fn eq(&self, rhs: &Self) -> bool {
+        self.0.eq(&rhs.0)
+    }
+}
+
+impl PartialOrd for Instant {
+    fn partial_cmp(&self, rhs: &Self) -> Option<Ordering> {
+        Some(self.cmp(rhs))
+    }
+}
+
+impl ops::Add<u32> for Instant {
+    type Output = Self;
+
+    fn add(self, rhs: u32) -> Self {
+        Instant(self.0.wrapping_add(rhs))
+    }
+}
+
+impl ops::Sub for Instant {
+    type Output = i32;
+
+    fn sub(self, rhs: Self) -> i32 {
+        (self.0 as i32).wrapping_sub(rhs.0 as i32)
+    }
+}
+
+const fn uninitialized<T>() -> T {
+    #[allow(unions_with_drop_fields)]
+    union U<T> {
+        some: T,
+        none: (),
+    }
+
+    unsafe { U { none: () }.some }
+}