diff options
Diffstat (limited to 'src/tq.rs')
| -rw-r--r-- | src/tq.rs | 169 |
1 files changed, 89 insertions, 80 deletions
@@ -1,28 +1,21 @@ -use core::{ - cmp::{self, Ordering}, - convert::TryInto, - mem, - ops::Sub, +use crate::{ + time::{Clock, Instant}, + Monotonic, }; - -use cortex_m::peripheral::{SCB, SYST}; +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 +24,16 @@ 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, + mono: &mut Mono, + ) 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 @@ -44,111 +46,118 @@ where }) .unwrap_or(true); if if_heap_max_greater_than_nr { - if is_empty { - mem::transmute::<_, SYST>(()).enable_interrupt(); + if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && is_empty { + // mem::transmute::<_, SYST>(()).enable_interrupt();A + mono.enable_timer(); + 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() + } + + #[inline] + fn unwrapper<T, E>(val: Result<T, E>) -> T { + if let Ok(v) = val { + v + } else { + unreachable!("Your monotonic is not infallible") + } + } + /// 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(); + pub fn dequeue<F>(&mut self, disable_interrupt: F, mono: &mut Mono) -> Option<(Task, u8)> + where + F: FnOnce(), + { + mono.clear_compare_flag(); + + if let Some(instant) = self.0.peek().map(|p| p.instant) { + if instant <= Self::unwrapper(Clock::try_now(mono)) { + // task became ready + let nr = unsafe { self.0.pop_unchecked() }; + + Some((nr.task, nr.index)) + } else { + // Set compare + mono.set_compare(&instant); - if instant < now { - // task became ready - let nr = self.0.pop_unchecked(); + // 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 <= Self::unwrapper(Clock::try_now(mono)) { + let nr = unsafe { 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 } + } else { + // The queue is empty, disable the interrupt. + if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE { + disable_interrupt(); + mono.disable_timer(); + } + + None } } } -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)) |