diff --git a/examples/tq.rs b/examples/tq.rs index 5a1b7ebd56..d46e232f4d 100644 --- a/examples/tq.rs +++ b/examples/tq.rs @@ -13,8 +13,9 @@ extern crate stm32f103xx; use core::cmp; use cortex_m::peripheral::syst::SystClkSource; -use cortex_m::peripheral::{DWT, ITM}; -use rtfm::ll::{Consumer, FreeList, Message, Node, Payload, Producer, RingBuffer, Slot, TimerQueue}; +use cortex_m::peripheral::ITM; +use rtfm::ll::{Consumer, FreeList, Instant, Node, Producer, RingBuffer, Slot, TaggedPayload, + TimerQueue}; use rtfm::{app, Resource, Threshold}; use stm32f103xx::Interrupt; @@ -27,25 +28,22 @@ app! { resources: { /* timer queue */ - static TQ: TimerQueue; 2]>; + static TQ: TimerQueue; 2]>; /* a */ // payloads w/ after static AN: [Node; 2] = [Node::new(), Node::new()]; static AFL: FreeList = FreeList::new(); - static AQ: RingBuffer<(u32, i32), [(u32, i32); ACAP + 1], u8> = RingBuffer::u8(); - static AQC: Consumer<'static, (u32, i32), [(u32, i32); ACAP + 1], u8>; - static AQP: Producer<'static, (u32, i32), [(u32, i32); ACAP + 1], u8>; - /* exti0 */ - static Q1: RingBuffer = RingBuffer::u8(); - static Q1C: Consumer<'static, Task1, [Task1; ACAP + 1], u8>; - static Q1P: Producer<'static, Task1, [Task1; ACAP + 1], u8>; + static Q1: RingBuffer, [TaggedPayload; ACAP + 1], u8> = + RingBuffer::u8(); + static Q1C: Consumer<'static, TaggedPayload, [TaggedPayload; ACAP + 1], u8>; + static Q1P: Producer<'static, TaggedPayload, [TaggedPayload; ACAP + 1], u8>; }, init: { - resources: [AN, Q1, AQ], + resources: [AN, Q1], }, tasks: { @@ -60,14 +58,14 @@ app! { // dispatch interrupt EXTI0: { path: exti0, - resources: [AQC, Q1C], + resources: [Q1C, AFL], priority: 1, }, // timer queue SYS_TICK: { path: sys_tick, - resources: [TQ, AQP, Q1P, AFL], + resources: [TQ, Q1P], priority: 2, }, }, @@ -88,12 +86,9 @@ pub fn init(mut p: ::init::Peripherals, r: init::Resources) -> init::LateResourc r.AFL.push(Slot::new(n)); } - let (aqp, aqc) = r.AQ.split(); let (q1p, q1c) = r.Q1.split(); init::LateResources { TQ: TimerQueue::new(p.core.SYST), - AQC: aqc, - AQP: aqp, Q1C: q1c, Q1P: q1p, } @@ -107,13 +102,12 @@ pub fn idle() -> ! { } } -fn a(_t: &mut Threshold, bl: u32, payload: i32) { - let now = DWT::get_cycle_count(); +fn a(_t: &mut Threshold, bl: Instant, payload: i32) { unsafe { iprintln!( &mut (*ITM::ptr()).stim[0], - "a(now={}, bl={}, payload={})", - now, + "a(now={:?}, bl={:?}, payload={})", + Instant::now(), bl, payload ) @@ -122,21 +116,24 @@ fn a(_t: &mut Threshold, bl: u32, payload: i32) { fn exti1(t: &mut Threshold, r: EXTI1::Resources) { /* expansion */ - let bl = DWT::get_cycle_count(); + let bl = Instant::now(); let mut async = a::Async::new(bl, r.TQ, r.AFL); /* end of expansion */ - unsafe { iprintln!(&mut (*ITM::ptr()).stim[0], "EXTI0(bl={})", bl) } + unsafe { iprintln!(&mut (*ITM::ptr()).stim[0], "EXTI0(bl={:?})", bl) } async.a(t, 100 * MS, 0).unwrap(); async.a(t, 50 * MS, 1).unwrap(); } /* auto generated */ -fn exti0(_t: &mut Threshold, mut r: EXTI0::Resources) { - while let Some(task) = r.Q1C.dequeue() { - match task { +fn exti0(t: &mut Threshold, mut r: EXTI0::Resources) { + while let Some(payload) = r.Q1C.dequeue() { + match payload.tag() { Task1::a => { - let (bl, payload) = r.AQC.dequeue().unwrap(); + let (bl, payload, slot) = unsafe { payload.coerce() }.read(); + + r.AFL.claim_mut(t, |afl, _| afl.push(slot)); + a(&mut unsafe { Threshold::new(1) }, bl, payload); } } @@ -145,29 +142,26 @@ fn exti0(_t: &mut Threshold, mut r: EXTI0::Resources) { fn sys_tick(t: &mut Threshold, r: SYS_TICK::Resources) { #[allow(non_snake_case)] - let SYS_TICK::Resources { - mut AFL, - mut AQP, - mut Q1P, - mut TQ, - } = r; + let SYS_TICK::Resources { mut Q1P, mut TQ } = r; - enum State { - Message(Message), - Baseline(u32), + enum State + where + T: Copy, + { + Payload(TaggedPayload), + Baseline(Instant), Done, } loop { let state = TQ.claim_mut(t, |tq, _| { - if let Some(m) = tq.queue.peek().cloned() { - if (DWT::get_cycle_count() as i32).wrapping_sub(m.baseline as i32) >= 0 { + if let Some(bl) = tq.queue.peek().map(|p| p.baseline()) { + if Instant::now() >= bl { // message ready - tq.queue.pop(); - State::Message(m) + State::Payload(tq.queue.pop().unwrap()) } else { - // set timeout - State::Baseline(m.baseline) + // new timeout + State::Baseline(bl) } } else { // empty queue @@ -177,30 +171,16 @@ fn sys_tick(t: &mut Threshold, r: SYS_TICK::Resources) { }); match state { - State::Message(m) => { - match m.task { - Task::a => { - // read payload - let (payload, slot) = unsafe { Payload::::from(m.payload) }.read(); - - // return free slot to the free list - AFL.claim_mut(t, |afl, _| afl.push(slot)); - - // enqueue a new `a` task - AQP.claim_mut(t, |aqp, t| { - aqp.enqueue_unchecked((m.baseline, payload)); - Q1P.claim_mut(t, |q1p, _| { - q1p.enqueue_unchecked(Task1::a); - rtfm::set_pending(Interrupt::EXTI0); - }); - }); - } + State::Payload(p) => match p.tag() { + Task::a => { + Q1P.claim_mut(t, |q1p, _| q1p.enqueue_unchecked(p.retag(Task1::a))); + rtfm::set_pending(Interrupt::EXTI0); } - } + }, State::Baseline(bl) => { const MAX: u32 = 0x00ffffff; - let diff = (bl as i32).wrapping_sub(DWT::get_cycle_count() as i32); + let diff = bl - Instant::now(); if diff < 0 { // message became ready @@ -237,21 +217,21 @@ pub enum Task { mod a { use cortex_m::peripheral::SCB; - use rtfm::ll::Message; + use rtfm::ll::Instant; use rtfm::{Resource, Threshold}; use Task; #[allow(non_snake_case)] pub struct Async { // inherited baseline - baseline: u32, + baseline: Instant, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL, } impl Async { #[allow(non_snake_case)] - pub fn new(bl: u32, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL) -> Self { + pub fn new(bl: Instant, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL) -> Self { Async { baseline: bl, TQ, @@ -267,10 +247,10 @@ mod a { // full Err(payload) } else { - let bl = baseline.wrapping_add(after); + let bl = baseline + after; if tq.queue .peek() - .map(|head| (bl as i32).wrapping_sub(head.baseline as i32) < 0) + .map(|head| bl < head.baseline()) .unwrap_or(true) { tq.syst.enable_interrupt(); @@ -278,9 +258,7 @@ mod a { unsafe { (*SCB::ptr()).icsr.write(1 << 26) } } - tq.queue - .push(Message::new(bl, Task::a, slot.write(payload))) - .ok(); + tq.queue.push(slot.write(bl, payload).tag(Task::a)).ok(); Ok(()) } diff --git a/src/lib.rs b/src/lib.rs index 9c98b23fa0..8692f28a0f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -81,6 +81,7 @@ // #![deny(warnings)] #![feature(const_fn)] #![feature(proc_macro)] +#![feature(untagged_unions)] #![feature(unsize)] #![no_std] diff --git a/src/ll.rs b/src/ll.rs index 21c658d6b2..d51302aa52 100644 --- a/src/ll.rs +++ b/src/ll.rs @@ -1,111 +1,48 @@ use core::cmp::Ordering; use core::marker::Unsize; -use core::ptr; +use core::ops; +use core::{mem, ptr}; -use cortex_m::peripheral::SYST; +use cortex_m::peripheral::{DWT, SYST}; use heapless::binary_heap::{BinaryHeap, Min}; pub use heapless::ring_buffer::{Consumer, Producer, RingBuffer}; -use untagged_option::UntaggedOption; - -pub struct TimerQueue -where - A: Unsize<[Message]>, -{ - pub syst: SYST, - pub queue: BinaryHeap, A, Min>, -} - -impl TimerQueue -where - A: Unsize<[Message]>, -{ - pub fn new(syst: SYST) -> Self { - TimerQueue { - syst, - queue: BinaryHeap::new(), - } - } -} - -#[derive(Clone, Copy)] -pub struct Message { - pub baseline: u32, - pub task: T, - pub payload: usize, -} - -impl Message { - pub fn new

(bl: u32, task: T, payload: Payload

) -> Self { - Message { - baseline: bl, - task, - payload: payload.erase(), - } - } -} - -impl PartialEq for Message { - fn eq(&self, other: &Self) -> bool { - self.baseline.eq(&other.baseline) - } -} - -impl Eq for Message {} - -impl PartialOrd for Message { - fn partial_cmp(&self, other: &Self) -> Option { - Some(self.cmp(other)) - } -} - -impl Ord for Message { - fn cmp(&self, other: &Self) -> Ordering { - (self.baseline as i32) - .wrapping_sub(other.baseline as i32) - .cmp(&0) - } -} +#[repr(C)] pub struct Node where T: 'static, { - data: UntaggedOption, - next: Option<&'static mut Node>, + baseline: Instant, + next: Option>, + payload: T, +} + +impl Eq for Node {} + +impl Ord for Node { + fn cmp(&self, rhs: &Self) -> Ordering { + self.baseline.cmp(&rhs.baseline) + } +} + +impl PartialEq for Node { + fn eq(&self, rhs: &Self) -> bool { + self.baseline.eq(&rhs.baseline) + } +} + +impl PartialOrd for Node { + fn partial_cmp(&self, rhs: &Self) -> Option { + Some(self.cmp(rhs)) + } } impl Node { pub const fn new() -> Self { Node { - data: UntaggedOption::none(), + baseline: Instant(0), next: None, - } - } -} - -pub struct Payload -where - T: 'static, -{ - node: &'static mut Node, -} - -impl Payload { - pub unsafe fn from(ptr: usize) -> Self { - Payload { - node: &mut *(ptr as *mut _), - } - } - - pub fn erase(self) -> usize { - self.node as *mut _ as usize - } - - pub fn read(self) -> (T, Slot) { - unsafe { - let payload = ptr::read(&self.node.data.some); - - (payload, Slot::new(self.node)) + payload: uninitialized(), } } } @@ -122,11 +59,10 @@ impl Slot { Slot { node } } - pub fn write(self, data: T) -> Payload { - unsafe { - ptr::write(&mut self.node.data.some, data); - Payload { node: self.node } - } + pub fn write(self, bl: Instant, data: T) -> Payload { + self.node.baseline = bl; + unsafe { ptr::write(&mut self.node.payload, data) } + Payload { node: self.node } } } @@ -142,19 +78,184 @@ impl FreeList { FreeList { head: None } } - pub fn is_empty(&self) -> bool { - self.head.is_none() - } - pub fn pop(&mut self) -> Option> { self.head.take().map(|head| { - self.head = head.node.next.take().map(Slot::new); + self.head = head.node.next.take(); head }) } - pub fn push(&mut self, free: Slot) { - free.node.next = self.head.take().map(|slot| slot.node); - self.head = Some(Slot::new(free.node)); + pub fn push(&mut self, slot: Slot) { + slot.node.next = self.head.take(); + self.head = Some(slot); } } + +pub struct Payload +where + T: 'static, +{ + node: &'static mut Node, +} + +impl Payload { + pub fn read(self) -> (Instant, T, Slot) { + let data = unsafe { ptr::read(&self.node.payload) }; + (self.node.baseline, data, Slot { node: self.node }) + } + + pub fn tag(self, tag: A) -> TaggedPayload + where + A: Copy, + { + TaggedPayload { + tag, + payload: unsafe { mem::transmute(self) }, + } + } +} + +pub struct TaggedPayload +where + A: Copy, +{ + tag: A, + payload: Payload, +} + +impl TaggedPayload +where + A: Copy, +{ + pub unsafe fn coerce(self) -> Payload { + mem::transmute(self.payload) + } + + pub fn baseline(&self) -> Instant { + self.payload.node.baseline + } + + pub fn tag(&self) -> A { + self.tag + } + + pub fn retag(self, tag: B) -> TaggedPayload + where + B: Copy, + { + TaggedPayload { + tag, + payload: self.payload, + } + } +} + +impl Eq for TaggedPayload +where + T: Copy, +{ +} + +impl Ord for TaggedPayload +where + T: Copy, +{ + fn cmp(&self, rhs: &Self) -> Ordering { + self.payload.node.cmp(&rhs.payload.node) + } +} + +impl PartialEq for TaggedPayload +where + T: Copy, +{ + fn eq(&self, rhs: &Self) -> bool { + self.payload.node.eq(&rhs.payload.node) + } +} + +impl PartialOrd for TaggedPayload +where + T: Copy, +{ + fn partial_cmp(&self, rhs: &Self) -> Option { + Some(self.cmp(rhs)) + } +} + +pub struct TimerQueue +where + A: Unsize<[TaggedPayload]>, + T: Copy, +{ + pub syst: SYST, + pub queue: BinaryHeap, A, Min>, +} + +impl TimerQueue +where + A: Unsize<[TaggedPayload]>, + T: Copy, +{ + pub const fn new(syst: SYST) -> Self { + TimerQueue { + syst, + queue: BinaryHeap::new(), + } + } +} + +#[derive(Clone, Copy, Debug)] +pub struct Instant(u32); + +impl Instant { + pub fn now() -> Self { + Instant(DWT::get_cycle_count()) + } +} + +impl Eq for Instant {} + +impl Ord for Instant { + fn cmp(&self, rhs: &Self) -> Ordering { + (self.0 as i32).wrapping_sub(rhs.0 as i32).cmp(&0) + } +} + +impl PartialEq for Instant { + fn eq(&self, rhs: &Self) -> bool { + self.0.eq(&rhs.0) + } +} + +impl PartialOrd for Instant { + fn partial_cmp(&self, rhs: &Self) -> Option { + Some(self.cmp(rhs)) + } +} + +impl ops::Add for Instant { + type Output = Self; + + fn add(self, rhs: u32) -> Self { + Instant(self.0.wrapping_add(rhs)) + } +} + +impl ops::Sub for Instant { + type Output = i32; + + fn sub(self, rhs: Self) -> i32 { + (self.0 as i32).wrapping_sub(rhs.0 as i32) + } +} + +const fn uninitialized() -> T { + #[allow(unions_with_drop_fields)] + union U { + some: T, + none: (), + } + + unsafe { U { none: () }.some } +}