diff options
Diffstat (limited to 'src/tq.rs')
| -rw-r--r-- | src/tq.rs | 307 |
1 files changed, 151 insertions, 156 deletions
@@ -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)) + } +} |