diff --git a/Cargo.toml b/Cargo.toml index 82704bff98..fc196a20ba 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,7 +15,7 @@ version = "0.3.2" [dependencies] cortex-m = "0.4.0" cortex-m-rtfm-macros = { path = "macros", version = "0.3.1" } -heapless = "0.2.6" +heapless = "0.2.7" rtfm-core = "0.2.0" untagged-option = "0.1.1" diff --git a/examples/tq.rs b/examples/tq.rs index 07cfcbd796..5a1b7ebd56 100644 --- a/examples/tq.rs +++ b/examples/tq.rs @@ -10,8 +10,10 @@ extern crate cortex_m_rtfm as rtfm; extern crate panic_abort; extern crate stm32f103xx; +use core::cmp; + use cortex_m::peripheral::syst::SystClkSource; -use cortex_m::peripheral::{DWT, ITM, SCB}; +use cortex_m::peripheral::{DWT, ITM}; use rtfm::ll::{Consumer, FreeList, Message, Node, Payload, Producer, RingBuffer, Slot, TimerQueue}; use rtfm::{app, Resource, Threshold}; use stm32f103xx::Interrupt; @@ -29,23 +31,21 @@ app! { /* a */ // payloads w/ after - static AN0: Node = Node::new(); - static AN1: Node = Node::new(); - static AFL: FreeList = FreeList::new(); + static AN: [Node; 2] = [Node::new(), Node::new()]; + static AFL: FreeList = FreeList::new(); - // payloads w/o after - static AQ: RingBuffer = RingBuffer::new(); - static AQC: Consumer<'static, u32, [u32; ACAP + 1]>; - static AQP: Producer<'static, u32, [u32; ACAP + 1]>; + 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::new(); - static Q1C: Consumer<'static, Task1, [Task1; ACAP + 1]>; - static Q1P: Producer<'static, Task1, [Task1; ACAP + 1]>; + static Q1: RingBuffer = RingBuffer::u8(); + static Q1C: Consumer<'static, Task1, [Task1; ACAP + 1], u8>; + static Q1P: Producer<'static, Task1, [Task1; ACAP + 1], u8>; }, init: { - resources: [AN0, AN1, Q1, AQ], + resources: [AN, Q1, AQ], }, tasks: { @@ -68,7 +68,7 @@ app! { SYS_TICK: { path: sys_tick, resources: [TQ, AQP, Q1P, AFL], - priority: 1, + priority: 2, }, }, } @@ -80,11 +80,13 @@ pub fn init(mut p: ::init::Peripherals, r: init::Resources) -> init::LateResourc p.core.DWT.enable_cycle_counter(); unsafe { p.core.DWT.cyccnt.write(0) }; p.core.SYST.set_clock_source(SystClkSource::Core); - p.core.SYST.enable_interrupt(); + p.core.SYST.enable_counter(); + p.core.SYST.disable_interrupt(); // populate the free list - r.AFL.push(Slot::new(r.AN0)); - r.AFL.push(Slot::new(r.AN1)); + for n in r.AN { + r.AFL.push(Slot::new(n)); + } let (aqp, aqc) = r.AQ.split(); let (q1p, q1c) = r.Q1.split(); @@ -105,12 +107,13 @@ pub fn idle() -> ! { } } -fn a(_t: &mut Threshold, payload: u32) { - let bl = DWT::get_cycle_count(); +fn a(_t: &mut Threshold, bl: u32, payload: i32) { + let now = DWT::get_cycle_count(); unsafe { iprintln!( &mut (*ITM::ptr()).stim[0], - "a(bl={}, payload={})", + "a(now={}, bl={}, payload={})", + now, bl, payload ) @@ -126,7 +129,6 @@ fn exti1(t: &mut Threshold, r: EXTI1::Resources) { unsafe { iprintln!(&mut (*ITM::ptr()).stim[0], "EXTI0(bl={})", bl) } async.a(t, 100 * MS, 0).unwrap(); async.a(t, 50 * MS, 1).unwrap(); - // rtfm::bkpt(); } /* auto generated */ @@ -134,8 +136,8 @@ fn exti0(_t: &mut Threshold, mut r: EXTI0::Resources) { while let Some(task) = r.Q1C.dequeue() { match task { Task1::a => { - let payload = r.AQC.dequeue().unwrap_or_else(|| unreachable!()); - a(&mut unsafe { Threshold::new(1) }, payload); + let (bl, payload) = r.AQC.dequeue().unwrap(); + a(&mut unsafe { Threshold::new(1) }, bl, payload); } } } @@ -150,54 +152,72 @@ fn sys_tick(t: &mut Threshold, r: SYS_TICK::Resources) { mut TQ, } = r; - TQ.claim_mut(t, |tq, t| { - tq.syst.disable_counter(); - - if let Some(m) = tq.queue.pop() { - match m.task { - Task::a => { - // read payload - let (payload, slot) = unsafe { Payload::::from(m.payload) }.read(); - - // enqueue a new `a` task - AQP.claim_mut(t, |aqp, t| { - aqp.enqueue(payload).ok().unwrap(); - Q1P.claim_mut(t, |q1p, _| { - q1p.enqueue(Task1::a).ok().unwrap_or_else(|| unreachable!()); - rtfm::set_pending(Interrupt::EXTI0); - }); - }); - - // return free slot to the free list - AFL.claim_mut(t, |afl, _| afl.push(slot)); - } - } + enum State { + Message(Message), + Baseline(u32), + Done, + } + loop { + let state = TQ.claim_mut(t, |tq, _| { if let Some(m) = tq.queue.peek().cloned() { - // set up a new interrupt - let now = DWT::get_cycle_count(); - - if let Some(timeout) = tq.baseline.wrapping_add(m.deadline).checked_sub(now) { - // TODO deal with the 24-bit limit - tq.syst.set_reload(timeout); - tq.syst.clear_current(); - tq.syst.enable_counter(); - - // update the timer queue baseline - tq.baseline = now; - tq.queue.iter_mut().for_each(|m| m.deadline -= timeout); + if (DWT::get_cycle_count() as i32).wrapping_sub(m.baseline as i32) >= 0 { + // message ready + tq.queue.pop(); + State::Message(m) } else { - // next message already expired, pend immediately - // NOTE(unsafe) atomic write to a stateless (from the programmer PoV) register - unsafe { (*SCB::ptr()).icsr.write(1 << 26) } + // set timeout + State::Baseline(m.baseline) } } else { - // no message left to process + // empty queue + tq.syst.disable_interrupt(); + State::Done + } + }); + + 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::Baseline(bl) => { + const MAX: u32 = 0x00ffffff; + + let diff = (bl as i32).wrapping_sub(DWT::get_cycle_count() as i32); + + if diff < 0 { + // message became ready + continue; + } else { + TQ.claim_mut(t, |tq, _| { + tq.syst.set_reload(cmp::min(MAX, diff as u32)); + tq.syst.clear_current(); + }); + return; + } + } + State::Done => { + return; } - } else { - unreachable!() } - }); + } } // Tasks dispatched at a priority of 1 @@ -215,12 +235,16 @@ pub enum Task { } mod a { + use cortex_m::peripheral::SCB; + + use rtfm::ll::Message; use rtfm::{Resource, Threshold}; use Task; #[allow(non_snake_case)] pub struct Async { - bl: u32, + // inherited baseline + baseline: u32, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL, } @@ -228,18 +252,39 @@ mod a { impl Async { #[allow(non_snake_case)] pub fn new(bl: u32, TQ: ::EXTI1::TQ, AFL: ::EXTI1::AFL) -> Self { - Async { bl, TQ, AFL } + Async { + baseline: bl, + TQ, + AFL, + } } - pub fn a(&mut self, t: &mut Threshold, after: u32, payload: u32) -> Result<(), u32> { + pub fn a(&mut self, t: &mut Threshold, after: u32, payload: i32) -> Result<(), i32> { if let Some(slot) = self.AFL.claim_mut(t, |afl, _| afl.pop()) { - let bl = self.bl; - self.TQ - .claim_mut(t, |tq, _| tq.insert(bl, after, Task::a, payload, slot)) - .map_err(|(p, slot)| { - self.AFL.claim_mut(t, |afl, _| afl.push(slot)); - p - }) + let baseline = self.baseline; + self.TQ.claim_mut(t, |tq, _| { + if tq.queue.capacity() == tq.queue.len() { + // full + Err(payload) + } else { + let bl = baseline.wrapping_add(after); + if tq.queue + .peek() + .map(|head| (bl as i32).wrapping_sub(head.baseline as i32) < 0) + .unwrap_or(true) + { + tq.syst.enable_interrupt(); + // Set SYST pending + unsafe { (*SCB::ptr()).icsr.write(1 << 26) } + } + + tq.queue + .push(Message::new(bl, Task::a, slot.write(payload))) + .ok(); + + Ok(()) + } + }) } else { Err(payload) } diff --git a/src/ll.rs b/src/ll.rs index 42873a160f..21c658d6b2 100644 --- a/src/ll.rs +++ b/src/ll.rs @@ -2,55 +2,16 @@ use core::cmp::Ordering; use core::marker::Unsize; use core::ptr; -use cortex_m::peripheral::{DWT, SCB, SYST}; +use cortex_m::peripheral::SYST; use heapless::binary_heap::{BinaryHeap, Min}; pub use heapless::ring_buffer::{Consumer, Producer, RingBuffer}; use untagged_option::UntaggedOption; -#[derive(Clone, Copy)] -pub struct Message { - // relative to the TimerQueue baseline - pub deadline: u32, - pub task: T, - pub payload: usize, -} - -impl Message { - fn new

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

) -> Self { - Message { - deadline: dl, - task, - payload: payload.erase(), - } - } -} - -impl PartialEq for Message { - fn eq(&self, other: &Self) -> bool { - self.deadline.eq(&other.deadline) - } -} - -impl Eq for Message {} - -impl PartialOrd for Message { - fn partial_cmp(&self, other: &Self) -> Option { - self.deadline.partial_cmp(&other.deadline) - } -} - -impl Ord for Message { - fn cmp(&self, other: &Self) -> Ordering { - self.deadline.cmp(&other.deadline) - } -} - pub struct TimerQueue where A: Unsize<[Message]>, { pub syst: SYST, - pub baseline: u32, pub queue: BinaryHeap, A, Min>, } @@ -60,54 +21,51 @@ where { pub fn new(syst: SYST) -> Self { TimerQueue { - baseline: 0, - queue: BinaryHeap::new(), syst, + queue: BinaryHeap::new(), } } +} - pub fn insert

( - &mut self, - bl: u32, - after: u32, - task: T, - payload: P, - slot: Slot

, - ) -> Result<(), (P, Slot

)> { - if self.queue.len() == self.queue.capacity() { - Err((payload, slot)) - } else { - if self.queue.is_empty() { - self.baseline = bl; - } +#[derive(Clone, Copy)] +pub struct Message { + pub baseline: u32, + pub task: T, + pub payload: usize, +} - let dl = bl.wrapping_add(after).wrapping_sub(self.baseline); - - if self.queue.peek().map(|m| dl < m.deadline).unwrap_or(true) { - // the new message is the most urgent; set a new timeout - let now = DWT::get_cycle_count(); - - if let Some(timeout) = dl.wrapping_add(self.baseline).checked_sub(now) { - self.syst.disable_counter(); - self.syst.set_reload(timeout); - self.syst.clear_current(); - self.syst.enable_counter(); - } else { - // message already expired, pend immediately - // NOTE(unsafe) atomic write to a stateless (from the programmer PoV) register - unsafe { (*SCB::ptr()).icsr.write(1 << 26) } - } - } - - self.queue - .push(Message::new(dl, task, slot.write(payload))) - .unwrap_or_else(|_| unreachable!()); - - Ok(()) +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) + } +} + pub struct Node where T: 'static,