1 files changed, 51 insertions, 0 deletions
diff --git a/src/types/wake_queue.rs b/src/types/wake_queue.rs
new file mode 100644
index 0000000..41d815b
--- /dev/null
+++ b/src/types/wake_queue.rs
@@ -0,0 +1,51 @@
+pub struct WakeQueue {
+    queue: Vec<u8>,
+}
+
+
+impl WakeQueue {
+    pub fn new() -> Self {
+        Self {
+            queue: [0x1,0x2,0x3,0x4,0x5,0x6,0x7,0x8,0x9,0xA,0xB,0xC,0xD,0xE,0xF].into(),
+        }
+    }
+
+    /// Iterate over all masked devices in last-woken order.
+    pub fn iter(&self, mask: u16) -> WakeIterator {
+        let mut queue = Vec::new();
+        for i in &self.queue {
+            if mask & (0x8000 >> i) != 0 {
+                queue.push(*i);
+            }
+        }
+        // Add system device last.
+        if mask & 0x8000 != 0 {
+            queue.push(0);
+        }
+        WakeIterator { queue, pointer: 0 }
+    }
+
+    /// Push a device to the back of the queue.
+    pub fn wake(&mut self, id: u8) {
+        if let Some(index) = self.queue.iter().position(|e| *e == id) {
+            self.queue.remove(index);
+            self.queue.push(id);
+        }
+    }
+}
+
+
+pub struct WakeIterator {
+    queue: Vec<u8>,
+    pointer: usize,
+}
+
+impl Iterator for WakeIterator {
+    type Item = u8;
+
+    fn next(&mut self) -> Option<u8> {
+        let pointer = self.pointer;
+        self.pointer += 1;
+        self.queue.get(pointer).copied()
+    }
+}