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 
dpaa2_eth_setup_consume_func(struct dpaa2_eth_priv * priv,struct dpaa2_eth_channel * ch,enum dpaa2_eth_fq_type type,dpaa2_eth_consume_cb_t * consume)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 
dpaa2_xsk_run_xdp(struct dpaa2_eth_priv * priv,struct dpaa2_eth_channel * ch,struct dpaa2_eth_fq * rx_fq,struct dpaa2_fd * fd,void * vaddr)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_buff);
56 	xdp_buff->rxq = &ch->xdp_rxq;
57 
58 	xsk_buff_dma_sync_for_cpu(xdp_buff, ch->xsk_pool);
59 	xdp_act = bpf_prog_run_xdp(xdp_prog, xdp_buff);
60 
61 	/* xdp.data pointer may have changed */
62 	dpaa2_fd_set_offset(fd, xdp_buff->data - vaddr);
63 	dpaa2_fd_set_len(fd, xdp_buff->data_end - xdp_buff->data);
64 
65 	if (likely(xdp_act == XDP_REDIRECT)) {
66 		err = xdp_do_redirect(priv->net_dev, xdp_buff, 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, vaddr, rx_fq->flowid);
83 		break;
84 	default:
85 		bpf_warn_invalid_xdp_action(priv->net_dev, xdp_prog, xdp_act);
86 		fallthrough;
87 	case XDP_ABORTED:
88 		trace_xdp_exception(priv->net_dev, xdp_prog, 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 */
dpaa2_xsk_rx(struct dpaa2_eth_priv * priv,struct dpaa2_eth_channel * ch,const struct dpaa2_fd * fd,struct dpaa2_eth_fq * fq)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(priv->net_dev, fd);
117 
118 	vaddr = dpaa2_iova_to_virt(priv->iommu_domain, 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, fq, (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, 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 
dpaa2_xsk_set_bp_per_qdbin(struct dpaa2_eth_priv * priv,struct dpni_pools_cfg * pools_params)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 
dpaa2_xsk_disable_pool(struct net_device * dev,u16 qid)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, 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, 0);
191 	err = xdp_rxq_info_reg_mem_model(&ch->xdp_rxq,
192 					 MEM_TYPE_PAGE_ORDER0, NULL);
193 	if (err)
194 		netdev_err(dev, "xsk_rxq_info_reg_mem_model() failed (err = %d)\n",
195 			   err);
196 
197 	dpaa2_eth_free_dpbp(priv, 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, DPAA2_RX_FQ, dpaa2_eth_rx);
205 
206 	dpaa2_xsk_set_bp_per_qdbin(priv, &pools_params);
207 	err = dpni_set_pools(priv->mc_io, 0, priv->mc_token, &pools_params);
208 	if (err)
209 		netdev_err(dev, "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 
dpaa2_xsk_enable_pool(struct net_device * dev,struct xsk_buff_pool * pool,u16 qid)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, "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, "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, priv->net_dev->dev.parent, 0);
245 	if (err) {
246 		netdev_err(dev, "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(&ch->xdp_rxq, MEM_TYPE_XSK_BUFF_POOL, NULL);
253 	if (err) {
254 		netdev_err(dev, "xdp_rxq_info_reg_mem_model() failed (err = %d)\n", err);
255 		goto err_mem_model;
256 	}
257 	xsk_pool_set_rxq_info(pool, &ch->xdp_rxq);
258 
259 	priv->bp[priv->num_bps] = dpaa2_eth_allocate_dpbp(priv);
260 	if (IS_ERR(priv->bp[priv->num_bps])) {
261 		err = PTR_ERR(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, DPAA2_RX_FQ, dpaa2_xsk_rx);
269 
270 	dpaa2_xsk_set_bp_per_qdbin(priv, &pools_params);
271 	err = dpni_set_pools(priv->mc_io, 0, priv->mc_token, &pools_params);
272 	if (err) {
273 		netdev_err(dev, "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, "dpaa2_xsk_disable_pool() failed %d\n", err2);
289 err_bp_alloc:
290 	err2 = xdp_rxq_info_reg_mem_model(&priv->channel[qid]->xdp_rxq,
291 					  MEM_TYPE_PAGE_ORDER0, NULL);
292 	if (err2)
293 		netdev_err(dev, "xsk_rxq_info_reg_mem_model() failed with %d)\n", err2);
294 err_mem_model:
295 	xsk_pool_dma_unmap(pool, 0);
296 err_dma_unmap:
297 	if (up)
298 		dev_open(dev, NULL);
299 
300 	return err;
301 }
302 
dpaa2_xsk_setup_pool(struct net_device * dev,struct xsk_buff_pool * pool,u16 qid)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 
dpaa2_xsk_wakeup(struct net_device * dev,u32 qid,u32 flags)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(&ch->napi))
327 		napi_schedule(&ch->napi);
328 
329 	return 0;
330 }
331 
dpaa2_xsk_tx_build_fd(struct dpaa2_eth_priv * priv,struct dpaa2_eth_channel * ch,struct dpaa2_fd * fd,struct xdp_desc * xdp_desc)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(ch->xsk_pool, xdp_desc->addr);
355 	xsk_buff_raw_dma_sync_for_device(ch->xsk_pool, addr, xdp_desc->len);
356 
357 	/* Fill in the HW SGT structure */
358 	dpaa2_sg_set_addr(sgt, addr);
359 	dpaa2_sg_set_len(sgt, xdp_desc->len);
360 	dpaa2_sg_set_final(sgt, 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, priv->tx_data_offset);
377 	dpaa2_fd_set_format(fd, dpaa2_fd_sg);
378 	dpaa2_fd_set_addr(fd, sgt_addr);
379 	dpaa2_fd_set_len(fd, 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 
dpaa2_xsk_tx(struct dpaa2_eth_priv * priv,struct dpaa2_eth_channel * ch)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 *percpu_extras;
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(ch->xsk_pool, 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, &fds[i], &xdp_descs[i]);
417 		if (err) {
418 			batch = i;
419 			break;
420 		}
421 
422 		trace_dpaa2_tx_xsk_fd(priv->net_dev, &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(&fds[i]);
446 	for (i = total_enqueued; i < batch; i++) {
447 		dpaa2_eth_free_tx_fd(priv, ch, fq, &fds[i], false);
448 		percpu_stats->tx_errors++;
449 	}
450 
451 	xsk_tx_release(ch->xsk_pool);
452 
453 	return total_enqueued == budget;
454 }
455