2018-04-19 18:38:12 +02:00
|
|
|
use core::cmp::Ordering;
|
|
|
|
use core::marker::Unsize;
|
2018-04-20 06:48:59 +02:00
|
|
|
use core::ops;
|
|
|
|
use core::{mem, ptr};
|
2018-04-19 18:38:12 +02:00
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
use cortex_m::peripheral::{DWT, SYST};
|
2018-04-19 18:38:12 +02:00
|
|
|
use heapless::binary_heap::{BinaryHeap, Min};
|
|
|
|
pub use heapless::ring_buffer::{Consumer, Producer, RingBuffer};
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
#[repr(C)]
|
|
|
|
pub struct Node<T>
|
2018-04-20 03:46:04 +02:00
|
|
|
where
|
2018-04-20 06:48:59 +02:00
|
|
|
T: 'static,
|
2018-04-20 03:46:04 +02:00
|
|
|
{
|
2018-04-20 06:48:59 +02:00
|
|
|
baseline: Instant,
|
|
|
|
next: Option<Slot<T>>,
|
|
|
|
payload: T,
|
2018-04-20 03:46:04 +02:00
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> Eq for Node<T> {}
|
|
|
|
|
|
|
|
impl<T> Ord for Node<T> {
|
|
|
|
fn cmp(&self, rhs: &Self) -> Ordering {
|
|
|
|
self.baseline.cmp(&rhs.baseline)
|
2018-04-20 03:46:04 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> PartialEq for Node<T> {
|
|
|
|
fn eq(&self, rhs: &Self) -> bool {
|
|
|
|
self.baseline.eq(&rhs.baseline)
|
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> PartialOrd for Node<T> {
|
|
|
|
fn partial_cmp(&self, rhs: &Self) -> Option<Ordering> {
|
|
|
|
Some(self.cmp(rhs))
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> Node<T> {
|
|
|
|
pub const fn new() -> Self {
|
|
|
|
Node {
|
|
|
|
baseline: Instant(0),
|
|
|
|
next: None,
|
|
|
|
payload: uninitialized(),
|
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
pub struct Slot<T>
|
|
|
|
where
|
|
|
|
T: 'static,
|
|
|
|
{
|
|
|
|
node: &'static mut Node<T>,
|
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> Slot<T> {
|
|
|
|
pub fn new(node: &'static mut Node<T>) -> Self {
|
|
|
|
Slot { node }
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
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 }
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
pub struct FreeList<T>
|
2018-04-19 18:38:12 +02:00
|
|
|
where
|
|
|
|
T: 'static,
|
|
|
|
{
|
2018-04-20 06:48:59 +02:00
|
|
|
head: Option<Slot<T>>,
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> FreeList<T> {
|
2018-04-19 18:38:12 +02:00
|
|
|
pub const fn new() -> Self {
|
2018-04-20 06:48:59 +02:00
|
|
|
FreeList { head: None }
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn pop(&mut self) -> Option<Slot<T>> {
|
|
|
|
self.head.take().map(|head| {
|
|
|
|
self.head = head.node.next.take();
|
|
|
|
head
|
|
|
|
})
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn push(&mut self, slot: Slot<T>) {
|
|
|
|
slot.node.next = self.head.take();
|
|
|
|
self.head = Some(slot);
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
pub struct Payload<T>
|
|
|
|
where
|
|
|
|
T: 'static,
|
|
|
|
{
|
|
|
|
node: &'static mut Node<T>,
|
|
|
|
}
|
|
|
|
|
|
|
|
impl<T> Payload<T> {
|
2018-04-20 06:48:59 +02:00
|
|
|
pub fn read(self) -> (Instant, T, Slot<T>) {
|
|
|
|
let data = unsafe { ptr::read(&self.node.payload) };
|
|
|
|
(self.node.baseline, data, Slot { node: self.node })
|
|
|
|
}
|
|
|
|
|
|
|
|
pub fn tag<A>(self, tag: A) -> TaggedPayload<A>
|
|
|
|
where
|
|
|
|
A: Copy,
|
|
|
|
{
|
|
|
|
TaggedPayload {
|
|
|
|
tag,
|
|
|
|
payload: unsafe { mem::transmute(self) },
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
}
|
2018-04-20 06:48:59 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
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)
|
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
pub fn baseline(&self) -> Instant {
|
|
|
|
self.payload.node.baseline
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
pub fn tag(&self) -> A {
|
|
|
|
self.tag
|
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
pub fn retag<B>(self, tag: B) -> TaggedPayload<B>
|
|
|
|
where
|
|
|
|
B: Copy,
|
|
|
|
{
|
|
|
|
TaggedPayload {
|
|
|
|
tag,
|
|
|
|
payload: self.payload,
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> Eq for TaggedPayload<T>
|
2018-04-19 18:38:12 +02:00
|
|
|
where
|
2018-04-20 06:48:59 +02:00
|
|
|
T: Copy,
|
2018-04-19 18:38:12 +02:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> Ord for TaggedPayload<T>
|
|
|
|
where
|
|
|
|
T: Copy,
|
|
|
|
{
|
|
|
|
fn cmp(&self, rhs: &Self) -> Ordering {
|
|
|
|
self.payload.node.cmp(&rhs.payload.node)
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
2018-04-20 06:48:59 +02:00
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> PartialEq for TaggedPayload<T>
|
|
|
|
where
|
|
|
|
T: Copy,
|
|
|
|
{
|
|
|
|
fn eq(&self, rhs: &Self) -> bool {
|
|
|
|
self.payload.node.eq(&rhs.payload.node)
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl<T> PartialOrd for TaggedPayload<T>
|
2018-04-19 18:38:12 +02:00
|
|
|
where
|
2018-04-20 06:48:59 +02:00
|
|
|
T: Copy,
|
2018-04-19 18:38:12 +02:00
|
|
|
{
|
2018-04-20 06:48:59 +02:00
|
|
|
fn partial_cmp(&self, rhs: &Self) -> Option<Ordering> {
|
|
|
|
Some(self.cmp(rhs))
|
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
pub struct TimerQueue<T, A>
|
|
|
|
where
|
|
|
|
A: Unsize<[TaggedPayload<T>]>,
|
|
|
|
T: Copy,
|
|
|
|
{
|
|
|
|
pub syst: SYST,
|
|
|
|
pub queue: BinaryHeap<TaggedPayload<T>, A, Min>,
|
|
|
|
}
|
|
|
|
|
|
|
|
impl<T, A> TimerQueue<T, A>
|
|
|
|
where
|
|
|
|
A: Unsize<[TaggedPayload<T>]>,
|
|
|
|
T: Copy,
|
|
|
|
{
|
|
|
|
pub const fn new(syst: SYST) -> Self {
|
|
|
|
TimerQueue {
|
|
|
|
syst,
|
|
|
|
queue: BinaryHeap::new(),
|
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
2018-04-20 06:48:59 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
#[derive(Clone, Copy, Debug)]
|
|
|
|
pub struct Instant(u32);
|
2018-04-19 18:38:12 +02:00
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl Instant {
|
|
|
|
pub fn now() -> Self {
|
|
|
|
Instant(DWT::get_cycle_count())
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
2018-04-20 06:48:59 +02:00
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
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<Ordering> {
|
|
|
|
Some(self.cmp(rhs))
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
impl ops::Add<u32> for Instant {
|
|
|
|
type Output = Self;
|
|
|
|
|
|
|
|
fn add(self, rhs: u32) -> Self {
|
|
|
|
Instant(self.0.wrapping_add(rhs))
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
2018-04-20 06:48:59 +02:00
|
|
|
}
|
2018-04-19 18:38:12 +02:00
|
|
|
|
2018-04-20 06:48:59 +02:00
|
|
|
impl ops::Sub for Instant {
|
|
|
|
type Output = i32;
|
|
|
|
|
|
|
|
fn sub(self, rhs: Self) -> i32 {
|
|
|
|
(self.0 as i32).wrapping_sub(rhs.0 as i32)
|
2018-04-19 18:38:12 +02:00
|
|
|
}
|
|
|
|
}
|
2018-04-20 06:48:59 +02:00
|
|
|
|
|
|
|
const fn uninitialized<T>() -> T {
|
|
|
|
#[allow(unions_with_drop_fields)]
|
|
|
|
union U<T> {
|
|
|
|
some: T,
|
|
|
|
none: (),
|
|
|
|
}
|
|
|
|
|
|
|
|
unsafe { U { none: () }.some }
|
|
|
|
}
|