1 /*
2 * Copyright (c) 2023 Enphase Energy
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "eth_ivshmem_priv.h"
8
9 #include <zephyr/arch/cpu.h>
10 #include <zephyr/cache.h>
11 #include <zephyr/kernel.h>
12
13 #include <stdatomic.h>
14 #include <string.h>
15
16 /* These defines must match on the peer */
17 #define ETH_IVSHMEM_VRING_ALIGNMENT 64
18 #define ETH_IVSHMEM_FRAME_SIZE(len) ROUND_UP(18 + (len), L1_CACHE_BYTES)
19
20 #define VRING_FLUSH(x) sys_cache_data_flush_range(&(x), sizeof(x))
21 #define VRING_INVALIDATE(x) sys_cache_data_invd_range(&(x), sizeof(x))
22
23 static int calc_vring_size(
24 size_t section_size, uint16_t *vring_desc_len,
25 uint32_t *vring_header_size);
26 static uint32_t tx_buffer_advance(uint32_t max_len, uint32_t *position, uint32_t *len);
27 static int tx_clean_used(struct eth_ivshmem_queue *q);
28 static int get_rx_avail_desc_idx(struct eth_ivshmem_queue *q, uint16_t *avail_desc_idx);
29
eth_ivshmem_queue_init(struct eth_ivshmem_queue * q,uintptr_t shmem,size_t shmem_section_size,bool tx_buffer_first)30 int eth_ivshmem_queue_init(
31 struct eth_ivshmem_queue *q, uintptr_t shmem,
32 size_t shmem_section_size, bool tx_buffer_first)
33 {
34 memset(q, 0, sizeof(*q));
35
36 uint16_t vring_desc_len;
37 uint32_t vring_header_size;
38 int res = calc_vring_size(shmem_section_size, &vring_desc_len, &vring_header_size);
39
40 if (res != 0) {
41 return res;
42 }
43
44 q->desc_max_len = vring_desc_len;
45 q->vring_data_max_len = shmem_section_size - vring_header_size;
46 q->vring_header_size = vring_header_size;
47
48 if (tx_buffer_first) {
49 q->tx.shmem = (void *)shmem;
50 q->rx.shmem = (void *)(shmem + shmem_section_size);
51 } else {
52 q->rx.shmem = (void *)shmem;
53 q->tx.shmem = (void *)(shmem + shmem_section_size);
54 }
55
56 /* Init vrings */
57 vring_init(&q->tx.vring, vring_desc_len, q->tx.shmem, ETH_IVSHMEM_VRING_ALIGNMENT);
58 vring_init(&q->rx.vring, vring_desc_len, q->rx.shmem, ETH_IVSHMEM_VRING_ALIGNMENT);
59
60 /* Swap "used" pointers.
61 * This is done so that each peer only ever writes to its output section,
62 * while maintaining vring code consistency elsewhere in this file.
63 */
64 struct vring_used *tmp_used = q->tx.vring.used;
65
66 q->tx.vring.used = q->rx.vring.used;
67 q->rx.vring.used = tmp_used;
68
69 eth_ivshmem_queue_reset(q);
70
71 return 0;
72 }
73
eth_ivshmem_queue_reset(struct eth_ivshmem_queue * q)74 void eth_ivshmem_queue_reset(struct eth_ivshmem_queue *q)
75 {
76 q->tx.desc_head = 0;
77 q->tx.desc_len = 0;
78 q->tx.data_head = 0;
79 q->tx.data_tail = 0;
80 q->tx.data_len = 0;
81 q->tx.avail_idx = 0;
82 q->tx.used_idx = 0;
83 q->tx.pending_data_head = 0;
84 q->tx.pending_data_len = 0;
85 q->rx.avail_idx = 0;
86 q->rx.used_idx = 0;
87
88 memset(q->tx.shmem, 0, q->vring_header_size);
89
90 /* Init TX ring descriptors */
91 for (unsigned int i = 0; i < q->tx.vring.num - 1; i++)
92 q->tx.vring.desc[i].next = i + 1;
93 q->tx.vring.desc[q->tx.vring.num - 1].next = 0;
94 }
95
eth_ivshmem_queue_tx_get_buff(struct eth_ivshmem_queue * q,void ** data,size_t len)96 int eth_ivshmem_queue_tx_get_buff(struct eth_ivshmem_queue *q, void **data, size_t len)
97 {
98 /* Clean used TX buffers */
99 int res = tx_clean_used(q);
100
101 if (res != 0) {
102 return res;
103 }
104
105 if (q->tx.desc_len >= q->desc_max_len) {
106 return -ENOBUFS;
107 }
108
109 uint32_t head = q->tx.data_head;
110 uint32_t consumed_len = len;
111 uint32_t new_head = tx_buffer_advance(q->vring_data_max_len, &head, &consumed_len);
112
113 if (q->vring_data_max_len - q->tx.data_len < consumed_len) {
114 return -ENOBUFS;
115 }
116
117 struct vring_desc *tx_desc = &q->tx.vring.desc[q->tx.desc_head];
118
119 tx_desc->addr = q->vring_header_size + head;
120 tx_desc->len = len;
121 tx_desc->flags = 0;
122 VRING_FLUSH(*tx_desc);
123
124 *data = (uint8_t *)q->tx.shmem + q->vring_header_size + head;
125
126 q->tx.pending_data_head = new_head;
127 q->tx.pending_data_len = q->tx.data_len + consumed_len;
128
129 return 0;
130 }
131
eth_ivshmem_queue_tx_commit_buff(struct eth_ivshmem_queue * q)132 int eth_ivshmem_queue_tx_commit_buff(struct eth_ivshmem_queue *q)
133 {
134 /* Ensure that a TX buffer is pending */
135 if (q->tx.pending_data_len == 0) {
136 return -EINVAL;
137 }
138
139 uint16_t desc_head = q->tx.desc_head;
140
141 q->tx.desc_len++;
142 q->tx.desc_head = (q->tx.desc_head + 1) % q->desc_max_len;
143
144 q->tx.data_head = q->tx.pending_data_head;
145 q->tx.data_len = q->tx.pending_data_len;
146
147 q->tx.vring.avail->ring[q->tx.avail_idx % q->desc_max_len] = desc_head;
148
149 VRING_FLUSH(q->tx.vring.avail->ring[q->tx.avail_idx % q->desc_max_len]);
150 atomic_thread_fence(memory_order_seq_cst);
151
152 q->tx.avail_idx++;
153 q->tx.vring.avail->idx = q->tx.avail_idx;
154
155 VRING_FLUSH(q->tx.vring.avail->idx);
156
157 q->tx.pending_data_len = 0;
158
159 return 0;
160 }
161
eth_ivshmem_queue_rx(struct eth_ivshmem_queue * q,const void ** data,size_t * len)162 int eth_ivshmem_queue_rx(struct eth_ivshmem_queue *q, const void **data, size_t *len)
163 {
164 *data = NULL;
165 *len = 0;
166
167 uint16_t avail_desc_idx;
168 int res = get_rx_avail_desc_idx(q, &avail_desc_idx);
169
170 if (res != 0) {
171 return res;
172 }
173
174 struct vring_desc *desc = &q->rx.vring.desc[avail_desc_idx];
175
176 VRING_INVALIDATE(*desc);
177
178 uint64_t offset = desc->addr - q->vring_header_size;
179 uint32_t rx_len = desc->len;
180
181 if (offset > q->vring_data_max_len ||
182 rx_len > q->vring_data_max_len ||
183 offset > q->vring_data_max_len - rx_len) {
184 return -EINVAL;
185 }
186
187 *data = (uint8_t *)q->rx.shmem + q->vring_header_size + offset;
188 *len = desc->len;
189
190 return 0;
191 }
192
eth_ivshmem_queue_rx_complete(struct eth_ivshmem_queue * q)193 int eth_ivshmem_queue_rx_complete(struct eth_ivshmem_queue *q)
194 {
195 uint16_t avail_desc_idx;
196 int res = get_rx_avail_desc_idx(q, &avail_desc_idx);
197
198 if (res != 0) {
199 return res;
200 }
201
202 uint16_t used_idx = q->rx.used_idx % q->desc_max_len;
203
204 q->rx.used_idx++;
205 q->rx.vring.used->ring[used_idx].id = avail_desc_idx;
206 q->rx.vring.used->ring[used_idx].len = 1;
207 VRING_FLUSH(q->rx.vring.used->ring[used_idx]);
208 atomic_thread_fence(memory_order_seq_cst);
209
210 q->rx.vring.used->idx = q->rx.used_idx;
211 VRING_FLUSH(q->rx.vring.used->idx);
212 atomic_thread_fence(memory_order_seq_cst);
213
214 q->rx.avail_idx++;
215 vring_avail_event(&q->rx.vring) = q->rx.avail_idx;
216 VRING_FLUSH(vring_avail_event(&q->rx.vring));
217
218 return 0;
219 }
220
221 /**
222 * Calculates the vring descriptor length and header size.
223 * This must match what is calculated by the peer.
224 */
calc_vring_size(size_t section_size,uint16_t * vring_desc_len,uint32_t * vring_header_size)225 static int calc_vring_size(
226 size_t section_size, uint16_t *vring_desc_len,
227 uint32_t *vring_header_size)
228 {
229 static const int eth_min_mtu = 68;
230 uint32_t header_size;
231 int16_t desc_len;
232
233 for (desc_len = 4096; desc_len > 32; desc_len >>= 1) {
234 header_size = vring_size(desc_len, ETH_IVSHMEM_VRING_ALIGNMENT);
235 header_size = ROUND_UP(header_size, ETH_IVSHMEM_VRING_ALIGNMENT);
236 if (header_size < section_size / 8) {
237 break;
238 }
239 }
240
241 if (header_size > section_size) {
242 return -EINVAL;
243 }
244
245 uint32_t vring_data_size = section_size - header_size;
246
247 if (vring_data_size < 4 * eth_min_mtu) {
248 return -EINVAL;
249 }
250
251 *vring_desc_len = desc_len;
252 *vring_header_size = header_size;
253
254 return 0;
255 }
256
tx_buffer_advance(uint32_t max_len,uint32_t * position,uint32_t * len)257 static uint32_t tx_buffer_advance(uint32_t max_len, uint32_t *position, uint32_t *len)
258 {
259 uint32_t aligned_len = ETH_IVSHMEM_FRAME_SIZE(*len);
260 uint32_t contiguous_len = max_len - *position;
261
262 *len = aligned_len;
263 if (aligned_len > contiguous_len) {
264 /* Wrap back to zero */
265 *position = 0;
266 *len += contiguous_len;
267 }
268
269 return *position + aligned_len;
270 }
271
tx_clean_used(struct eth_ivshmem_queue * q)272 static int tx_clean_used(struct eth_ivshmem_queue *q)
273 {
274 while (true) {
275 VRING_INVALIDATE(q->tx.vring.used->idx);
276 if (q->tx.used_idx == q->tx.vring.used->idx) {
277 break;
278 }
279
280 struct vring_used_elem *used = &q->tx.vring.used->ring[
281 q->tx.used_idx % q->desc_max_len];
282
283 atomic_thread_fence(memory_order_seq_cst);
284 VRING_INVALIDATE(*used);
285
286 if (used->id >= q->desc_max_len || used->len != 1) {
287 return -EINVAL;
288 }
289
290 struct vring_desc *desc = &q->tx.vring.desc[used->id];
291
292 uint64_t offset = desc->addr - q->vring_header_size;
293 uint32_t len = desc->len;
294
295 uint32_t tail = q->tx.data_tail;
296 uint32_t consumed_len = len;
297 uint32_t new_tail = tx_buffer_advance(q->vring_data_max_len, &tail, &consumed_len);
298
299 if (consumed_len > q->tx.data_len ||
300 offset != tail) {
301 return -EINVAL;
302 }
303
304 q->tx.data_tail = new_tail;
305 q->tx.data_len -= consumed_len;
306 q->tx.desc_len--;
307 q->tx.used_idx++;
308 }
309 return 0;
310 }
311
get_rx_avail_desc_idx(struct eth_ivshmem_queue * q,uint16_t * avail_desc_idx)312 static int get_rx_avail_desc_idx(struct eth_ivshmem_queue *q, uint16_t *avail_desc_idx)
313 {
314 atomic_thread_fence(memory_order_seq_cst);
315 VRING_INVALIDATE(q->rx.vring.avail->idx);
316
317 uint16_t avail_idx = q->rx.vring.avail->idx;
318
319 if (avail_idx == q->rx.avail_idx) {
320 return -EWOULDBLOCK;
321 }
322
323 VRING_INVALIDATE(q->rx.vring.avail->ring[q->rx.avail_idx % q->desc_max_len]);
324 *avail_desc_idx = q->rx.vring.avail->ring[q->rx.avail_idx % q->desc_max_len];
325 if (*avail_desc_idx >= q->desc_max_len) {
326 return -EINVAL;
327 }
328
329 return 0;
330 }
331
