From 53c407017f50d0fde17d38afed714b2fcb54194b Mon Sep 17 00:00:00 2001 From: Emil Fresk Date: Sun, 14 Mar 2021 21:27:21 +0100 Subject: [PATCH] Cancel and reschedule working Support cfgs in the imports Account for extern tasks --- Cargo.toml | 1 + macros/src/codegen/module.rs | 65 +++- macros/src/codegen/pre_init.rs | 12 +- macros/src/codegen/timer_queue.rs | 11 +- src/lib.rs | 4 +- src/linked_list.rs | 599 ++++++++++++++++++++++++++++++ src/tq.rs | 59 ++- 7 files changed, 716 insertions(+), 35 deletions(-) create mode 100644 src/linked_list.rs diff --git a/Cargo.toml b/Cargo.toml index f0ca605178..b02df9f7c8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -58,6 +58,7 @@ rtic-monotonic = "0.1.0-alpha.0" rtic-core = "0.3.1" heapless = "0.6.1" bare-metal = "1.0.0" +generic-array = "*" [dependencies.dwt-systick-monotonic] version = "0.1.0-alpha.0" diff --git a/macros/src/codegen/module.rs b/macros/src/codegen/module.rs index fb028e0119..5a594c66b3 100644 --- a/macros/src/codegen/module.rs +++ b/macros/src/codegen/module.rs @@ -21,6 +21,33 @@ pub fn codegen( let app_name = &app.name; let app_path = quote! {crate::#app_name}; + let all_task_names: Vec<_> = app + .software_tasks + .iter() + .map(|(name, st)| { + if !st.is_extern { + let cfgs = &st.cfgs; + quote! { + #(#cfgs)* + #[allow(unused_imports)] + use #app_path::#name as #name; + } + } else { + quote!() + } + }) + .chain(app.hardware_tasks.iter().map(|(name, ht)| { + if !ht.is_extern { + quote! { + #[allow(unused_imports)] + use #app_path::#name as #name; + } + } else { + quote!() + } + })) + .collect(); + let mut lt = None; match ctxt { Context::Init => { @@ -202,6 +229,9 @@ pub fn codegen( // Spawn caller items.push(quote!( + + #(#all_task_names)* + #(#cfgs)* /// Spawns the task directly pub fn spawn(#(#args,)*) -> Result<(), #ty> { @@ -247,6 +277,7 @@ pub fn codegen( if monotonic.args.default { items.push(quote!(pub use #m::spawn_after;)); items.push(quote!(pub use #m::spawn_at;)); + items.push(quote!(pub use #m::SpawnHandle;)); } let (enable_interrupt, pend) = if &*m_isr.to_string() == "SysTick" { @@ -269,6 +300,11 @@ pub fn codegen( items.push(quote!( /// Holds methods related to this monotonic pub mod #m { + // #( + // #[allow(unused_imports)] + // use #app_path::#all_task_names as #all_task_names; + // )* + use super::*; #[allow(unused_imports)] use #app_path::#tq_marker; #[allow(unused_imports)] @@ -297,10 +333,19 @@ pub fn codegen( impl SpawnHandle { pub fn cancel(self) -> Result<#ty, ()> { - // TODO: Actually cancel... - // &mut #app_path::#tq; + rtic::export::interrupt::free(|_| unsafe { + let tq = &mut *#app_path::#tq.as_mut_ptr(); + if let Some((_task, index)) = tq.cancel_marker(self.marker) { + // Get the message + let msg = #app_path::#inputs.get_unchecked(usize::from(index)).as_ptr().read(); + // Return the index to the free queue + #app_path::#fq.split().0.enqueue_unchecked(index); - Err(()) + Ok(msg) + } else { + Err(()) + } + }) } #[inline] @@ -313,12 +358,14 @@ pub fn codegen( pub fn reschedule_at(self, instant: rtic::time::Instant<#app_path::#mono_type>) -> Result { - let _ = instant; + rtic::export::interrupt::free(|_| unsafe { + let marker = #tq_marker; + #tq_marker = #tq_marker.wrapping_add(1); - // TODO: Actually reschedule... - // &mut #app_path::#tq; + let tq = &mut *#app_path::#tq.as_mut_ptr(); - Err(()) + tq.update_marker(self.marker, marker, instant, || #pend).map(|_| SpawnHandle { marker }) + }) } } @@ -374,8 +421,10 @@ pub fn codegen( #tq_marker = #tq_marker.wrapping_add(1); + let tq = unsafe { &mut *#app_path::#tq.as_mut_ptr() }; + if let Some(mono) = #app_path::#m_ident.as_mut() { - #app_path::#tq.enqueue_unchecked( + tq.enqueue_unchecked( nr, || #enable_interrupt, || #pend, diff --git a/macros/src/codegen/pre_init.rs b/macros/src/codegen/pre_init.rs index d5105445dd..287f41a43e 100644 --- a/macros/src/codegen/pre_init.rs +++ b/macros/src/codegen/pre_init.rs @@ -77,12 +77,16 @@ pub fn codegen(app: &App, analysis: &Analysis, extra: &Extra) -> Vec Vec); + let tq_ty = + quote!(core::mem::MaybeUninit>); items.push(quote!( #[doc(hidden)] - static mut #tq: #tq_ty = rtic::export::TimerQueue( - rtic::export::BinaryHeap( - rtic::export::iBinaryHeap::new() - ) - ); + static mut #tq: #tq_ty = core::mem::MaybeUninit::uninit(); )); let mono = util::monotonic_ident(&monotonic_name); @@ -138,7 +135,7 @@ pub fn codegen(app: &App, analysis: &Analysis, _extra: &Extra) -> Vec Self { + LinkedIndex(value) + } + + #[inline] + const fn none() -> Self { + LinkedIndex(u16::MAX) + } + + #[inline] + const fn option(self) -> Option { + if self.0 == u16::MAX { + None + } else { + Some(self.0) + } + } +} + +/// A node in the linked list. +pub struct Node { + val: MaybeUninit, + next: LinkedIndex, +} + +/// Iterator for the linked list. +pub struct Iter<'a, T, Kind, N> +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + list: &'a LinkedList, + index: LinkedIndex, +} + +impl<'a, T, Kind, N> Iterator for Iter<'a, T, Kind, N> +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + type Item = &'a T; + + fn next(&mut self) -> Option { + let index = self.index.option()?; + + let node = self.list.node_at(index as usize); + self.index = node.next; + + Some(self.list.read_data_in_node_at(index as usize)) + } +} + +/// Comes from [`LinkedList::find_mut`]. +pub struct FindMut<'a, T, Kind, N> +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + list: &'a mut LinkedList, + is_head: bool, + prev_index: LinkedIndex, + index: LinkedIndex, + maybe_changed: bool, +} + +impl<'a, T, Kind, N> FindMut<'a, T, Kind, N> +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + fn pop_internal(&mut self) -> T { + if self.is_head { + // If it is the head element, we can do a normal pop + unsafe { self.list.pop_unchecked() } + } else { + // Somewhere in the list + + // Re-point the previous index + self.list.node_at_mut(self.prev_index.0 as usize).next = + self.list.node_at_mut(self.index.0 as usize).next; + + // Release the index into the free queue + self.list.node_at_mut(self.index.0 as usize).next = self.list.free; + self.list.free = self.index; + + self.list.extract_data_in_node_at(self.index.0 as usize) + } + } + + /// This will pop the element from the list. + /// + /// Complexity is O(1). + #[inline] + pub fn pop(mut self) -> T { + self.pop_internal() + } + + /// This will resort the element into the correct position in the list in needed. + /// Same as calling `drop`. + /// + /// Complexity is worst-case O(N). + #[inline] + pub fn finish(self) { + drop(self) + } +} + +impl Drop for FindMut<'_, T, Kind, N> +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + fn drop(&mut self) { + // Only resort the list if the element has changed + if self.maybe_changed { + let val = self.pop_internal(); + unsafe { self.list.push_unchecked(val) }; + } + } +} + +impl Deref for FindMut<'_, T, Kind, N> +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + type Target = T; + + fn deref(&self) -> &Self::Target { + self.list.read_data_in_node_at(self.index.0 as usize) + } +} + +impl DerefMut for FindMut<'_, T, Kind, N> +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + fn deref_mut(&mut self) -> &mut Self::Target { + self.maybe_changed = true; + self.list.read_mut_data_in_node_at(self.index.0 as usize) + } +} + +impl fmt::Debug for FindMut<'_, T, Kind, N> +where + T: PartialEq + PartialOrd + core::fmt::Debug, + Kind: kind::Kind, + N: ArrayLength>, +{ + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + f.debug_struct("FindMut") + .field("prev_index", &self.prev_index) + .field("index", &self.index) + .field( + "prev_value", + &self + .list + .read_data_in_node_at(self.prev_index.option().unwrap() as usize), + ) + .field( + "value", + &self + .list + .read_data_in_node_at(self.index.option().unwrap() as usize), + ) + .finish() + } +} + +/// The linked list. +pub struct LinkedList +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + list: MaybeUninit, N>>, + head: LinkedIndex, + free: LinkedIndex, + _kind: PhantomData, +} + +impl LinkedList +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + /// Internal helper to not do pointer arithmetic all over the place. + #[inline] + fn node_at(&self, index: usize) -> &Node { + // Safety: The entire `self.list` is initialized in `new`, which makes this safe. + unsafe { &*(self.list.as_ptr() as *const Node).add(index) } + } + + /// Internal helper to not do pointer arithmetic all over the place. + #[inline] + fn node_at_mut(&mut self, index: usize) -> &mut Node { + // Safety: The entire `self.list` is initialized in `new`, which makes this safe. + unsafe { &mut *(self.list.as_mut_ptr() as *mut Node).add(index) } + } + + /// Internal helper to not do pointer arithmetic all over the place. + #[inline] + fn write_data_in_node_at(&mut self, index: usize, data: T) { + unsafe { + self.node_at_mut(index).val.as_mut_ptr().write(data); + } + } + + /// Internal helper to not do pointer arithmetic all over the place. + #[inline] + fn read_data_in_node_at(&self, index: usize) -> &T { + unsafe { &*self.node_at(index).val.as_ptr() } + } + + /// Internal helper to not do pointer arithmetic all over the place. + #[inline] + fn read_mut_data_in_node_at(&mut self, index: usize) -> &mut T { + unsafe { &mut *self.node_at_mut(index).val.as_mut_ptr() } + } + + /// Internal helper to not do pointer arithmetic all over the place. + #[inline] + fn extract_data_in_node_at(&mut self, index: usize) -> T { + unsafe { self.node_at(index).val.as_ptr().read() } + } + + /// Internal helper to not do pointer arithmetic all over the place. + /// Safety: This can overwrite existing allocated nodes if used improperly, meaning their + /// `Drop` methods won't run. + #[inline] + unsafe fn write_node_at(&mut self, index: usize, node: Node) { + (self.list.as_mut_ptr() as *mut Node) + .add(index) + .write(node) + } + + /// Create a new linked list. + pub fn new() -> Self { + let mut list = LinkedList { + list: MaybeUninit::uninit(), + head: LinkedIndex::none(), + free: unsafe { LinkedIndex::new_unchecked(0) }, + _kind: PhantomData, + }; + + let len = N::U16; + let mut free = 0; + + // Initialize indexes + while free < len - 1 { + unsafe { + list.write_node_at( + free as usize, + Node { + val: MaybeUninit::uninit(), + next: LinkedIndex::new_unchecked(free + 1), + }, + ); + } + free += 1; + } + + // Initialize final index + unsafe { + list.write_node_at( + free as usize, + Node { + val: MaybeUninit::uninit(), + next: LinkedIndex::none(), + }, + ); + } + + list + } + + /// Push unchecked + /// + /// Complexity is O(N). + /// + /// # Safety + /// + /// Assumes that the list is not full. + pub unsafe fn push_unchecked(&mut self, value: T) { + let new = self.free.0; + // Store the data and update the next free spot + self.write_data_in_node_at(new as usize, value); + self.free = self.node_at(new as usize).next; + + if let Some(head) = self.head.option() { + // Check if we need to replace head + if self + .read_data_in_node_at(head as usize) + .partial_cmp(self.read_data_in_node_at(new as usize)) + != Kind::ordering() + { + self.node_at_mut(new as usize).next = self.head; + self.head = LinkedIndex::new_unchecked(new); + } else { + // It's not head, search the list for the correct placement + let mut current = head; + + while let Some(next) = self.node_at(current as usize).next.option() { + if self + .read_data_in_node_at(next as usize) + .partial_cmp(self.read_data_in_node_at(new as usize)) + != Kind::ordering() + { + break; + } + + current = next; + } + + self.node_at_mut(new as usize).next = self.node_at(current as usize).next; + self.node_at_mut(current as usize).next = LinkedIndex::new_unchecked(new); + } + } else { + self.node_at_mut(new as usize).next = self.head; + self.head = LinkedIndex::new_unchecked(new); + } + } + + /// Pushes an element to the linked list and sorts it into place. + /// + /// Complexity is O(N). + pub fn push(&mut self, value: T) -> Result<(), T> { + if !self.is_full() { + Ok(unsafe { self.push_unchecked(value) }) + } else { + Err(value) + } + } + + /// Get an iterator over the sorted list. + pub fn iter(&self) -> Iter<'_, T, Kind, N> { + Iter { + list: self, + index: self.head, + } + } + + /// Find an element in the list. + pub fn find_mut(&mut self, mut f: F) -> Option> + where + F: FnMut(&T) -> bool, + { + let head = self.head.option()?; + + // Special-case, first element + if f(self.read_data_in_node_at(head as usize)) { + return Some(FindMut { + is_head: true, + prev_index: LinkedIndex::none(), + index: self.head, + list: self, + maybe_changed: false, + }); + } + + let mut current = head; + + while let Some(next) = self.node_at(current as usize).next.option() { + if f(self.read_data_in_node_at(next as usize)) { + return Some(FindMut { + is_head: false, + prev_index: unsafe { LinkedIndex::new_unchecked(current) }, + index: unsafe { LinkedIndex::new_unchecked(next) }, + list: self, + maybe_changed: false, + }); + } + + current = next; + } + + None + } + + /// Peek at the first element. + pub fn peek(&self) -> Option<&T> { + self.head + .option() + .map(|head| self.read_data_in_node_at(head as usize)) + } + + /// Pop unchecked + /// + /// # Safety + /// + /// Assumes that the list is not empty. + pub unsafe fn pop_unchecked(&mut self) -> T { + let head = self.head.0; + let current = head; + self.head = self.node_at(head as usize).next; + self.node_at_mut(current as usize).next = self.free; + self.free = LinkedIndex::new_unchecked(current); + + self.extract_data_in_node_at(current as usize) + } + + /// Pops the first element in the list. + /// + /// Complexity is O(1). + pub fn pop(&mut self) -> Result { + if !self.is_empty() { + Ok(unsafe { self.pop_unchecked() }) + } else { + Err(()) + } + } + + /// Checks if the linked list is full. + #[inline] + pub fn is_full(&self) -> bool { + self.free.option().is_none() + } + + /// Checks if the linked list is empty. + #[inline] + pub fn is_empty(&self) -> bool { + self.head.option().is_none() + } +} + +impl Drop for LinkedList +where + T: PartialEq + PartialOrd, + Kind: kind::Kind, + N: ArrayLength>, +{ + fn drop(&mut self) { + let mut index = self.head; + + while let Some(i) = index.option() { + let node = self.node_at_mut(i as usize); + index = node.next; + + unsafe { + ptr::drop_in_place(node.val.as_mut_ptr()); + } + } + } +} + +impl fmt::Debug for LinkedList +where + T: PartialEq + PartialOrd + core::fmt::Debug, + Kind: kind::Kind, + N: ArrayLength>, +{ + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + f.debug_list().entries(self.iter()).finish() + } +} + +/// Min sorted linked list. +pub struct Min; + +/// Max sorted linked list. +pub struct Max; + +/// Sealed traits and implementations for `linked_list` +pub mod kind { + use super::{Max, Min}; + use core::cmp::Ordering; + + /// The linked list kind: min first or max first + pub unsafe trait Kind { + #[doc(hidden)] + fn ordering() -> Option; + } + + unsafe impl Kind for Min { + #[inline] + fn ordering() -> Option { + Some(Ordering::Less) + } + } + + unsafe impl Kind for Max { + #[inline] + fn ordering() -> Option { + Some(Ordering::Greater) + } + } +} + +#[cfg(test)] +mod tests { + // Note this useful idiom: importing names from outer (for mod tests) scope. + use super::*; + use generic_array::typenum::consts::*; + + #[test] + fn test_peek() { + let mut ll: LinkedList = LinkedList::new(); + + ll.push(1).unwrap(); + assert_eq!(ll.peek().unwrap(), &1); + + ll.push(2).unwrap(); + assert_eq!(ll.peek().unwrap(), &2); + + ll.push(3).unwrap(); + assert_eq!(ll.peek().unwrap(), &3); + + let mut ll: LinkedList = LinkedList::new(); + + ll.push(2).unwrap(); + assert_eq!(ll.peek().unwrap(), &2); + + ll.push(1).unwrap(); + assert_eq!(ll.peek().unwrap(), &1); + + ll.push(3).unwrap(); + assert_eq!(ll.peek().unwrap(), &1); + } + + #[test] + fn test_full() { + let mut ll: LinkedList = LinkedList::new(); + ll.push(1).unwrap(); + ll.push(2).unwrap(); + ll.push(3).unwrap(); + + assert!(ll.is_full()) + } + + #[test] + fn test_empty() { + let ll: LinkedList = LinkedList::new(); + + assert!(ll.is_empty()) + } + + #[test] + fn test_rejected_push() { + let mut ll: LinkedList = LinkedList::new(); + ll.push(1).unwrap(); + ll.push(2).unwrap(); + ll.push(3).unwrap(); + + // This won't fit + let r = ll.push(4); + + assert_eq!(r, Err(4)); + } + + #[test] + fn test_updating() { + let mut ll: LinkedList = LinkedList::new(); + ll.push(1).unwrap(); + ll.push(2).unwrap(); + ll.push(3).unwrap(); + + let mut find = ll.find_mut(|v| *v == 2).unwrap(); + + *find += 1000; + find.finish(); + + assert_eq!(ll.peek().unwrap(), &1002); + + let mut find = ll.find_mut(|v| *v == 3).unwrap(); + + *find += 1000; + find.finish(); + + assert_eq!(ll.peek().unwrap(), &1003); + + // Remove largest element + ll.find_mut(|v| *v == 1003).unwrap().pop(); + + assert_eq!(ll.peek().unwrap(), &1002); + } +} diff --git a/src/tq.rs b/src/tq.rs index f341d8cc3e..3864025924 100644 --- a/src/tq.rs +++ b/src/tq.rs @@ -1,11 +1,11 @@ use crate::{ + linked_list::{ArrayLength, LinkedList, Min, Node}, time::{Clock, Instant}, Monotonic, }; use core::cmp::Ordering; -use heapless::{binary_heap::Min, ArrayLength, BinaryHeap}; -#[inline] +#[inline(always)] fn unwrapper(val: Result) -> T { if let Ok(v) = val { v @@ -14,18 +14,22 @@ fn unwrapper(val: Result) -> T { } } -pub struct TimerQueue(pub BinaryHeap, N, Min>) +pub struct TimerQueue(pub LinkedList, Min, N>) where Mono: Monotonic, - N: ArrayLength>, + N: ArrayLength>>, Task: Copy; impl TimerQueue where Mono: Monotonic, - N: ArrayLength>, + N: ArrayLength>>, Task: Copy, { + pub fn new() -> Self { + TimerQueue(LinkedList::new()) + } + /// # Safety /// /// Writing to memory with a transmute in order to enable @@ -43,26 +47,20 @@ 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 - }) + .map(|head| nr.instant < head.instant) .unwrap_or(true); + if if_heap_max_greater_than_nr { - if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && is_empty { - // mem::transmute::<_, SYST>(()).enable_interrupt();A + if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && self.0.is_empty() { mono.enable_timer(); enable_interrupt(); } - // Set SysTick pending - // SCB::set_pendst(); pend_handler(); } @@ -75,8 +73,39 @@ where self.0.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) { + let nr = val.pop(); + + Some((nr.task, nr.index)) + } else { + None + } + } + + /// Update the instant at an marker value to a new instant + pub fn update_marker( + &mut self, + marker: u32, + new_marker: u32, + instant: Instant, + pend_handler: F, + ) -> Result<(), ()> { + if let Some(mut val) = self.0.find_mut(|nr| nr.marker == marker) { + val.instant = instant; + val.marker = new_marker; + + // On update pend the handler to reconfigure the next compare match + pend_handler(); + + Ok(()) + } else { + Err(()) + } + } + /// Dequeue a task from the TimerQueue - #[inline] pub fn dequeue(&mut self, disable_interrupt: F, mono: &mut Mono) -> Option<(Task, u8)> where F: FnOnce(),