Monotonic codegen now passing compile stage

8 files changed, 190 insertions, 220 deletions

diff --git a/macros/src/codegen/dispatchers.rs b/macros/src/codegen/dispatchers.rs
index 01fb511..d3adee0 100644
--- a/macros/src/codegen/dispatchers.rs
+++ b/macros/src/codegen/dispatchers.rs
@@ -70,22 +70,6 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
                 let inputs = util::inputs_ident(name);
                 let (_, tupled, pats, _) = util::regroup_inputs(&task.inputs);
 
-                // TODO: Fix for new monotonics
-                // let (let_instant, instant) = if extra.monotonic.is_some() {
-                //     let instants = util::instants_ident(name);
-
-                //     (
-                //         quote!(
-                //             let instant =
-                //                 #instants.get_unchecked(usize::from(index)).as_ptr().read();
-                //         ),
-                //         quote!(, instant),
-                //     )
-                // } else {
-                //     (quote!(), quote!())
-                // };
-                let (let_instant, instant) = (quote!(), quote!());
-
                 let locals_new = if task.locals.is_empty() {
                     quote!()
                 } else {
@@ -99,12 +83,11 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
                     #t::#name => {
                         let #tupled =
                             #inputs.get_unchecked(usize::from(index)).as_ptr().read();
-                        #let_instant
                         #fq.split().0.enqueue_unchecked(index);
                         let priority = &rtic::export::Priority::new(PRIORITY);
                         #app_path::#name(
                             #locals_new
-                            #name::Context::new(priority #instant)
+                            #name::Context::new(priority)
                             #(,#pats)*
                         )
                     }
diff --git a/macros/src/codegen/module.rs b/macros/src/codegen/module.rs
index 6dd6e9a..0f495d9 100644
--- a/macros/src/codegen/module.rs
+++ b/macros/src/codegen/module.rs
@@ -18,6 +18,8 @@ pub fn codegen(
     let mut task_cfgs = vec![];
 
     let name = ctxt.ident(app);
+    let app_name = &app.name;
+    let app_path = quote! {crate::#app_name};
 
     let mut lt = None;
     match ctxt {
@@ -125,7 +127,7 @@ pub fn codegen(
             .iter()
             .map(|(_, monotonic)| {
                 let mono = &monotonic.ident;
-                quote! {#mono}
+                quote! {#app_path::#mono}
             })
             .collect();
 
@@ -190,9 +192,6 @@ pub fn codegen(
         let rq = util::rq_ident(priority);
         let inputs = util::inputs_ident(name);
 
-        let app_name = &app.name;
-        let app_path = quote! {crate::#app_name};
-
         let device = &extra.device;
         let enum_ = util::interrupt_ident();
         let interrupt = &analysis
@@ -234,11 +233,13 @@ pub fn codegen(
 
         // Schedule caller
         for (_, monotonic) in &app.monotonics {
-            let instants = util::instants_ident(name);
+            let instants = util::monotonic_instants_ident(name, &monotonic.ident);
 
             let tq = util::tq_ident(&monotonic.ident.to_string());
             let t = util::schedule_t_ident();
             let m = &monotonic.ident;
+            let m_isr = &monotonic.args.binds;
+            let enum_ = util::interrupt_ident();
 
             if monotonic.args.default {
                 items.push(quote!(pub use #m::spawn_after;));
@@ -259,7 +260,7 @@ pub fn codegen(
 
                 #(#cfgs)*
                 pub fn spawn_at(
-                    instant: Instant<#app_path::#m as rtic::Monotonic>
+                    instant: rtic::Instant<#app_path::#m>
                     #(,#args)*
                 ) -> Result<(), #ty> {
                     unsafe {
@@ -284,9 +285,11 @@ pub fn codegen(
                                 task: #app_path::#t::#name,
                             };
 
-                            rtic::export::interrupt::free(|_| #app_path::#tq.enqueue_unchecked(nr));
-
-                            // TODO: After adding the scheduled task, check and setup the timer.
+                            rtic::export::interrupt::free(|_| #app_path::#tq.enqueue_unchecked(
+                                    nr,
+                                    || rtic::export::NVIC::unmask(#app_path::you_must_enable_the_rt_feature_for_the_pac_in_your_cargo_toml::#enum_::#m_isr),
+                                    || rtic::pend(#app_path::you_must_enable_the_rt_feature_for_the_pac_in_your_cargo_toml::#enum_::#m_isr),
+                            ));
 
                             Ok(())
                         } else {
diff --git a/macros/src/codegen/software_tasks.rs b/macros/src/codegen/software_tasks.rs
index 7b884ea..ebe7bdf 100644
--- a/macros/src/codegen/software_tasks.rs
+++ b/macros/src/codegen/software_tasks.rs
@@ -57,19 +57,22 @@ pub fn codegen(
             .map(|_| quote!(core::mem::MaybeUninit::uninit()))
             .collect::<Vec<_>>();
 
-        // TODO: Update for new monotonic
-        // if let Some(m) = &extra.monotonic {
-        //     let instants = util::instants_ident(name);
-
-        //     let uninit = mk_uninit();
-        //     mod_app.push(quote!(
-        //         #uninit
-        //         /// Buffer that holds the instants associated to the inputs of a task
-        //         static mut #instants:
-        //             [core::mem::MaybeUninit<<#m as rtic::Monotonic>::Instant>; #cap_lit] =
-        //             [#(#elems,)*];
-        //     ));
-        // }
+        let app_name = &app.name;
+        let app_path = quote! {crate::#app_name};
+
+        for (_, monotonic) in &app.monotonics {
+            let instants = util::monotonic_instants_ident(name, &monotonic.ident);
+            let m = &monotonic.ident;
+
+            let uninit = mk_uninit();
+            mod_app.push(quote!(
+                #uninit
+                /// Buffer that holds the instants associated to the inputs of a task
+                static mut #instants:
+                    [core::mem::MaybeUninit<rtic::Instant<#app_path::#m>>; #cap_lit] =
+                    [#(#elems,)*];
+            ));
+        }
 
         let uninit = mk_uninit();
         let inputs_ident = util::inputs_ident(name);
diff --git a/macros/src/codegen/timer_queue.rs b/macros/src/codegen/timer_queue.rs
index f219eef..dc29835 100644
--- a/macros/src/codegen/timer_queue.rs
+++ b/macros/src/codegen/timer_queue.rs
@@ -67,6 +67,8 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
 
         // Timer queue handler
         {
+            let enum_ = util::interrupt_ident();
+
             let arms = app
                 .software_tasks
                 .iter()
@@ -75,7 +77,6 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
                     let priority = task.args.priority;
                     let rq = util::rq_ident(priority);
                     let rqt = util::spawn_t_ident(priority);
-                    let enum_ = util::interrupt_ident();
 
                     // The interrupt that runs the task dispatcher
                     let interrupt = &analysis.interrupts.get(&priority).expect("RTIC-ICE: interrupt not found").0;
@@ -98,12 +99,15 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec<TokenStrea
                 .collect::<Vec<_>>();
 
             let bound_interrupt = &monotonic.args.binds;
+
             items.push(quote!(
                 #[no_mangle]
                 unsafe fn #bound_interrupt() {
                     use rtic::Mutex as _;
 
-                    while let Some((task, index)) = rtic::export::interrupt::free(|_| #tq.dequeue())
+                    while let Some((task, index)) = rtic::export::interrupt::free(|_| #tq.dequeue(
+                                || rtic::export::NVIC::unmask(you_must_enable_the_rt_feature_for_the_pac_in_your_cargo_toml::#enum_::#bound_interrupt),
+                            ))
                     {
                         match task {
                             #(#arms)*
diff --git a/macros/src/codegen/util.rs b/macros/src/codegen/util.rs
index d9310dd..329a7dd 100644
--- a/macros/src/codegen/util.rs
+++ b/macros/src/codegen/util.rs
@@ -77,8 +77,8 @@ pub fn inputs_ident(task: &Ident) -> Ident {
 }
 
 /// Generates an identifier for the `INSTANTS` buffer (`schedule` API)
-pub fn instants_ident(task: &Ident) -> Ident {
-    Ident::new(&format!("{}_INSTANTS", task), Span::call_site())
+pub fn monotonic_instants_ident(task: &Ident, monotonic: &Ident) -> Ident {
+    Ident::new(&format!("{}_{}_INSTANTS", task, monotonic), Span::call_site())
 }
 
 pub fn interrupt_ident() -> Ident {
@@ -103,22 +103,6 @@ pub fn is_exception(name: &Ident) -> bool {
     )
 }
 
-/// Generates a pre-reexport identifier for the "late resources" struct
-pub fn late_resources_ident(init: &Ident) -> Ident {
-    Ident::new(
-        &format!("{}LateResources", init.to_string()),
-        Span::call_site(),
-    )
-}
-
-/// Generates a pre-reexport identifier for the "monotonics" struct
-pub fn monotonics_ident(init: &Ident) -> Ident {
-    Ident::new(
-        &format!("{}Monotonics", init.to_string()),
-        Span::call_site(),
-    )
-}
-
 /// Mangle an ident
 pub fn mangle_ident(ident: &Ident) -> Ident {
     Ident::new(
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))
+    }
+}