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