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
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
|
use crate::Monotonic;
use core::cmp::Ordering;
use heapless::sorted_linked_list::{LinkedIndexU16, Min, SortedLinkedList};
pub struct TimerQueue<Mono, Task, const N: usize>(
pub SortedLinkedList<NotReady<Mono, Task>, LinkedIndexU16, Min, N>,
)
where
Mono: Monotonic,
Task: Copy;
impl<Mono, Task, const N: usize> TimerQueue<Mono, Task, N>
where
Mono: Monotonic,
Task: 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<F1, F2>(
&mut self,
nr: NotReady<Mono, Task>,
enable_interrupt: F1,
pend_handler: F2,
mono: Option<&mut Mono>,
) where
F1: FnOnce(),
F2: FnOnce(),
{
// 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| nr.instant < head.instant)
.unwrap_or(true);
if if_heap_max_greater_than_nr {
if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE && self.0.is_empty() {
if let Some(mono) = mono {
mono.enable_timer();
}
enable_interrupt();
}
pend_handler();
}
self.0.push_unchecked(nr);
}
/// Check if the timer queue is empty.
#[inline]
pub fn is_empty(&self) -> bool {
self.0.is_empty()
}
/// Cancel the marker value
pub fn cancel_marker(&mut self, marker: u32) -> Option<(Task, u8)> {
if let Some(val) = self.0.find_mut(|nr| nr.marker == marker) {
let nr = val.pop();
Some((nr.task, nr.index))
} else {
None
}
}
/// Update the instant at an marker value to a new instant
#[allow(clippy::result_unit_err)]
pub fn update_marker<F: FnOnce()>(
&mut self,
marker: u32,
new_marker: u32,
instant: Mono::Instant,
pend_handler: F,
) -> Result<(), ()> {
if let Some(mut val) = self.0.find_mut(|nr| nr.marker == marker) {
val.instant = instant;
val.marker = new_marker;
// On update pend the handler to reconfigure the next compare match
pend_handler();
Ok(())
} else {
Err(())
}
}
/// Dequeue a task from the TimerQueue
pub fn dequeue<F>(&mut self, disable_interrupt: F, mono: &mut Mono) -> Option<(Task, u8)>
where
F: FnOnce(),
{
mono.clear_compare_flag();
if let Some(instant) = self.0.peek().map(|p| p.instant) {
if instant <= mono.now() {
// task became ready
let nr = unsafe { self.0.pop_unchecked() };
Some((nr.task, nr.index))
} else {
// Set compare
mono.set_compare(instant);
// Double check that the instant we set is really in the future, else
// dequeue. If the monotonic is fast enough it can happen that from the
// read of now to the set of the compare, the time can overflow. This is to
// guard against this.
if instant <= mono.now() {
let nr = unsafe { self.0.pop_unchecked() };
Some((nr.task, nr.index))
} else {
None
}
}
} else {
// The queue is empty, disable the interrupt.
if Mono::DISABLE_INTERRUPT_ON_EMPTY_QUEUE {
disable_interrupt();
mono.disable_timer();
}
None
}
}
}
pub struct NotReady<Mono, Task>
where
Task: Copy,
Mono: Monotonic,
{
pub index: u8,
pub instant: Mono::Instant,
pub task: Task,
pub marker: u32,
}
impl<Mono, Task> Eq for NotReady<Mono, Task>
where
Task: Copy,
Mono: Monotonic,
{
}
impl<Mono, Task> Ord for NotReady<Mono, Task>
where
Task: Copy,
Mono: Monotonic,
{
fn cmp(&self, other: &Self) -> Ordering {
self.instant.cmp(&other.instant)
}
}
impl<Mono, Task> PartialEq for NotReady<Mono, Task>
where
Task: Copy,
Mono: Monotonic,
{
fn eq(&self, other: &Self) -> bool {
self.instant == other.instant
}
}
impl<Mono, Task> PartialOrd for NotReady<Mono, Task>
where
Task: Copy,
Mono: Monotonic,
{
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
|
