From 582c602912592ec7ebea3096aefa02aea99c2143 Mon Sep 17 00:00:00 2001 From: Emil Fresk Date: Mon, 2 Jan 2023 14:34:05 +0100 Subject: Old xtask test pass --- src/tq.rs | 275 ++++++++++++++++++++++++++++++++++++++++++++++++-------------- 1 file changed, 212 insertions(+), 63 deletions(-) (limited to 'src/tq.rs') diff --git a/src/tq.rs b/src/tq.rs index 0f585ba..daa91c8 100644 --- a/src/tq.rs +++ b/src/tq.rs @@ -1,29 +1,28 @@ -use crate::Monotonic; +use crate::{ + sll::{IntrusiveSortedLinkedList, Min as IsslMin, Node as IntrusiveNode}, + Monotonic, +}; use core::cmp::Ordering; -use heapless::sorted_linked_list::{LinkedIndexU16, Min, SortedLinkedList}; +use core::task::Waker; +use heapless::sorted_linked_list::{LinkedIndexU16, Min as SllMin, SortedLinkedList}; -pub struct TimerQueue( - pub SortedLinkedList, LinkedIndexU16, Min, N>, -) +pub struct TimerQueue<'a, Mono, Task, const N_TASK: usize> where Mono: Monotonic, - Task: Copy; + Task: Copy, +{ + pub task_queue: SortedLinkedList, LinkedIndexU16, SllMin, N_TASK>, + pub waker_queue: IntrusiveSortedLinkedList<'a, WakerNotReady, IsslMin>, +} -impl TimerQueue +impl<'a, Mono, Task, const N_TASK: usize> TimerQueue<'a, Mono, Task, N_TASK> where - Mono: Monotonic, + Mono: Monotonic + 'a, Task: 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, + fn check_if_enable( + &self, + instant: Mono::Instant, enable_interrupt: F1, pend_handler: F2, mono: Option<&mut Mono>, @@ -33,11 +32,17 @@ where { // 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_or(true, |head| nr.instant < head.instant); + let if_task_heap_max_greater_than_nr = self + .task_queue + .peek() + .map_or(true, |head| instant < head.instant); + let if_waker_heap_max_greater_than_nr = self + .waker_queue + .peek() + .map_or(true, |head| instant < head.instant); - if if_heap_max_greater_than_nr { - if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && self.0.is_empty() { + if if_task_heap_max_greater_than_nr || if_waker_heap_max_greater_than_nr { + if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && self.is_empty() { if let Some(mono) = mono { mono.enable_timer(); } @@ -46,19 +51,49 @@ where pend_handler(); } + } - self.0.push_unchecked(nr); + /// Enqueue a task without checking if it is full + #[inline] + pub unsafe fn enqueue_task_unchecked( + &mut self, + nr: TaskNotReady, + enable_interrupt: F1, + pend_handler: F2, + mono: Option<&mut Mono>, + ) where + F1: FnOnce(), + F2: FnOnce(), + { + self.check_if_enable(nr.instant, enable_interrupt, pend_handler, mono); + self.task_queue.push_unchecked(nr); } - /// Check if the timer queue is empty. + /// Enqueue a waker + #[inline] + pub fn enqueue_waker( + &mut self, + nr: &'a mut IntrusiveNode>, + enable_interrupt: F1, + pend_handler: F2, + mono: Option<&mut Mono>, + ) where + F1: FnOnce(), + F2: FnOnce(), + { + self.check_if_enable(nr.val.instant, enable_interrupt, pend_handler, mono); + self.waker_queue.push(nr); + } + + /// Check if all the timer queue is empty. #[inline] pub fn is_empty(&self) -> bool { - self.0.is_empty() + self.task_queue.is_empty() && self.waker_queue.is_empty() } - /// Cancel the marker value - pub fn cancel_marker(&mut self, marker: u32) -> Option<(Task, u8)> { - if let Some(val) = self.0.find_mut(|nr| nr.marker == marker) { + /// Cancel the marker value for a task + pub fn cancel_task_marker(&mut self, marker: u32) -> Option<(Task, u8)> { + if let Some(val) = self.task_queue.find_mut(|nr| nr.marker == marker) { let nr = val.pop(); Some((nr.task, nr.index)) @@ -67,16 +102,23 @@ where } } - /// Update the instant at an marker value to a new instant + /// Cancel the marker value for a waker + pub fn cancel_waker_marker(&mut self, marker: u32) { + if let Some(val) = self.waker_queue.find_mut(|nr| nr.marker == marker) { + let _ = val.pop(); + } + } + + /// Update the instant at an marker value for a task to a new instant #[allow(clippy::result_unit_err)] - pub fn update_marker( + pub fn update_task_marker( &mut self, marker: u32, new_marker: u32, instant: Mono::Instant, pend_handler: F, ) -> Result<(), ()> { - if let Some(mut val) = self.0.find_mut(|nr| nr.marker == marker) { + if let Some(mut val) = self.task_queue.find_mut(|nr| nr.marker == marker) { val.instant = instant; val.marker = new_marker; @@ -89,6 +131,62 @@ where } } + fn dequeue_task_queue( + &mut self, + instant: Mono::Instant, + mono: &mut Mono, + ) -> Option<(Task, u8)> { + if instant <= mono.now() { + // task became ready + let nr = unsafe { self.task_queue.pop_unchecked() }; + Some((nr.task, nr.index)) + } else { + // Set compare + mono.set_compare(instant); + + // 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 = unsafe { self.task_queue.pop_unchecked() }; + Some((nr.task, nr.index)) + } else { + None + } + } + } + + fn dequeue_waker_queue(&mut self, instant: Mono::Instant, mono: &mut Mono) -> bool { + let mut did_wake = false; + + if instant <= mono.now() { + // Task became ready, wake the waker + if let Some(v) = self.waker_queue.pop() { + v.val.waker.wake_by_ref(); + + did_wake = true; + } + } else { + // Set compare + mono.set_compare(instant); + + // 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() { + if let Some(v) = self.waker_queue.pop() { + v.val.waker.wake_by_ref(); + + did_wake = true; + } + } + } + + did_wake + } + /// Dequeue a task from the ``TimerQueue`` pub fn dequeue(&mut self, disable_interrupt: F, mono: &mut Mono) -> Option<(Task, u8)> where @@ -96,59 +194,72 @@ where { mono.clear_compare_flag(); - if let Some(instant) = self.0.peek().map(|p| p.instant) { - if instant <= mono.now() { - // task became ready - let nr = unsafe { self.0.pop_unchecked() }; + loop { + let tq = self.task_queue.peek().map(|p| p.instant); + let wq = self.waker_queue.peek().map(|p| p.instant); - Some((nr.task, nr.index)) - } else { - // Set compare - mono.set_compare(instant); - - // 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 = unsafe { self.0.pop_unchecked() }; - - Some((nr.task, nr.index)) - } else { - None + let dequeue_task; + let instant; + + match (tq, wq) { + (Some(tq_instant), Some(wq_instant)) => { + if tq_instant <= wq_instant { + dequeue_task = true; + instant = tq_instant; + } else { + dequeue_task = false; + instant = wq_instant; + } + } + (Some(tq_instant), None) => { + dequeue_task = true; + instant = tq_instant; + } + (None, Some(wq_instant)) => { + dequeue_task = false; + instant = wq_instant; + } + (None, None) => { + // The queue is empty, disable the interrupt. + if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE { + disable_interrupt(); + mono.disable_timer(); + } + + return None; } - } - } else { - // The queue is empty, disable the interrupt. - if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE { - disable_interrupt(); - mono.disable_timer(); } - None + if dequeue_task { + return self.dequeue_task_queue(instant, mono); + } else if !self.dequeue_waker_queue(instant, mono) { + return None; + } else { + // Run the dequeue again + } } } } -pub struct NotReady +pub struct TaskNotReady where Task: Copy, Mono: Monotonic, { + pub task: Task, pub index: u8, pub instant: Mono::Instant, - pub task: Task, pub marker: u32, } -impl Eq for NotReady +impl Eq for TaskNotReady where Task: Copy, Mono: Monotonic, { } -impl Ord for NotReady +impl Ord for TaskNotReady where Task: Copy, Mono: Monotonic, @@ -158,7 +269,7 @@ where } } -impl PartialEq for NotReady +impl PartialEq for TaskNotReady where Task: Copy, Mono: Monotonic, @@ -168,7 +279,7 @@ where } } -impl PartialOrd for NotReady +impl PartialOrd for TaskNotReady where Task: Copy, Mono: Monotonic, @@ -177,3 +288,41 @@ where Some(self.cmp(other)) } } + +pub struct WakerNotReady +where + Mono: Monotonic, +{ + pub waker: Waker, + pub instant: Mono::Instant, + pub marker: u32, +} + +impl Eq for WakerNotReady where Mono: Monotonic {} + +impl Ord for WakerNotReady +where + Mono: Monotonic, +{ + fn cmp(&self, other: &Self) -> Ordering { + self.instant.cmp(&other.instant) + } +} + +impl PartialEq for WakerNotReady +where + Mono: Monotonic, +{ + fn eq(&self, other: &Self) -> bool { + self.instant == other.instant + } +} + +impl PartialOrd for WakerNotReady +where + Mono: Monotonic, +{ + fn partial_cmp(&self, other: &Self) -> Option { + Some(self.cmp(other)) + } +} -- cgit v1.2.3