reduce memory overhead

by storing indices (u8) in the queues instead of pointers (*mut u8)

in the binary heap we store the baseline inline along with the index and the task name. Before we
stored a pointer to the message and had to lookup the baseline when comparing two nodes in the heap.
This commit is contained in:
Jorge Aparicio 2018-05-04 10:59:23 +02:00
parent 6de27b9a64
commit 0cc456ba80
10 changed files with 227 additions and 233 deletions

View file

@ -52,7 +52,7 @@ required-features = ["timer-queue"]
[dependencies] [dependencies]
cortex-m = "0.4.0" cortex-m = "0.4.0"
cortex-m-rtfm-macros = { path = "macros", version = "0.3.1" } cortex-m-rtfm-macros = { path = "macros", version = "0.3.1" }
heapless = "0.3.5" heapless = "0.3.6"
typenum = "1.10.0" typenum = "1.10.0"
[target.'cfg(target_arch = "x86_64")'.dev-dependencies] [target.'cfg(target_arch = "x86_64")'.dev-dependencies]

View file

@ -1,3 +1,11 @@
// # Pointers (old)
//
// ~40~ 32 bytes .bss
//
// # Indices (new)
//
// 12 bytes .bss
#![deny(unsafe_code)] #![deny(unsafe_code)]
#![deny(warnings)] #![deny(warnings)]
#![feature(proc_macro)] #![feature(proc_macro)]

View file

@ -1,16 +1,36 @@
// 52 bytes .bss // # Pointers (old)
//
// ~52~ 48 bytes .bss
// //
// # -Os // # -Os
//
// init // init
// a(bl=8000000, now=8000180, input=0) // a(bl=8000000, now=8000180, input=0)
// a(bl=16000000, now=16000180, input=1) // a(bl=16000000, now=16000180, input=1)
// a(bl=24000000, now=24000180, input=2) // a(bl=24000000, now=24000180, input=2)
// //
// # -O3 // # -O3
//
// init // init
// a(bl=8000000, now=8000168, input=0) // a(bl=8000000, now=8000168, input=0)
// a(bl=16000000, now=16000168, input=1) // a(bl=16000000, now=16000168, input=1)
// a(bl=24000000, now=24000168, input=2) // a(bl=24000000, now=24000168, input=2)
//
// # Indices (new)
//
// 32 bytes .bss
//
// ## -O3
//
// init
// a(bl=8000000, now=8000170, input=0)
// a(bl=16000000, now=16000170, input=1)
//
// ## -Os
//
// init
// a(bl=8000000, now=8000179, input=0)
// a(bl=16000000, now=16000179, input=1)
#![deny(unsafe_code)] #![deny(unsafe_code)]
#![deny(warnings)] #![deny(warnings)]

View file

@ -1,6 +1,9 @@
// 104 bytes .bss // # Pointers (old)
//
// ~104~ 88 bytes .bss
//
// ## -Os
// //
// # -Os
// a(bl=16000000, now=16000248, input=0) // a(bl=16000000, now=16000248, input=0)
// b(bl=24000000, now=24000251, input=0) // b(bl=24000000, now=24000251, input=0)
// a(bl=32000000, now=32000248, input=1) // a(bl=32000000, now=32000248, input=1)
@ -11,8 +14,9 @@
// a(bl=80000000, now=80000248, input=4) // a(bl=80000000, now=80000248, input=4)
// b(bl=96000000, now=96000283, input=3) // b(bl=96000000, now=96000283, input=3)
// a(bl=96000000, now=96002427, input=5) // a(bl=96000000, now=96002427, input=5)
//
// # -O3 // ## -O3
//
// init // init
// a(bl=16000000, now=16000231, input=0) // a(bl=16000000, now=16000231, input=0)
// b(bl=24000000, now=24000230, input=0) // b(bl=24000000, now=24000230, input=0)
@ -24,6 +28,37 @@
// a(bl=80000000, now=80000231, input=4) // a(bl=80000000, now=80000231, input=4)
// b(bl=96000000, now=96000259, input=3) // b(bl=96000000, now=96000259, input=3)
// a(bl=96000000, now=96002397, input=5) // a(bl=96000000, now=96002397, input=5)
//
// # Indices (new)
//
// 56 bytes .bss
//
// ## -O3
//
// a(bl=16000000, now=16000215, input=0)
// b(bl=24000000, now=24000214, input=0)
// a(bl=32000000, now=32000215, input=1)
// b(bl=48000000, now=48000236, input=1)
// a(bl=48000000, now=48002281, input=2)
// a(bl=64000000, now=64000215, input=3)
// b(bl=72000000, now=72000214, input=2)
// a(bl=80000000, now=80000215, input=4)
// b(bl=96000000, now=96000236, input=3)
// a(bl=96000000, now=96002281, input=5)
//
// ## -Os
//
// init
// a(bl=16000000, now=16000257, input=0)
// b(bl=24000000, now=24000252, input=0)
// a(bl=32000000, now=32000257, input=1)
// b(bl=48000000, now=48000284, input=1)
// a(bl=48000000, now=48002326, input=2)
// a(bl=64000000, now=64000257, input=3)
// b(bl=72000000, now=72000252, input=2)
// a(bl=80000000, now=80000257, input=4)
// b(bl=96000000, now=96000284, input=3)
// a(bl=96000000, now=96002326, input=5)
#![deny(unsafe_code)] #![deny(unsafe_code)]
#![deny(warnings)] #![deny(warnings)]

View file

@ -1,6 +1,9 @@
// 96 bytes .bss // # Pointers (old)
//
// ~96~ 80 bytes .bss
// //
// # -Os // # -Os
//
// init // init
// a(bl=16000000, now=16000249) // a(bl=16000000, now=16000249)
// b(bl=24000000, now=24000248) // b(bl=24000000, now=24000248)
@ -12,8 +15,9 @@
// a(bl=80000000, now=80000249) // a(bl=80000000, now=80000249)
// b(bl=96000000, now=96000282) // b(bl=96000000, now=96000282)
// a(bl=96000000, now=96001731) // a(bl=96000000, now=96001731)
//
// # -O3 // # -O3
//
// init // init
// a(bl=16000000, now=16000228) // a(bl=16000000, now=16000228)
// b(bl=24000000, now=24000231) // b(bl=24000000, now=24000231)
@ -25,6 +29,38 @@
// a(bl=80000000, now=80000228) // a(bl=80000000, now=80000228)
// b(bl=96000000, now=96000257) // b(bl=96000000, now=96000257)
// a(bl=96000000, now=96001705) // a(bl=96000000, now=96001705)
//
// # Indices (new)
//
// 48 bytes .bss
//
// ## -O3
//
// init
// a(bl=16000000, now=16000213)
// b(bl=24000000, now=24000212)
// a(bl=32000000, now=32000213)
// b(bl=48000000, now=48000234)
// a(bl=48000000, now=48001650)
// a(bl=64000000, now=64000213)
// b(bl=72000000, now=72000212)
// a(bl=80000000, now=80000213)
// b(bl=96000000, now=96000234)
// a(bl=96000000, now=96001650)
//
// ## -Os
//
// init
// a(bl=16000000, now=16000253)
// b(bl=24000000, now=24000251)
// a(bl=32000000, now=32000253)
// b(bl=48000000, now=48000283)
// a(bl=48000000, now=48001681)
// a(bl=64000000, now=64000253)
// b(bl=72000000, now=72000251)
// a(bl=80000000, now=80000253)
// b(bl=96000000, now=96000283)
// a(bl=96000000, now=96001681)
#![deny(unsafe_code)] #![deny(unsafe_code)]
#![deny(warnings)] #![deny(warnings)]

View file

@ -1,13 +1,33 @@
// 52 bytes .bss // # Pointers (old)
//
// ~52~ 40 bytes .bss
//
// ## -Os
// //
// # -Os
// init // init
// a(bl=8000000, now=8000180) // a(bl=8000000, now=8000180)
// a(bl=16000000, now=16000180) // a(bl=16000000, now=16000180)
// //
// # -O3 // ## -O3
//
// a(bl=8000000, now=8000168) // a(bl=8000000, now=8000168)
// a(bl=16000000, now=16000168) // a(bl=16000000, now=16000168)
//
// # Indices (new)
//
// 28 bytes .bss
//
// ## -Os
//
// init
// a(bl=8000000, now=8000176)
// a(bl=16000000, now=16000176)
//
// ## -O3
//
// init
// a(bl=8000000, now=8000167)
// a(bl=16000000, now=16000167)
#![deny(unsafe_code)] #![deny(unsafe_code)]
#![deny(warnings)] #![deny(warnings)]

View file

@ -280,18 +280,19 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
}); });
} }
Either::Right(capacity) => { Either::Right(capacity) => {
let capacity = Ident::from(format!("U{}", capacity)); let ucapacity = Ident::from(format!("U{}", capacity));
let capacity = capacity as usize;
root.push(quote! { root.push(quote! {
#[allow(unsafe_code)] #[allow(unsafe_code)]
unsafe impl #hidden::#krate::Resource for #name::SQ { unsafe impl #hidden::#krate::Resource for #name::SQ {
const NVIC_PRIO_BITS: u8 = ::#device::NVIC_PRIO_BITS; const NVIC_PRIO_BITS: u8 = ::#device::NVIC_PRIO_BITS;
type Ceiling = #name::Ceiling; type Ceiling = #name::Ceiling;
type Data = #hidden::#krate::SlotQueue<#input, #hidden::#krate::#capacity>; type Data = #hidden::#krate::SlotQueue<#hidden::#krate::#ucapacity>;
unsafe fn get() -> &'static mut Self::Data { unsafe fn get() -> &'static mut Self::Data {
static mut SQ: static mut SQ:
#hidden::#krate::SlotQueue<#input, #hidden::#krate::#capacity> = #hidden::#krate::SlotQueue<#hidden::#krate::#ucapacity> =
#hidden::#krate::SlotQueue::u8(); #hidden::#krate::SlotQueue::u8();
&mut SQ &mut SQ
@ -306,6 +307,10 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
.unwrap_or(0) .unwrap_or(0)
)); ));
mod_.push(quote! { mod_.push(quote! {
#[allow(unsafe_code)]
pub static mut BUFFER: [#krate::Node<#input>; #capacity] =
unsafe { #krate::uninitialized() };
pub struct SQ { _0: () } pub struct SQ { _0: () }
#[allow(unsafe_code)] #[allow(unsafe_code)]
@ -372,13 +377,15 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
{ {
unsafe { unsafe {
let slot = ::#name::SQ::new().claim_mut(t, |sq, _| sq.dequeue()); let slot = ::#name::SQ::new().claim_mut(t, |sq, _| sq.dequeue());
if let Some(slot) = slot { if let Some(index) = slot {
let tp = slot let task = ::#__priority::Task::#name;
.write(self.baseline, payload) core::ptr::write(
.tag(::#__priority::Task::#name); ::#name::BUFFER.get_unchecked_mut(index as usize),
#krate::Node { baseline: self.baseline, payload }
);
::#__priority::Q::new().claim_mut(t, |q, _| { ::#__priority::Q::new().claim_mut(t, |q, _| {
q.split().0.enqueue_unchecked(tp); q.split().0.enqueue_unchecked((task, index));
}); });
#krate::set_pending(#device::Interrupt::#interrupt); #krate::set_pending(#device::Interrupt::#interrupt);
@ -417,14 +424,16 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
#krate::Maximum<P, #krate::#qc>: #krate::Unsigned, #krate::Maximum<P, #krate::#qc>: #krate::Unsigned,
{ {
unsafe { unsafe {
if let Some(slot) = if let Some(index) =
::#name::SQ::new().claim_mut(t, |sq, _| sq.dequeue()) { ::#name::SQ::new().claim_mut(t, |sq, _| sq.dequeue()) {
let tp = slot let task = ::#__priority::Task::#name;
.write(payload) core::ptr::write(
.tag(::#__priority::Task::#name); ::#name::BUFFER.get_unchecked_mut(index as usize),
#krate::Node { payload }
);
::#__priority::Q::new().claim_mut(t, |q, _| { ::#__priority::Q::new().claim_mut(t, |q, _| {
q.split().0.enqueue_unchecked(tp); q.split().0.enqueue_unchecked((task, index));
}); });
#krate::set_pending(#device::Interrupt::#interrupt); #krate::set_pending(#device::Interrupt::#interrupt);
@ -487,14 +496,21 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
#krate::Maximum<P, #krate::#tqc>: #krate::Unsigned, #krate::Maximum<P, #krate::#tqc>: #krate::Unsigned,
{ {
unsafe { unsafe {
if let Some(slot) = if let Some(index) =
::#name::SQ::new().claim_mut(t, |sq, _| sq.dequeue()) { ::#name::SQ::new().claim_mut(t, |sq, _| sq.dequeue()) {
let bl = self.baseline + after; let bl = self.baseline + after;
let tp = slot let task = ::__tq::Task::#name;
.write(bl, payload) core::ptr::write(
.tag(::__tq::Task::#name); ::#name::BUFFER.get_unchecked_mut(index as usize),
#krate::Node { baseline: bl, payload },
);
let m = #krate::Message {
baseline: bl,
index,
task,
};
::__tq::TQ::new().claim_mut(t, |tq, _| tq.enqueue(bl, tp)); ::__tq::TQ::new().claim_mut(t, |tq, _| tq.enqueue(m));
Ok(()) Ok(())
} else { } else {
@ -531,7 +547,7 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
quote! { quote! {
__tq::Task::#name => { __tq::Task::#name => {
#__priority::Q::new().claim_mut(t, |q, _| { #__priority::Q::new().claim_mut(t, |q, _| {
q.split().0.enqueue_unchecked(tp.retag(#__priority::Task::#name)) q.split().0.enqueue_unchecked((#__priority::Task::#name, index))
}); });
#hidden::#krate::set_pending(#device::Interrupt::#interrupt); #hidden::#krate::set_pending(#device::Interrupt::#interrupt);
} }
@ -585,8 +601,8 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
#hidden::#krate::dispatch( #hidden::#krate::dispatch(
&mut #hidden::#krate::Threshold::<__tq::Priority>::new(), &mut #hidden::#krate::Threshold::<__tq::Priority>::new(),
&mut __tq::TQ::new(), &mut __tq::TQ::new(),
|t, tp| { |t, task, index| {
match tp.tag() { match task {
#(#arms,)* #(#arms,)*
} }
}) })
@ -644,20 +660,18 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
if cfg!(feature = "timer-queue") { if cfg!(feature = "timer-queue") {
quote! { quote! {
#__priority::Task::#name => { #__priority::Task::#name => {
let (bl, payload, slot) = payload.coerce().read(); let node = core::ptr::read(::#name::BUFFER.get_unchecked(index as usize));
// priority #name::SQ::get().split().0.enqueue_unchecked(index);
#name::SQ::get().split().0.enqueue_unchecked(slot); #name::HANDLER(#name::Context::new(node.baseline, node.payload));
#name::HANDLER(#name::Context::new(bl, payload));
} }
} }
} else { } else {
quote! { quote! {
#__priority::Task::#name => { #__priority::Task::#name => {
let (payload, slot) = payload.coerce().read(); let node = core::ptr::read(::#name::BUFFER.get_unchecked(index as usize));
// priority #name::SQ::get().split().0.enqueue_unchecked(index);
#name::SQ::get().split().0.enqueue_unchecked(slot); #name::HANDLER(#name::Context::new(node.payload));
#name::HANDLER(#name::Context::new(payload));
} }
} }
} }
@ -675,8 +689,8 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
use #hidden::#krate::Resource; use #hidden::#krate::Resource;
// NOTE(get) the dispatcher is the only consumer of this queue // NOTE(get) the dispatcher is the only consumer of this queue
while let Some(payload) = #__priority::Q::get().split().1.dequeue() { while let Some((task, index)) = #__priority::Q::get().split().1.dequeue() {
match payload.tag() { match task {
#(#arms,)* #(#arms,)*
} }
} }
@ -691,16 +705,9 @@ pub fn app(ctxt: &Context, app: &App) -> Tokens {
let input = &task.input; let input = &task.input;
if let Either::Right(capacity) = task.interrupt_or_capacity { if let Either::Right(capacity) = task.interrupt_or_capacity {
let capacity = capacity as usize;
pre_init.push(quote! { pre_init.push(quote! {
{ for i in 0..#capacity {
static mut N: [#hidden::#krate::Node<#input>; #capacity] = #name::SQ::get().enqueue_unchecked(i);
unsafe { #hidden::#krate::uninitialized() };
for node in N.iter_mut() {
#name::SQ::get().enqueue_unchecked(node.into());
}
} }
}) })
} }

View file

@ -37,13 +37,12 @@ pub use typenum::{Max, Maximum, Unsigned};
pub use instant::Instant; pub use instant::Instant;
pub use node::Node; pub use node::Node;
use node::{Slot, TaggedPayload};
pub use resource::{Resource, Threshold}; pub use resource::{Resource, Threshold};
#[cfg(feature = "timer-queue")] #[cfg(feature = "timer-queue")]
pub use tq::{dispatch, TimerQueue}; pub use tq::{dispatch, Message, TimerQueue};
pub type PayloadQueue<T, N> = RingBuffer<TaggedPayload<T>, N, u8>; pub type PayloadQueue<T, N> = RingBuffer<(T, u8), N, u8>;
pub type SlotQueue<T, N> = RingBuffer<Slot<T>, N, u8>; pub type SlotQueue<N> = RingBuffer<u8, N, u8>;
pub type Ceiling<R> = <R as Resource>::Ceiling; pub type Ceiling<R> = <R as Resource>::Ceiling;
pub struct Core { pub struct Core {

View file

@ -4,171 +4,11 @@ use core::{mem, ptr};
use instant::Instant; use instant::Instant;
#[doc(hidden)] #[doc(hidden)]
#[repr(C)]
pub struct Node<T> pub struct Node<T>
where where
T: 'static, T: 'static,
{ {
#[cfg(feature = "timer-queue")] #[cfg(feature = "timer-queue")]
baseline: Instant, pub baseline: Instant,
payload: T, pub payload: T,
}
#[cfg(feature = "timer-queue")]
impl<T> Eq for Node<T> {}
#[cfg(feature = "timer-queue")]
impl<T> PartialEq for Node<T> {
fn eq(&self, other: &Node<T>) -> bool {
self.baseline == other.baseline
}
}
#[cfg(feature = "timer-queue")]
impl<T> Ord for Node<T> {
fn cmp(&self, other: &Node<T>) -> Ordering {
self.baseline.cmp(&other.baseline)
}
}
#[cfg(feature = "timer-queue")]
impl<T> PartialOrd for Node<T> {
fn partial_cmp(&self, other: &Node<T>) -> Option<Ordering> {
Some(self.cmp(other))
}
}
#[doc(hidden)]
pub struct Slot<T>
where
T: 'static,
{
node: &'static mut Node<T>,
}
impl<T> Slot<T> {
#[cfg(feature = "timer-queue")]
pub fn write(self, bl: Instant, data: T) -> Payload<T> {
self.node.baseline = bl;
unsafe { ptr::write(&mut self.node.payload, data) }
Payload { node: self.node }
}
#[cfg(not(feature = "timer-queue"))]
pub fn write(self, data: T) -> Payload<T> {
unsafe { ptr::write(&mut self.node.payload, data) }
Payload { node: self.node }
}
}
impl<T> Into<Slot<T>> for &'static mut Node<T> {
fn into(self) -> Slot<T> {
Slot { node: self }
}
}
#[doc(hidden)]
pub struct Payload<T>
where
T: 'static,
{
node: &'static mut Node<T>,
}
impl<T> Payload<T> {
#[cfg(feature = "timer-queue")]
pub fn read(self) -> (Instant, T, Slot<T>) {
let data = unsafe { ptr::read(&self.node.payload) };
(self.node.baseline, data, Slot { node: self.node })
}
#[cfg(not(feature = "timer-queue"))]
pub fn read(self) -> (T, Slot<T>) {
let data = unsafe { ptr::read(&self.node.payload) };
(data, Slot { node: self.node })
}
pub fn tag<A>(self, tag: A) -> TaggedPayload<A>
where
A: Copy,
{
TaggedPayload {
tag,
payload: unsafe { mem::transmute(self) },
}
}
}
#[doc(hidden)]
pub struct TaggedPayload<A>
where
A: Copy,
{
tag: A,
payload: Payload<!>,
}
impl<A> TaggedPayload<A>
where
A: Copy,
{
pub unsafe fn coerce<T>(self) -> Payload<T> {
mem::transmute(self.payload)
}
#[cfg(feature = "timer-queue")]
pub fn baseline(&self) -> Instant {
self.payload.node.baseline
}
pub fn tag(&self) -> A {
self.tag
}
pub fn retag<B>(self, tag: B) -> TaggedPayload<B>
where
B: Copy,
{
TaggedPayload {
tag,
payload: self.payload,
}
}
}
#[cfg(feature = "timer-queue")]
impl<T> Eq for TaggedPayload<T>
where
T: Copy,
{
}
#[cfg(feature = "timer-queue")]
impl<T> Ord for TaggedPayload<T>
where
T: Copy,
{
fn cmp(&self, rhs: &Self) -> Ordering {
self.payload.node.cmp(&rhs.payload.node)
}
}
#[cfg(feature = "timer-queue")]
impl<T> PartialEq for TaggedPayload<T>
where
T: Copy,
{
fn eq(&self, rhs: &Self) -> bool {
self.payload.node.eq(&rhs.payload.node)
}
}
#[cfg(feature = "timer-queue")]
impl<T> PartialOrd for TaggedPayload<T>
where
T: Copy,
{
fn partial_cmp(&self, rhs: &Self) -> Option<Ordering> {
Some(self.cmp(rhs))
}
} }

View file

@ -1,4 +1,4 @@
use core::cmp; use core::cmp::{self, Ordering};
use cortex_m::peripheral::{SCB, SYST}; use cortex_m::peripheral::{SCB, SYST};
use heapless::binary_heap::{BinaryHeap, Min}; use heapless::binary_heap::{BinaryHeap, Min};
@ -6,14 +6,39 @@ use heapless::ArrayLength;
use typenum::{Max, Maximum, Unsigned}; use typenum::{Max, Maximum, Unsigned};
use instant::Instant; use instant::Instant;
use node::{Slot, TaggedPayload};
use resource::{Resource, Threshold}; use resource::{Resource, Threshold};
pub struct Message<T> {
pub baseline: Instant,
pub index: u8,
pub task: T,
}
impl<T> Eq for Message<T> {}
impl<T> Ord for Message<T> {
fn cmp(&self, other: &Message<T>) -> Ordering {
self.baseline.cmp(&other.baseline)
}
}
impl<T> PartialEq for Message<T> {
fn eq(&self, other: &Message<T>) -> bool {
self.baseline == other.baseline
}
}
impl<T> PartialOrd for Message<T> {
fn partial_cmp(&self, other: &Message<T>) -> Option<Ordering> {
Some(self.cmp(other))
}
}
enum State<T> enum State<T>
where where
T: Copy, T: Copy,
{ {
Payload(TaggedPayload<T>), Payload { task: T, index: u8 },
Baseline(Instant), Baseline(Instant),
Done, Done,
} }
@ -21,16 +46,16 @@ where
#[doc(hidden)] #[doc(hidden)]
pub struct TimerQueue<T, N> pub struct TimerQueue<T, N>
where where
N: ArrayLength<TaggedPayload<T>>, N: ArrayLength<Message<T>>,
T: Copy, T: Copy,
{ {
pub syst: SYST, pub syst: SYST,
pub queue: BinaryHeap<TaggedPayload<T>, N, Min>, pub queue: BinaryHeap<Message<T>, N, Min>,
} }
impl<T, N> TimerQueue<T, N> impl<T, N> TimerQueue<T, N>
where where
N: ArrayLength<TaggedPayload<T>>, N: ArrayLength<Message<T>>,
T: Copy, T: Copy,
{ {
pub const fn new(syst: SYST) -> Self { pub const fn new(syst: SYST) -> Self {
@ -41,10 +66,10 @@ where
} }
#[inline] #[inline]
pub unsafe fn enqueue(&mut self, bl: Instant, tp: TaggedPayload<T>) { pub unsafe fn enqueue(&mut self, m: Message<T>) {
if self.queue if self.queue
.peek() .peek()
.map(|head| bl < head.baseline()) .map(|head| m.baseline < head.baseline)
.unwrap_or(true) .unwrap_or(true)
{ {
self.syst.enable_interrupt(); self.syst.enable_interrupt();
@ -52,25 +77,29 @@ where
unsafe { (*SCB::ptr()).icsr.write(1 << 26) } unsafe { (*SCB::ptr()).icsr.write(1 << 26) }
} }
self.queue.push_unchecked(tp); self.queue.push_unchecked(m);
} }
} }
pub fn dispatch<T, TQ, N, F, P>(t: &mut Threshold<P>, tq: &mut TQ, mut f: F) pub fn dispatch<T, TQ, N, F, P>(t: &mut Threshold<P>, tq: &mut TQ, mut f: F)
where where
F: FnMut(&mut Threshold<P>, TaggedPayload<T>), F: FnMut(&mut Threshold<P>, T, u8),
Maximum<P, TQ::Ceiling>: Unsigned, Maximum<P, TQ::Ceiling>: Unsigned,
N: 'static + ArrayLength<TaggedPayload<T>>, N: 'static + ArrayLength<Message<T>>,
P: Unsigned + Max<TQ::Ceiling>, P: Unsigned + Max<TQ::Ceiling>,
T: 'static + Copy + Send, T: 'static + Copy + Send,
TQ: Resource<Data = TimerQueue<T, N>>, TQ: Resource<Data = TimerQueue<T, N>>,
{ {
loop { loop {
let state = tq.claim_mut(t, |tq, _| { let state = tq.claim_mut(t, |tq, _| {
if let Some(bl) = tq.queue.peek().map(|p| p.baseline()) { if let Some(bl) = tq.queue.peek().map(|p| p.baseline) {
if Instant::now() >= bl { if Instant::now() >= bl {
// message ready // message ready
State::Payload(unsafe { tq.queue.pop_unchecked() }) let m = unsafe { tq.queue.pop_unchecked() };
State::Payload {
task: m.task,
index: m.index,
}
} else { } else {
// set a new timeout // set a new timeout
State::Baseline(bl) State::Baseline(bl)
@ -83,7 +112,7 @@ where
}); });
match state { match state {
State::Payload(p) => f(t, p), State::Payload { task, index } => f(t, task, index),
State::Baseline(bl) => { State::Baseline(bl) => {
const MAX: u32 = 0x00ffffff; const MAX: u32 = 0x00ffffff;