1 | // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) |
2 | /* Copyright 2022 NXP |
3 | */ |
4 | #include <linux/filter.h> |
5 | #include <linux/compiler.h> |
6 | #include <linux/bpf_trace.h> |
7 | #include <net/xdp.h> |
8 | #include <net/xdp_sock_drv.h> |
9 | |
10 | #include "dpaa2-eth.h" |
11 | |
12 | static void dpaa2_eth_setup_consume_func(struct dpaa2_eth_priv *priv, |
13 | struct dpaa2_eth_channel *ch, |
14 | enum dpaa2_eth_fq_type type, |
15 | dpaa2_eth_consume_cb_t *consume) |
16 | { |
17 | struct dpaa2_eth_fq *fq; |
18 | int i; |
19 | |
20 | for (i = 0; i < priv->num_fqs; i++) { |
21 | fq = &priv->fq[i]; |
22 | |
23 | if (fq->type != type) |
24 | continue; |
25 | if (fq->channel != ch) |
26 | continue; |
27 | |
28 | fq->consume = consume; |
29 | } |
30 | } |
31 | |
32 | static u32 dpaa2_xsk_run_xdp(struct dpaa2_eth_priv *priv, |
33 | struct dpaa2_eth_channel *ch, |
34 | struct dpaa2_eth_fq *rx_fq, |
35 | struct dpaa2_fd *fd, void *vaddr) |
36 | { |
37 | dma_addr_t addr = dpaa2_fd_get_addr(fd); |
38 | struct bpf_prog *xdp_prog; |
39 | struct xdp_buff *xdp_buff; |
40 | struct dpaa2_eth_swa *swa; |
41 | u32 xdp_act = XDP_PASS; |
42 | int err; |
43 | |
44 | xdp_prog = READ_ONCE(ch->xdp.prog); |
45 | if (!xdp_prog) |
46 | goto out; |
47 | |
48 | swa = (struct dpaa2_eth_swa *)(vaddr + DPAA2_ETH_RX_HWA_SIZE + |
49 | ch->xsk_pool->umem->headroom); |
50 | xdp_buff = swa->xsk.xdp_buff; |
51 | |
52 | xdp_buff->data_hard_start = vaddr; |
53 | xdp_buff->data = vaddr + dpaa2_fd_get_offset(fd); |
54 | xdp_buff->data_end = xdp_buff->data + dpaa2_fd_get_len(fd); |
55 | xdp_set_data_meta_invalid(xdp: xdp_buff); |
56 | xdp_buff->rxq = &ch->xdp_rxq; |
57 | |
58 | xsk_buff_dma_sync_for_cpu(xdp: xdp_buff, pool: ch->xsk_pool); |
59 | xdp_act = bpf_prog_run_xdp(prog: xdp_prog, xdp: xdp_buff); |
60 | |
61 | /* xdp.data pointer may have changed */ |
62 | dpaa2_fd_set_offset(fd, offset: xdp_buff->data - vaddr); |
63 | dpaa2_fd_set_len(fd, len: xdp_buff->data_end - xdp_buff->data); |
64 | |
65 | if (likely(xdp_act == XDP_REDIRECT)) { |
66 | err = xdp_do_redirect(dev: priv->net_dev, xdp: xdp_buff, prog: xdp_prog); |
67 | if (unlikely(err)) { |
68 | ch->stats.xdp_drop++; |
69 | dpaa2_eth_recycle_buf(priv, ch, addr); |
70 | } else { |
71 | ch->buf_count--; |
72 | ch->stats.xdp_redirect++; |
73 | } |
74 | |
75 | goto xdp_redir; |
76 | } |
77 | |
78 | switch (xdp_act) { |
79 | case XDP_PASS: |
80 | break; |
81 | case XDP_TX: |
82 | dpaa2_eth_xdp_enqueue(priv, ch, fd, buf_start: vaddr, queue_id: rx_fq->flowid); |
83 | break; |
84 | default: |
85 | bpf_warn_invalid_xdp_action(dev: priv->net_dev, prog: xdp_prog, act: xdp_act); |
86 | fallthrough; |
87 | case XDP_ABORTED: |
88 | trace_xdp_exception(dev: priv->net_dev, xdp: xdp_prog, act: xdp_act); |
89 | fallthrough; |
90 | case XDP_DROP: |
91 | dpaa2_eth_recycle_buf(priv, ch, addr); |
92 | ch->stats.xdp_drop++; |
93 | break; |
94 | } |
95 | |
96 | xdp_redir: |
97 | ch->xdp.res |= xdp_act; |
98 | out: |
99 | return xdp_act; |
100 | } |
101 | |
102 | /* Rx frame processing routine for the AF_XDP fast path */ |
103 | static void dpaa2_xsk_rx(struct dpaa2_eth_priv *priv, |
104 | struct dpaa2_eth_channel *ch, |
105 | const struct dpaa2_fd *fd, |
106 | struct dpaa2_eth_fq *fq) |
107 | { |
108 | dma_addr_t addr = dpaa2_fd_get_addr(fd); |
109 | u8 fd_format = dpaa2_fd_get_format(fd); |
110 | struct rtnl_link_stats64 *percpu_stats; |
111 | u32 fd_length = dpaa2_fd_get_len(fd); |
112 | struct sk_buff *skb; |
113 | void *vaddr; |
114 | u32 xdp_act; |
115 | |
116 | trace_dpaa2_rx_xsk_fd(netdev: priv->net_dev, fd); |
117 | |
118 | vaddr = dpaa2_iova_to_virt(domain: priv->iommu_domain, iova_addr: addr); |
119 | percpu_stats = this_cpu_ptr(priv->percpu_stats); |
120 | |
121 | if (fd_format != dpaa2_fd_single) { |
122 | WARN_ON(priv->xdp_prog); |
123 | /* AF_XDP doesn't support any other formats */ |
124 | goto err_frame_format; |
125 | } |
126 | |
127 | xdp_act = dpaa2_xsk_run_xdp(priv, ch, rx_fq: fq, fd: (struct dpaa2_fd *)fd, vaddr); |
128 | if (xdp_act != XDP_PASS) { |
129 | percpu_stats->rx_packets++; |
130 | percpu_stats->rx_bytes += dpaa2_fd_get_len(fd); |
131 | return; |
132 | } |
133 | |
134 | /* Build skb */ |
135 | skb = dpaa2_eth_alloc_skb(priv, ch, fd, fd_length, fd_vaddr: vaddr); |
136 | if (!skb) |
137 | /* Nothing else we can do, recycle the buffer and |
138 | * drop the frame. |
139 | */ |
140 | goto err_alloc_skb; |
141 | |
142 | /* Send the skb to the Linux networking stack */ |
143 | dpaa2_eth_receive_skb(priv, ch, fd, vaddr, fq, percpu_stats, skb); |
144 | |
145 | return; |
146 | |
147 | err_alloc_skb: |
148 | dpaa2_eth_recycle_buf(priv, ch, addr); |
149 | err_frame_format: |
150 | percpu_stats->rx_dropped++; |
151 | } |
152 | |
153 | static void dpaa2_xsk_set_bp_per_qdbin(struct dpaa2_eth_priv *priv, |
154 | struct dpni_pools_cfg *pools_params) |
155 | { |
156 | int curr_bp = 0, i, j; |
157 | |
158 | pools_params->pool_options = DPNI_POOL_ASSOC_QDBIN; |
159 | for (i = 0; i < priv->num_bps; i++) { |
160 | for (j = 0; j < priv->num_channels; j++) |
161 | if (priv->bp[i] == priv->channel[j]->bp) |
162 | pools_params->pools[curr_bp].priority_mask |= (1 << j); |
163 | if (!pools_params->pools[curr_bp].priority_mask) |
164 | continue; |
165 | |
166 | pools_params->pools[curr_bp].dpbp_id = priv->bp[i]->bpid; |
167 | pools_params->pools[curr_bp].buffer_size = priv->rx_buf_size; |
168 | pools_params->pools[curr_bp++].backup_pool = 0; |
169 | } |
170 | pools_params->num_dpbp = curr_bp; |
171 | } |
172 | |
173 | static int dpaa2_xsk_disable_pool(struct net_device *dev, u16 qid) |
174 | { |
175 | struct xsk_buff_pool *pool = xsk_get_pool_from_qid(dev, queue_id: qid); |
176 | struct dpaa2_eth_priv *priv = netdev_priv(dev); |
177 | struct dpni_pools_cfg pools_params = { 0 }; |
178 | struct dpaa2_eth_channel *ch; |
179 | int err; |
180 | bool up; |
181 | |
182 | ch = priv->channel[qid]; |
183 | if (!ch->xsk_pool) |
184 | return -EINVAL; |
185 | |
186 | up = netif_running(dev); |
187 | if (up) |
188 | dev_close(dev); |
189 | |
190 | xsk_pool_dma_unmap(pool, attrs: 0); |
191 | err = xdp_rxq_info_reg_mem_model(xdp_rxq: &ch->xdp_rxq, |
192 | type: MEM_TYPE_PAGE_ORDER0, NULL); |
193 | if (err) |
194 | netdev_err(dev, format: "xsk_rxq_info_reg_mem_model() failed (err = %d)\n" , |
195 | err); |
196 | |
197 | dpaa2_eth_free_dpbp(priv, bp: ch->bp); |
198 | |
199 | ch->xsk_zc = false; |
200 | ch->xsk_pool = NULL; |
201 | ch->xsk_tx_pkts_sent = 0; |
202 | ch->bp = priv->bp[DPAA2_ETH_DEFAULT_BP_IDX]; |
203 | |
204 | dpaa2_eth_setup_consume_func(priv, ch, type: DPAA2_RX_FQ, consume: dpaa2_eth_rx); |
205 | |
206 | dpaa2_xsk_set_bp_per_qdbin(priv, pools_params: &pools_params); |
207 | err = dpni_set_pools(mc_io: priv->mc_io, cmd_flags: 0, token: priv->mc_token, cfg: &pools_params); |
208 | if (err) |
209 | netdev_err(dev, format: "dpni_set_pools() failed\n" ); |
210 | |
211 | if (up) { |
212 | err = dev_open(dev, NULL); |
213 | if (err) |
214 | return err; |
215 | } |
216 | |
217 | return 0; |
218 | } |
219 | |
220 | static int dpaa2_xsk_enable_pool(struct net_device *dev, |
221 | struct xsk_buff_pool *pool, |
222 | u16 qid) |
223 | { |
224 | struct dpaa2_eth_priv *priv = netdev_priv(dev); |
225 | struct dpni_pools_cfg pools_params = { 0 }; |
226 | struct dpaa2_eth_channel *ch; |
227 | int err, err2; |
228 | bool up; |
229 | |
230 | if (priv->dpni_attrs.wriop_version < DPAA2_WRIOP_VERSION(3, 0, 0)) { |
231 | netdev_err(dev, format: "AF_XDP zero-copy not supported on devices <= WRIOP(3, 0, 0)\n" ); |
232 | return -EOPNOTSUPP; |
233 | } |
234 | |
235 | if (priv->dpni_attrs.num_queues > 8) { |
236 | netdev_err(dev, format: "AF_XDP zero-copy not supported on DPNI with more then 8 queues\n" ); |
237 | return -EOPNOTSUPP; |
238 | } |
239 | |
240 | up = netif_running(dev); |
241 | if (up) |
242 | dev_close(dev); |
243 | |
244 | err = xsk_pool_dma_map(pool, dev: priv->net_dev->dev.parent, attrs: 0); |
245 | if (err) { |
246 | netdev_err(dev, format: "xsk_pool_dma_map() failed (err = %d)\n" , |
247 | err); |
248 | goto err_dma_unmap; |
249 | } |
250 | |
251 | ch = priv->channel[qid]; |
252 | err = xdp_rxq_info_reg_mem_model(xdp_rxq: &ch->xdp_rxq, type: MEM_TYPE_XSK_BUFF_POOL, NULL); |
253 | if (err) { |
254 | netdev_err(dev, format: "xdp_rxq_info_reg_mem_model() failed (err = %d)\n" , err); |
255 | goto err_mem_model; |
256 | } |
257 | xsk_pool_set_rxq_info(pool, rxq: &ch->xdp_rxq); |
258 | |
259 | priv->bp[priv->num_bps] = dpaa2_eth_allocate_dpbp(priv); |
260 | if (IS_ERR(ptr: priv->bp[priv->num_bps])) { |
261 | err = PTR_ERR(ptr: priv->bp[priv->num_bps]); |
262 | goto err_bp_alloc; |
263 | } |
264 | ch->xsk_zc = true; |
265 | ch->xsk_pool = pool; |
266 | ch->bp = priv->bp[priv->num_bps++]; |
267 | |
268 | dpaa2_eth_setup_consume_func(priv, ch, type: DPAA2_RX_FQ, consume: dpaa2_xsk_rx); |
269 | |
270 | dpaa2_xsk_set_bp_per_qdbin(priv, pools_params: &pools_params); |
271 | err = dpni_set_pools(mc_io: priv->mc_io, cmd_flags: 0, token: priv->mc_token, cfg: &pools_params); |
272 | if (err) { |
273 | netdev_err(dev, format: "dpni_set_pools() failed\n" ); |
274 | goto err_set_pools; |
275 | } |
276 | |
277 | if (up) { |
278 | err = dev_open(dev, NULL); |
279 | if (err) |
280 | return err; |
281 | } |
282 | |
283 | return 0; |
284 | |
285 | err_set_pools: |
286 | err2 = dpaa2_xsk_disable_pool(dev, qid); |
287 | if (err2) |
288 | netdev_err(dev, format: "dpaa2_xsk_disable_pool() failed %d\n" , err2); |
289 | err_bp_alloc: |
290 | err2 = xdp_rxq_info_reg_mem_model(xdp_rxq: &priv->channel[qid]->xdp_rxq, |
291 | type: MEM_TYPE_PAGE_ORDER0, NULL); |
292 | if (err2) |
293 | netdev_err(dev, format: "xsk_rxq_info_reg_mem_model() failed with %d)\n" , err2); |
294 | err_mem_model: |
295 | xsk_pool_dma_unmap(pool, attrs: 0); |
296 | err_dma_unmap: |
297 | if (up) |
298 | dev_open(dev, NULL); |
299 | |
300 | return err; |
301 | } |
302 | |
303 | int dpaa2_xsk_setup_pool(struct net_device *dev, struct xsk_buff_pool *pool, u16 qid) |
304 | { |
305 | return pool ? dpaa2_xsk_enable_pool(dev, pool, qid) : |
306 | dpaa2_xsk_disable_pool(dev, qid); |
307 | } |
308 | |
309 | int dpaa2_xsk_wakeup(struct net_device *dev, u32 qid, u32 flags) |
310 | { |
311 | struct dpaa2_eth_priv *priv = netdev_priv(dev); |
312 | struct dpaa2_eth_channel *ch = priv->channel[qid]; |
313 | |
314 | if (!priv->link_state.up) |
315 | return -ENETDOWN; |
316 | |
317 | if (!priv->xdp_prog) |
318 | return -EINVAL; |
319 | |
320 | if (!ch->xsk_zc) |
321 | return -EINVAL; |
322 | |
323 | /* We do not have access to a per channel SW interrupt, so instead we |
324 | * schedule a NAPI instance. |
325 | */ |
326 | if (!napi_if_scheduled_mark_missed(n: &ch->napi)) |
327 | napi_schedule(n: &ch->napi); |
328 | |
329 | return 0; |
330 | } |
331 | |
332 | static int dpaa2_xsk_tx_build_fd(struct dpaa2_eth_priv *priv, |
333 | struct dpaa2_eth_channel *ch, |
334 | struct dpaa2_fd *fd, |
335 | struct xdp_desc *xdp_desc) |
336 | { |
337 | struct device *dev = priv->net_dev->dev.parent; |
338 | struct dpaa2_sg_entry *sgt; |
339 | struct dpaa2_eth_swa *swa; |
340 | void *sgt_buf = NULL; |
341 | dma_addr_t sgt_addr; |
342 | int sgt_buf_size; |
343 | dma_addr_t addr; |
344 | int err = 0; |
345 | |
346 | /* Prepare the HW SGT structure */ |
347 | sgt_buf_size = priv->tx_data_offset + sizeof(struct dpaa2_sg_entry); |
348 | sgt_buf = dpaa2_eth_sgt_get(priv); |
349 | if (unlikely(!sgt_buf)) |
350 | return -ENOMEM; |
351 | sgt = (struct dpaa2_sg_entry *)(sgt_buf + priv->tx_data_offset); |
352 | |
353 | /* Get the address of the XSK Tx buffer */ |
354 | addr = xsk_buff_raw_get_dma(pool: ch->xsk_pool, addr: xdp_desc->addr); |
355 | xsk_buff_raw_dma_sync_for_device(pool: ch->xsk_pool, dma: addr, size: xdp_desc->len); |
356 | |
357 | /* Fill in the HW SGT structure */ |
358 | dpaa2_sg_set_addr(sg: sgt, addr); |
359 | dpaa2_sg_set_len(sg: sgt, len: xdp_desc->len); |
360 | dpaa2_sg_set_final(sg: sgt, final: true); |
361 | |
362 | /* Store the necessary info in the SGT buffer */ |
363 | swa = (struct dpaa2_eth_swa *)sgt_buf; |
364 | swa->type = DPAA2_ETH_SWA_XSK; |
365 | swa->xsk.sgt_size = sgt_buf_size; |
366 | |
367 | /* Separately map the SGT buffer */ |
368 | sgt_addr = dma_map_single(dev, sgt_buf, sgt_buf_size, DMA_BIDIRECTIONAL); |
369 | if (unlikely(dma_mapping_error(dev, sgt_addr))) { |
370 | err = -ENOMEM; |
371 | goto sgt_map_failed; |
372 | } |
373 | |
374 | /* Initialize FD fields */ |
375 | memset(fd, 0, sizeof(struct dpaa2_fd)); |
376 | dpaa2_fd_set_offset(fd, offset: priv->tx_data_offset); |
377 | dpaa2_fd_set_format(fd, format: dpaa2_fd_sg); |
378 | dpaa2_fd_set_addr(fd, addr: sgt_addr); |
379 | dpaa2_fd_set_len(fd, len: xdp_desc->len); |
380 | dpaa2_fd_set_ctrl(fd, FD_CTRL_PTA); |
381 | |
382 | return 0; |
383 | |
384 | sgt_map_failed: |
385 | dpaa2_eth_sgt_recycle(priv, sgt_buf); |
386 | |
387 | return err; |
388 | } |
389 | |
390 | bool dpaa2_xsk_tx(struct dpaa2_eth_priv *priv, |
391 | struct dpaa2_eth_channel *ch) |
392 | { |
393 | struct xdp_desc *xdp_descs = ch->xsk_pool->tx_descs; |
394 | struct dpaa2_eth_drv_stats *; |
395 | struct rtnl_link_stats64 *percpu_stats; |
396 | int budget = DPAA2_ETH_TX_ZC_PER_NAPI; |
397 | int total_enqueued, enqueued; |
398 | int retries, max_retries; |
399 | struct dpaa2_eth_fq *fq; |
400 | struct dpaa2_fd *fds; |
401 | int batch, i, err; |
402 | |
403 | percpu_stats = this_cpu_ptr(priv->percpu_stats); |
404 | percpu_extras = this_cpu_ptr(priv->percpu_extras); |
405 | fds = (this_cpu_ptr(priv->fd))->array; |
406 | |
407 | /* Use the FQ with the same idx as the affine CPU */ |
408 | fq = &priv->fq[ch->nctx.desired_cpu]; |
409 | |
410 | batch = xsk_tx_peek_release_desc_batch(pool: ch->xsk_pool, max: budget); |
411 | if (!batch) |
412 | return false; |
413 | |
414 | /* Create a FD for each XSK frame to be sent */ |
415 | for (i = 0; i < batch; i++) { |
416 | err = dpaa2_xsk_tx_build_fd(priv, ch, fd: &fds[i], xdp_desc: &xdp_descs[i]); |
417 | if (err) { |
418 | batch = i; |
419 | break; |
420 | } |
421 | |
422 | trace_dpaa2_tx_xsk_fd(netdev: priv->net_dev, fd: &fds[i]); |
423 | } |
424 | |
425 | /* Enqueue all the created FDs */ |
426 | max_retries = batch * DPAA2_ETH_ENQUEUE_RETRIES; |
427 | total_enqueued = 0; |
428 | enqueued = 0; |
429 | retries = 0; |
430 | while (total_enqueued < batch && retries < max_retries) { |
431 | err = priv->enqueue(priv, fq, &fds[total_enqueued], 0, |
432 | batch - total_enqueued, &enqueued); |
433 | if (err == -EBUSY) { |
434 | retries++; |
435 | continue; |
436 | } |
437 | |
438 | total_enqueued += enqueued; |
439 | } |
440 | percpu_extras->tx_portal_busy += retries; |
441 | |
442 | /* Update statistics */ |
443 | percpu_stats->tx_packets += total_enqueued; |
444 | for (i = 0; i < total_enqueued; i++) |
445 | percpu_stats->tx_bytes += dpaa2_fd_get_len(fd: &fds[i]); |
446 | for (i = total_enqueued; i < batch; i++) { |
447 | dpaa2_eth_free_tx_fd(priv, ch, fq, fd: &fds[i], in_napi: false); |
448 | percpu_stats->tx_errors++; |
449 | } |
450 | |
451 | xsk_tx_release(pool: ch->xsk_pool); |
452 | |
453 | return total_enqueued == budget; |
454 | } |
455 | |