Monotonic codegen now passing compile stage

3 files changed, 153 insertions, 160 deletions

diff --git a/src/export.rs b/src/export.rs
index 080b1f6..ab5984e 100644
--- a/src/export.rs
+++ b/src/export.rs
@@ -3,7 +3,7 @@ use core::{
     sync::atomic::{AtomicBool, Ordering},
 };
 
-// pub use crate::tq::{NotReady, TimerQueue};
+pub use crate::tq::{NotReady, TimerQueue};
 pub use bare_metal::CriticalSection;
 #[cfg(armv7m)]
 pub use cortex_m::register::basepri;
diff --git a/src/lib.rs b/src/lib.rs
index ade1267..c85090f 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -35,12 +35,10 @@
 // #![deny(warnings)]
 #![no_std]
 
-use core::ops::Sub;
-
 use cortex_m::{interrupt::Nr, peripheral::NVIC};
 pub use cortex_m_rtic_macros::app;
 pub use rtic_core::{
-    monotonic::{Clock, Instant, Monotonic},
+    monotonic::{self, Clock, Duration, Instant, Monotonic},
     prelude as mutex_prelude, Exclusive, Mutex,
 };
 
diff --git a/src/tq.rs b/src/tq.rs
index f2539a9..2bfb651 100644
--- a/src/tq.rs
+++ b/src/tq.rs
@@ -1,156 +1,151 @@
-// 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::Monotonic;
-//
-// pub struct TimerQueue<M, T, N>(pub BinaryHeap<NotReady<M, T>, N, Min>)
-// where
-//     M: Monotonic,
-//     <M::Instant as Sub>::Output: TryInto<u32>,
-//     N: ArrayLength<NotReady<M, T>>,
-//     T: Copy;
-//
-// impl<M, T, N> TimerQueue<M, T, N>
-// where
-//     M: Monotonic,
-//     <M::Instant as Sub>::Output: TryInto<u32>,
-//     N: ArrayLength<NotReady<M, T>>,
-//     T: Copy,
-// {
-//     /// # Safety
-//     ///
-//     /// Writing to memory with a transmute in order to enable
-//     /// interrupts of the SysTick timer
-//     ///
-//     /// Enqueue a task without checking if it is full
-//     #[inline]
-//     pub unsafe fn enqueue_unchecked(&mut self, nr: NotReady<M, T>) {
-//         let mut is_empty = true;
-//         // Check if the top contains a non-empty element and if that element is
-//         // greater than nr
-//         let if_heap_max_greater_than_nr = self
-//             .0
-//             .peek()
-//             .map(|head| {
-//                 is_empty = false;
-//                 nr.instant < head.instant
-//             })
-//             .unwrap_or(true);
-//         if if_heap_max_greater_than_nr {
-//             if is_empty {
-//                 mem::transmute::<_, SYST>(()).enable_interrupt();
-//             }
-//
-//             // Set SysTick pending
-//             SCB::set_pendst();
-//         }
-//
-//         self.0.push_unchecked(nr);
-//     }
-//
-//     /// Dequeue a task from the TimerQueue
-//     #[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<M, T>
-// where
-//     T: Copy,
-//     M: Monotonic,
-//     <M::Instant as Sub>::Output: TryInto<u32>,
-// {
-//     pub index: u8,
-//     pub instant: M::Instant,
-//     pub task: T,
-// }
-//
-// impl<M, T> Eq for NotReady<M, T>
-// where
-//     T: Copy,
-//     M: Monotonic,
-//     <M::Instant as Sub>::Output: TryInto<u32>,
-// {
-// }
-//
-// 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<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<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))
-//     }
-// }
+use crate::{Instant, Monotonic};
+use core::cmp::Ordering;
+use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
+
+pub struct TimerQueue<M, T, N>(pub BinaryHeap<NotReady<M, T>, N, Min>)
+where
+    M: Monotonic,
+    N: ArrayLength<NotReady<M, T>>,
+    T: Copy;
+
+impl<M, T, N> TimerQueue<M, T, N>
+where
+    M: Monotonic,
+    N: ArrayLength<NotReady<M, T>>,
+    T: Copy,
+{
+    /// # Safety
+    ///
+    /// Writing to memory with a transmute in order to enable
+    /// interrupts of the SysTick timer
+    ///
+    /// Enqueue a task without checking if it is full
+    #[inline]
+    pub unsafe fn enqueue_unchecked<F1, F2>(
+        &mut self,
+        nr: NotReady<M, T>,
+        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
+        let if_heap_max_greater_than_nr = self
+            .0
+            .peek()
+            .map(|head| {
+                is_empty = false;
+                nr.instant < head.instant
+            })
+            .unwrap_or(true);
+        if if_heap_max_greater_than_nr {
+            if is_empty {
+                // mem::transmute::<_, SYST>(()).enable_interrupt();
+                enable_interrupt();
+            }
+
+            // Set SysTick pending
+            // 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<F>(&mut self, disable_interrupt: F) -> Option<(T, u8)>
+    where
+        F: FnOnce(),
+    {
+        unsafe {
+            M::clear_compare();
+
+            if let Some(instant) = self.0.peek().map(|p| p.instant) {
+                let now = M::now();
+
+                match instant.checked_duration_since(&now) {
+                    None => {
+                        // instant < now
+                        // task became ready
+                        let nr = self.0.pop_unchecked();
+
+                        Some((nr.task, nr.index))
+                    }
+                    Some(dur) => {
+                        // TODO: Fix this hack...
+                        let new_instant = *now.duration_since_epoch().integer() + *dur.integer();
+                        M::set_compare(new_instant);
+
+                        // Start counting down from the new reload
+                        // mem::transmute::<_, SYST>(()).clear_current();
+
+                        None
+                    }
+                }
+            } else {
+                // The queue is empty
+                // mem::transmute::<_, SYST>(()).disable_interrupt();
+                disable_interrupt();
+
+                None
+            }
+        }
+    }
+}
+
+pub struct NotReady<M, T>
+where
+    T: Copy,
+    M: Monotonic,
+{
+    pub index: u8,
+    pub instant: Instant<M>,
+    pub task: T,
+}
+
+impl<M, T> Eq for NotReady<M, T>
+where
+    T: Copy,
+    M: Monotonic,
+{
+}
+
+impl<M, T> Ord for NotReady<M, T>
+where
+    T: Copy,
+    M: Monotonic,
+{
+    fn cmp(&self, other: &Self) -> Ordering {
+        self.instant.cmp(&other.instant)
+    }
+}
+
+impl<M, T> PartialEq for NotReady<M, T>
+where
+    T: Copy,
+    M: Monotonic,
+{
+    fn eq(&self, other: &Self) -> bool {
+        self.instant == other.instant
+    }
+}
+
+impl<M, T> PartialOrd for NotReady<M, T>
+where
+    T: Copy,
+    M: Monotonic,
+{
+    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
+        Some(self.cmp(&other))
+    }
+}