zencan_node/
priority_queue.rs

1//! A prioritized queue for handling CAN messages
2use core::{cell::RefCell, mem::MaybeUninit};
3
4use critical_section::Mutex;
5
6#[derive(Clone, Copy, Debug)]
7struct Prio<T: Copy>(u32, MaybeUninit<T>);
8
9impl<T: Copy> Prio<T> {
10    const EMPTY: Prio<T> = Prio(1 << 31, MaybeUninit::uninit());
11
12    pub fn new(prio: u32, value: T) -> Self {
13        let prio = prio & 0x7FFFFFFF;
14        Self(prio, MaybeUninit::new(value))
15    }
16
17    pub fn is_empty(&self) -> bool {
18        self.0 & (1 << 31) != 0
19    }
20
21    pub fn prio(&self) -> Option<u32> {
22        if self.is_empty() {
23            None
24        } else {
25            Some(self.0)
26        }
27    }
28
29    pub fn value(&self) -> Option<T> {
30        if self.is_empty() {
31            None
32        } else {
33            Some(unsafe { self.1.assume_init() })
34        }
35    }
36
37    pub fn take(&mut self) -> Option<T> {
38        let value = self.value();
39        *self = Prio::EMPTY;
40        value
41    }
42}
43
44/// A simple prioritized queue
45#[derive(Debug)]
46pub struct PriorityQueue<const N: usize, T: Copy> {
47    buffer: Mutex<RefCell<[Prio<T>; N]>>,
48}
49
50impl<const N: usize, T: Copy + Send> Default for PriorityQueue<N, T> {
51    fn default() -> Self {
52        Self::new()
53    }
54}
55
56impl<const N: usize, T> PriorityQueue<N, T>
57where
58    T: Copy + Send,
59{
60    /// Create a new PriorityQueue
61    pub const fn new() -> Self {
62        Self {
63            buffer: Mutex::new(RefCell::new([Prio::EMPTY; N])),
64        }
65    }
66
67    /// Write an item to the queue
68    ///
69    /// # Arguments
70    /// - `prio`: The priority of the item. Lower priority values will be read first. Bit 31 is
71    ///   reserved and must always be zero, so the maximum priority value is (2**31-1)
72    /// - `item`: The item to queue
73    pub fn push(&self, prio: u32, item: T) -> Result<(), T> {
74        critical_section::with(|cs| {
75            let mut buffer = self.buffer.borrow_ref_mut(cs);
76            for loc in buffer.iter_mut() {
77                if loc.is_empty() {
78                    *loc = Prio::new(prio, item);
79                    return Ok(());
80                }
81            }
82
83            Err(item)
84        })
85    }
86
87    /// Remove the queue item with the lowest priority value
88    ///
89    /// Returns: The item with the lowest priority value in the queue, or None if the queue is empty
90    pub fn pop(&self) -> Option<T> {
91        critical_section::with(|cs| {
92            let mut min_prio = u32::MAX;
93            let mut selected_index = None;
94            let mut buffer = self.buffer.borrow_ref_mut(cs);
95            // Traverse the list and find the lowest priority
96            for (i, loc) in buffer.iter().enumerate() {
97                if let Some(prio) = loc.prio() {
98                    if prio < min_prio {
99                        min_prio = prio;
100                        selected_index = Some(i);
101                    }
102                }
103            }
104
105            selected_index.map(|i| buffer[i].take())?
106        })
107    }
108}
109
110#[cfg(test)]
111mod test {
112    use super::*;
113    #[test]
114    fn test_priority_queue() {
115        let queue: PriorityQueue<4, u8> = PriorityQueue::new();
116
117        queue.push(87, 2).unwrap();
118        queue.push(100, 3).unwrap();
119        queue.push(1, 0).unwrap();
120        queue.push(10, 1).unwrap();
121
122        // Now the queue is full
123        assert_eq!(Err(12), queue.push(100, 12));
124
125        assert_eq!(Some(0), queue.pop());
126        assert_eq!(Some(1), queue.pop());
127        assert_eq!(Some(2), queue.pop());
128        assert_eq!(Some(3), queue.pop());
129    }
130}
zencan_node/priority_queue.rs

zencan_node/
priority_queue.rs
