1 files changed, 88 insertions, 76 deletions

diff --git a/src/tq.rs b/src/tq.rs
index 8d52051..9300dbf 100644
--- a/src/tq.rs
+++ b/src/tq.rs
@@ -1,36 +1,34 @@
-use core::cmp::{self, Ordering};
+use core::{
+    cmp::{self, Ordering},
+    convert::TryInto,
+    mem,
+    ops::Sub,
+};
 
 use cortex_m::peripheral::{SCB, SYST};
 use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
 
-use crate::{Instant, Mutex};
+use crate::Monotonic;
 
-pub struct TimerQueue<T, N>
+pub struct TimerQueue<M, T, N>(pub BinaryHeap<NotReady<M, T>, N, Min>)
 where
-    N: ArrayLength<NotReady<T>>,
-    T: Copy,
-{
-    pub syst: SYST,
-    pub queue: BinaryHeap<NotReady<T>, N, Min>,
-}
+    M: Monotonic,
+    <M::Instant as Sub>::Output: TryInto<u32>,
+    N: ArrayLength<NotReady<M, T>>,
+    T: Copy;
 
-impl<T, N> TimerQueue<T, N>
+impl<M, T, N> TimerQueue<M, T, N>
 where
-    N: ArrayLength<NotReady<T>>,
+    M: Monotonic,
+    <M::Instant as Sub>::Output: TryInto<u32>,
+    N: ArrayLength<NotReady<M, T>>,
     T: Copy,
 {
-    pub fn new(syst: SYST) -> Self {
-        TimerQueue {
-            syst,
-            queue: BinaryHeap::new(),
-        }
-    }
-
     #[inline]
-    pub unsafe fn enqueue_unchecked(&mut self, nr: NotReady<T>) {
+    pub unsafe fn enqueue_unchecked(&mut self, nr: NotReady<M, T>) {
         let mut is_empty = true;
         if self
-            .queue
+            .0
             .peek()
             .map(|head| {
                 is_empty = false;
@@ -39,97 +37,111 @@ where
             .unwrap_or(true)
         {
             if is_empty {
-                self.syst.enable_interrupt();
+                mem::transmute::<_, SYST>(()).enable_interrupt();
             }
 
-            // set SysTick pending
-            (*SCB::ptr()).icsr.write(1 << 26);
+            // Set SysTick pending
+            SCB::set_pendst();
         }
 
-        self.queue.push_unchecked(nr);
+        self.0.push_unchecked(nr);
+    }
+
+    #[inline]
+    pub fn dequeue(&mut self) -> Option<(T, u8)> {
+        unsafe {
+            if let Some(instant) = self.0.peek().map(|p| p.instant) {
+                let now = M::now();
+
+                if instant < now {
+                    // task became ready
+                    let nr = self.0.pop_unchecked();
+
+                    Some((nr.task, nr.index))
+                } else {
+                    // set a new timeout
+                    const MAX: u32 = 0x00ffffff;
+
+                    let ratio = M::ratio();
+                    let dur = match (instant - now).try_into().ok().and_then(|x| {
+                        x.checked_mul(ratio.numerator)
+                            .map(|x| x / ratio.denominator)
+                    }) {
+                        None => MAX,
+
+                        // ARM Architecture Reference Manual says:
+                        // "Setting SYST_RVR to zero has the effect of
+                        // disabling the SysTick counter independently
+                        // of the counter enable bit."
+                        Some(0) => 1,
+
+                        Some(x) => cmp::min(MAX, x),
+                    };
+                    mem::transmute::<_, SYST>(()).set_reload(dur);
+
+                    // Start counting down from the new reload
+                    mem::transmute::<_, SYST>(()).clear_current();
+
+                    None
+                }
+            } else {
+                // The queue is empty
+                mem::transmute::<_, SYST>(()).disable_interrupt();
+
+                None
+            }
+        }
     }
 }
 
-pub struct NotReady<T>
+pub struct NotReady<M, T>
 where
     T: Copy,
+    M: Monotonic,
+    <M::Instant as Sub>::Output: TryInto<u32>,
 {
     pub index: u8,
-    pub instant: Instant,
+    pub instant: M::Instant,
     pub task: T,
 }
 
-impl<T> Eq for NotReady<T> where T: Copy {}
+impl<M, T> Eq for NotReady<M, T>
+where
+    T: Copy,
+    M: Monotonic,
+    <M::Instant as Sub>::Output: TryInto<u32>,
+{
+}
 
-impl<T> Ord for NotReady<T>
+impl<M, T> Ord for NotReady<M, T>
 where
     T: Copy,
+    M: Monotonic,
+    <M::Instant as Sub>::Output: TryInto<u32>,
 {
     fn cmp(&self, other: &Self) -> Ordering {
         self.instant.cmp(&other.instant)
     }
 }
 
-impl<T> PartialEq for NotReady<T>
+impl<M, T> PartialEq for NotReady<M, T>
 where
     T: Copy,
+    M: Monotonic,
+    <M::Instant as Sub>::Output: TryInto<u32>,
 {
     fn eq(&self, other: &Self) -> bool {
         self.instant == other.instant
     }
 }
 
-impl<T> PartialOrd for NotReady<T>
+impl<M, T> PartialOrd for NotReady<M, T>
 where
     T: Copy,
+    M: Monotonic,
+    <M::Instant as Sub>::Output: TryInto<u32>,
 {
     fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
         Some(self.cmp(&other))
     }
 }
-
-#[inline(always)]
-pub fn isr<TQ, T, N, F>(mut tq: TQ, mut f: F)
-where
-    TQ: Mutex<T = TimerQueue<T, N>>,
-    T: Copy + Send,
-    N: ArrayLength<NotReady<T>>,
-    F: FnMut(T, u8),
-{
-    loop {
-        // XXX does `#[inline(always)]` improve performance or not?
-        let next = tq.lock(#[inline(always)]
-        |tq| {
-            if let Some(instant) = tq.queue.peek().map(|p| p.instant) {
-                let diff = instant.0.wrapping_sub(Instant::now().0);
-
-                if diff < 0 {
-                    // task became ready
-                    let m = unsafe { tq.queue.pop_unchecked() };
-
-                    Some((m.task, m.index))
-                } else {
-                    // set a new timeout
-                    const MAX: u32 = 0x00ffffff;
-
-                    tq.syst.set_reload(cmp::min(MAX, diff as u32));
-
-                    // start counting down from the new reload
-                    tq.syst.clear_current();
-
-                    None
-                }
-            } else {
-                // the queue is empty
-                tq.syst.disable_interrupt();
-                None
-            }
-        });
-
-        if let Some((task, index)) = next {
-            f(task, index)
-        } else {
-            return;
-        }
-    }
-}