1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
|
use core::{
cmp::{self, Ordering},
convert::TryInto,
mem,
ops::Sub,
};
use cortex_m::peripheral::{SCB, SYST};
use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
use crate::Monotonic;
pub struct TimerQueue<M, T, N>(pub BinaryHeap<NotReady<M, T>, N, Min>)
where
M: Monotonic,
<M::Instant as Sub>::Output: TryInto<u32>,
N: ArrayLength<NotReady<M, T>>,
T: Copy;
impl<M, T, N> TimerQueue<M, T, N>
where
M: Monotonic,
<M::Instant as Sub>::Output: TryInto<u32>,
N: ArrayLength<NotReady<M, T>>,
T: Copy,
{
#[inline]
pub unsafe fn enqueue_unchecked(&mut self, nr: NotReady<M, T>) {
let mut is_empty = true;
if self
.0
.peek()
.map(|head| {
is_empty = false;
nr.instant < head.instant
})
.unwrap_or(true)
{
if is_empty {
mem::transmute::<_, SYST>(()).enable_interrupt();
}
// set SysTick pending
SCB::set_pendst();
}
self.0.push_unchecked(nr);
}
#[inline]
pub fn dequeue(&mut self) -> Option<(T, u8)> {
unsafe {
if let Some(instant) = self.0.peek().map(|p| p.instant) {
let now = M::now();
if instant < now {
// task became ready
let nr = self.0.pop_unchecked();
Some((nr.task, nr.index))
} else {
// set a new timeout
const MAX: u32 = 0x00ffffff;
let dur = match (instant - now)
.try_into()
.ok()
.and_then(|x| x.checked_mul(M::ratio()))
{
None => MAX,
Some(x) => cmp::min(MAX, x),
};
mem::transmute::<_, SYST>(()).set_reload(dur);
// start counting down from the new reload
mem::transmute::<_, SYST>(()).clear_current();
None
}
} else {
// the queue is empty
mem::transmute::<_, SYST>(()).disable_interrupt();
None
}
}
}
}
pub struct NotReady<M, T>
where
T: Copy,
M: Monotonic,
<M::Instant as Sub>::Output: TryInto<u32>,
{
pub index: u8,
pub instant: M::Instant,
pub task: T,
}
impl<M, T> Eq for NotReady<M, T>
where
T: Copy,
M: Monotonic,
<M::Instant as Sub>::Output: TryInto<u32>,
{
}
impl<M, T> Ord for NotReady<M, T>
where
T: Copy,
M: Monotonic,
<M::Instant as Sub>::Output: TryInto<u32>,
{
fn cmp(&self, other: &Self) -> Ordering {
self.instant.cmp(&other.instant)
}
}
impl<M, T> PartialEq for NotReady<M, T>
where
T: Copy,
M: Monotonic,
<M::Instant as Sub>::Output: TryInto<u32>,
{
fn eq(&self, other: &Self) -> bool {
self.instant == other.instant
}
}
impl<M, T> PartialOrd for NotReady<M, T>
where
T: Copy,
M: Monotonic,
<M::Instant as Sub>::Output: TryInto<u32>,
{
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(&other))
}
}
|
