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
|
use core::cmp::{self, Ordering};
use cortex_m::peripheral::{SCB, SYST};
use heapless::{binary_heap::Min, ArrayLength, BinaryHeap};
use crate::{Instant, Mutex};
pub struct TimerQueue<T, N>
where
N: ArrayLength<NotReady<T>>,
T: Copy,
{
pub syst: SYST,
pub queue: BinaryHeap<NotReady<T>, N, Min>,
}
impl<T, N> TimerQueue<T, N>
where
N: ArrayLength<NotReady<T>>,
T: Copy,
{
pub fn new(syst: SYST) -> Self {
TimerQueue {
syst,
queue: BinaryHeap::new(),
}
}
#[inline]
pub unsafe fn enqueue_unchecked(&mut self, nr: NotReady<T>) {
let mut is_empty = true;
if self
.queue
.peek()
.map(|head| {
is_empty = false;
nr.instant < head.instant
})
.unwrap_or(true)
{
if is_empty {
self.syst.enable_interrupt();
}
// set SysTick pending
(*SCB::ptr()).icsr.write(1 << 26);
}
self.queue.push_unchecked(nr);
}
}
pub struct NotReady<T>
where
T: Copy,
{
pub index: u8,
pub instant: Instant,
pub task: T,
}
impl<T> Eq for NotReady<T> where T: Copy {}
impl<T> Ord for NotReady<T>
where
T: Copy,
{
fn cmp(&self, other: &Self) -> Ordering {
self.instant.cmp(&other.instant)
}
}
impl<T> PartialEq for NotReady<T>
where
T: Copy,
{
fn eq(&self, other: &Self) -> bool {
self.instant == other.instant
}
}
impl<T> PartialOrd for NotReady<T>
where
T: Copy,
{
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(&other))
}
}
#[inline(always)]
pub fn isr<TQ, T, N, F>(mut tq: TQ, mut f: F)
where
TQ: Mutex<Data = TimerQueue<T, N>>,
T: Copy + Send,
N: ArrayLength<NotReady<T>>,
F: FnMut(T, u8),
{
loop {
// XXX does `#[inline(always)]` improve performance or not?
let next = tq.lock(#[inline(always)]
|tq| {
if let Some(instant) = tq.queue.peek().map(|p| p.instant) {
let diff = instant.0.wrapping_sub(Instant::now().0);
if diff < 0 {
// task became ready
let m = unsafe { tq.queue.pop_unchecked() };
Some((m.task, m.index))
} else {
// set a new timeout
const MAX: u32 = 0x00ffffff;
tq.syst.set_reload(cmp::min(MAX, diff as u32));
// start counting down from the new reload
tq.syst.clear_current();
None
}
} else {
// the queue is empty
tq.syst.disable_interrupt();
None
}
});
if let Some((task, index)) = next {
f(task, index)
} else {
return;
}
}
}
|
