1 files changed, 70 insertions, 75 deletions

diff --git a/src/tq.rs b/src/tq.rs
index b2a84c8..4c89a66 100644
--- a/src/tq.rs
+++ b/src/tq.rs
@@ -1,28 +1,18 @@
-use core::{
-    cmp::{self, Ordering},
-    convert::TryInto,
-    mem,
-    ops::Sub,
-};
-
-use cortex_m::peripheral::{SCB, SYST};
+use crate::{time::Instant, Monotonic};
+use core::cmp::Ordering;
 use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
 
-use crate::Monotonic;
-
-pub struct TimerQueue<M, T, N>(pub BinaryHeap<NotReady<M, T>, N, Min>)
+pub struct TimerQueue<Mono, Task, N>(pub BinaryHeap<NotReady<Mono, Task>, N, Min>)
 where
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
-    N: ArrayLength<NotReady<M, T>>,
-    T: Copy;
+    Mono: Monotonic,
+    N: ArrayLength<NotReady<Mono, Task>>,
+    Task: Copy;
 
-impl<M, T, N> TimerQueue<M, T, N>
+impl<Mono, Task, N> TimerQueue<Mono, Task, N>
 where
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
-    N: ArrayLength<NotReady<M, T>>,
-    T: Copy,
+    Mono: Monotonic,
+    N: ArrayLength<NotReady<Mono, Task>>,
+    Task: Copy,
 {
     /// # Safety
     ///
@@ -31,7 +21,15 @@ where
     ///
     /// Enqueue a task without checking if it is full
     #[inline]
-    pub unsafe fn enqueue_unchecked(&mut self, nr: NotReady<M, T>) {
+    pub unsafe fn enqueue_unchecked<F1, F2>(
+        &mut self,
+        nr: NotReady<Mono, Task>,
+        enable_interrupt: F1,
+        pend_handler: F2,
+    ) where
+        F1: FnOnce(),
+        F2: FnOnce(),
+    {
         let mut is_empty = true;
         // Check if the top contains a non-empty element and if that element is
         // greater than nr
@@ -45,57 +43,59 @@ where
             .unwrap_or(true);
         if if_heap_max_greater_than_nr {
             if is_empty {
-                mem::transmute::<_, SYST>(()).enable_interrupt();
+                // mem::transmute::<_, SYST>(()).enable_interrupt();
+                enable_interrupt();
             }
 
             // Set SysTick pending
-            SCB::set_pendst();
+            // SCB::set_pendst();
+            pend_handler();
         }
 
         self.0.push_unchecked(nr);
     }
 
+    /// Check if the timer queue is empty.
+    #[inline]
+    pub fn is_empty(&self) -> bool {
+        self.0.is_empty()
+    }
+
     /// Dequeue a task from the TimerQueue
     #[inline]
-    pub fn dequeue(&mut self) -> Option<(T, u8)> {
+    pub fn dequeue<F>(&mut self, disable_interrupt: F) -> Option<(Task, u8)>
+    where
+        F: FnOnce(),
+    {
         unsafe {
-            if let Some(instant) = self.0.peek().map(|p| p.instant) {
-                let now = M::now();
+            Mono::clear_compare();
 
-                if instant < now {
+            if let Some(instant) = self.0.peek().map(|p| p.instant) {
+                if instant < Mono::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
+                    // TODO: Fix this hack...
+                    // Extract the compare time.
+                    Mono::set_compare(*instant.duration_since_epoch().integer());
+
+                    // Double check that the instant we set is really in the future, else
+                    // dequeue. If the monotonic is fast enough it can happen that from the
+                    // read of now to the set of the compare, the time can overflow. This is to
+                    // guard against this.
+                    if instant < Mono::now() {
+                        let nr = self.0.pop_unchecked();
+
+                        Some((nr.task, nr.index))
+                    } else {
+                        None
+                    }
                 }
             } else {
-                // The queue is empty
-                mem::transmute::<_, SYST>(()).disable_interrupt();
+                // The queue is empty, disable the interrupt.
+                disable_interrupt();
 
                 None
             }
@@ -103,52 +103,47 @@ where
     }
 }
 
-pub struct NotReady<M, T>
+pub struct NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
     pub index: u8,
-    pub instant: M::Instant,
-    pub task: T,
+    pub instant: Instant<Mono>,
+    pub task: Task,
 }
 
-impl<M, T> Eq for NotReady<M, T>
+impl<Mono, Task> Eq for NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
 }
 
-impl<M, T> Ord for NotReady<M, T>
+impl<Mono, Task> Ord for NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
     fn cmp(&self, other: &Self) -> Ordering {
         self.instant.cmp(&other.instant)
     }
 }
 
-impl<M, T> PartialEq for NotReady<M, T>
+impl<Mono, Task> PartialEq for NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
     fn eq(&self, other: &Self) -> bool {
         self.instant == other.instant
     }
 }
 
-impl<M, T> PartialOrd for NotReady<M, T>
+impl<Mono, Task> PartialOrd for NotReady<Mono, Task>
 where
-    T: Copy,
-    M: Monotonic,
-    <M::Instant as Sub>::Output: TryInto<u32>,
+    Task: Copy,
+    Mono: Monotonic,
 {
     fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
         Some(self.cmp(&other))