Nothing to see here, move along meow
1use core::sync::atomic::{Ordering, fence};
2
3pub const QUEUE_SIZE: u16 = 64;
4
5const VRING_DESC_F_NEXT: u16 = 1;
6const VRING_DESC_F_WRITE: u16 = 2;
7
8#[repr(C)]
9#[derive(Clone, Copy)]
10pub struct VringDesc {
11 pub addr: u64,
12 pub len: u32,
13 pub flags: u16,
14 pub next: u16,
15}
16
17#[repr(C)]
18pub struct VringAvail {
19 pub flags: u16,
20 pub idx: u16,
21 pub ring: [u16; QUEUE_SIZE as usize],
22 pub used_event: u16,
23}
24
25#[repr(C)]
26#[derive(Clone, Copy)]
27pub struct VringUsedElem {
28 pub id: u32,
29 pub len: u32,
30}
31
32#[repr(C)]
33pub struct VringUsed {
34 pub flags: u16,
35 pub idx: u16,
36 pub ring: [VringUsedElem; QUEUE_SIZE as usize],
37 pub avail_event: u16,
38}
39
40const DESC_OFF_ADDR: usize = core::mem::offset_of!(VringDesc, addr);
41const DESC_OFF_LEN: usize = core::mem::offset_of!(VringDesc, len);
42const DESC_OFF_FLAGS: usize = core::mem::offset_of!(VringDesc, flags);
43const DESC_OFF_NEXT: usize = core::mem::offset_of!(VringDesc, next);
44const DESC_STRIDE: usize = core::mem::size_of::<VringDesc>();
45
46const AVAIL_OFF_FLAGS: usize = core::mem::offset_of!(VringAvail, flags);
47const AVAIL_OFF_IDX: usize = core::mem::offset_of!(VringAvail, idx);
48const AVAIL_OFF_RING: usize = core::mem::offset_of!(VringAvail, ring);
49
50const USED_OFF_IDX: usize = core::mem::offset_of!(VringUsed, idx);
51const USED_OFF_RING: usize = core::mem::offset_of!(VringUsed, ring);
52const USED_ELEM_SIZE: usize = core::mem::size_of::<VringUsedElem>();
53
54pub struct Virtqueue {
55 desc_base: *mut u8,
56 avail_base: *mut u8,
57 used_base: *const u8,
58 pub desc_phys: u64,
59 pub avail_phys: u64,
60 pub used_phys: u64,
61 free_head: u16,
62 num_free: u16,
63 last_used_idx: u16,
64 queue_size: u16,
65}
66
67fn desc_field_ptr<T>(base: *mut u8, idx: usize, field_off: usize) -> *mut T {
68 unsafe { base.add(idx * DESC_STRIDE + field_off) as *mut T }
69}
70
71fn write_desc(base: *mut u8, idx: usize, addr: u64, len: u32, flags: u16, next: u16) {
72 unsafe {
73 desc_field_ptr::<u64>(base, idx, DESC_OFF_ADDR).write(addr);
74 desc_field_ptr::<u32>(base, idx, DESC_OFF_LEN).write(len);
75 desc_field_ptr::<u16>(base, idx, DESC_OFF_FLAGS).write(flags);
76 desc_field_ptr::<u16>(base, idx, DESC_OFF_NEXT).write(next);
77 }
78}
79
80fn read_desc_flags(base: *mut u8, idx: usize) -> u16 {
81 unsafe { desc_field_ptr::<u16>(base, idx, DESC_OFF_FLAGS).read() }
82}
83
84fn read_desc_next(base: *mut u8, idx: usize) -> u16 {
85 unsafe { desc_field_ptr::<u16>(base, idx, DESC_OFF_NEXT).read() }
86}
87
88fn avail_idx_ptr(base: *mut u8) -> *mut u16 {
89 unsafe { base.add(AVAIL_OFF_IDX) as *mut u16 }
90}
91
92fn avail_ring_ptr(base: *mut u8, slot: usize) -> *mut u16 {
93 unsafe { base.add(AVAIL_OFF_RING + slot * 2) as *mut u16 }
94}
95
96fn used_idx_ptr(base: *const u8) -> *const u16 {
97 unsafe { base.add(USED_OFF_IDX) as *const u16 }
98}
99
100fn used_ring_elem(base: *const u8, slot: usize) -> VringUsedElem {
101 let ptr = unsafe { base.add(USED_OFF_RING + slot * USED_ELEM_SIZE) };
102 let id = unsafe { (ptr as *const u32).read() };
103 let len = unsafe { (ptr.add(4) as *const u32).read() };
104 VringUsedElem { id, len }
105}
106
107impl Virtqueue {
108 pub fn new(
109 desc_virt: usize,
110 avail_virt: usize,
111 used_virt: usize,
112 desc_phys: u64,
113 avail_phys: u64,
114 used_phys: u64,
115 queue_size: u16,
116 ) -> Self {
117 let desc_base = desc_virt as *mut u8;
118
119 (0..queue_size).for_each(|i| {
120 let flags = match i + 1 < queue_size {
121 true => VRING_DESC_F_NEXT,
122 false => 0,
123 };
124 let next = match i + 1 < queue_size {
125 true => i + 1,
126 false => 0,
127 };
128 write_desc(desc_base, i as usize, 0, 0, flags, next);
129 });
130
131 let avail_base = avail_virt as *mut u8;
132 unsafe {
133 (avail_base.add(AVAIL_OFF_FLAGS) as *mut u16).write(0);
134 (avail_base.add(AVAIL_OFF_IDX) as *mut u16).write(0);
135 }
136
137 Self {
138 desc_base,
139 avail_base,
140 used_base: used_virt as *const u8,
141 desc_phys,
142 avail_phys,
143 used_phys,
144 free_head: 0,
145 num_free: queue_size,
146 last_used_idx: 0,
147 queue_size,
148 }
149 }
150
151 pub fn add_buf_single(
152 &mut self,
153 buf_phys: u64,
154 buf_len: u32,
155 device_writable: bool,
156 ) -> Option<u16> {
157 match self.num_free {
158 0 => None,
159 _ => {
160 let idx = self.free_head;
161 let old_next = read_desc_next(self.desc_base, idx as usize);
162 self.free_head = old_next;
163 self.num_free -= 1;
164
165 let flags = match device_writable {
166 true => VRING_DESC_F_WRITE,
167 false => 0,
168 };
169 write_desc(self.desc_base, idx as usize, buf_phys, buf_len, flags, 0);
170
171 let avail_idx = unsafe { avail_idx_ptr(self.avail_base).read_volatile() };
172 let ring_slot = (avail_idx % self.queue_size) as usize;
173 unsafe {
174 avail_ring_ptr(self.avail_base, ring_slot).write_volatile(idx);
175 }
176
177 fence(Ordering::Release);
178 unsafe {
179 avail_idx_ptr(self.avail_base).write_volatile(avail_idx.wrapping_add(1));
180 }
181 fence(Ordering::Release);
182
183 Some(idx)
184 }
185 }
186 }
187
188 #[allow(dead_code)]
189 pub fn add_buf_chain(
190 &mut self,
191 hdr_phys: u64,
192 hdr_len: u32,
193 data_phys: u64,
194 data_len: u32,
195 device_writable: bool,
196 ) -> Option<u16> {
197 match self.num_free >= 2 {
198 false => None,
199 true => {
200 let head_idx = self.free_head;
201 let data_idx = read_desc_next(self.desc_base, head_idx as usize);
202 let after_data = read_desc_next(self.desc_base, data_idx as usize);
203
204 self.free_head = after_data;
205 self.num_free -= 2;
206
207 let write_flag = match device_writable {
208 true => VRING_DESC_F_WRITE,
209 false => 0,
210 };
211
212 write_desc(
213 self.desc_base,
214 head_idx as usize,
215 hdr_phys,
216 hdr_len,
217 VRING_DESC_F_NEXT | write_flag,
218 data_idx,
219 );
220
221 write_desc(
222 self.desc_base,
223 data_idx as usize,
224 data_phys,
225 data_len,
226 write_flag,
227 0,
228 );
229
230 let avail_idx = unsafe { avail_idx_ptr(self.avail_base).read_volatile() };
231 let ring_slot = (avail_idx % self.queue_size) as usize;
232 unsafe {
233 avail_ring_ptr(self.avail_base, ring_slot).write_volatile(head_idx);
234 }
235
236 fence(Ordering::Release);
237 unsafe {
238 avail_idx_ptr(self.avail_base).write_volatile(avail_idx.wrapping_add(1));
239 }
240 fence(Ordering::Release);
241
242 Some(head_idx)
243 }
244 }
245 }
246
247 pub fn poll_used(&mut self) -> Option<(u16, u32)> {
248 fence(Ordering::Acquire);
249 let used_idx = unsafe { used_idx_ptr(self.used_base).read_volatile() };
250 match used_idx == self.last_used_idx {
251 true => None,
252 false => {
253 let ring_idx = (self.last_used_idx % self.queue_size) as usize;
254 let elem = used_ring_elem(self.used_base, ring_idx);
255 fence(Ordering::Acquire);
256 self.last_used_idx = self.last_used_idx.wrapping_add(1);
257
258 self.reclaim_desc_chain(elem.id as u16);
259
260 Some((elem.id as u16, elem.len))
261 }
262 }
263 }
264
265 fn reclaim_desc_chain(&mut self, mut idx: u16) {
266 core::iter::from_fn(|| match idx < self.queue_size {
267 false => None,
268 true => {
269 let flags = read_desc_flags(self.desc_base, idx as usize);
270 let next = read_desc_next(self.desc_base, idx as usize);
271 let has_next = flags & VRING_DESC_F_NEXT != 0;
272 let current = idx;
273 idx = match has_next {
274 true => next,
275 false => self.queue_size,
276 };
277 Some(current)
278 }
279 })
280 .take(self.queue_size as usize)
281 .for_each(|desc_idx| {
282 write_desc(
283 self.desc_base,
284 desc_idx as usize,
285 0,
286 0,
287 VRING_DESC_F_NEXT,
288 self.free_head,
289 );
290 self.free_head = desc_idx;
291 self.num_free = (self.num_free + 1).min(self.queue_size);
292 });
293 }
294
295 #[allow(dead_code)]
296 pub fn num_free(&self) -> u16 {
297 self.num_free
298 }
299}