Nothing to see here, move along meow
1use core::cell::Cell;
2use core::sync::atomic::{AtomicU32, Ordering};
3
4const HEADER_SIZE: u32 = 64;
5const WRITE_HEAD_OFFSET: usize = 0;
6const READ_TAIL_OFFSET: usize = 4;
7const SLOT_COUNT_OFFSET: usize = 8;
8const SLOT_SIZE_OFFSET: usize = 12;
9const LEN_FIELD_SIZE: u32 = 2;
10
11unsafe fn atomic_at(ptr: *mut u8, offset: usize) -> &'static AtomicU32 {
12 unsafe { &*(ptr.add(offset) as *const AtomicU32) }
13}
14
15unsafe fn vol_read_u32(ptr: *const u8, offset: usize) -> u32 {
16 unsafe { core::ptr::read_volatile(ptr.add(offset) as *const u32) }
17}
18
19unsafe fn vol_write_u32(ptr: *mut u8, offset: usize, val: u32) {
20 unsafe { core::ptr::write_volatile(ptr.add(offset) as *mut u32, val) }
21}
22
23unsafe fn vol_read_u16(ptr: *const u8, offset: usize) -> u16 {
24 unsafe { core::ptr::read_volatile(ptr.add(offset) as *const u16) }
25}
26
27unsafe fn vol_write_u16(ptr: *mut u8, offset: usize, val: u16) {
28 unsafe { core::ptr::write_volatile(ptr.add(offset) as *mut u16, val) }
29}
30
31pub struct PacketRingWriter {
32 base: *mut u8,
33 slot_count: u32,
34 slot_size: u32,
35}
36
37unsafe impl Send for PacketRingWriter {}
38
39pub struct PacketRingReader {
40 base: *mut u8,
41 slot_count: Cell<u32>,
42 slot_size: Cell<u32>,
43}
44
45unsafe impl Send for PacketRingReader {}
46
47impl PacketRingWriter {
48 #[allow(clippy::missing_safety_doc)]
49 pub unsafe fn attach(base: *mut u8, _len: usize) -> Self {
50 let slot_count = unsafe { vol_read_u32(base, SLOT_COUNT_OFFSET) };
51 let slot_size = unsafe { vol_read_u32(base, SLOT_SIZE_OFFSET) };
52 Self {
53 base,
54 slot_count,
55 slot_size,
56 }
57 }
58
59 #[allow(clippy::missing_safety_doc)]
60 pub unsafe fn init(base: *mut u8, len: usize, slot_size: u32) -> Self {
61 assert!(slot_size > LEN_FIELD_SIZE);
62 assert!(len >= HEADER_SIZE as usize);
63 let usable = (len as u32) - HEADER_SIZE;
64 let slot_count = usable / slot_size;
65 assert!(slot_count > 0);
66
67 unsafe {
68 vol_write_u32(base, SLOT_COUNT_OFFSET, slot_count);
69 vol_write_u32(base, SLOT_SIZE_OFFSET, slot_size);
70 atomic_at(base, READ_TAIL_OFFSET).store(0, Ordering::Relaxed);
71 atomic_at(base, WRITE_HEAD_OFFSET).store(0, Ordering::Release);
72 }
73
74 Self {
75 base,
76 slot_count,
77 slot_size,
78 }
79 }
80
81 pub fn has_space(&self) -> bool {
82 let head = unsafe { atomic_at(self.base, WRITE_HEAD_OFFSET) }.load(Ordering::Relaxed);
83 let tail = unsafe { atomic_at(self.base, READ_TAIL_OFFSET) }.load(Ordering::Acquire);
84 (head + 1) % self.slot_count != tail
85 }
86
87 pub fn try_push(&self, data: &[u8]) -> bool {
88 let max_payload = (self.slot_size - LEN_FIELD_SIZE) as usize;
89 if data.len() > max_payload || data.is_empty() {
90 return false;
91 }
92
93 let head = unsafe { atomic_at(self.base, WRITE_HEAD_OFFSET) }.load(Ordering::Relaxed);
94 let tail = unsafe { atomic_at(self.base, READ_TAIL_OFFSET) }.load(Ordering::Acquire);
95
96 let next_head = (head + 1) % self.slot_count;
97 if next_head == tail {
98 return false;
99 }
100
101 let slot_offset = HEADER_SIZE + head * self.slot_size;
102 unsafe {
103 vol_write_u16(self.base, slot_offset as usize, data.len() as u16);
104 core::ptr::copy_nonoverlapping(
105 data.as_ptr(),
106 self.base
107 .add(slot_offset as usize + LEN_FIELD_SIZE as usize),
108 data.len(),
109 );
110 }
111
112 unsafe { atomic_at(self.base, WRITE_HEAD_OFFSET) }.store(next_head, Ordering::Release);
113
114 true
115 }
116}
117
118impl PacketRingReader {
119 #[allow(clippy::missing_safety_doc)]
120 pub unsafe fn attach(base: *mut u8, len: usize) -> Self {
121 let slot_count = unsafe { vol_read_u32(base, SLOT_COUNT_OFFSET) };
122 let slot_size = unsafe { vol_read_u32(base, SLOT_SIZE_OFFSET) };
123 let valid = slot_count > 0
124 && slot_size > LEN_FIELD_SIZE
125 && (HEADER_SIZE as usize) + (slot_count as usize) * (slot_size as usize) <= len;
126 let (sc, ss) = match valid {
127 true => (slot_count, slot_size),
128 false => (0, 0),
129 };
130 Self {
131 base,
132 slot_count: Cell::new(sc),
133 slot_size: Cell::new(ss),
134 }
135 }
136
137 fn params(&self) -> Option<(u32, u32)> {
138 let sc = self.slot_count.get();
139 let ss = self.slot_size.get();
140 match sc > 0 && ss > LEN_FIELD_SIZE {
141 true => Some((sc, ss)),
142 false => {
143 let sc = unsafe { vol_read_u32(self.base, SLOT_COUNT_OFFSET) };
144 let ss = unsafe { vol_read_u32(self.base, SLOT_SIZE_OFFSET) };
145 match sc > 0 && ss > LEN_FIELD_SIZE {
146 true => {
147 self.slot_count.set(sc);
148 self.slot_size.set(ss);
149 Some((sc, ss))
150 }
151 false => None,
152 }
153 }
154 }
155 }
156
157 pub fn try_pop(&self, buf: &mut [u8]) -> Option<usize> {
158 let tail = unsafe { atomic_at(self.base, READ_TAIL_OFFSET) }.load(Ordering::Relaxed);
159 let head = unsafe { atomic_at(self.base, WRITE_HEAD_OFFSET) }.load(Ordering::Acquire);
160
161 if head == tail {
162 return None;
163 }
164
165 let (slot_count, slot_size) = self.params()?;
166
167 let slot_offset = HEADER_SIZE + tail * slot_size;
168 let pkt_len = unsafe { vol_read_u16(self.base, slot_offset as usize) } as usize;
169 let max_payload = (slot_size - LEN_FIELD_SIZE) as usize;
170 let copy_len = pkt_len.min(max_payload).min(buf.len());
171
172 unsafe {
173 core::ptr::copy_nonoverlapping(
174 (self.base as *const u8).add(slot_offset as usize + LEN_FIELD_SIZE as usize),
175 buf.as_mut_ptr(),
176 copy_len,
177 );
178 }
179
180 let next_tail = (tail + 1) % slot_count;
181 unsafe { atomic_at(self.base, READ_TAIL_OFFSET) }.store(next_tail, Ordering::Release);
182
183 Some(copy_len)
184 }
185}