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
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
|
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,
{
/// # Safety
///
/// Writing to memory with a transmute in order to enable
/// interrupts of the SysTick timer
///
/// Enqueue a task without checking if it is full
#[inline]
pub unsafe fn enqueue_unchecked(&mut self, nr: NotReady<M, T>) {
let mut is_empty = true;
// Check if the top contains a non-empty element and if that element is
// greater than nr
let if_heap_max_greater_than_nr = self
.0
.peek()
.map(|head| {
is_empty = false;
nr.instant < head.instant
})
.unwrap_or(true);
if if_heap_max_greater_than_nr {
if is_empty {
mem::transmute::<_, SYST>(()).enable_interrupt();
}
// Set SysTick pending
SCB::set_pendst();
}
self.0.push_unchecked(nr);
}
/// Dequeue a task from the TimerQueue
#[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 ratio = M::ratio();
let dur = match (instant - now).try_into().ok().and_then(|x| {
x.checked_mul(ratio.numerator)
.map(|x| x / ratio.denominator)
}) {
None => MAX,
// ARM Architecture Reference Manual says:
// "Setting SYST_RVR to zero has the effect of
// disabling the SysTick counter independently
// of the counter enable bit."
Some(0) => 1,
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))
}
}
|
