rtic_common/
wait_queue.rs

Help
1//! A wait queue implementation using a doubly linked list.
2
3use core::marker::PhantomPinned;
4use core::pin::Pin;
5use core::ptr::null_mut;
6use core::task::Waker;
7use critical_section as cs;
8use portable_atomic::{AtomicBool, AtomicPtr, Ordering};
9
10/// A helper definition of a wait queue.
11pub type WaitQueue = DoublyLinkedList<Waker>;
12
13/// An atomic, doubly linked, FIFO list for a wait queue.
14///
15/// Atomicity is guaranteed by short [`critical_section`]s, so this list is _not_ lock free,
16/// but it will not deadlock.
17pub struct DoublyLinkedList<T> {
18    head: AtomicPtr<Link<T>>, // UnsafeCell<*mut Link<T>>
19    tail: AtomicPtr<Link<T>>,
20}
21
22impl<T> DoublyLinkedList<T> {
23    /// Create a new linked list.
24    pub const fn new() -> Self {
25        Self {
26            head: AtomicPtr::new(null_mut()),
27            tail: AtomicPtr::new(null_mut()),
28        }
29    }
30}
31
32impl<T> Default for DoublyLinkedList<T> {
33    fn default() -> Self {
34        Self::new()
35    }
36}
37
38impl<T: Clone> DoublyLinkedList<T> {
39    const R: Ordering = Ordering::Relaxed;
40
41    /// Pop the first element in the queue.
42    pub fn pop(&self) -> Option<T> {
43        cs::with(|_| {
44            // Make sure all previous writes are visible
45            core::sync::atomic::fence(Ordering::SeqCst);
46
47            let head = self.head.load(Self::R);
48
49            // SAFETY: `as_ref` is safe as `insert` requires a valid reference to a link
50            if let Some(head_ref) = unsafe { head.as_ref() } {
51                // Move head to the next element
52                self.head.store(head_ref.next.load(Self::R), Self::R);
53
54                // We read the value at head
55                let head_val = head_ref.val.clone();
56
57                let tail = self.tail.load(Self::R);
58                if head == tail {
59                    // The queue is empty
60                    self.tail.store(null_mut(), Self::R);
61                }
62
63                if let Some(next_ref) = unsafe { head_ref.next.load(Self::R).as_ref() } {
64                    next_ref.prev.store(null_mut(), Self::R);
65                }
66
67                // Clear the pointers in the node.
68                head_ref.next.store(null_mut(), Self::R);
69                head_ref.prev.store(null_mut(), Self::R);
70                head_ref.is_popped.store(true, Self::R);
71
72                return Some(head_val);
73            }
74
75            None
76        })
77    }
78
79    /// Put an element at the back of the queue.
80    ///
81    /// # Safety
82    ///
83    /// The link must live until it is removed from the queue.
84    pub unsafe fn push(&self, link: Pin<&Link<T>>) {
85        cs::with(|_| {
86            // Make sure all previous writes are visible
87            core::sync::atomic::fence(Ordering::SeqCst);
88
89            let tail = self.tail.load(Self::R);
90
91            // SAFETY: This datastructure does not move the underlying value.
92            let link = link.get_ref();
93
94            if let Some(tail_ref) = unsafe { tail.as_ref() } {
95                // Queue is not empty
96                link.prev.store(tail, Self::R);
97                self.tail.store(link as *const _ as *mut _, Self::R);
98                tail_ref.next.store(link as *const _ as *mut _, Self::R);
99            } else {
100                // Queue is empty
101                self.tail.store(link as *const _ as *mut _, Self::R);
102                self.head.store(link as *const _ as *mut _, Self::R);
103            }
104        });
105    }
106
107    /// Check if the queue is empty.
108    pub fn is_empty(&self) -> bool {
109        self.head.load(Self::R).is_null()
110    }
111}
112
113/// A link in the linked list.
114pub struct Link<T> {
115    pub(crate) val: T,
116    next: AtomicPtr<Link<T>>,
117    prev: AtomicPtr<Link<T>>,
118    is_popped: AtomicBool,
119    _up: PhantomPinned,
120}
121
122impl<T: Clone> Link<T> {
123    const R: Ordering = Ordering::Relaxed;
124
125    /// Create a new link.
126    pub const fn new(val: T) -> Self {
127        Self {
128            val,
129            next: AtomicPtr::new(null_mut()),
130            prev: AtomicPtr::new(null_mut()),
131            is_popped: AtomicBool::new(false),
132            _up: PhantomPinned,
133        }
134    }
135
136    /// Return true if this link has been poped from the list.
137    pub fn is_popped(&self) -> bool {
138        self.is_popped.load(Self::R)
139    }
140
141    /// Remove this link from a linked list.
142    pub fn remove_from_list(&self, list: &DoublyLinkedList<T>) {
143        cs::with(|_| {
144            // Make sure all previous writes are visible
145            core::sync::atomic::fence(Ordering::SeqCst);
146
147            if self.is_popped() {
148                return;
149            }
150
151            let prev = self.prev.load(Self::R);
152            let next = self.next.load(Self::R);
153            self.is_popped.store(true, Self::R);
154
155            match unsafe { (prev.as_ref(), next.as_ref()) } {
156                (None, None) => {
157                    // Not in the list or alone in the list, check if list head == node address
158                    let sp = self as *const _;
159
160                    if sp == list.head.load(Ordering::Relaxed) {
161                        list.head.store(null_mut(), Self::R);
162                        list.tail.store(null_mut(), Self::R);
163                    }
164                }
165                (None, Some(next_ref)) => {
166                    // First in the list
167                    next_ref.prev.store(null_mut(), Self::R);
168                    list.head.store(next, Self::R);
169                }
170                (Some(prev_ref), None) => {
171                    // Last in the list
172                    prev_ref.next.store(null_mut(), Self::R);
173                    list.tail.store(prev, Self::R);
174                }
175                (Some(prev_ref), Some(next_ref)) => {
176                    // Somewhere in the list
177
178                    // Connect the `prev.next` and `next.prev` with each other to remove the node
179                    prev_ref.next.store(next, Self::R);
180                    next_ref.prev.store(prev, Self::R);
181                }
182            }
183        })
184    }
185}
186
187#[cfg(test)]
188impl<T: core::fmt::Debug + Clone> DoublyLinkedList<T> {
189    fn print(&self) {
190        cs::with(|_| {
191            // Make sure all previous writes are visible
192            core::sync::atomic::fence(Ordering::SeqCst);
193
194            let mut head = self.head.load(Self::R);
195            let tail = self.tail.load(Self::R);
196
197            println!(
198                "List - h = 0x{:x}, t = 0x{:x}",
199                head as usize, tail as usize
200            );
201
202            let mut i = 0;
203
204            // SAFETY: `as_ref` is safe as `insert` requires a valid reference to a link
205            while let Some(head_ref) = unsafe { head.as_ref() } {
206                println!(
207                    "    {}: {:?}, s = 0x{:x}, n = 0x{:x}, p = 0x{:x}",
208                    i,
209                    head_ref.val,
210                    head as usize,
211                    head_ref.next.load(Ordering::Relaxed) as usize,
212                    head_ref.prev.load(Ordering::Relaxed) as usize
213                );
214
215                head = head_ref.next.load(Self::R);
216
217                i += 1;
218            }
219        });
220    }
221}
222
223#[cfg(test)]
224impl<T: core::fmt::Debug + Clone> Link<T> {
225    fn print(&self) {
226        cs::with(|_| {
227            // Make sure all previous writes are visible
228            core::sync::atomic::fence(Ordering::SeqCst);
229
230            println!("Link:");
231
232            println!(
233                "    val = {:?}, n = 0x{:x}, p = 0x{:x}",
234                self.val,
235                self.next.load(Ordering::Relaxed) as usize,
236                self.prev.load(Ordering::Relaxed) as usize
237            );
238        });
239    }
240}
241
242#[cfg(test)]
243mod tests {
244    use super::*;
245
246    #[test]
247    fn linked_list() {
248        let wq = DoublyLinkedList::<u32>::new();
249
250        let i1 = Link::new(10);
251        let i2 = Link::new(11);
252        let i3 = Link::new(12);
253        let i4 = Link::new(13);
254        let i5 = Link::new(14);
255
256        unsafe { wq.push(Pin::new_unchecked(&i1)) };
257        unsafe { wq.push(Pin::new_unchecked(&i2)) };
258        unsafe { wq.push(Pin::new_unchecked(&i3)) };
259        unsafe { wq.push(Pin::new_unchecked(&i4)) };
260        unsafe { wq.push(Pin::new_unchecked(&i5)) };
261
262        wq.print();
263
264        wq.pop();
265        i1.print();
266
267        wq.print();
268
269        i4.remove_from_list(&wq);
270
271        wq.print();
272
273        // i1.remove_from_list(&wq);
274        // wq.print();
275
276        println!("i2");
277        i2.remove_from_list(&wq);
278        wq.print();
279
280        println!("i3");
281        i3.remove_from_list(&wq);
282        wq.print();
283
284        println!("i5");
285        i5.remove_from_list(&wq);
286        wq.print();
287    }
288}
rtic_common/wait_queue.rs

rtic_common/
wait_queue.rs
