1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0-or-later
2 /*
3  * Copyright 2008 - 2016 Freescale Semiconductor Inc.
4  * Copyright 2020 NXP
5  */
6 
7 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
8 
9 #include <linux/init.h>
10 #include <linux/module.h>
11 #include <linux/of_platform.h>
12 #include <linux/of_mdio.h>
13 #include <linux/of_net.h>
14 #include <linux/io.h>
15 #include <linux/if_arp.h>
16 #include <linux/if_vlan.h>
17 #include <linux/icmp.h>
18 #include <linux/ip.h>
19 #include <linux/ipv6.h>
20 #include <linux/udp.h>
21 #include <linux/tcp.h>
22 #include <linux/net.h>
23 #include <linux/skbuff.h>
24 #include <linux/etherdevice.h>
25 #include <linux/if_ether.h>
26 #include <linux/highmem.h>
27 #include <linux/percpu.h>
28 #include <linux/dma-mapping.h>
29 #include <linux/sort.h>
30 #include <linux/phy_fixed.h>
31 #include <linux/bpf.h>
32 #include <linux/bpf_trace.h>
33 #include <soc/fsl/bman.h>
34 #include <soc/fsl/qman.h>
35 #include "fman.h"
36 #include "fman_port.h"
37 #include "mac.h"
38 #include "dpaa_eth.h"
39 
40 /* CREATE_TRACE_POINTS only needs to be defined once. Other dpaa files
41  * using trace events only need to #include <trace/events/sched.h>
42  */
43 #define CREATE_TRACE_POINTS
44 #include "dpaa_eth_trace.h"
45 
46 static int debug = -1;
47 module_param(debug, int, 0444);
48 MODULE_PARM_DESC(debug, "Module/Driver verbosity level (0=none,...,16=all)");
49 
50 static u16 tx_timeout = 1000;
51 module_param(tx_timeout, ushort, 0444);
52 MODULE_PARM_DESC(tx_timeout, "The Tx timeout in ms");
53 
54 #define FM_FD_STAT_RX_ERRORS						\
55 	(FM_FD_ERR_DMA | FM_FD_ERR_PHYSICAL	| \
56 	 FM_FD_ERR_SIZE | FM_FD_ERR_CLS_DISCARD | \
57 	 FM_FD_ERR_EXTRACTION | FM_FD_ERR_NO_SCHEME	| \
58 	 FM_FD_ERR_PRS_TIMEOUT | FM_FD_ERR_PRS_ILL_INSTRUCT | \
59 	 FM_FD_ERR_PRS_HDR_ERR)
60 
61 #define FM_FD_STAT_TX_ERRORS \
62 	(FM_FD_ERR_UNSUPPORTED_FORMAT | \
63 	 FM_FD_ERR_LENGTH | FM_FD_ERR_DMA)
64 
65 #define DPAA_MSG_DEFAULT (NETIF_MSG_DRV | NETIF_MSG_PROBE | \
66 			  NETIF_MSG_LINK | NETIF_MSG_IFUP | \
67 			  NETIF_MSG_IFDOWN | NETIF_MSG_HW)
68 
69 #define DPAA_INGRESS_CS_THRESHOLD 0x10000000
70 /* Ingress congestion threshold on FMan ports
71  * The size in bytes of the ingress tail-drop threshold on FMan ports.
72  * Traffic piling up above this value will be rejected by QMan and discarded
73  * by FMan.
74  */
75 
76 /* Size in bytes of the FQ taildrop threshold */
77 #define DPAA_FQ_TD 0x200000
78 
79 #define DPAA_CS_THRESHOLD_1G 0x06000000
80 /* Egress congestion threshold on 1G ports, range 0x1000 .. 0x10000000
81  * The size in bytes of the egress Congestion State notification threshold on
82  * 1G ports. The 1G dTSECs can quite easily be flooded by cores doing Tx in a
83  * tight loop (e.g. by sending UDP datagrams at "while(1) speed"),
84  * and the larger the frame size, the more acute the problem.
85  * So we have to find a balance between these factors:
86  * - avoiding the device staying congested for a prolonged time (risking
87  *   the netdev watchdog to fire - see also the tx_timeout module param);
88  * - affecting performance of protocols such as TCP, which otherwise
89  *   behave well under the congestion notification mechanism;
90  * - preventing the Tx cores from tightly-looping (as if the congestion
91  *   threshold was too low to be effective);
92  * - running out of memory if the CS threshold is set too high.
93  */
94 
95 #define DPAA_CS_THRESHOLD_10G 0x10000000
96 /* The size in bytes of the egress Congestion State notification threshold on
97  * 10G ports, range 0x1000 .. 0x10000000
98  */
99 
100 /* Largest value that the FQD's OAL field can hold */
101 #define FSL_QMAN_MAX_OAL	127
102 
103 /* Default alignment for start of data in an Rx FD */
104 #ifdef CONFIG_DPAA_ERRATUM_A050385
105 /* aligning data start to 64 avoids DMA transaction splits, unless the buffer
106  * is crossing a 4k page boundary
107  */
108 #define DPAA_FD_DATA_ALIGNMENT  (fman_has_errata_a050385() ? 64 : 16)
109 /* aligning to 256 avoids DMA transaction splits caused by 4k page boundary
110  * crossings; also, all SG fragments except the last must have a size multiple
111  * of 256 to avoid DMA transaction splits
112  */
113 #define DPAA_A050385_ALIGN 256
114 #define DPAA_FD_RX_DATA_ALIGNMENT (fman_has_errata_a050385() ? \
115 				   DPAA_A050385_ALIGN : 16)
116 #else
117 #define DPAA_FD_DATA_ALIGNMENT  16
118 #define DPAA_FD_RX_DATA_ALIGNMENT DPAA_FD_DATA_ALIGNMENT
119 #endif
120 
121 /* The DPAA requires 256 bytes reserved and mapped for the SGT */
122 #define DPAA_SGT_SIZE 256
123 
124 /* Values for the L3R field of the FM Parse Results
125  */
126 /* L3 Type field: First IP Present IPv4 */
127 #define FM_L3_PARSE_RESULT_IPV4	0x8000
128 /* L3 Type field: First IP Present IPv6 */
129 #define FM_L3_PARSE_RESULT_IPV6	0x4000
130 /* Values for the L4R field of the FM Parse Results */
131 /* L4 Type field: UDP */
132 #define FM_L4_PARSE_RESULT_UDP	0x40
133 /* L4 Type field: TCP */
134 #define FM_L4_PARSE_RESULT_TCP	0x20
135 
136 /* FD status field indicating whether the FM Parser has attempted to validate
137  * the L4 csum of the frame.
138  * Note that having this bit set doesn't necessarily imply that the checksum
139  * is valid. One would have to check the parse results to find that out.
140  */
141 #define FM_FD_STAT_L4CV         0x00000004
142 
143 #define DPAA_SGT_MAX_ENTRIES 16 /* maximum number of entries in SG Table */
144 #define DPAA_BUFF_RELEASE_MAX 8 /* maximum number of buffers released at once */
145 
146 #define FSL_DPAA_BPID_INV		0xff
147 #define FSL_DPAA_ETH_MAX_BUF_COUNT	128
148 #define FSL_DPAA_ETH_REFILL_THRESHOLD	80
149 
150 #define DPAA_TX_PRIV_DATA_SIZE	16
151 #define DPAA_PARSE_RESULTS_SIZE sizeof(struct fman_prs_result)
152 #define DPAA_TIME_STAMP_SIZE 8
153 #define DPAA_HASH_RESULTS_SIZE 8
154 #define DPAA_HWA_SIZE (DPAA_PARSE_RESULTS_SIZE + DPAA_TIME_STAMP_SIZE \
155 		       + DPAA_HASH_RESULTS_SIZE)
156 #define DPAA_RX_PRIV_DATA_DEFAULT_SIZE (DPAA_TX_PRIV_DATA_SIZE + \
157 					XDP_PACKET_HEADROOM - DPAA_HWA_SIZE)
158 #ifdef CONFIG_DPAA_ERRATUM_A050385
159 #define DPAA_RX_PRIV_DATA_A050385_SIZE (DPAA_A050385_ALIGN - DPAA_HWA_SIZE)
160 #define DPAA_RX_PRIV_DATA_SIZE (fman_has_errata_a050385() ? \
161 				DPAA_RX_PRIV_DATA_A050385_SIZE : \
162 				DPAA_RX_PRIV_DATA_DEFAULT_SIZE)
163 #else
164 #define DPAA_RX_PRIV_DATA_SIZE DPAA_RX_PRIV_DATA_DEFAULT_SIZE
165 #endif
166 
167 #define DPAA_ETH_PCD_RXQ_NUM	128
168 
169 #define DPAA_ENQUEUE_RETRIES	100000
170 
171 enum port_type {RX, TX};
172 
173 struct fm_port_fqs {
174 	struct dpaa_fq *tx_defq;
175 	struct dpaa_fq *tx_errq;
176 	struct dpaa_fq *rx_defq;
177 	struct dpaa_fq *rx_errq;
178 	struct dpaa_fq *rx_pcdq;
179 };
180 
181 /* All the dpa bps in use at any moment */
182 static struct dpaa_bp *dpaa_bp_array[BM_MAX_NUM_OF_POOLS];
183 
184 #define DPAA_BP_RAW_SIZE 4096
185 
186 #ifdef CONFIG_DPAA_ERRATUM_A050385
187 #define dpaa_bp_size(raw_size) (SKB_WITH_OVERHEAD(raw_size) & \
188 				~(DPAA_A050385_ALIGN - 1))
189 #else
190 #define dpaa_bp_size(raw_size) SKB_WITH_OVERHEAD(raw_size)
191 #endif
192 
193 static int dpaa_max_frm;
194 
195 static int dpaa_rx_extra_headroom;
196 
197 #define dpaa_get_max_mtu()	\
198 	(dpaa_max_frm - (VLAN_ETH_HLEN + ETH_FCS_LEN))
199 
200 static void dpaa_eth_cgr_set_speed(struct mac_device *mac_dev, int speed);
201 
202 static int dpaa_netdev_init(struct net_device *net_dev,
203 			    const struct net_device_ops *dpaa_ops,
204 			    u16 tx_timeout)
205 {
206 	struct dpaa_priv *priv = netdev_priv(net_dev);
207 	struct device *dev = net_dev->dev.parent;
208 	struct mac_device *mac_dev = priv->mac_dev;
209 	struct dpaa_percpu_priv *percpu_priv;
210 	const u8 *mac_addr;
211 	int i, err;
212 
213 	/* Although we access another CPU's private data here
214 	 * we do it at initialization so it is safe
215 	 */
216 	for_each_possible_cpu(i) {
217 		percpu_priv = per_cpu_ptr(priv->percpu_priv, i);
218 		percpu_priv->net_dev = net_dev;
219 	}
220 
221 	net_dev->netdev_ops = dpaa_ops;
222 	mac_addr = mac_dev->addr;
223 
224 	net_dev->mem_start = (unsigned long)priv->mac_dev->res->start;
225 	net_dev->mem_end = (unsigned long)priv->mac_dev->res->end;
226 
227 	net_dev->min_mtu = ETH_MIN_MTU;
228 	net_dev->max_mtu = dpaa_get_max_mtu();
229 
230 	net_dev->hw_features |= (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
231 				 NETIF_F_LLTX | NETIF_F_RXHASH);
232 
233 	net_dev->hw_features |= NETIF_F_SG | NETIF_F_HIGHDMA;
234 	/* The kernels enables GSO automatically, if we declare NETIF_F_SG.
235 	 * For conformity, we'll still declare GSO explicitly.
236 	 */
237 	net_dev->features |= NETIF_F_GSO;
238 	net_dev->features |= NETIF_F_RXCSUM;
239 
240 	net_dev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
241 	/* we do not want shared skbs on TX */
242 	net_dev->priv_flags &= ~IFF_TX_SKB_SHARING;
243 
244 	net_dev->features |= net_dev->hw_features;
245 	net_dev->vlan_features = net_dev->features;
246 
247 	net_dev->xdp_features = NETDEV_XDP_ACT_BASIC |
248 				NETDEV_XDP_ACT_REDIRECT |
249 				NETDEV_XDP_ACT_NDO_XMIT;
250 
251 	if (is_valid_ether_addr(mac_addr)) {
252 		memcpy(net_dev->perm_addr, mac_addr, net_dev->addr_len);
253 		eth_hw_addr_set(net_dev, mac_addr);
254 	} else {
255 		eth_hw_addr_random(net_dev);
256 		err = mac_dev->change_addr(mac_dev->fman_mac,
257 			(const enet_addr_t *)net_dev->dev_addr);
258 		if (err) {
259 			dev_err(dev, "Failed to set random MAC address\n");
260 			return -EINVAL;
261 		}
262 		dev_info(dev, "Using random MAC address: %pM\n",
263 			 net_dev->dev_addr);
264 	}
265 
266 	net_dev->ethtool_ops = &dpaa_ethtool_ops;
267 
268 	net_dev->needed_headroom = priv->tx_headroom;
269 	net_dev->watchdog_timeo = msecs_to_jiffies(tx_timeout);
270 
271 	/* The rest of the config is filled in by the mac device already */
272 	mac_dev->phylink_config.dev = &net_dev->dev;
273 	mac_dev->phylink_config.type = PHYLINK_NETDEV;
274 	mac_dev->update_speed = dpaa_eth_cgr_set_speed;
275 	mac_dev->phylink = phylink_create(&mac_dev->phylink_config,
276 					  dev_fwnode(mac_dev->dev),
277 					  mac_dev->phy_if,
278 					  mac_dev->phylink_ops);
279 	if (IS_ERR(mac_dev->phylink)) {
280 		err = PTR_ERR(mac_dev->phylink);
281 		dev_err_probe(dev, err, "Could not create phylink\n");
282 		return err;
283 	}
284 
285 	/* start without the RUNNING flag, phylib controls it later */
286 	netif_carrier_off(net_dev);
287 
288 	err = register_netdev(net_dev);
289 	if (err < 0) {
290 		dev_err(dev, "register_netdev() = %d\n", err);
291 		phylink_destroy(mac_dev->phylink);
292 		return err;
293 	}
294 
295 	return 0;
296 }
297 
298 static int dpaa_stop(struct net_device *net_dev)
299 {
300 	struct mac_device *mac_dev;
301 	struct dpaa_priv *priv;
302 	int i, error;
303 	int err = 0;
304 
305 	priv = netdev_priv(net_dev);
306 	mac_dev = priv->mac_dev;
307 
308 	netif_tx_stop_all_queues(net_dev);
309 	/* Allow the Fman (Tx) port to process in-flight frames before we
310 	 * try switching it off.
311 	 */
312 	msleep(200);
313 
314 	phylink_stop(mac_dev->phylink);
315 	mac_dev->disable(mac_dev->fman_mac);
316 
317 	for (i = 0; i < ARRAY_SIZE(mac_dev->port); i++) {
318 		error = fman_port_disable(mac_dev->port[i]);
319 		if (error)
320 			err = error;
321 	}
322 
323 	phylink_disconnect_phy(mac_dev->phylink);
324 	net_dev->phydev = NULL;
325 
326 	msleep(200);
327 
328 	return err;
329 }
330 
331 static void dpaa_tx_timeout(struct net_device *net_dev, unsigned int txqueue)
332 {
333 	struct dpaa_percpu_priv *percpu_priv;
334 	const struct dpaa_priv	*priv;
335 
336 	priv = netdev_priv(net_dev);
337 	percpu_priv = this_cpu_ptr(priv->percpu_priv);
338 
339 	netif_crit(priv, timer, net_dev, "Transmit timeout latency: %u ms\n",
340 		   jiffies_to_msecs(jiffies - dev_trans_start(net_dev)));
341 
342 	percpu_priv->stats.tx_errors++;
343 }
344 
345 /* Calculates the statistics for the given device by adding the statistics
346  * collected by each CPU.
347  */
348 static void dpaa_get_stats64(struct net_device *net_dev,
349 			     struct rtnl_link_stats64 *s)
350 {
351 	int numstats = sizeof(struct rtnl_link_stats64) / sizeof(u64);
352 	struct dpaa_priv *priv = netdev_priv(net_dev);
353 	struct dpaa_percpu_priv *percpu_priv;
354 	u64 *netstats = (u64 *)s;
355 	u64 *cpustats;
356 	int i, j;
357 
358 	for_each_possible_cpu(i) {
359 		percpu_priv = per_cpu_ptr(priv->percpu_priv, i);
360 
361 		cpustats = (u64 *)&percpu_priv->stats;
362 
363 		/* add stats from all CPUs */
364 		for (j = 0; j < numstats; j++)
365 			netstats[j] += cpustats[j];
366 	}
367 }
368 
369 static int dpaa_setup_tc(struct net_device *net_dev, enum tc_setup_type type,
370 			 void *type_data)
371 {
372 	struct dpaa_priv *priv = netdev_priv(net_dev);
373 	struct tc_mqprio_qopt *mqprio = type_data;
374 	u8 num_tc;
375 	int i;
376 
377 	if (type != TC_SETUP_QDISC_MQPRIO)
378 		return -EOPNOTSUPP;
379 
380 	mqprio->hw = TC_MQPRIO_HW_OFFLOAD_TCS;
381 	num_tc = mqprio->num_tc;
382 
383 	if (num_tc == priv->num_tc)
384 		return 0;
385 
386 	if (!num_tc) {
387 		netdev_reset_tc(net_dev);
388 		goto out;
389 	}
390 
391 	if (num_tc > DPAA_TC_NUM) {
392 		netdev_err(net_dev, "Too many traffic classes: max %d supported.\n",
393 			   DPAA_TC_NUM);
394 		return -EINVAL;
395 	}
396 
397 	netdev_set_num_tc(net_dev, num_tc);
398 
399 	for (i = 0; i < num_tc; i++)
400 		netdev_set_tc_queue(net_dev, i, DPAA_TC_TXQ_NUM,
401 				    i * DPAA_TC_TXQ_NUM);
402 
403 out:
404 	priv->num_tc = num_tc ? : 1;
405 	netif_set_real_num_tx_queues(net_dev, priv->num_tc * DPAA_TC_TXQ_NUM);
406 	return 0;
407 }
408 
409 static struct mac_device *dpaa_mac_dev_get(struct platform_device *pdev)
410 {
411 	struct dpaa_eth_data *eth_data;
412 	struct device *dpaa_dev;
413 	struct mac_device *mac_dev;
414 
415 	dpaa_dev = &pdev->dev;
416 	eth_data = dpaa_dev->platform_data;
417 	if (!eth_data) {
418 		dev_err(dpaa_dev, "eth_data missing\n");
419 		return ERR_PTR(-ENODEV);
420 	}
421 	mac_dev = eth_data->mac_dev;
422 	if (!mac_dev) {
423 		dev_err(dpaa_dev, "mac_dev missing\n");
424 		return ERR_PTR(-EINVAL);
425 	}
426 
427 	return mac_dev;
428 }
429 
430 static int dpaa_set_mac_address(struct net_device *net_dev, void *addr)
431 {
432 	const struct dpaa_priv *priv;
433 	struct mac_device *mac_dev;
434 	struct sockaddr old_addr;
435 	int err;
436 
437 	priv = netdev_priv(net_dev);
438 
439 	memcpy(old_addr.sa_data, net_dev->dev_addr,  ETH_ALEN);
440 
441 	err = eth_mac_addr(net_dev, addr);
442 	if (err < 0) {
443 		netif_err(priv, drv, net_dev, "eth_mac_addr() = %d\n", err);
444 		return err;
445 	}
446 
447 	mac_dev = priv->mac_dev;
448 
449 	err = mac_dev->change_addr(mac_dev->fman_mac,
450 				   (const enet_addr_t *)net_dev->dev_addr);
451 	if (err < 0) {
452 		netif_err(priv, drv, net_dev, "mac_dev->change_addr() = %d\n",
453 			  err);
454 		/* reverting to previous address */
455 		eth_mac_addr(net_dev, &old_addr);
456 
457 		return err;
458 	}
459 
460 	return 0;
461 }
462 
463 static void dpaa_set_rx_mode(struct net_device *net_dev)
464 {
465 	const struct dpaa_priv	*priv;
466 	int err;
467 
468 	priv = netdev_priv(net_dev);
469 
470 	if (!!(net_dev->flags & IFF_PROMISC) != priv->mac_dev->promisc) {
471 		priv->mac_dev->promisc = !priv->mac_dev->promisc;
472 		err = priv->mac_dev->set_promisc(priv->mac_dev->fman_mac,
473 						 priv->mac_dev->promisc);
474 		if (err < 0)
475 			netif_err(priv, drv, net_dev,
476 				  "mac_dev->set_promisc() = %d\n",
477 				  err);
478 	}
479 
480 	if (!!(net_dev->flags & IFF_ALLMULTI) != priv->mac_dev->allmulti) {
481 		priv->mac_dev->allmulti = !priv->mac_dev->allmulti;
482 		err = priv->mac_dev->set_allmulti(priv->mac_dev->fman_mac,
483 						  priv->mac_dev->allmulti);
484 		if (err < 0)
485 			netif_err(priv, drv, net_dev,
486 				  "mac_dev->set_allmulti() = %d\n",
487 				  err);
488 	}
489 
490 	err = priv->mac_dev->set_multi(net_dev, priv->mac_dev);
491 	if (err < 0)
492 		netif_err(priv, drv, net_dev, "mac_dev->set_multi() = %d\n",
493 			  err);
494 }
495 
496 static struct dpaa_bp *dpaa_bpid2pool(int bpid)
497 {
498 	if (WARN_ON(bpid < 0 || bpid >= BM_MAX_NUM_OF_POOLS))
499 		return NULL;
500 
501 	return dpaa_bp_array[bpid];
502 }
503 
504 /* checks if this bpool is already allocated */
505 static bool dpaa_bpid2pool_use(int bpid)
506 {
507 	if (dpaa_bpid2pool(bpid)) {
508 		refcount_inc(&dpaa_bp_array[bpid]->refs);
509 		return true;
510 	}
511 
512 	return false;
513 }
514 
515 /* called only once per bpid by dpaa_bp_alloc_pool() */
516 static void dpaa_bpid2pool_map(int bpid, struct dpaa_bp *dpaa_bp)
517 {
518 	dpaa_bp_array[bpid] = dpaa_bp;
519 	refcount_set(&dpaa_bp->refs, 1);
520 }
521 
522 static int dpaa_bp_alloc_pool(struct dpaa_bp *dpaa_bp)
523 {
524 	int err;
525 
526 	if (dpaa_bp->size == 0 || dpaa_bp->config_count == 0) {
527 		pr_err("%s: Buffer pool is not properly initialized! Missing size or initial number of buffers\n",
528 		       __func__);
529 		return -EINVAL;
530 	}
531 
532 	/* If the pool is already specified, we only create one per bpid */
533 	if (dpaa_bp->bpid != FSL_DPAA_BPID_INV &&
534 	    dpaa_bpid2pool_use(dpaa_bp->bpid))
535 		return 0;
536 
537 	if (dpaa_bp->bpid == FSL_DPAA_BPID_INV) {
538 		dpaa_bp->pool = bman_new_pool();
539 		if (!dpaa_bp->pool) {
540 			pr_err("%s: bman_new_pool() failed\n",
541 			       __func__);
542 			return -ENODEV;
543 		}
544 
545 		dpaa_bp->bpid = (u8)bman_get_bpid(dpaa_bp->pool);
546 	}
547 
548 	if (dpaa_bp->seed_cb) {
549 		err = dpaa_bp->seed_cb(dpaa_bp);
550 		if (err)
551 			goto pool_seed_failed;
552 	}
553 
554 	dpaa_bpid2pool_map(dpaa_bp->bpid, dpaa_bp);
555 
556 	return 0;
557 
558 pool_seed_failed:
559 	pr_err("%s: pool seeding failed\n", __func__);
560 	bman_free_pool(dpaa_bp->pool);
561 
562 	return err;
563 }
564 
565 /* remove and free all the buffers from the given buffer pool */
566 static void dpaa_bp_drain(struct dpaa_bp *bp)
567 {
568 	u8 num = 8;
569 	int ret;
570 
571 	do {
572 		struct bm_buffer bmb[8];
573 		int i;
574 
575 		ret = bman_acquire(bp->pool, bmb, num);
576 		if (ret < 0) {
577 			if (num == 8) {
578 				/* we have less than 8 buffers left;
579 				 * drain them one by one
580 				 */
581 				num = 1;
582 				ret = 1;
583 				continue;
584 			} else {
585 				/* Pool is fully drained */
586 				break;
587 			}
588 		}
589 
590 		if (bp->free_buf_cb)
591 			for (i = 0; i < num; i++)
592 				bp->free_buf_cb(bp, &bmb[i]);
593 	} while (ret > 0);
594 }
595 
596 static void dpaa_bp_free(struct dpaa_bp *dpaa_bp)
597 {
598 	struct dpaa_bp *bp = dpaa_bpid2pool(dpaa_bp->bpid);
599 
600 	/* the mapping between bpid and dpaa_bp is done very late in the
601 	 * allocation procedure; if something failed before the mapping, the bp
602 	 * was not configured, therefore we don't need the below instructions
603 	 */
604 	if (!bp)
605 		return;
606 
607 	if (!refcount_dec_and_test(&bp->refs))
608 		return;
609 
610 	if (bp->free_buf_cb)
611 		dpaa_bp_drain(bp);
612 
613 	dpaa_bp_array[bp->bpid] = NULL;
614 	bman_free_pool(bp->pool);
615 }
616 
617 static void dpaa_bps_free(struct dpaa_priv *priv)
618 {
619 	dpaa_bp_free(priv->dpaa_bp);
620 }
621 
622 /* Use multiple WQs for FQ assignment:
623  *	- Tx Confirmation queues go to WQ1.
624  *	- Rx Error and Tx Error queues go to WQ5 (giving them a better chance
625  *	  to be scheduled, in case there are many more FQs in WQ6).
626  *	- Rx Default goes to WQ6.
627  *	- Tx queues go to different WQs depending on their priority. Equal
628  *	  chunks of NR_CPUS queues go to WQ6 (lowest priority), WQ2, WQ1 and
629  *	  WQ0 (highest priority).
630  * This ensures that Tx-confirmed buffers are timely released. In particular,
631  * it avoids congestion on the Tx Confirm FQs, which can pile up PFDRs if they
632  * are greatly outnumbered by other FQs in the system, while
633  * dequeue scheduling is round-robin.
634  */
635 static inline void dpaa_assign_wq(struct dpaa_fq *fq, int idx)
636 {
637 	switch (fq->fq_type) {
638 	case FQ_TYPE_TX_CONFIRM:
639 	case FQ_TYPE_TX_CONF_MQ:
640 		fq->wq = 1;
641 		break;
642 	case FQ_TYPE_RX_ERROR:
643 	case FQ_TYPE_TX_ERROR:
644 		fq->wq = 5;
645 		break;
646 	case FQ_TYPE_RX_DEFAULT:
647 	case FQ_TYPE_RX_PCD:
648 		fq->wq = 6;
649 		break;
650 	case FQ_TYPE_TX:
651 		switch (idx / DPAA_TC_TXQ_NUM) {
652 		case 0:
653 			/* Low priority (best effort) */
654 			fq->wq = 6;
655 			break;
656 		case 1:
657 			/* Medium priority */
658 			fq->wq = 2;
659 			break;
660 		case 2:
661 			/* High priority */
662 			fq->wq = 1;
663 			break;
664 		case 3:
665 			/* Very high priority */
666 			fq->wq = 0;
667 			break;
668 		default:
669 			WARN(1, "Too many TX FQs: more than %d!\n",
670 			     DPAA_ETH_TXQ_NUM);
671 		}
672 		break;
673 	default:
674 		WARN(1, "Invalid FQ type %d for FQID %d!\n",
675 		     fq->fq_type, fq->fqid);
676 	}
677 }
678 
679 static struct dpaa_fq *dpaa_fq_alloc(struct device *dev,
680 				     u32 start, u32 count,
681 				     struct list_head *list,
682 				     enum dpaa_fq_type fq_type)
683 {
684 	struct dpaa_fq *dpaa_fq;
685 	int i;
686 
687 	dpaa_fq = devm_kcalloc(dev, count, sizeof(*dpaa_fq),
688 			       GFP_KERNEL);
689 	if (!dpaa_fq)
690 		return NULL;
691 
692 	for (i = 0; i < count; i++) {
693 		dpaa_fq[i].fq_type = fq_type;
694 		dpaa_fq[i].fqid = start ? start + i : 0;
695 		list_add_tail(&dpaa_fq[i].list, list);
696 	}
697 
698 	for (i = 0; i < count; i++)
699 		dpaa_assign_wq(dpaa_fq + i, i);
700 
701 	return dpaa_fq;
702 }
703 
704 static int dpaa_alloc_all_fqs(struct device *dev, struct list_head *list,
705 			      struct fm_port_fqs *port_fqs)
706 {
707 	struct dpaa_fq *dpaa_fq;
708 	u32 fq_base, fq_base_aligned, i;
709 
710 	dpaa_fq = dpaa_fq_alloc(dev, 0, 1, list, FQ_TYPE_RX_ERROR);
711 	if (!dpaa_fq)
712 		goto fq_alloc_failed;
713 
714 	port_fqs->rx_errq = &dpaa_fq[0];
715 
716 	dpaa_fq = dpaa_fq_alloc(dev, 0, 1, list, FQ_TYPE_RX_DEFAULT);
717 	if (!dpaa_fq)
718 		goto fq_alloc_failed;
719 
720 	port_fqs->rx_defq = &dpaa_fq[0];
721 
722 	/* the PCD FQIDs range needs to be aligned for correct operation */
723 	if (qman_alloc_fqid_range(&fq_base, 2 * DPAA_ETH_PCD_RXQ_NUM))
724 		goto fq_alloc_failed;
725 
726 	fq_base_aligned = ALIGN(fq_base, DPAA_ETH_PCD_RXQ_NUM);
727 
728 	for (i = fq_base; i < fq_base_aligned; i++)
729 		qman_release_fqid(i);
730 
731 	for (i = fq_base_aligned + DPAA_ETH_PCD_RXQ_NUM;
732 	     i < (fq_base + 2 * DPAA_ETH_PCD_RXQ_NUM); i++)
733 		qman_release_fqid(i);
734 
735 	dpaa_fq = dpaa_fq_alloc(dev, fq_base_aligned, DPAA_ETH_PCD_RXQ_NUM,
736 				list, FQ_TYPE_RX_PCD);
737 	if (!dpaa_fq)
738 		goto fq_alloc_failed;
739 
740 	port_fqs->rx_pcdq = &dpaa_fq[0];
741 
742 	if (!dpaa_fq_alloc(dev, 0, DPAA_ETH_TXQ_NUM, list, FQ_TYPE_TX_CONF_MQ))
743 		goto fq_alloc_failed;
744 
745 	dpaa_fq = dpaa_fq_alloc(dev, 0, 1, list, FQ_TYPE_TX_ERROR);
746 	if (!dpaa_fq)
747 		goto fq_alloc_failed;
748 
749 	port_fqs->tx_errq = &dpaa_fq[0];
750 
751 	dpaa_fq = dpaa_fq_alloc(dev, 0, 1, list, FQ_TYPE_TX_CONFIRM);
752 	if (!dpaa_fq)
753 		goto fq_alloc_failed;
754 
755 	port_fqs->tx_defq = &dpaa_fq[0];
756 
757 	if (!dpaa_fq_alloc(dev, 0, DPAA_ETH_TXQ_NUM, list, FQ_TYPE_TX))
758 		goto fq_alloc_failed;
759 
760 	return 0;
761 
762 fq_alloc_failed:
763 	dev_err(dev, "dpaa_fq_alloc() failed\n");
764 	return -ENOMEM;
765 }
766 
767 static u32 rx_pool_channel;
768 static DEFINE_SPINLOCK(rx_pool_channel_init);
769 
770 static int dpaa_get_channel(void)
771 {
772 	spin_lock(&rx_pool_channel_init);
773 	if (!rx_pool_channel) {
774 		u32 pool;
775 		int ret;
776 
777 		ret = qman_alloc_pool(&pool);
778 
779 		if (!ret)
780 			rx_pool_channel = pool;
781 	}
782 	spin_unlock(&rx_pool_channel_init);
783 	if (!rx_pool_channel)
784 		return -ENOMEM;
785 	return rx_pool_channel;
786 }
787 
788 static void dpaa_release_channel(void)
789 {
790 	qman_release_pool(rx_pool_channel);
791 }
792 
793 static void dpaa_eth_add_channel(u16 channel, struct device *dev)
794 {
795 	u32 pool = QM_SDQCR_CHANNELS_POOL_CONV(channel);
796 	const cpumask_t *cpus = qman_affine_cpus();
797 	struct qman_portal *portal;
798 	int cpu;
799 
800 	for_each_cpu_and(cpu, cpus, cpu_online_mask) {
801 		portal = qman_get_affine_portal(cpu);
802 		qman_p_static_dequeue_add(portal, pool);
803 		qman_start_using_portal(portal, dev);
804 	}
805 }
806 
807 /* Congestion group state change notification callback.
808  * Stops the device's egress queues while they are congested and
809  * wakes them upon exiting congested state.
810  * Also updates some CGR-related stats.
811  */
812 static void dpaa_eth_cgscn(struct qman_portal *qm, struct qman_cgr *cgr,
813 			   int congested)
814 {
815 	struct dpaa_priv *priv = (struct dpaa_priv *)container_of(cgr,
816 		struct dpaa_priv, cgr_data.cgr);
817 
818 	if (congested) {
819 		priv->cgr_data.congestion_start_jiffies = jiffies;
820 		netif_tx_stop_all_queues(priv->net_dev);
821 		priv->cgr_data.cgr_congested_count++;
822 	} else {
823 		priv->cgr_data.congested_jiffies +=
824 			(jiffies - priv->cgr_data.congestion_start_jiffies);
825 		netif_tx_wake_all_queues(priv->net_dev);
826 	}
827 }
828 
829 static int dpaa_eth_cgr_init(struct dpaa_priv *priv)
830 {
831 	struct qm_mcc_initcgr initcgr;
832 	u32 cs_th;
833 	int err;
834 
835 	err = qman_alloc_cgrid(&priv->cgr_data.cgr.cgrid);
836 	if (err < 0) {
837 		if (netif_msg_drv(priv))
838 			pr_err("%s: Error %d allocating CGR ID\n",
839 			       __func__, err);
840 		goto out_error;
841 	}
842 	priv->cgr_data.cgr.cb = dpaa_eth_cgscn;
843 
844 	/* Enable Congestion State Change Notifications and CS taildrop */
845 	memset(&initcgr, 0, sizeof(initcgr));
846 	initcgr.we_mask = cpu_to_be16(QM_CGR_WE_CSCN_EN | QM_CGR_WE_CS_THRES);
847 	initcgr.cgr.cscn_en = QM_CGR_EN;
848 
849 	/* Set different thresholds based on the configured MAC speed.
850 	 * This may turn suboptimal if the MAC is reconfigured at another
851 	 * speed, so MACs must call dpaa_eth_cgr_set_speed in their link_up
852 	 * callback.
853 	 */
854 	if (priv->mac_dev->phylink_config.mac_capabilities & MAC_10000FD)
855 		cs_th = DPAA_CS_THRESHOLD_10G;
856 	else
857 		cs_th = DPAA_CS_THRESHOLD_1G;
858 	qm_cgr_cs_thres_set64(&initcgr.cgr.cs_thres, cs_th, 1);
859 
860 	initcgr.we_mask |= cpu_to_be16(QM_CGR_WE_CSTD_EN);
861 	initcgr.cgr.cstd_en = QM_CGR_EN;
862 
863 	err = qman_create_cgr(&priv->cgr_data.cgr, QMAN_CGR_FLAG_USE_INIT,
864 			      &initcgr);
865 	if (err < 0) {
866 		if (netif_msg_drv(priv))
867 			pr_err("%s: Error %d creating CGR with ID %d\n",
868 			       __func__, err, priv->cgr_data.cgr.cgrid);
869 		qman_release_cgrid(priv->cgr_data.cgr.cgrid);
870 		goto out_error;
871 	}
872 	if (netif_msg_drv(priv))
873 		pr_debug("Created CGR %d for netdev with hwaddr %pM on QMan channel %d\n",
874 			 priv->cgr_data.cgr.cgrid, priv->mac_dev->addr,
875 			 priv->cgr_data.cgr.chan);
876 
877 out_error:
878 	return err;
879 }
880 
881 static void dpaa_eth_cgr_set_speed(struct mac_device *mac_dev, int speed)
882 {
883 	struct net_device *net_dev = to_net_dev(mac_dev->phylink_config.dev);
884 	struct dpaa_priv *priv = netdev_priv(net_dev);
885 	struct qm_mcc_initcgr opts = { };
886 	u32 cs_th;
887 	int err;
888 
889 	opts.we_mask = cpu_to_be16(QM_CGR_WE_CS_THRES);
890 	switch (speed) {
891 	case SPEED_10000:
892 		cs_th = DPAA_CS_THRESHOLD_10G;
893 		break;
894 	case SPEED_1000:
895 	default:
896 		cs_th = DPAA_CS_THRESHOLD_1G;
897 		break;
898 	}
899 	qm_cgr_cs_thres_set64(&opts.cgr.cs_thres, cs_th, 1);
900 
901 	err = qman_update_cgr_safe(&priv->cgr_data.cgr, &opts);
902 	if (err)
903 		netdev_err(net_dev, "could not update speed: %d\n", err);
904 }
905 
906 static inline void dpaa_setup_ingress(const struct dpaa_priv *priv,
907 				      struct dpaa_fq *fq,
908 				      const struct qman_fq *template)
909 {
910 	fq->fq_base = *template;
911 	fq->net_dev = priv->net_dev;
912 
913 	fq->flags = QMAN_FQ_FLAG_NO_ENQUEUE;
914 	fq->channel = priv->channel;
915 }
916 
917 static inline void dpaa_setup_egress(const struct dpaa_priv *priv,
918 				     struct dpaa_fq *fq,
919 				     struct fman_port *port,
920 				     const struct qman_fq *template)
921 {
922 	fq->fq_base = *template;
923 	fq->net_dev = priv->net_dev;
924 
925 	if (port) {
926 		fq->flags = QMAN_FQ_FLAG_TO_DCPORTAL;
927 		fq->channel = (u16)fman_port_get_qman_channel_id(port);
928 	} else {
929 		fq->flags = QMAN_FQ_FLAG_NO_MODIFY;
930 	}
931 }
932 
933 static void dpaa_fq_setup(struct dpaa_priv *priv,
934 			  const struct dpaa_fq_cbs *fq_cbs,
935 			  struct fman_port *tx_port)
936 {
937 	int egress_cnt = 0, conf_cnt = 0, num_portals = 0, portal_cnt = 0, cpu;
938 	const cpumask_t *affine_cpus = qman_affine_cpus();
939 	u16 channels[NR_CPUS];
940 	struct dpaa_fq *fq;
941 
942 	for_each_cpu_and(cpu, affine_cpus, cpu_online_mask)
943 		channels[num_portals++] = qman_affine_channel(cpu);
944 
945 	if (num_portals == 0)
946 		dev_err(priv->net_dev->dev.parent,
947 			"No Qman software (affine) channels found\n");
948 
949 	/* Initialize each FQ in the list */
950 	list_for_each_entry(fq, &priv->dpaa_fq_list, list) {
951 		switch (fq->fq_type) {
952 		case FQ_TYPE_RX_DEFAULT:
953 			dpaa_setup_ingress(priv, fq, &fq_cbs->rx_defq);
954 			break;
955 		case FQ_TYPE_RX_ERROR:
956 			dpaa_setup_ingress(priv, fq, &fq_cbs->rx_errq);
957 			break;
958 		case FQ_TYPE_RX_PCD:
959 			if (!num_portals)
960 				continue;
961 			dpaa_setup_ingress(priv, fq, &fq_cbs->rx_defq);
962 			fq->channel = channels[portal_cnt++ % num_portals];
963 			break;
964 		case FQ_TYPE_TX:
965 			dpaa_setup_egress(priv, fq, tx_port,
966 					  &fq_cbs->egress_ern);
967 			/* If we have more Tx queues than the number of cores,
968 			 * just ignore the extra ones.
969 			 */
970 			if (egress_cnt < DPAA_ETH_TXQ_NUM)
971 				priv->egress_fqs[egress_cnt++] = &fq->fq_base;
972 			break;
973 		case FQ_TYPE_TX_CONF_MQ:
974 			priv->conf_fqs[conf_cnt++] = &fq->fq_base;
975 			fallthrough;
976 		case FQ_TYPE_TX_CONFIRM:
977 			dpaa_setup_ingress(priv, fq, &fq_cbs->tx_defq);
978 			break;
979 		case FQ_TYPE_TX_ERROR:
980 			dpaa_setup_ingress(priv, fq, &fq_cbs->tx_errq);
981 			break;
982 		default:
983 			dev_warn(priv->net_dev->dev.parent,
984 				 "Unknown FQ type detected!\n");
985 			break;
986 		}
987 	}
988 
989 	 /* Make sure all CPUs receive a corresponding Tx queue. */
990 	while (egress_cnt < DPAA_ETH_TXQ_NUM) {
991 		list_for_each_entry(fq, &priv->dpaa_fq_list, list) {
992 			if (fq->fq_type != FQ_TYPE_TX)
993 				continue;
994 			priv->egress_fqs[egress_cnt++] = &fq->fq_base;
995 			if (egress_cnt == DPAA_ETH_TXQ_NUM)
996 				break;
997 		}
998 	}
999 }
1000 
1001 static inline int dpaa_tx_fq_to_id(const struct dpaa_priv *priv,
1002 				   struct qman_fq *tx_fq)
1003 {
1004 	int i;
1005 
1006 	for (i = 0; i < DPAA_ETH_TXQ_NUM; i++)
1007 		if (priv->egress_fqs[i] == tx_fq)
1008 			return i;
1009 
1010 	return -EINVAL;
1011 }
1012 
1013 static int dpaa_fq_init(struct dpaa_fq *dpaa_fq, bool td_enable)
1014 {
1015 	const struct dpaa_priv	*priv;
1016 	struct qman_fq *confq = NULL;
1017 	struct qm_mcc_initfq initfq;
1018 	struct device *dev;
1019 	struct qman_fq *fq;
1020 	int queue_id;
1021 	int err;
1022 
1023 	priv = netdev_priv(dpaa_fq->net_dev);
1024 	dev = dpaa_fq->net_dev->dev.parent;
1025 
1026 	if (dpaa_fq->fqid == 0)
1027 		dpaa_fq->flags |= QMAN_FQ_FLAG_DYNAMIC_FQID;
1028 
1029 	dpaa_fq->init = !(dpaa_fq->flags & QMAN_FQ_FLAG_NO_MODIFY);
1030 
1031 	err = qman_create_fq(dpaa_fq->fqid, dpaa_fq->flags, &dpaa_fq->fq_base);
1032 	if (err) {
1033 		dev_err(dev, "qman_create_fq() failed\n");
1034 		return err;
1035 	}
1036 	fq = &dpaa_fq->fq_base;
1037 
1038 	if (dpaa_fq->init) {
1039 		memset(&initfq, 0, sizeof(initfq));
1040 
1041 		initfq.we_mask = cpu_to_be16(QM_INITFQ_WE_FQCTRL);
1042 		/* Note: we may get to keep an empty FQ in cache */
1043 		initfq.fqd.fq_ctrl = cpu_to_be16(QM_FQCTRL_PREFERINCACHE);
1044 
1045 		/* Try to reduce the number of portal interrupts for
1046 		 * Tx Confirmation FQs.
1047 		 */
1048 		if (dpaa_fq->fq_type == FQ_TYPE_TX_CONFIRM)
1049 			initfq.fqd.fq_ctrl |= cpu_to_be16(QM_FQCTRL_AVOIDBLOCK);
1050 
1051 		/* FQ placement */
1052 		initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_DESTWQ);
1053 
1054 		qm_fqd_set_destwq(&initfq.fqd, dpaa_fq->channel, dpaa_fq->wq);
1055 
1056 		/* Put all egress queues in a congestion group of their own.
1057 		 * Sensu stricto, the Tx confirmation queues are Rx FQs,
1058 		 * rather than Tx - but they nonetheless account for the
1059 		 * memory footprint on behalf of egress traffic. We therefore
1060 		 * place them in the netdev's CGR, along with the Tx FQs.
1061 		 */
1062 		if (dpaa_fq->fq_type == FQ_TYPE_TX ||
1063 		    dpaa_fq->fq_type == FQ_TYPE_TX_CONFIRM ||
1064 		    dpaa_fq->fq_type == FQ_TYPE_TX_CONF_MQ) {
1065 			initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_CGID);
1066 			initfq.fqd.fq_ctrl |= cpu_to_be16(QM_FQCTRL_CGE);
1067 			initfq.fqd.cgid = (u8)priv->cgr_data.cgr.cgrid;
1068 			/* Set a fixed overhead accounting, in an attempt to
1069 			 * reduce the impact of fixed-size skb shells and the
1070 			 * driver's needed headroom on system memory. This is
1071 			 * especially the case when the egress traffic is
1072 			 * composed of small datagrams.
1073 			 * Unfortunately, QMan's OAL value is capped to an
1074 			 * insufficient value, but even that is better than
1075 			 * no overhead accounting at all.
1076 			 */
1077 			initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_OAC);
1078 			qm_fqd_set_oac(&initfq.fqd, QM_OAC_CG);
1079 			qm_fqd_set_oal(&initfq.fqd,
1080 				       min(sizeof(struct sk_buff) +
1081 				       priv->tx_headroom,
1082 				       (size_t)FSL_QMAN_MAX_OAL));
1083 		}
1084 
1085 		if (td_enable) {
1086 			initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_TDTHRESH);
1087 			qm_fqd_set_taildrop(&initfq.fqd, DPAA_FQ_TD, 1);
1088 			initfq.fqd.fq_ctrl = cpu_to_be16(QM_FQCTRL_TDE);
1089 		}
1090 
1091 		if (dpaa_fq->fq_type == FQ_TYPE_TX) {
1092 			queue_id = dpaa_tx_fq_to_id(priv, &dpaa_fq->fq_base);
1093 			if (queue_id >= 0)
1094 				confq = priv->conf_fqs[queue_id];
1095 			if (confq) {
1096 				initfq.we_mask |=
1097 					cpu_to_be16(QM_INITFQ_WE_CONTEXTA);
1098 			/* ContextA: OVOM=1(use contextA2 bits instead of ICAD)
1099 			 *	     A2V=1 (contextA A2 field is valid)
1100 			 *	     A0V=1 (contextA A0 field is valid)
1101 			 *	     B0V=1 (contextB field is valid)
1102 			 * ContextA A2: EBD=1 (deallocate buffers inside FMan)
1103 			 * ContextB B0(ASPID): 0 (absolute Virtual Storage ID)
1104 			 */
1105 				qm_fqd_context_a_set64(&initfq.fqd,
1106 						       0x1e00000080000000ULL);
1107 			}
1108 		}
1109 
1110 		/* Put all the ingress queues in our "ingress CGR". */
1111 		if (priv->use_ingress_cgr &&
1112 		    (dpaa_fq->fq_type == FQ_TYPE_RX_DEFAULT ||
1113 		     dpaa_fq->fq_type == FQ_TYPE_RX_ERROR ||
1114 		     dpaa_fq->fq_type == FQ_TYPE_RX_PCD)) {
1115 			initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_CGID);
1116 			initfq.fqd.fq_ctrl |= cpu_to_be16(QM_FQCTRL_CGE);
1117 			initfq.fqd.cgid = (u8)priv->ingress_cgr.cgrid;
1118 			/* Set a fixed overhead accounting, just like for the
1119 			 * egress CGR.
1120 			 */
1121 			initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_OAC);
1122 			qm_fqd_set_oac(&initfq.fqd, QM_OAC_CG);
1123 			qm_fqd_set_oal(&initfq.fqd,
1124 				       min(sizeof(struct sk_buff) +
1125 				       priv->tx_headroom,
1126 				       (size_t)FSL_QMAN_MAX_OAL));
1127 		}
1128 
1129 		/* Initialization common to all ingress queues */
1130 		if (dpaa_fq->flags & QMAN_FQ_FLAG_NO_ENQUEUE) {
1131 			initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_CONTEXTA);
1132 			initfq.fqd.fq_ctrl |= cpu_to_be16(QM_FQCTRL_HOLDACTIVE |
1133 						QM_FQCTRL_CTXASTASHING);
1134 			initfq.fqd.context_a.stashing.exclusive =
1135 				QM_STASHING_EXCL_DATA | QM_STASHING_EXCL_CTX |
1136 				QM_STASHING_EXCL_ANNOTATION;
1137 			qm_fqd_set_stashing(&initfq.fqd, 1, 2,
1138 					    DIV_ROUND_UP(sizeof(struct qman_fq),
1139 							 64));
1140 		}
1141 
1142 		err = qman_init_fq(fq, QMAN_INITFQ_FLAG_SCHED, &initfq);
1143 		if (err < 0) {
1144 			dev_err(dev, "qman_init_fq(%u) = %d\n",
1145 				qman_fq_fqid(fq), err);
1146 			qman_destroy_fq(fq);
1147 			return err;
1148 		}
1149 	}
1150 
1151 	dpaa_fq->fqid = qman_fq_fqid(fq);
1152 
1153 	if (dpaa_fq->fq_type == FQ_TYPE_RX_DEFAULT ||
1154 	    dpaa_fq->fq_type == FQ_TYPE_RX_PCD) {
1155 		err = xdp_rxq_info_reg(&dpaa_fq->xdp_rxq, dpaa_fq->net_dev,
1156 				       dpaa_fq->fqid, 0);
1157 		if (err) {
1158 			dev_err(dev, "xdp_rxq_info_reg() = %d\n", err);
1159 			return err;
1160 		}
1161 
1162 		err = xdp_rxq_info_reg_mem_model(&dpaa_fq->xdp_rxq,
1163 						 MEM_TYPE_PAGE_ORDER0, NULL);
1164 		if (err) {
1165 			dev_err(dev, "xdp_rxq_info_reg_mem_model() = %d\n",
1166 				err);
1167 			xdp_rxq_info_unreg(&dpaa_fq->xdp_rxq);
1168 			return err;
1169 		}
1170 	}
1171 
1172 	return 0;
1173 }
1174 
1175 static int dpaa_fq_free_entry(struct device *dev, struct qman_fq *fq)
1176 {
1177 	const struct dpaa_priv  *priv;
1178 	struct dpaa_fq *dpaa_fq;
1179 	int err, error;
1180 
1181 	err = 0;
1182 
1183 	dpaa_fq = container_of(fq, struct dpaa_fq, fq_base);
1184 	priv = netdev_priv(dpaa_fq->net_dev);
1185 
1186 	if (dpaa_fq->init) {
1187 		err = qman_retire_fq(fq, NULL);
1188 		if (err < 0 && netif_msg_drv(priv))
1189 			dev_err(dev, "qman_retire_fq(%u) = %d\n",
1190 				qman_fq_fqid(fq), err);
1191 
1192 		error = qman_oos_fq(fq);
1193 		if (error < 0 && netif_msg_drv(priv)) {
1194 			dev_err(dev, "qman_oos_fq(%u) = %d\n",
1195 				qman_fq_fqid(fq), error);
1196 			if (err >= 0)
1197 				err = error;
1198 		}
1199 	}
1200 
1201 	if ((dpaa_fq->fq_type == FQ_TYPE_RX_DEFAULT ||
1202 	     dpaa_fq->fq_type == FQ_TYPE_RX_PCD) &&
1203 	    xdp_rxq_info_is_reg(&dpaa_fq->xdp_rxq))
1204 		xdp_rxq_info_unreg(&dpaa_fq->xdp_rxq);
1205 
1206 	qman_destroy_fq(fq);
1207 	list_del(&dpaa_fq->list);
1208 
1209 	return err;
1210 }
1211 
1212 static int dpaa_fq_free(struct device *dev, struct list_head *list)
1213 {
1214 	struct dpaa_fq *dpaa_fq, *tmp;
1215 	int err, error;
1216 
1217 	err = 0;
1218 	list_for_each_entry_safe(dpaa_fq, tmp, list, list) {
1219 		error = dpaa_fq_free_entry(dev, (struct qman_fq *)dpaa_fq);
1220 		if (error < 0 && err >= 0)
1221 			err = error;
1222 	}
1223 
1224 	return err;
1225 }
1226 
1227 static int dpaa_eth_init_tx_port(struct fman_port *port, struct dpaa_fq *errq,
1228 				 struct dpaa_fq *defq,
1229 				 struct dpaa_buffer_layout *buf_layout)
1230 {
1231 	struct fman_buffer_prefix_content buf_prefix_content;
1232 	struct fman_port_params params;
1233 	int err;
1234 
1235 	memset(&params, 0, sizeof(params));
1236 	memset(&buf_prefix_content, 0, sizeof(buf_prefix_content));
1237 
1238 	buf_prefix_content.priv_data_size = buf_layout->priv_data_size;
1239 	buf_prefix_content.pass_prs_result = true;
1240 	buf_prefix_content.pass_hash_result = true;
1241 	buf_prefix_content.pass_time_stamp = true;
1242 	buf_prefix_content.data_align = DPAA_FD_DATA_ALIGNMENT;
1243 
1244 	params.specific_params.non_rx_params.err_fqid = errq->fqid;
1245 	params.specific_params.non_rx_params.dflt_fqid = defq->fqid;
1246 
1247 	err = fman_port_config(port, &params);
1248 	if (err) {
1249 		pr_err("%s: fman_port_config failed\n", __func__);
1250 		return err;
1251 	}
1252 
1253 	err = fman_port_cfg_buf_prefix_content(port, &buf_prefix_content);
1254 	if (err) {
1255 		pr_err("%s: fman_port_cfg_buf_prefix_content failed\n",
1256 		       __func__);
1257 		return err;
1258 	}
1259 
1260 	err = fman_port_init(port);
1261 	if (err)
1262 		pr_err("%s: fm_port_init failed\n", __func__);
1263 
1264 	return err;
1265 }
1266 
1267 static int dpaa_eth_init_rx_port(struct fman_port *port, struct dpaa_bp *bp,
1268 				 struct dpaa_fq *errq,
1269 				 struct dpaa_fq *defq, struct dpaa_fq *pcdq,
1270 				 struct dpaa_buffer_layout *buf_layout)
1271 {
1272 	struct fman_buffer_prefix_content buf_prefix_content;
1273 	struct fman_port_rx_params *rx_p;
1274 	struct fman_port_params params;
1275 	int err;
1276 
1277 	memset(&params, 0, sizeof(params));
1278 	memset(&buf_prefix_content, 0, sizeof(buf_prefix_content));
1279 
1280 	buf_prefix_content.priv_data_size = buf_layout->priv_data_size;
1281 	buf_prefix_content.pass_prs_result = true;
1282 	buf_prefix_content.pass_hash_result = true;
1283 	buf_prefix_content.pass_time_stamp = true;
1284 	buf_prefix_content.data_align = DPAA_FD_RX_DATA_ALIGNMENT;
1285 
1286 	rx_p = &params.specific_params.rx_params;
1287 	rx_p->err_fqid = errq->fqid;
1288 	rx_p->dflt_fqid = defq->fqid;
1289 	if (pcdq) {
1290 		rx_p->pcd_base_fqid = pcdq->fqid;
1291 		rx_p->pcd_fqs_count = DPAA_ETH_PCD_RXQ_NUM;
1292 	}
1293 
1294 	rx_p->ext_buf_pools.num_of_pools_used = 1;
1295 	rx_p->ext_buf_pools.ext_buf_pool[0].id =  bp->bpid;
1296 	rx_p->ext_buf_pools.ext_buf_pool[0].size = (u16)bp->size;
1297 
1298 	err = fman_port_config(port, &params);
1299 	if (err) {
1300 		pr_err("%s: fman_port_config failed\n", __func__);
1301 		return err;
1302 	}
1303 
1304 	err = fman_port_cfg_buf_prefix_content(port, &buf_prefix_content);
1305 	if (err) {
1306 		pr_err("%s: fman_port_cfg_buf_prefix_content failed\n",
1307 		       __func__);
1308 		return err;
1309 	}
1310 
1311 	err = fman_port_init(port);
1312 	if (err)
1313 		pr_err("%s: fm_port_init failed\n", __func__);
1314 
1315 	return err;
1316 }
1317 
1318 static int dpaa_eth_init_ports(struct mac_device *mac_dev,
1319 			       struct dpaa_bp *bp,
1320 			       struct fm_port_fqs *port_fqs,
1321 			       struct dpaa_buffer_layout *buf_layout,
1322 			       struct device *dev)
1323 {
1324 	struct fman_port *rxport = mac_dev->port[RX];
1325 	struct fman_port *txport = mac_dev->port[TX];
1326 	int err;
1327 
1328 	err = dpaa_eth_init_tx_port(txport, port_fqs->tx_errq,
1329 				    port_fqs->tx_defq, &buf_layout[TX]);
1330 	if (err)
1331 		return err;
1332 
1333 	err = dpaa_eth_init_rx_port(rxport, bp, port_fqs->rx_errq,
1334 				    port_fqs->rx_defq, port_fqs->rx_pcdq,
1335 				    &buf_layout[RX]);
1336 
1337 	return err;
1338 }
1339 
1340 static int dpaa_bman_release(const struct dpaa_bp *dpaa_bp,
1341 			     struct bm_buffer *bmb, int cnt)
1342 {
1343 	int err;
1344 
1345 	err = bman_release(dpaa_bp->pool, bmb, cnt);
1346 	/* Should never occur, address anyway to avoid leaking the buffers */
1347 	if (WARN_ON(err) && dpaa_bp->free_buf_cb)
1348 		while (cnt-- > 0)
1349 			dpaa_bp->free_buf_cb(dpaa_bp, &bmb[cnt]);
1350 
1351 	return cnt;
1352 }
1353 
1354 static void dpaa_release_sgt_members(struct qm_sg_entry *sgt)
1355 {
1356 	struct bm_buffer bmb[DPAA_BUFF_RELEASE_MAX];
1357 	struct dpaa_bp *dpaa_bp;
1358 	int i = 0, j;
1359 
1360 	memset(bmb, 0, sizeof(bmb));
1361 
1362 	do {
1363 		dpaa_bp = dpaa_bpid2pool(sgt[i].bpid);
1364 		if (!dpaa_bp)
1365 			return;
1366 
1367 		j = 0;
1368 		do {
1369 			WARN_ON(qm_sg_entry_is_ext(&sgt[i]));
1370 
1371 			bm_buffer_set64(&bmb[j], qm_sg_entry_get64(&sgt[i]));
1372 
1373 			j++; i++;
1374 		} while (j < ARRAY_SIZE(bmb) &&
1375 				!qm_sg_entry_is_final(&sgt[i - 1]) &&
1376 				sgt[i - 1].bpid == sgt[i].bpid);
1377 
1378 		dpaa_bman_release(dpaa_bp, bmb, j);
1379 	} while (!qm_sg_entry_is_final(&sgt[i - 1]));
1380 }
1381 
1382 static void dpaa_fd_release(const struct net_device *net_dev,
1383 			    const struct qm_fd *fd)
1384 {
1385 	struct qm_sg_entry *sgt;
1386 	struct dpaa_bp *dpaa_bp;
1387 	struct bm_buffer bmb;
1388 	dma_addr_t addr;
1389 	void *vaddr;
1390 
1391 	bmb.data = 0;
1392 	bm_buffer_set64(&bmb, qm_fd_addr(fd));
1393 
1394 	dpaa_bp = dpaa_bpid2pool(fd->bpid);
1395 	if (!dpaa_bp)
1396 		return;
1397 
1398 	if (qm_fd_get_format(fd) == qm_fd_sg) {
1399 		vaddr = phys_to_virt(qm_fd_addr(fd));
1400 		sgt = vaddr + qm_fd_get_offset(fd);
1401 
1402 		dma_unmap_page(dpaa_bp->priv->rx_dma_dev, qm_fd_addr(fd),
1403 			       DPAA_BP_RAW_SIZE, DMA_FROM_DEVICE);
1404 
1405 		dpaa_release_sgt_members(sgt);
1406 
1407 		addr = dma_map_page(dpaa_bp->priv->rx_dma_dev,
1408 				    virt_to_page(vaddr), 0, DPAA_BP_RAW_SIZE,
1409 				    DMA_FROM_DEVICE);
1410 		if (dma_mapping_error(dpaa_bp->priv->rx_dma_dev, addr)) {
1411 			netdev_err(net_dev, "DMA mapping failed\n");
1412 			return;
1413 		}
1414 		bm_buffer_set64(&bmb, addr);
1415 	}
1416 
1417 	dpaa_bman_release(dpaa_bp, &bmb, 1);
1418 }
1419 
1420 static void count_ern(struct dpaa_percpu_priv *percpu_priv,
1421 		      const union qm_mr_entry *msg)
1422 {
1423 	switch (msg->ern.rc & QM_MR_RC_MASK) {
1424 	case QM_MR_RC_CGR_TAILDROP:
1425 		percpu_priv->ern_cnt.cg_tdrop++;
1426 		break;
1427 	case QM_MR_RC_WRED:
1428 		percpu_priv->ern_cnt.wred++;
1429 		break;
1430 	case QM_MR_RC_ERROR:
1431 		percpu_priv->ern_cnt.err_cond++;
1432 		break;
1433 	case QM_MR_RC_ORPWINDOW_EARLY:
1434 		percpu_priv->ern_cnt.early_window++;
1435 		break;
1436 	case QM_MR_RC_ORPWINDOW_LATE:
1437 		percpu_priv->ern_cnt.late_window++;
1438 		break;
1439 	case QM_MR_RC_FQ_TAILDROP:
1440 		percpu_priv->ern_cnt.fq_tdrop++;
1441 		break;
1442 	case QM_MR_RC_ORPWINDOW_RETIRED:
1443 		percpu_priv->ern_cnt.fq_retired++;
1444 		break;
1445 	case QM_MR_RC_ORP_ZERO:
1446 		percpu_priv->ern_cnt.orp_zero++;
1447 		break;
1448 	}
1449 }
1450 
1451 /* Turn on HW checksum computation for this outgoing frame.
1452  * If the current protocol is not something we support in this regard
1453  * (or if the stack has already computed the SW checksum), we do nothing.
1454  *
1455  * Returns 0 if all goes well (or HW csum doesn't apply), and a negative value
1456  * otherwise.
1457  *
1458  * Note that this function may modify the fd->cmd field and the skb data buffer
1459  * (the Parse Results area).
1460  */
1461 static int dpaa_enable_tx_csum(struct dpaa_priv *priv,
1462 			       struct sk_buff *skb,
1463 			       struct qm_fd *fd,
1464 			       void *parse_results)
1465 {
1466 	struct fman_prs_result *parse_result;
1467 	u16 ethertype = ntohs(skb->protocol);
1468 	struct ipv6hdr *ipv6h = NULL;
1469 	struct iphdr *iph;
1470 	int retval = 0;
1471 	u8 l4_proto;
1472 
1473 	if (skb->ip_summed != CHECKSUM_PARTIAL)
1474 		return 0;
1475 
1476 	/* Note: L3 csum seems to be already computed in sw, but we can't choose
1477 	 * L4 alone from the FM configuration anyway.
1478 	 */
1479 
1480 	/* Fill in some fields of the Parse Results array, so the FMan
1481 	 * can find them as if they came from the FMan Parser.
1482 	 */
1483 	parse_result = (struct fman_prs_result *)parse_results;
1484 
1485 	/* If we're dealing with VLAN, get the real Ethernet type */
1486 	if (ethertype == ETH_P_8021Q)
1487 		ethertype = ntohs(skb_vlan_eth_hdr(skb)->h_vlan_encapsulated_proto);
1488 
1489 	/* Fill in the relevant L3 parse result fields
1490 	 * and read the L4 protocol type
1491 	 */
1492 	switch (ethertype) {
1493 	case ETH_P_IP:
1494 		parse_result->l3r = cpu_to_be16(FM_L3_PARSE_RESULT_IPV4);
1495 		iph = ip_hdr(skb);
1496 		WARN_ON(!iph);
1497 		l4_proto = iph->protocol;
1498 		break;
1499 	case ETH_P_IPV6:
1500 		parse_result->l3r = cpu_to_be16(FM_L3_PARSE_RESULT_IPV6);
1501 		ipv6h = ipv6_hdr(skb);
1502 		WARN_ON(!ipv6h);
1503 		l4_proto = ipv6h->nexthdr;
1504 		break;
1505 	default:
1506 		/* We shouldn't even be here */
1507 		if (net_ratelimit())
1508 			netif_alert(priv, tx_err, priv->net_dev,
1509 				    "Can't compute HW csum for L3 proto 0x%x\n",
1510 				    ntohs(skb->protocol));
1511 		retval = -EIO;
1512 		goto return_error;
1513 	}
1514 
1515 	/* Fill in the relevant L4 parse result fields */
1516 	switch (l4_proto) {
1517 	case IPPROTO_UDP:
1518 		parse_result->l4r = FM_L4_PARSE_RESULT_UDP;
1519 		break;
1520 	case IPPROTO_TCP:
1521 		parse_result->l4r = FM_L4_PARSE_RESULT_TCP;
1522 		break;
1523 	default:
1524 		if (net_ratelimit())
1525 			netif_alert(priv, tx_err, priv->net_dev,
1526 				    "Can't compute HW csum for L4 proto 0x%x\n",
1527 				    l4_proto);
1528 		retval = -EIO;
1529 		goto return_error;
1530 	}
1531 
1532 	/* At index 0 is IPOffset_1 as defined in the Parse Results */
1533 	parse_result->ip_off[0] = (u8)skb_network_offset(skb);
1534 	parse_result->l4_off = (u8)skb_transport_offset(skb);
1535 
1536 	/* Enable L3 (and L4, if TCP or UDP) HW checksum. */
1537 	fd->cmd |= cpu_to_be32(FM_FD_CMD_RPD | FM_FD_CMD_DTC);
1538 
1539 	/* On P1023 and similar platforms fd->cmd interpretation could
1540 	 * be disabled by setting CONTEXT_A bit ICMD; currently this bit
1541 	 * is not set so we do not need to check; in the future, if/when
1542 	 * using context_a we need to check this bit
1543 	 */
1544 
1545 return_error:
1546 	return retval;
1547 }
1548 
1549 static int dpaa_bp_add_8_bufs(const struct dpaa_bp *dpaa_bp)
1550 {
1551 	struct net_device *net_dev = dpaa_bp->priv->net_dev;
1552 	struct bm_buffer bmb[8];
1553 	dma_addr_t addr;
1554 	struct page *p;
1555 	u8 i;
1556 
1557 	for (i = 0; i < 8; i++) {
1558 		p = dev_alloc_pages(0);
1559 		if (unlikely(!p)) {
1560 			netdev_err(net_dev, "dev_alloc_pages() failed\n");
1561 			goto release_previous_buffs;
1562 		}
1563 
1564 		addr = dma_map_page(dpaa_bp->priv->rx_dma_dev, p, 0,
1565 				    DPAA_BP_RAW_SIZE, DMA_FROM_DEVICE);
1566 		if (unlikely(dma_mapping_error(dpaa_bp->priv->rx_dma_dev,
1567 					       addr))) {
1568 			netdev_err(net_dev, "DMA map failed\n");
1569 			goto release_previous_buffs;
1570 		}
1571 
1572 		bmb[i].data = 0;
1573 		bm_buffer_set64(&bmb[i], addr);
1574 	}
1575 
1576 release_bufs:
1577 	return dpaa_bman_release(dpaa_bp, bmb, i);
1578 
1579 release_previous_buffs:
1580 	WARN_ONCE(1, "dpaa_eth: failed to add buffers on Rx\n");
1581 
1582 	bm_buffer_set64(&bmb[i], 0);
1583 	/* Avoid releasing a completely null buffer; bman_release() requires
1584 	 * at least one buffer.
1585 	 */
1586 	if (likely(i))
1587 		goto release_bufs;
1588 
1589 	return 0;
1590 }
1591 
1592 static int dpaa_bp_seed(struct dpaa_bp *dpaa_bp)
1593 {
1594 	int i;
1595 
1596 	/* Give each CPU an allotment of "config_count" buffers */
1597 	for_each_possible_cpu(i) {
1598 		int *count_ptr = per_cpu_ptr(dpaa_bp->percpu_count, i);
1599 		int j;
1600 
1601 		/* Although we access another CPU's counters here
1602 		 * we do it at boot time so it is safe
1603 		 */
1604 		for (j = 0; j < dpaa_bp->config_count; j += 8)
1605 			*count_ptr += dpaa_bp_add_8_bufs(dpaa_bp);
1606 	}
1607 	return 0;
1608 }
1609 
1610 /* Add buffers/(pages) for Rx processing whenever bpool count falls below
1611  * REFILL_THRESHOLD.
1612  */
1613 static int dpaa_eth_refill_bpool(struct dpaa_bp *dpaa_bp, int *countptr)
1614 {
1615 	int count = *countptr;
1616 	int new_bufs;
1617 
1618 	if (unlikely(count < FSL_DPAA_ETH_REFILL_THRESHOLD)) {
1619 		do {
1620 			new_bufs = dpaa_bp_add_8_bufs(dpaa_bp);
1621 			if (unlikely(!new_bufs)) {
1622 				/* Avoid looping forever if we've temporarily
1623 				 * run out of memory. We'll try again at the
1624 				 * next NAPI cycle.
1625 				 */
1626 				break;
1627 			}
1628 			count += new_bufs;
1629 		} while (count < FSL_DPAA_ETH_MAX_BUF_COUNT);
1630 
1631 		*countptr = count;
1632 		if (unlikely(count < FSL_DPAA_ETH_MAX_BUF_COUNT))
1633 			return -ENOMEM;
1634 	}
1635 
1636 	return 0;
1637 }
1638 
1639 static int dpaa_eth_refill_bpools(struct dpaa_priv *priv)
1640 {
1641 	struct dpaa_bp *dpaa_bp;
1642 	int *countptr;
1643 
1644 	dpaa_bp = priv->dpaa_bp;
1645 	if (!dpaa_bp)
1646 		return -EINVAL;
1647 	countptr = this_cpu_ptr(dpaa_bp->percpu_count);
1648 
1649 	return dpaa_eth_refill_bpool(dpaa_bp, countptr);
1650 }
1651 
1652 /* Cleanup function for outgoing frame descriptors that were built on Tx path,
1653  * either contiguous frames or scatter/gather ones.
1654  * Skb freeing is not handled here.
1655  *
1656  * This function may be called on error paths in the Tx function, so guard
1657  * against cases when not all fd relevant fields were filled in. To avoid
1658  * reading the invalid transmission timestamp for the error paths set ts to
1659  * false.
1660  *
1661  * Return the skb backpointer, since for S/G frames the buffer containing it
1662  * gets freed here.
1663  *
1664  * No skb backpointer is set when transmitting XDP frames. Cleanup the buffer
1665  * and return NULL in this case.
1666  */
1667 static struct sk_buff *dpaa_cleanup_tx_fd(const struct dpaa_priv *priv,
1668 					  const struct qm_fd *fd, bool ts)
1669 {
1670 	const enum dma_data_direction dma_dir = DMA_TO_DEVICE;
1671 	struct device *dev = priv->net_dev->dev.parent;
1672 	struct skb_shared_hwtstamps shhwtstamps;
1673 	dma_addr_t addr = qm_fd_addr(fd);
1674 	void *vaddr = phys_to_virt(addr);
1675 	const struct qm_sg_entry *sgt;
1676 	struct dpaa_eth_swbp *swbp;
1677 	struct sk_buff *skb;
1678 	u64 ns;
1679 	int i;
1680 
1681 	if (unlikely(qm_fd_get_format(fd) == qm_fd_sg)) {
1682 		dma_unmap_page(priv->tx_dma_dev, addr,
1683 			       qm_fd_get_offset(fd) + DPAA_SGT_SIZE,
1684 			       dma_dir);
1685 
1686 		/* The sgt buffer has been allocated with netdev_alloc_frag(),
1687 		 * it's from lowmem.
1688 		 */
1689 		sgt = vaddr + qm_fd_get_offset(fd);
1690 
1691 		/* sgt[0] is from lowmem, was dma_map_single()-ed */
1692 		dma_unmap_single(priv->tx_dma_dev, qm_sg_addr(&sgt[0]),
1693 				 qm_sg_entry_get_len(&sgt[0]), dma_dir);
1694 
1695 		/* remaining pages were mapped with skb_frag_dma_map() */
1696 		for (i = 1; (i < DPAA_SGT_MAX_ENTRIES) &&
1697 		     !qm_sg_entry_is_final(&sgt[i - 1]); i++) {
1698 			WARN_ON(qm_sg_entry_is_ext(&sgt[i]));
1699 
1700 			dma_unmap_page(priv->tx_dma_dev, qm_sg_addr(&sgt[i]),
1701 				       qm_sg_entry_get_len(&sgt[i]), dma_dir);
1702 		}
1703 	} else {
1704 		dma_unmap_single(priv->tx_dma_dev, addr,
1705 				 qm_fd_get_offset(fd) + qm_fd_get_length(fd),
1706 				 dma_dir);
1707 	}
1708 
1709 	swbp = (struct dpaa_eth_swbp *)vaddr;
1710 	skb = swbp->skb;
1711 
1712 	/* No skb backpointer is set when running XDP. An xdp_frame
1713 	 * backpointer is saved instead.
1714 	 */
1715 	if (!skb) {
1716 		xdp_return_frame(swbp->xdpf);
1717 		return NULL;
1718 	}
1719 
1720 	/* DMA unmapping is required before accessing the HW provided info */
1721 	if (ts && priv->tx_tstamp &&
1722 	    skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) {
1723 		memset(&shhwtstamps, 0, sizeof(shhwtstamps));
1724 
1725 		if (!fman_port_get_tstamp(priv->mac_dev->port[TX], vaddr,
1726 					  &ns)) {
1727 			shhwtstamps.hwtstamp = ns_to_ktime(ns);
1728 			skb_tstamp_tx(skb, &shhwtstamps);
1729 		} else {
1730 			dev_warn(dev, "fman_port_get_tstamp failed!\n");
1731 		}
1732 	}
1733 
1734 	if (qm_fd_get_format(fd) == qm_fd_sg)
1735 		/* Free the page that we allocated on Tx for the SGT */
1736 		free_pages((unsigned long)vaddr, 0);
1737 
1738 	return skb;
1739 }
1740 
1741 static u8 rx_csum_offload(const struct dpaa_priv *priv, const struct qm_fd *fd)
1742 {
1743 	/* The parser has run and performed L4 checksum validation.
1744 	 * We know there were no parser errors (and implicitly no
1745 	 * L4 csum error), otherwise we wouldn't be here.
1746 	 */
1747 	if ((priv->net_dev->features & NETIF_F_RXCSUM) &&
1748 	    (be32_to_cpu(fd->status) & FM_FD_STAT_L4CV))
1749 		return CHECKSUM_UNNECESSARY;
1750 
1751 	/* We're here because either the parser didn't run or the L4 checksum
1752 	 * was not verified. This may include the case of a UDP frame with
1753 	 * checksum zero or an L4 proto other than TCP/UDP
1754 	 */
1755 	return CHECKSUM_NONE;
1756 }
1757 
1758 #define PTR_IS_ALIGNED(x, a) (IS_ALIGNED((unsigned long)(x), (a)))
1759 
1760 /* Build a linear skb around the received buffer.
1761  * We are guaranteed there is enough room at the end of the data buffer to
1762  * accommodate the shared info area of the skb.
1763  */
1764 static struct sk_buff *contig_fd_to_skb(const struct dpaa_priv *priv,
1765 					const struct qm_fd *fd)
1766 {
1767 	ssize_t fd_off = qm_fd_get_offset(fd);
1768 	dma_addr_t addr = qm_fd_addr(fd);
1769 	struct dpaa_bp *dpaa_bp;
1770 	struct sk_buff *skb;
1771 	void *vaddr;
1772 
1773 	vaddr = phys_to_virt(addr);
1774 	WARN_ON(!IS_ALIGNED((unsigned long)vaddr, SMP_CACHE_BYTES));
1775 
1776 	dpaa_bp = dpaa_bpid2pool(fd->bpid);
1777 	if (!dpaa_bp)
1778 		goto free_buffer;
1779 
1780 	skb = build_skb(vaddr, dpaa_bp->size +
1781 			SKB_DATA_ALIGN(sizeof(struct skb_shared_info)));
1782 	if (WARN_ONCE(!skb, "Build skb failure on Rx\n"))
1783 		goto free_buffer;
1784 	skb_reserve(skb, fd_off);
1785 	skb_put(skb, qm_fd_get_length(fd));
1786 
1787 	skb->ip_summed = rx_csum_offload(priv, fd);
1788 
1789 	return skb;
1790 
1791 free_buffer:
1792 	free_pages((unsigned long)vaddr, 0);
1793 	return NULL;
1794 }
1795 
1796 /* Build an skb with the data of the first S/G entry in the linear portion and
1797  * the rest of the frame as skb fragments.
1798  *
1799  * The page fragment holding the S/G Table is recycled here.
1800  */
1801 static struct sk_buff *sg_fd_to_skb(const struct dpaa_priv *priv,
1802 				    const struct qm_fd *fd)
1803 {
1804 	ssize_t fd_off = qm_fd_get_offset(fd);
1805 	dma_addr_t addr = qm_fd_addr(fd);
1806 	const struct qm_sg_entry *sgt;
1807 	struct page *page, *head_page;
1808 	struct dpaa_bp *dpaa_bp;
1809 	void *vaddr, *sg_vaddr;
1810 	int frag_off, frag_len;
1811 	struct sk_buff *skb;
1812 	dma_addr_t sg_addr;
1813 	int page_offset;
1814 	unsigned int sz;
1815 	int *count_ptr;
1816 	int i, j;
1817 
1818 	vaddr = phys_to_virt(addr);
1819 	WARN_ON(!IS_ALIGNED((unsigned long)vaddr, SMP_CACHE_BYTES));
1820 
1821 	/* Iterate through the SGT entries and add data buffers to the skb */
1822 	sgt = vaddr + fd_off;
1823 	skb = NULL;
1824 	for (i = 0; i < DPAA_SGT_MAX_ENTRIES; i++) {
1825 		/* Extension bit is not supported */
1826 		WARN_ON(qm_sg_entry_is_ext(&sgt[i]));
1827 
1828 		sg_addr = qm_sg_addr(&sgt[i]);
1829 		sg_vaddr = phys_to_virt(sg_addr);
1830 		WARN_ON(!PTR_IS_ALIGNED(sg_vaddr, SMP_CACHE_BYTES));
1831 
1832 		dma_unmap_page(priv->rx_dma_dev, sg_addr,
1833 			       DPAA_BP_RAW_SIZE, DMA_FROM_DEVICE);
1834 
1835 		/* We may use multiple Rx pools */
1836 		dpaa_bp = dpaa_bpid2pool(sgt[i].bpid);
1837 		if (!dpaa_bp)
1838 			goto free_buffers;
1839 
1840 		if (!skb) {
1841 			sz = dpaa_bp->size +
1842 				SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
1843 			skb = build_skb(sg_vaddr, sz);
1844 			if (WARN_ON(!skb))
1845 				goto free_buffers;
1846 
1847 			skb->ip_summed = rx_csum_offload(priv, fd);
1848 
1849 			/* Make sure forwarded skbs will have enough space
1850 			 * on Tx, if extra headers are added.
1851 			 */
1852 			WARN_ON(fd_off != priv->rx_headroom);
1853 			skb_reserve(skb, fd_off);
1854 			skb_put(skb, qm_sg_entry_get_len(&sgt[i]));
1855 		} else {
1856 			/* Not the first S/G entry; all data from buffer will
1857 			 * be added in an skb fragment; fragment index is offset
1858 			 * by one since first S/G entry was incorporated in the
1859 			 * linear part of the skb.
1860 			 *
1861 			 * Caution: 'page' may be a tail page.
1862 			 */
1863 			page = virt_to_page(sg_vaddr);
1864 			head_page = virt_to_head_page(sg_vaddr);
1865 
1866 			/* Compute offset in (possibly tail) page */
1867 			page_offset = ((unsigned long)sg_vaddr &
1868 					(PAGE_SIZE - 1)) +
1869 				(page_address(page) - page_address(head_page));
1870 			/* page_offset only refers to the beginning of sgt[i];
1871 			 * but the buffer itself may have an internal offset.
1872 			 */
1873 			frag_off = qm_sg_entry_get_off(&sgt[i]) + page_offset;
1874 			frag_len = qm_sg_entry_get_len(&sgt[i]);
1875 			/* skb_add_rx_frag() does no checking on the page; if
1876 			 * we pass it a tail page, we'll end up with
1877 			 * bad page accounting and eventually with segafults.
1878 			 */
1879 			skb_add_rx_frag(skb, i - 1, head_page, frag_off,
1880 					frag_len, dpaa_bp->size);
1881 		}
1882 
1883 		/* Update the pool count for the current {cpu x bpool} */
1884 		count_ptr = this_cpu_ptr(dpaa_bp->percpu_count);
1885 		(*count_ptr)--;
1886 
1887 		if (qm_sg_entry_is_final(&sgt[i]))
1888 			break;
1889 	}
1890 	WARN_ONCE(i == DPAA_SGT_MAX_ENTRIES, "No final bit on SGT\n");
1891 
1892 	/* free the SG table buffer */
1893 	free_pages((unsigned long)vaddr, 0);
1894 
1895 	return skb;
1896 
1897 free_buffers:
1898 	/* free all the SG entries */
1899 	for (j = 0; j < DPAA_SGT_MAX_ENTRIES ; j++) {
1900 		sg_addr = qm_sg_addr(&sgt[j]);
1901 		sg_vaddr = phys_to_virt(sg_addr);
1902 		/* all pages 0..i were unmaped */
1903 		if (j > i)
1904 			dma_unmap_page(priv->rx_dma_dev, qm_sg_addr(&sgt[j]),
1905 				       DPAA_BP_RAW_SIZE, DMA_FROM_DEVICE);
1906 		free_pages((unsigned long)sg_vaddr, 0);
1907 		/* counters 0..i-1 were decremented */
1908 		if (j >= i) {
1909 			dpaa_bp = dpaa_bpid2pool(sgt[j].bpid);
1910 			if (dpaa_bp) {
1911 				count_ptr = this_cpu_ptr(dpaa_bp->percpu_count);
1912 				(*count_ptr)--;
1913 			}
1914 		}
1915 
1916 		if (qm_sg_entry_is_final(&sgt[j]))
1917 			break;
1918 	}
1919 	/* free the SGT fragment */
1920 	free_pages((unsigned long)vaddr, 0);
1921 
1922 	return NULL;
1923 }
1924 
1925 static int skb_to_contig_fd(struct dpaa_priv *priv,
1926 			    struct sk_buff *skb, struct qm_fd *fd,
1927 			    int *offset)
1928 {
1929 	struct net_device *net_dev = priv->net_dev;
1930 	enum dma_data_direction dma_dir;
1931 	struct dpaa_eth_swbp *swbp;
1932 	unsigned char *buff_start;
1933 	dma_addr_t addr;
1934 	int err;
1935 
1936 	/* We are guaranteed to have at least tx_headroom bytes
1937 	 * available, so just use that for offset.
1938 	 */
1939 	fd->bpid = FSL_DPAA_BPID_INV;
1940 	buff_start = skb->data - priv->tx_headroom;
1941 	dma_dir = DMA_TO_DEVICE;
1942 
1943 	swbp = (struct dpaa_eth_swbp *)buff_start;
1944 	swbp->skb = skb;
1945 
1946 	/* Enable L3/L4 hardware checksum computation.
1947 	 *
1948 	 * We must do this before dma_map_single(DMA_TO_DEVICE), because we may
1949 	 * need to write into the skb.
1950 	 */
1951 	err = dpaa_enable_tx_csum(priv, skb, fd,
1952 				  buff_start + DPAA_TX_PRIV_DATA_SIZE);
1953 	if (unlikely(err < 0)) {
1954 		if (net_ratelimit())
1955 			netif_err(priv, tx_err, net_dev, "HW csum error: %d\n",
1956 				  err);
1957 		return err;
1958 	}
1959 
1960 	/* Fill in the rest of the FD fields */
1961 	qm_fd_set_contig(fd, priv->tx_headroom, skb->len);
1962 	fd->cmd |= cpu_to_be32(FM_FD_CMD_FCO);
1963 
1964 	/* Map the entire buffer size that may be seen by FMan, but no more */
1965 	addr = dma_map_single(priv->tx_dma_dev, buff_start,
1966 			      priv->tx_headroom + skb->len, dma_dir);
1967 	if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
1968 		if (net_ratelimit())
1969 			netif_err(priv, tx_err, net_dev, "dma_map_single() failed\n");
1970 		return -EINVAL;
1971 	}
1972 	qm_fd_addr_set64(fd, addr);
1973 
1974 	return 0;
1975 }
1976 
1977 static int skb_to_sg_fd(struct dpaa_priv *priv,
1978 			struct sk_buff *skb, struct qm_fd *fd)
1979 {
1980 	const enum dma_data_direction dma_dir = DMA_TO_DEVICE;
1981 	const int nr_frags = skb_shinfo(skb)->nr_frags;
1982 	struct net_device *net_dev = priv->net_dev;
1983 	struct dpaa_eth_swbp *swbp;
1984 	struct qm_sg_entry *sgt;
1985 	void *buff_start;
1986 	skb_frag_t *frag;
1987 	dma_addr_t addr;
1988 	size_t frag_len;
1989 	struct page *p;
1990 	int i, j, err;
1991 
1992 	/* get a page to store the SGTable */
1993 	p = dev_alloc_pages(0);
1994 	if (unlikely(!p)) {
1995 		netdev_err(net_dev, "dev_alloc_pages() failed\n");
1996 		return -ENOMEM;
1997 	}
1998 	buff_start = page_address(p);
1999 
2000 	/* Enable L3/L4 hardware checksum computation.
2001 	 *
2002 	 * We must do this before dma_map_single(DMA_TO_DEVICE), because we may
2003 	 * need to write into the skb.
2004 	 */
2005 	err = dpaa_enable_tx_csum(priv, skb, fd,
2006 				  buff_start + DPAA_TX_PRIV_DATA_SIZE);
2007 	if (unlikely(err < 0)) {
2008 		if (net_ratelimit())
2009 			netif_err(priv, tx_err, net_dev, "HW csum error: %d\n",
2010 				  err);
2011 		goto csum_failed;
2012 	}
2013 
2014 	/* SGT[0] is used by the linear part */
2015 	sgt = (struct qm_sg_entry *)(buff_start + priv->tx_headroom);
2016 	frag_len = skb_headlen(skb);
2017 	qm_sg_entry_set_len(&sgt[0], frag_len);
2018 	sgt[0].bpid = FSL_DPAA_BPID_INV;
2019 	sgt[0].offset = 0;
2020 	addr = dma_map_single(priv->tx_dma_dev, skb->data,
2021 			      skb_headlen(skb), dma_dir);
2022 	if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
2023 		netdev_err(priv->net_dev, "DMA mapping failed\n");
2024 		err = -EINVAL;
2025 		goto sg0_map_failed;
2026 	}
2027 	qm_sg_entry_set64(&sgt[0], addr);
2028 
2029 	/* populate the rest of SGT entries */
2030 	for (i = 0; i < nr_frags; i++) {
2031 		frag = &skb_shinfo(skb)->frags[i];
2032 		frag_len = skb_frag_size(frag);
2033 		WARN_ON(!skb_frag_page(frag));
2034 		addr = skb_frag_dma_map(priv->tx_dma_dev, frag, 0,
2035 					frag_len, dma_dir);
2036 		if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
2037 			netdev_err(priv->net_dev, "DMA mapping failed\n");
2038 			err = -EINVAL;
2039 			goto sg_map_failed;
2040 		}
2041 
2042 		qm_sg_entry_set_len(&sgt[i + 1], frag_len);
2043 		sgt[i + 1].bpid = FSL_DPAA_BPID_INV;
2044 		sgt[i + 1].offset = 0;
2045 
2046 		/* keep the offset in the address */
2047 		qm_sg_entry_set64(&sgt[i + 1], addr);
2048 	}
2049 
2050 	/* Set the final bit in the last used entry of the SGT */
2051 	qm_sg_entry_set_f(&sgt[nr_frags], frag_len);
2052 
2053 	/* set fd offset to priv->tx_headroom */
2054 	qm_fd_set_sg(fd, priv->tx_headroom, skb->len);
2055 
2056 	/* DMA map the SGT page */
2057 	swbp = (struct dpaa_eth_swbp *)buff_start;
2058 	swbp->skb = skb;
2059 
2060 	addr = dma_map_page(priv->tx_dma_dev, p, 0,
2061 			    priv->tx_headroom + DPAA_SGT_SIZE, dma_dir);
2062 	if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
2063 		netdev_err(priv->net_dev, "DMA mapping failed\n");
2064 		err = -EINVAL;
2065 		goto sgt_map_failed;
2066 	}
2067 
2068 	fd->bpid = FSL_DPAA_BPID_INV;
2069 	fd->cmd |= cpu_to_be32(FM_FD_CMD_FCO);
2070 	qm_fd_addr_set64(fd, addr);
2071 
2072 	return 0;
2073 
2074 sgt_map_failed:
2075 sg_map_failed:
2076 	for (j = 0; j < i; j++)
2077 		dma_unmap_page(priv->tx_dma_dev, qm_sg_addr(&sgt[j]),
2078 			       qm_sg_entry_get_len(&sgt[j]), dma_dir);
2079 sg0_map_failed:
2080 csum_failed:
2081 	free_pages((unsigned long)buff_start, 0);
2082 
2083 	return err;
2084 }
2085 
2086 static inline int dpaa_xmit(struct dpaa_priv *priv,
2087 			    struct rtnl_link_stats64 *percpu_stats,
2088 			    int queue,
2089 			    struct qm_fd *fd)
2090 {
2091 	struct qman_fq *egress_fq;
2092 	int err, i;
2093 
2094 	egress_fq = priv->egress_fqs[queue];
2095 	if (fd->bpid == FSL_DPAA_BPID_INV)
2096 		fd->cmd |= cpu_to_be32(qman_fq_fqid(priv->conf_fqs[queue]));
2097 
2098 	/* Trace this Tx fd */
2099 	trace_dpaa_tx_fd(priv->net_dev, egress_fq, fd);
2100 
2101 	for (i = 0; i < DPAA_ENQUEUE_RETRIES; i++) {
2102 		err = qman_enqueue(egress_fq, fd);
2103 		if (err != -EBUSY)
2104 			break;
2105 	}
2106 
2107 	if (unlikely(err < 0)) {
2108 		percpu_stats->tx_fifo_errors++;
2109 		return err;
2110 	}
2111 
2112 	percpu_stats->tx_packets++;
2113 	percpu_stats->tx_bytes += qm_fd_get_length(fd);
2114 
2115 	return 0;
2116 }
2117 
2118 #ifdef CONFIG_DPAA_ERRATUM_A050385
2119 static int dpaa_a050385_wa_skb(struct net_device *net_dev, struct sk_buff **s)
2120 {
2121 	struct dpaa_priv *priv = netdev_priv(net_dev);
2122 	struct sk_buff *new_skb, *skb = *s;
2123 	unsigned char *start, i;
2124 
2125 	/* check linear buffer alignment */
2126 	if (!PTR_IS_ALIGNED(skb->data, DPAA_A050385_ALIGN))
2127 		goto workaround;
2128 
2129 	/* linear buffers just need to have an aligned start */
2130 	if (!skb_is_nonlinear(skb))
2131 		return 0;
2132 
2133 	/* linear data size for nonlinear skbs needs to be aligned */
2134 	if (!IS_ALIGNED(skb_headlen(skb), DPAA_A050385_ALIGN))
2135 		goto workaround;
2136 
2137 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
2138 		skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
2139 
2140 		/* all fragments need to have aligned start addresses */
2141 		if (!IS_ALIGNED(skb_frag_off(frag), DPAA_A050385_ALIGN))
2142 			goto workaround;
2143 
2144 		/* all but last fragment need to have aligned sizes */
2145 		if (!IS_ALIGNED(skb_frag_size(frag), DPAA_A050385_ALIGN) &&
2146 		    (i < skb_shinfo(skb)->nr_frags - 1))
2147 			goto workaround;
2148 	}
2149 
2150 	return 0;
2151 
2152 workaround:
2153 	/* copy all the skb content into a new linear buffer */
2154 	new_skb = netdev_alloc_skb(net_dev, skb->len + DPAA_A050385_ALIGN - 1 +
2155 						priv->tx_headroom);
2156 	if (!new_skb)
2157 		return -ENOMEM;
2158 
2159 	/* NET_SKB_PAD bytes already reserved, adding up to tx_headroom */
2160 	skb_reserve(new_skb, priv->tx_headroom - NET_SKB_PAD);
2161 
2162 	/* Workaround for DPAA_A050385 requires data start to be aligned */
2163 	start = PTR_ALIGN(new_skb->data, DPAA_A050385_ALIGN);
2164 	if (start - new_skb->data)
2165 		skb_reserve(new_skb, start - new_skb->data);
2166 
2167 	skb_put(new_skb, skb->len);
2168 	skb_copy_bits(skb, 0, new_skb->data, skb->len);
2169 	skb_copy_header(new_skb, skb);
2170 	new_skb->dev = skb->dev;
2171 
2172 	/* Copy relevant timestamp info from the old skb to the new */
2173 	if (priv->tx_tstamp) {
2174 		skb_shinfo(new_skb)->tx_flags = skb_shinfo(skb)->tx_flags;
2175 		skb_shinfo(new_skb)->hwtstamps = skb_shinfo(skb)->hwtstamps;
2176 		skb_shinfo(new_skb)->tskey = skb_shinfo(skb)->tskey;
2177 		if (skb->sk)
2178 			skb_set_owner_w(new_skb, skb->sk);
2179 	}
2180 
2181 	/* We move the headroom when we align it so we have to reset the
2182 	 * network and transport header offsets relative to the new data
2183 	 * pointer. The checksum offload relies on these offsets.
2184 	 */
2185 	skb_set_network_header(new_skb, skb_network_offset(skb));
2186 	skb_set_transport_header(new_skb, skb_transport_offset(skb));
2187 
2188 	dev_kfree_skb(skb);
2189 	*s = new_skb;
2190 
2191 	return 0;
2192 }
2193 
2194 static int dpaa_a050385_wa_xdpf(struct dpaa_priv *priv,
2195 				struct xdp_frame **init_xdpf)
2196 {
2197 	struct xdp_frame *new_xdpf, *xdpf = *init_xdpf;
2198 	void *new_buff, *aligned_data;
2199 	struct page *p;
2200 	u32 data_shift;
2201 	int headroom;
2202 
2203 	/* Check the data alignment and make sure the headroom is large
2204 	 * enough to store the xdpf backpointer. Use an aligned headroom
2205 	 * value.
2206 	 *
2207 	 * Due to alignment constraints, we give XDP access to the full 256
2208 	 * byte frame headroom. If the XDP program uses all of it, copy the
2209 	 * data to a new buffer and make room for storing the backpointer.
2210 	 */
2211 	if (PTR_IS_ALIGNED(xdpf->data, DPAA_FD_DATA_ALIGNMENT) &&
2212 	    xdpf->headroom >= priv->tx_headroom) {
2213 		xdpf->headroom = priv->tx_headroom;
2214 		return 0;
2215 	}
2216 
2217 	/* Try to move the data inside the buffer just enough to align it and
2218 	 * store the xdpf backpointer. If the available headroom isn't large
2219 	 * enough, resort to allocating a new buffer and copying the data.
2220 	 */
2221 	aligned_data = PTR_ALIGN_DOWN(xdpf->data, DPAA_FD_DATA_ALIGNMENT);
2222 	data_shift = xdpf->data - aligned_data;
2223 
2224 	/* The XDP frame's headroom needs to be large enough to accommodate
2225 	 * shifting the data as well as storing the xdpf backpointer.
2226 	 */
2227 	if (xdpf->headroom  >= data_shift + priv->tx_headroom) {
2228 		memmove(aligned_data, xdpf->data, xdpf->len);
2229 		xdpf->data = aligned_data;
2230 		xdpf->headroom = priv->tx_headroom;
2231 		return 0;
2232 	}
2233 
2234 	/* The new xdp_frame is stored in the new buffer. Reserve enough space
2235 	 * in the headroom for storing it along with the driver's private
2236 	 * info. The headroom needs to be aligned to DPAA_FD_DATA_ALIGNMENT to
2237 	 * guarantee the data's alignment in the buffer.
2238 	 */
2239 	headroom = ALIGN(sizeof(*new_xdpf) + priv->tx_headroom,
2240 			 DPAA_FD_DATA_ALIGNMENT);
2241 
2242 	/* Assure the extended headroom and data don't overflow the buffer,
2243 	 * while maintaining the mandatory tailroom.
2244 	 */
2245 	if (headroom + xdpf->len > DPAA_BP_RAW_SIZE -
2246 			SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))
2247 		return -ENOMEM;
2248 
2249 	p = dev_alloc_pages(0);
2250 	if (unlikely(!p))
2251 		return -ENOMEM;
2252 
2253 	/* Copy the data to the new buffer at a properly aligned offset */
2254 	new_buff = page_address(p);
2255 	memcpy(new_buff + headroom, xdpf->data, xdpf->len);
2256 
2257 	/* Create an XDP frame around the new buffer in a similar fashion
2258 	 * to xdp_convert_buff_to_frame.
2259 	 */
2260 	new_xdpf = new_buff;
2261 	new_xdpf->data = new_buff + headroom;
2262 	new_xdpf->len = xdpf->len;
2263 	new_xdpf->headroom = priv->tx_headroom;
2264 	new_xdpf->frame_sz = DPAA_BP_RAW_SIZE;
2265 	new_xdpf->mem.type = MEM_TYPE_PAGE_ORDER0;
2266 
2267 	/* Release the initial buffer */
2268 	xdp_return_frame_rx_napi(xdpf);
2269 
2270 	*init_xdpf = new_xdpf;
2271 	return 0;
2272 }
2273 #endif
2274 
2275 static netdev_tx_t
2276 dpaa_start_xmit(struct sk_buff *skb, struct net_device *net_dev)
2277 {
2278 	const int queue_mapping = skb_get_queue_mapping(skb);
2279 	bool nonlinear = skb_is_nonlinear(skb);
2280 	struct rtnl_link_stats64 *percpu_stats;
2281 	struct dpaa_percpu_priv *percpu_priv;
2282 	struct netdev_queue *txq;
2283 	struct dpaa_priv *priv;
2284 	struct qm_fd fd;
2285 	int offset = 0;
2286 	int err = 0;
2287 
2288 	priv = netdev_priv(net_dev);
2289 	percpu_priv = this_cpu_ptr(priv->percpu_priv);
2290 	percpu_stats = &percpu_priv->stats;
2291 
2292 	qm_fd_clear_fd(&fd);
2293 
2294 	if (!nonlinear) {
2295 		/* We're going to store the skb backpointer at the beginning
2296 		 * of the data buffer, so we need a privately owned skb
2297 		 *
2298 		 * We've made sure skb is not shared in dev->priv_flags,
2299 		 * we need to verify the skb head is not cloned
2300 		 */
2301 		if (skb_cow_head(skb, priv->tx_headroom))
2302 			goto enomem;
2303 
2304 		WARN_ON(skb_is_nonlinear(skb));
2305 	}
2306 
2307 	/* MAX_SKB_FRAGS is equal or larger than our dpaa_SGT_MAX_ENTRIES;
2308 	 * make sure we don't feed FMan with more fragments than it supports.
2309 	 */
2310 	if (unlikely(nonlinear &&
2311 		     (skb_shinfo(skb)->nr_frags >= DPAA_SGT_MAX_ENTRIES))) {
2312 		/* If the egress skb contains more fragments than we support
2313 		 * we have no choice but to linearize it ourselves.
2314 		 */
2315 		if (__skb_linearize(skb))
2316 			goto enomem;
2317 
2318 		nonlinear = skb_is_nonlinear(skb);
2319 	}
2320 
2321 #ifdef CONFIG_DPAA_ERRATUM_A050385
2322 	if (unlikely(fman_has_errata_a050385())) {
2323 		if (dpaa_a050385_wa_skb(net_dev, &skb))
2324 			goto enomem;
2325 		nonlinear = skb_is_nonlinear(skb);
2326 	}
2327 #endif
2328 
2329 	if (nonlinear) {
2330 		/* Just create a S/G fd based on the skb */
2331 		err = skb_to_sg_fd(priv, skb, &fd);
2332 		percpu_priv->tx_frag_skbuffs++;
2333 	} else {
2334 		/* Create a contig FD from this skb */
2335 		err = skb_to_contig_fd(priv, skb, &fd, &offset);
2336 	}
2337 	if (unlikely(err < 0))
2338 		goto skb_to_fd_failed;
2339 
2340 	txq = netdev_get_tx_queue(net_dev, queue_mapping);
2341 
2342 	/* LLTX requires to do our own update of trans_start */
2343 	txq_trans_cond_update(txq);
2344 
2345 	if (priv->tx_tstamp && skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) {
2346 		fd.cmd |= cpu_to_be32(FM_FD_CMD_UPD);
2347 		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
2348 	}
2349 
2350 	if (likely(dpaa_xmit(priv, percpu_stats, queue_mapping, &fd) == 0))
2351 		return NETDEV_TX_OK;
2352 
2353 	dpaa_cleanup_tx_fd(priv, &fd, false);
2354 skb_to_fd_failed:
2355 enomem:
2356 	percpu_stats->tx_errors++;
2357 	dev_kfree_skb(skb);
2358 	return NETDEV_TX_OK;
2359 }
2360 
2361 static void dpaa_rx_error(struct net_device *net_dev,
2362 			  const struct dpaa_priv *priv,
2363 			  struct dpaa_percpu_priv *percpu_priv,
2364 			  const struct qm_fd *fd,
2365 			  u32 fqid)
2366 {
2367 	if (net_ratelimit())
2368 		netif_err(priv, hw, net_dev, "Err FD status = 0x%08x\n",
2369 			  be32_to_cpu(fd->status) & FM_FD_STAT_RX_ERRORS);
2370 
2371 	percpu_priv->stats.rx_errors++;
2372 
2373 	if (be32_to_cpu(fd->status) & FM_FD_ERR_DMA)
2374 		percpu_priv->rx_errors.dme++;
2375 	if (be32_to_cpu(fd->status) & FM_FD_ERR_PHYSICAL)
2376 		percpu_priv->rx_errors.fpe++;
2377 	if (be32_to_cpu(fd->status) & FM_FD_ERR_SIZE)
2378 		percpu_priv->rx_errors.fse++;
2379 	if (be32_to_cpu(fd->status) & FM_FD_ERR_PRS_HDR_ERR)
2380 		percpu_priv->rx_errors.phe++;
2381 
2382 	dpaa_fd_release(net_dev, fd);
2383 }
2384 
2385 static void dpaa_tx_error(struct net_device *net_dev,
2386 			  const struct dpaa_priv *priv,
2387 			  struct dpaa_percpu_priv *percpu_priv,
2388 			  const struct qm_fd *fd,
2389 			  u32 fqid)
2390 {
2391 	struct sk_buff *skb;
2392 
2393 	if (net_ratelimit())
2394 		netif_warn(priv, hw, net_dev, "FD status = 0x%08x\n",
2395 			   be32_to_cpu(fd->status) & FM_FD_STAT_TX_ERRORS);
2396 
2397 	percpu_priv->stats.tx_errors++;
2398 
2399 	skb = dpaa_cleanup_tx_fd(priv, fd, false);
2400 	dev_kfree_skb(skb);
2401 }
2402 
2403 static int dpaa_eth_poll(struct napi_struct *napi, int budget)
2404 {
2405 	struct dpaa_napi_portal *np =
2406 			container_of(napi, struct dpaa_napi_portal, napi);
2407 	int cleaned;
2408 
2409 	np->xdp_act = 0;
2410 
2411 	cleaned = qman_p_poll_dqrr(np->p, budget);
2412 
2413 	if (np->xdp_act & XDP_REDIRECT)
2414 		xdp_do_flush();
2415 
2416 	if (cleaned < budget) {
2417 		napi_complete_done(napi, cleaned);
2418 		qman_p_irqsource_add(np->p, QM_PIRQ_DQRI);
2419 	} else if (np->down) {
2420 		qman_p_irqsource_add(np->p, QM_PIRQ_DQRI);
2421 	}
2422 
2423 	return cleaned;
2424 }
2425 
2426 static void dpaa_tx_conf(struct net_device *net_dev,
2427 			 const struct dpaa_priv *priv,
2428 			 struct dpaa_percpu_priv *percpu_priv,
2429 			 const struct qm_fd *fd,
2430 			 u32 fqid)
2431 {
2432 	struct sk_buff	*skb;
2433 
2434 	if (unlikely(be32_to_cpu(fd->status) & FM_FD_STAT_TX_ERRORS)) {
2435 		if (net_ratelimit())
2436 			netif_warn(priv, hw, net_dev, "FD status = 0x%08x\n",
2437 				   be32_to_cpu(fd->status) &
2438 				   FM_FD_STAT_TX_ERRORS);
2439 
2440 		percpu_priv->stats.tx_errors++;
2441 	}
2442 
2443 	percpu_priv->tx_confirm++;
2444 
2445 	skb = dpaa_cleanup_tx_fd(priv, fd, true);
2446 
2447 	consume_skb(skb);
2448 }
2449 
2450 static inline int dpaa_eth_napi_schedule(struct dpaa_percpu_priv *percpu_priv,
2451 					 struct qman_portal *portal, bool sched_napi)
2452 {
2453 	if (sched_napi) {
2454 		/* Disable QMan IRQ and invoke NAPI */
2455 		qman_p_irqsource_remove(portal, QM_PIRQ_DQRI);
2456 
2457 		percpu_priv->np.p = portal;
2458 		napi_schedule(&percpu_priv->np.napi);
2459 		percpu_priv->in_interrupt++;
2460 		return 1;
2461 	}
2462 	return 0;
2463 }
2464 
2465 static enum qman_cb_dqrr_result rx_error_dqrr(struct qman_portal *portal,
2466 					      struct qman_fq *fq,
2467 					      const struct qm_dqrr_entry *dq,
2468 					      bool sched_napi)
2469 {
2470 	struct dpaa_fq *dpaa_fq = container_of(fq, struct dpaa_fq, fq_base);
2471 	struct dpaa_percpu_priv *percpu_priv;
2472 	struct net_device *net_dev;
2473 	struct dpaa_bp *dpaa_bp;
2474 	struct dpaa_priv *priv;
2475 
2476 	net_dev = dpaa_fq->net_dev;
2477 	priv = netdev_priv(net_dev);
2478 	dpaa_bp = dpaa_bpid2pool(dq->fd.bpid);
2479 	if (!dpaa_bp)
2480 		return qman_cb_dqrr_consume;
2481 
2482 	percpu_priv = this_cpu_ptr(priv->percpu_priv);
2483 
2484 	if (dpaa_eth_napi_schedule(percpu_priv, portal, sched_napi))
2485 		return qman_cb_dqrr_stop;
2486 
2487 	dpaa_eth_refill_bpools(priv);
2488 	dpaa_rx_error(net_dev, priv, percpu_priv, &dq->fd, fq->fqid);
2489 
2490 	return qman_cb_dqrr_consume;
2491 }
2492 
2493 static int dpaa_xdp_xmit_frame(struct net_device *net_dev,
2494 			       struct xdp_frame *xdpf)
2495 {
2496 	struct dpaa_priv *priv = netdev_priv(net_dev);
2497 	struct rtnl_link_stats64 *percpu_stats;
2498 	struct dpaa_percpu_priv *percpu_priv;
2499 	struct dpaa_eth_swbp *swbp;
2500 	struct netdev_queue *txq;
2501 	void *buff_start;
2502 	struct qm_fd fd;
2503 	dma_addr_t addr;
2504 	int err;
2505 
2506 	percpu_priv = this_cpu_ptr(priv->percpu_priv);
2507 	percpu_stats = &percpu_priv->stats;
2508 
2509 #ifdef CONFIG_DPAA_ERRATUM_A050385
2510 	if (unlikely(fman_has_errata_a050385())) {
2511 		if (dpaa_a050385_wa_xdpf(priv, &xdpf)) {
2512 			err = -ENOMEM;
2513 			goto out_error;
2514 		}
2515 	}
2516 #endif
2517 
2518 	if (xdpf->headroom < DPAA_TX_PRIV_DATA_SIZE) {
2519 		err = -EINVAL;
2520 		goto out_error;
2521 	}
2522 
2523 	buff_start = xdpf->data - xdpf->headroom;
2524 
2525 	/* Leave empty the skb backpointer at the start of the buffer.
2526 	 * Save the XDP frame for easy cleanup on confirmation.
2527 	 */
2528 	swbp = (struct dpaa_eth_swbp *)buff_start;
2529 	swbp->skb = NULL;
2530 	swbp->xdpf = xdpf;
2531 
2532 	qm_fd_clear_fd(&fd);
2533 	fd.bpid = FSL_DPAA_BPID_INV;
2534 	fd.cmd |= cpu_to_be32(FM_FD_CMD_FCO);
2535 	qm_fd_set_contig(&fd, xdpf->headroom, xdpf->len);
2536 
2537 	addr = dma_map_single(priv->tx_dma_dev, buff_start,
2538 			      xdpf->headroom + xdpf->len,
2539 			      DMA_TO_DEVICE);
2540 	if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
2541 		err = -EINVAL;
2542 		goto out_error;
2543 	}
2544 
2545 	qm_fd_addr_set64(&fd, addr);
2546 
2547 	/* Bump the trans_start */
2548 	txq = netdev_get_tx_queue(net_dev, smp_processor_id());
2549 	txq_trans_cond_update(txq);
2550 
2551 	err = dpaa_xmit(priv, percpu_stats, smp_processor_id(), &fd);
2552 	if (err) {
2553 		dma_unmap_single(priv->tx_dma_dev, addr,
2554 				 qm_fd_get_offset(&fd) + qm_fd_get_length(&fd),
2555 				 DMA_TO_DEVICE);
2556 		goto out_error;
2557 	}
2558 
2559 	return 0;
2560 
2561 out_error:
2562 	percpu_stats->tx_errors++;
2563 	return err;
2564 }
2565 
2566 static u32 dpaa_run_xdp(struct dpaa_priv *priv, struct qm_fd *fd, void *vaddr,
2567 			struct dpaa_fq *dpaa_fq, unsigned int *xdp_meta_len)
2568 {
2569 	ssize_t fd_off = qm_fd_get_offset(fd);
2570 	struct bpf_prog *xdp_prog;
2571 	struct xdp_frame *xdpf;
2572 	struct xdp_buff xdp;
2573 	u32 xdp_act;
2574 	int err;
2575 
2576 	xdp_prog = READ_ONCE(priv->xdp_prog);
2577 	if (!xdp_prog)
2578 		return XDP_PASS;
2579 
2580 	xdp_init_buff(&xdp, DPAA_BP_RAW_SIZE - DPAA_TX_PRIV_DATA_SIZE,
2581 		      &dpaa_fq->xdp_rxq);
2582 	xdp_prepare_buff(&xdp, vaddr + fd_off - XDP_PACKET_HEADROOM,
2583 			 XDP_PACKET_HEADROOM, qm_fd_get_length(fd), true);
2584 
2585 	/* We reserve a fixed headroom of 256 bytes under the erratum and we
2586 	 * offer it all to XDP programs to use. If no room is left for the
2587 	 * xdpf backpointer on TX, we will need to copy the data.
2588 	 * Disable metadata support since data realignments might be required
2589 	 * and the information can be lost.
2590 	 */
2591 #ifdef CONFIG_DPAA_ERRATUM_A050385
2592 	if (unlikely(fman_has_errata_a050385())) {
2593 		xdp_set_data_meta_invalid(&xdp);
2594 		xdp.data_hard_start = vaddr;
2595 		xdp.frame_sz = DPAA_BP_RAW_SIZE;
2596 	}
2597 #endif
2598 
2599 	xdp_act = bpf_prog_run_xdp(xdp_prog, &xdp);
2600 
2601 	/* Update the length and the offset of the FD */
2602 	qm_fd_set_contig(fd, xdp.data - vaddr, xdp.data_end - xdp.data);
2603 
2604 	switch (xdp_act) {
2605 	case XDP_PASS:
2606 #ifdef CONFIG_DPAA_ERRATUM_A050385
2607 		*xdp_meta_len = xdp_data_meta_unsupported(&xdp) ? 0 :
2608 				xdp.data - xdp.data_meta;
2609 #else
2610 		*xdp_meta_len = xdp.data - xdp.data_meta;
2611 #endif
2612 		break;
2613 	case XDP_TX:
2614 		/* We can access the full headroom when sending the frame
2615 		 * back out
2616 		 */
2617 		xdp.data_hard_start = vaddr;
2618 		xdp.frame_sz = DPAA_BP_RAW_SIZE;
2619 		xdpf = xdp_convert_buff_to_frame(&xdp);
2620 		if (unlikely(!xdpf)) {
2621 			free_pages((unsigned long)vaddr, 0);
2622 			break;
2623 		}
2624 
2625 		if (dpaa_xdp_xmit_frame(priv->net_dev, xdpf))
2626 			xdp_return_frame_rx_napi(xdpf);
2627 
2628 		break;
2629 	case XDP_REDIRECT:
2630 		/* Allow redirect to use the full headroom */
2631 		xdp.data_hard_start = vaddr;
2632 		xdp.frame_sz = DPAA_BP_RAW_SIZE;
2633 
2634 		err = xdp_do_redirect(priv->net_dev, &xdp, xdp_prog);
2635 		if (err) {
2636 			trace_xdp_exception(priv->net_dev, xdp_prog, xdp_act);
2637 			free_pages((unsigned long)vaddr, 0);
2638 		}
2639 		break;
2640 	default:
2641 		bpf_warn_invalid_xdp_action(priv->net_dev, xdp_prog, xdp_act);
2642 		fallthrough;
2643 	case XDP_ABORTED:
2644 		trace_xdp_exception(priv->net_dev, xdp_prog, xdp_act);
2645 		fallthrough;
2646 	case XDP_DROP:
2647 		/* Free the buffer */
2648 		free_pages((unsigned long)vaddr, 0);
2649 		break;
2650 	}
2651 
2652 	return xdp_act;
2653 }
2654 
2655 static enum qman_cb_dqrr_result rx_default_dqrr(struct qman_portal *portal,
2656 						struct qman_fq *fq,
2657 						const struct qm_dqrr_entry *dq,
2658 						bool sched_napi)
2659 {
2660 	bool ts_valid = false, hash_valid = false;
2661 	struct skb_shared_hwtstamps *shhwtstamps;
2662 	unsigned int skb_len, xdp_meta_len = 0;
2663 	struct rtnl_link_stats64 *percpu_stats;
2664 	struct dpaa_percpu_priv *percpu_priv;
2665 	const struct qm_fd *fd = &dq->fd;
2666 	dma_addr_t addr = qm_fd_addr(fd);
2667 	struct dpaa_napi_portal *np;
2668 	enum qm_fd_format fd_format;
2669 	struct net_device *net_dev;
2670 	u32 fd_status, hash_offset;
2671 	struct qm_sg_entry *sgt;
2672 	struct dpaa_bp *dpaa_bp;
2673 	struct dpaa_fq *dpaa_fq;
2674 	struct dpaa_priv *priv;
2675 	struct sk_buff *skb;
2676 	int *count_ptr;
2677 	u32 xdp_act;
2678 	void *vaddr;
2679 	u32 hash;
2680 	u64 ns;
2681 
2682 	dpaa_fq = container_of(fq, struct dpaa_fq, fq_base);
2683 	fd_status = be32_to_cpu(fd->status);
2684 	fd_format = qm_fd_get_format(fd);
2685 	net_dev = dpaa_fq->net_dev;
2686 	priv = netdev_priv(net_dev);
2687 	dpaa_bp = dpaa_bpid2pool(dq->fd.bpid);
2688 	if (!dpaa_bp)
2689 		return qman_cb_dqrr_consume;
2690 
2691 	/* Trace the Rx fd */
2692 	trace_dpaa_rx_fd(net_dev, fq, &dq->fd);
2693 
2694 	percpu_priv = this_cpu_ptr(priv->percpu_priv);
2695 	percpu_stats = &percpu_priv->stats;
2696 	np = &percpu_priv->np;
2697 
2698 	if (unlikely(dpaa_eth_napi_schedule(percpu_priv, portal, sched_napi)))
2699 		return qman_cb_dqrr_stop;
2700 
2701 	/* Make sure we didn't run out of buffers */
2702 	if (unlikely(dpaa_eth_refill_bpools(priv))) {
2703 		/* Unable to refill the buffer pool due to insufficient
2704 		 * system memory. Just release the frame back into the pool,
2705 		 * otherwise we'll soon end up with an empty buffer pool.
2706 		 */
2707 		dpaa_fd_release(net_dev, &dq->fd);
2708 		return qman_cb_dqrr_consume;
2709 	}
2710 
2711 	if (unlikely(fd_status & FM_FD_STAT_RX_ERRORS) != 0) {
2712 		if (net_ratelimit())
2713 			netif_warn(priv, hw, net_dev, "FD status = 0x%08x\n",
2714 				   fd_status & FM_FD_STAT_RX_ERRORS);
2715 
2716 		percpu_stats->rx_errors++;
2717 		dpaa_fd_release(net_dev, fd);
2718 		return qman_cb_dqrr_consume;
2719 	}
2720 
2721 	dma_unmap_page(dpaa_bp->priv->rx_dma_dev, addr, DPAA_BP_RAW_SIZE,
2722 		       DMA_FROM_DEVICE);
2723 
2724 	/* prefetch the first 64 bytes of the frame or the SGT start */
2725 	vaddr = phys_to_virt(addr);
2726 	prefetch(vaddr + qm_fd_get_offset(fd));
2727 
2728 	/* The only FD types that we may receive are contig and S/G */
2729 	WARN_ON((fd_format != qm_fd_contig) && (fd_format != qm_fd_sg));
2730 
2731 	/* Account for either the contig buffer or the SGT buffer (depending on
2732 	 * which case we were in) having been removed from the pool.
2733 	 */
2734 	count_ptr = this_cpu_ptr(dpaa_bp->percpu_count);
2735 	(*count_ptr)--;
2736 
2737 	/* Extract the timestamp stored in the headroom before running XDP */
2738 	if (priv->rx_tstamp) {
2739 		if (!fman_port_get_tstamp(priv->mac_dev->port[RX], vaddr, &ns))
2740 			ts_valid = true;
2741 		else
2742 			WARN_ONCE(1, "fman_port_get_tstamp failed!\n");
2743 	}
2744 
2745 	/* Extract the hash stored in the headroom before running XDP */
2746 	if (net_dev->features & NETIF_F_RXHASH && priv->keygen_in_use &&
2747 	    !fman_port_get_hash_result_offset(priv->mac_dev->port[RX],
2748 					      &hash_offset)) {
2749 		hash = be32_to_cpu(*(u32 *)(vaddr + hash_offset));
2750 		hash_valid = true;
2751 	}
2752 
2753 	if (likely(fd_format == qm_fd_contig)) {
2754 		xdp_act = dpaa_run_xdp(priv, (struct qm_fd *)fd, vaddr,
2755 				       dpaa_fq, &xdp_meta_len);
2756 		np->xdp_act |= xdp_act;
2757 		if (xdp_act != XDP_PASS) {
2758 			percpu_stats->rx_packets++;
2759 			percpu_stats->rx_bytes += qm_fd_get_length(fd);
2760 			return qman_cb_dqrr_consume;
2761 		}
2762 		skb = contig_fd_to_skb(priv, fd);
2763 	} else {
2764 		/* XDP doesn't support S/G frames. Return the fragments to the
2765 		 * buffer pool and release the SGT.
2766 		 */
2767 		if (READ_ONCE(priv->xdp_prog)) {
2768 			WARN_ONCE(1, "S/G frames not supported under XDP\n");
2769 			sgt = vaddr + qm_fd_get_offset(fd);
2770 			dpaa_release_sgt_members(sgt);
2771 			free_pages((unsigned long)vaddr, 0);
2772 			return qman_cb_dqrr_consume;
2773 		}
2774 		skb = sg_fd_to_skb(priv, fd);
2775 	}
2776 	if (!skb)
2777 		return qman_cb_dqrr_consume;
2778 
2779 	if (xdp_meta_len)
2780 		skb_metadata_set(skb, xdp_meta_len);
2781 
2782 	/* Set the previously extracted timestamp */
2783 	if (ts_valid) {
2784 		shhwtstamps = skb_hwtstamps(skb);
2785 		memset(shhwtstamps, 0, sizeof(*shhwtstamps));
2786 		shhwtstamps->hwtstamp = ns_to_ktime(ns);
2787 	}
2788 
2789 	skb->protocol = eth_type_trans(skb, net_dev);
2790 
2791 	/* Set the previously extracted hash */
2792 	if (hash_valid) {
2793 		enum pkt_hash_types type;
2794 
2795 		/* if L4 exists, it was used in the hash generation */
2796 		type = be32_to_cpu(fd->status) & FM_FD_STAT_L4CV ?
2797 			PKT_HASH_TYPE_L4 : PKT_HASH_TYPE_L3;
2798 		skb_set_hash(skb, hash, type);
2799 	}
2800 
2801 	skb_len = skb->len;
2802 
2803 	if (unlikely(netif_receive_skb(skb) == NET_RX_DROP)) {
2804 		percpu_stats->rx_dropped++;
2805 		return qman_cb_dqrr_consume;
2806 	}
2807 
2808 	percpu_stats->rx_packets++;
2809 	percpu_stats->rx_bytes += skb_len;
2810 
2811 	return qman_cb_dqrr_consume;
2812 }
2813 
2814 static enum qman_cb_dqrr_result conf_error_dqrr(struct qman_portal *portal,
2815 						struct qman_fq *fq,
2816 						const struct qm_dqrr_entry *dq,
2817 						bool sched_napi)
2818 {
2819 	struct dpaa_percpu_priv *percpu_priv;
2820 	struct net_device *net_dev;
2821 	struct dpaa_priv *priv;
2822 
2823 	net_dev = ((struct dpaa_fq *)fq)->net_dev;
2824 	priv = netdev_priv(net_dev);
2825 
2826 	percpu_priv = this_cpu_ptr(priv->percpu_priv);
2827 
2828 	if (dpaa_eth_napi_schedule(percpu_priv, portal, sched_napi))
2829 		return qman_cb_dqrr_stop;
2830 
2831 	dpaa_tx_error(net_dev, priv, percpu_priv, &dq->fd, fq->fqid);
2832 
2833 	return qman_cb_dqrr_consume;
2834 }
2835 
2836 static enum qman_cb_dqrr_result conf_dflt_dqrr(struct qman_portal *portal,
2837 					       struct qman_fq *fq,
2838 					       const struct qm_dqrr_entry *dq,
2839 					       bool sched_napi)
2840 {
2841 	struct dpaa_percpu_priv *percpu_priv;
2842 	struct net_device *net_dev;
2843 	struct dpaa_priv *priv;
2844 
2845 	net_dev = ((struct dpaa_fq *)fq)->net_dev;
2846 	priv = netdev_priv(net_dev);
2847 
2848 	/* Trace the fd */
2849 	trace_dpaa_tx_conf_fd(net_dev, fq, &dq->fd);
2850 
2851 	percpu_priv = this_cpu_ptr(priv->percpu_priv);
2852 
2853 	if (dpaa_eth_napi_schedule(percpu_priv, portal, sched_napi))
2854 		return qman_cb_dqrr_stop;
2855 
2856 	dpaa_tx_conf(net_dev, priv, percpu_priv, &dq->fd, fq->fqid);
2857 
2858 	return qman_cb_dqrr_consume;
2859 }
2860 
2861 static void egress_ern(struct qman_portal *portal,
2862 		       struct qman_fq *fq,
2863 		       const union qm_mr_entry *msg)
2864 {
2865 	const struct qm_fd *fd = &msg->ern.fd;
2866 	struct dpaa_percpu_priv *percpu_priv;
2867 	const struct dpaa_priv *priv;
2868 	struct net_device *net_dev;
2869 	struct sk_buff *skb;
2870 
2871 	net_dev = ((struct dpaa_fq *)fq)->net_dev;
2872 	priv = netdev_priv(net_dev);
2873 	percpu_priv = this_cpu_ptr(priv->percpu_priv);
2874 
2875 	percpu_priv->stats.tx_dropped++;
2876 	percpu_priv->stats.tx_fifo_errors++;
2877 	count_ern(percpu_priv, msg);
2878 
2879 	skb = dpaa_cleanup_tx_fd(priv, fd, false);
2880 	dev_kfree_skb_any(skb);
2881 }
2882 
2883 static const struct dpaa_fq_cbs dpaa_fq_cbs = {
2884 	.rx_defq = { .cb = { .dqrr = rx_default_dqrr } },
2885 	.tx_defq = { .cb = { .dqrr = conf_dflt_dqrr } },
2886 	.rx_errq = { .cb = { .dqrr = rx_error_dqrr } },
2887 	.tx_errq = { .cb = { .dqrr = conf_error_dqrr } },
2888 	.egress_ern = { .cb = { .ern = egress_ern } }
2889 };
2890 
2891 static void dpaa_eth_napi_enable(struct dpaa_priv *priv)
2892 {
2893 	struct dpaa_percpu_priv *percpu_priv;
2894 	int i;
2895 
2896 	for_each_online_cpu(i) {
2897 		percpu_priv = per_cpu_ptr(priv->percpu_priv, i);
2898 
2899 		percpu_priv->np.down = false;
2900 		napi_enable(&percpu_priv->np.napi);
2901 	}
2902 }
2903 
2904 static void dpaa_eth_napi_disable(struct dpaa_priv *priv)
2905 {
2906 	struct dpaa_percpu_priv *percpu_priv;
2907 	int i;
2908 
2909 	for_each_online_cpu(i) {
2910 		percpu_priv = per_cpu_ptr(priv->percpu_priv, i);
2911 
2912 		percpu_priv->np.down = true;
2913 		napi_disable(&percpu_priv->np.napi);
2914 	}
2915 }
2916 
2917 static int dpaa_open(struct net_device *net_dev)
2918 {
2919 	struct mac_device *mac_dev;
2920 	struct dpaa_priv *priv;
2921 	int err, i;
2922 
2923 	priv = netdev_priv(net_dev);
2924 	mac_dev = priv->mac_dev;
2925 	dpaa_eth_napi_enable(priv);
2926 
2927 	err = phylink_of_phy_connect(mac_dev->phylink,
2928 				     mac_dev->dev->of_node, 0);
2929 	if (err)
2930 		goto phy_init_failed;
2931 
2932 	for (i = 0; i < ARRAY_SIZE(mac_dev->port); i++) {
2933 		err = fman_port_enable(mac_dev->port[i]);
2934 		if (err)
2935 			goto mac_start_failed;
2936 	}
2937 
2938 	err = priv->mac_dev->enable(mac_dev->fman_mac);
2939 	if (err < 0) {
2940 		netif_err(priv, ifup, net_dev, "mac_dev->enable() = %d\n", err);
2941 		goto mac_start_failed;
2942 	}
2943 	phylink_start(mac_dev->phylink);
2944 
2945 	netif_tx_start_all_queues(net_dev);
2946 
2947 	return 0;
2948 
2949 mac_start_failed:
2950 	for (i = 0; i < ARRAY_SIZE(mac_dev->port); i++)
2951 		fman_port_disable(mac_dev->port[i]);
2952 	phylink_disconnect_phy(mac_dev->phylink);
2953 
2954 phy_init_failed:
2955 	dpaa_eth_napi_disable(priv);
2956 
2957 	return err;
2958 }
2959 
2960 static int dpaa_eth_stop(struct net_device *net_dev)
2961 {
2962 	struct dpaa_priv *priv;
2963 	int err;
2964 
2965 	err = dpaa_stop(net_dev);
2966 
2967 	priv = netdev_priv(net_dev);
2968 	dpaa_eth_napi_disable(priv);
2969 
2970 	return err;
2971 }
2972 
2973 static bool xdp_validate_mtu(struct dpaa_priv *priv, int mtu)
2974 {
2975 	int max_contig_data = priv->dpaa_bp->size - priv->rx_headroom;
2976 
2977 	/* We do not support S/G fragments when XDP is enabled.
2978 	 * Limit the MTU in relation to the buffer size.
2979 	 */
2980 	if (mtu + VLAN_ETH_HLEN + ETH_FCS_LEN > max_contig_data) {
2981 		dev_warn(priv->net_dev->dev.parent,
2982 			 "The maximum MTU for XDP is %d\n",
2983 			 max_contig_data - VLAN_ETH_HLEN - ETH_FCS_LEN);
2984 		return false;
2985 	}
2986 
2987 	return true;
2988 }
2989 
2990 static int dpaa_change_mtu(struct net_device *net_dev, int new_mtu)
2991 {
2992 	struct dpaa_priv *priv = netdev_priv(net_dev);
2993 
2994 	if (priv->xdp_prog && !xdp_validate_mtu(priv, new_mtu))
2995 		return -EINVAL;
2996 
2997 	net_dev->mtu = new_mtu;
2998 	return 0;
2999 }
3000 
3001 static int dpaa_setup_xdp(struct net_device *net_dev, struct netdev_bpf *bpf)
3002 {
3003 	struct dpaa_priv *priv = netdev_priv(net_dev);
3004 	struct bpf_prog *old_prog;
3005 	int err;
3006 	bool up;
3007 
3008 	/* S/G fragments are not supported in XDP-mode */
3009 	if (bpf->prog && !xdp_validate_mtu(priv, net_dev->mtu)) {
3010 		NL_SET_ERR_MSG_MOD(bpf->extack, "MTU too large for XDP");
3011 		return -EINVAL;
3012 	}
3013 
3014 	up = netif_running(net_dev);
3015 
3016 	if (up)
3017 		dpaa_eth_stop(net_dev);
3018 
3019 	old_prog = xchg(&priv->xdp_prog, bpf->prog);
3020 	if (old_prog)
3021 		bpf_prog_put(old_prog);
3022 
3023 	if (up) {
3024 		err = dpaa_open(net_dev);
3025 		if (err) {
3026 			NL_SET_ERR_MSG_MOD(bpf->extack, "dpaa_open() failed");
3027 			return err;
3028 		}
3029 	}
3030 
3031 	return 0;
3032 }
3033 
3034 static int dpaa_xdp(struct net_device *net_dev, struct netdev_bpf *xdp)
3035 {
3036 	switch (xdp->command) {
3037 	case XDP_SETUP_PROG:
3038 		return dpaa_setup_xdp(net_dev, xdp);
3039 	default:
3040 		return -EINVAL;
3041 	}
3042 }
3043 
3044 static int dpaa_xdp_xmit(struct net_device *net_dev, int n,
3045 			 struct xdp_frame **frames, u32 flags)
3046 {
3047 	struct xdp_frame *xdpf;
3048 	int i, nxmit = 0;
3049 
3050 	if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
3051 		return -EINVAL;
3052 
3053 	if (!netif_running(net_dev))
3054 		return -ENETDOWN;
3055 
3056 	for (i = 0; i < n; i++) {
3057 		xdpf = frames[i];
3058 		if (dpaa_xdp_xmit_frame(net_dev, xdpf))
3059 			break;
3060 		nxmit++;
3061 	}
3062 
3063 	return nxmit;
3064 }
3065 
3066 static int dpaa_ts_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
3067 {
3068 	struct dpaa_priv *priv = netdev_priv(dev);
3069 	struct hwtstamp_config config;
3070 
3071 	if (copy_from_user(&config, rq->ifr_data, sizeof(config)))
3072 		return -EFAULT;
3073 
3074 	switch (config.tx_type) {
3075 	case HWTSTAMP_TX_OFF:
3076 		/* Couldn't disable rx/tx timestamping separately.
3077 		 * Do nothing here.
3078 		 */
3079 		priv->tx_tstamp = false;
3080 		break;
3081 	case HWTSTAMP_TX_ON:
3082 		priv->mac_dev->set_tstamp(priv->mac_dev->fman_mac, true);
3083 		priv->tx_tstamp = true;
3084 		break;
3085 	default:
3086 		return -ERANGE;
3087 	}
3088 
3089 	if (config.rx_filter == HWTSTAMP_FILTER_NONE) {
3090 		/* Couldn't disable rx/tx timestamping separately.
3091 		 * Do nothing here.
3092 		 */
3093 		priv->rx_tstamp = false;
3094 	} else {
3095 		priv->mac_dev->set_tstamp(priv->mac_dev->fman_mac, true);
3096 		priv->rx_tstamp = true;
3097 		/* TS is set for all frame types, not only those requested */
3098 		config.rx_filter = HWTSTAMP_FILTER_ALL;
3099 	}
3100 
3101 	return copy_to_user(rq->ifr_data, &config, sizeof(config)) ?
3102 			-EFAULT : 0;
3103 }
3104 
3105 static int dpaa_ioctl(struct net_device *net_dev, struct ifreq *rq, int cmd)
3106 {
3107 	int ret = -EINVAL;
3108 	struct dpaa_priv *priv = netdev_priv(net_dev);
3109 
3110 	if (cmd == SIOCGMIIREG) {
3111 		if (net_dev->phydev)
3112 			return phylink_mii_ioctl(priv->mac_dev->phylink, rq,
3113 						 cmd);
3114 	}
3115 
3116 	if (cmd == SIOCSHWTSTAMP)
3117 		return dpaa_ts_ioctl(net_dev, rq, cmd);
3118 
3119 	return ret;
3120 }
3121 
3122 static const struct net_device_ops dpaa_ops = {
3123 	.ndo_open = dpaa_open,
3124 	.ndo_start_xmit = dpaa_start_xmit,
3125 	.ndo_stop = dpaa_eth_stop,
3126 	.ndo_tx_timeout = dpaa_tx_timeout,
3127 	.ndo_get_stats64 = dpaa_get_stats64,
3128 	.ndo_change_carrier = fixed_phy_change_carrier,
3129 	.ndo_set_mac_address = dpaa_set_mac_address,
3130 	.ndo_validate_addr = eth_validate_addr,
3131 	.ndo_set_rx_mode = dpaa_set_rx_mode,
3132 	.ndo_eth_ioctl = dpaa_ioctl,
3133 	.ndo_setup_tc = dpaa_setup_tc,
3134 	.ndo_change_mtu = dpaa_change_mtu,
3135 	.ndo_bpf = dpaa_xdp,
3136 	.ndo_xdp_xmit = dpaa_xdp_xmit,
3137 };
3138 
3139 static int dpaa_napi_add(struct net_device *net_dev)
3140 {
3141 	struct dpaa_priv *priv = netdev_priv(net_dev);
3142 	struct dpaa_percpu_priv *percpu_priv;
3143 	int cpu;
3144 
3145 	for_each_possible_cpu(cpu) {
3146 		percpu_priv = per_cpu_ptr(priv->percpu_priv, cpu);
3147 
3148 		netif_napi_add(net_dev, &percpu_priv->np.napi, dpaa_eth_poll);
3149 	}
3150 
3151 	return 0;
3152 }
3153 
3154 static void dpaa_napi_del(struct net_device *net_dev)
3155 {
3156 	struct dpaa_priv *priv = netdev_priv(net_dev);
3157 	struct dpaa_percpu_priv *percpu_priv;
3158 	int cpu;
3159 
3160 	for_each_possible_cpu(cpu) {
3161 		percpu_priv = per_cpu_ptr(priv->percpu_priv, cpu);
3162 
3163 		netif_napi_del(&percpu_priv->np.napi);
3164 	}
3165 }
3166 
3167 static inline void dpaa_bp_free_pf(const struct dpaa_bp *bp,
3168 				   struct bm_buffer *bmb)
3169 {
3170 	dma_addr_t addr = bm_buf_addr(bmb);
3171 
3172 	dma_unmap_page(bp->priv->rx_dma_dev, addr, DPAA_BP_RAW_SIZE,
3173 		       DMA_FROM_DEVICE);
3174 
3175 	skb_free_frag(phys_to_virt(addr));
3176 }
3177 
3178 /* Alloc the dpaa_bp struct and configure default values */
3179 static struct dpaa_bp *dpaa_bp_alloc(struct device *dev)
3180 {
3181 	struct dpaa_bp *dpaa_bp;
3182 
3183 	dpaa_bp = devm_kzalloc(dev, sizeof(*dpaa_bp), GFP_KERNEL);
3184 	if (!dpaa_bp)
3185 		return ERR_PTR(-ENOMEM);
3186 
3187 	dpaa_bp->bpid = FSL_DPAA_BPID_INV;
3188 	dpaa_bp->percpu_count = devm_alloc_percpu(dev, *dpaa_bp->percpu_count);
3189 	if (!dpaa_bp->percpu_count)
3190 		return ERR_PTR(-ENOMEM);
3191 
3192 	dpaa_bp->config_count = FSL_DPAA_ETH_MAX_BUF_COUNT;
3193 
3194 	dpaa_bp->seed_cb = dpaa_bp_seed;
3195 	dpaa_bp->free_buf_cb = dpaa_bp_free_pf;
3196 
3197 	return dpaa_bp;
3198 }
3199 
3200 /* Place all ingress FQs (Rx Default, Rx Error) in a dedicated CGR.
3201  * We won't be sending congestion notifications to FMan; for now, we just use
3202  * this CGR to generate enqueue rejections to FMan in order to drop the frames
3203  * before they reach our ingress queues and eat up memory.
3204  */
3205 static int dpaa_ingress_cgr_init(struct dpaa_priv *priv)
3206 {
3207 	struct qm_mcc_initcgr initcgr;
3208 	u32 cs_th;
3209 	int err;
3210 
3211 	err = qman_alloc_cgrid(&priv->ingress_cgr.cgrid);
3212 	if (err < 0) {
3213 		if (netif_msg_drv(priv))
3214 			pr_err("Error %d allocating CGR ID\n", err);
3215 		goto out_error;
3216 	}
3217 
3218 	/* Enable CS TD, but disable Congestion State Change Notifications. */
3219 	memset(&initcgr, 0, sizeof(initcgr));
3220 	initcgr.we_mask = cpu_to_be16(QM_CGR_WE_CS_THRES);
3221 	initcgr.cgr.cscn_en = QM_CGR_EN;
3222 	cs_th = DPAA_INGRESS_CS_THRESHOLD;
3223 	qm_cgr_cs_thres_set64(&initcgr.cgr.cs_thres, cs_th, 1);
3224 
3225 	initcgr.we_mask |= cpu_to_be16(QM_CGR_WE_CSTD_EN);
3226 	initcgr.cgr.cstd_en = QM_CGR_EN;
3227 
3228 	/* This CGR will be associated with the SWP affined to the current CPU.
3229 	 * However, we'll place all our ingress FQs in it.
3230 	 */
3231 	err = qman_create_cgr(&priv->ingress_cgr, QMAN_CGR_FLAG_USE_INIT,
3232 			      &initcgr);
3233 	if (err < 0) {
3234 		if (netif_msg_drv(priv))
3235 			pr_err("Error %d creating ingress CGR with ID %d\n",
3236 			       err, priv->ingress_cgr.cgrid);
3237 		qman_release_cgrid(priv->ingress_cgr.cgrid);
3238 		goto out_error;
3239 	}
3240 	if (netif_msg_drv(priv))
3241 		pr_debug("Created ingress CGR %d for netdev with hwaddr %pM\n",
3242 			 priv->ingress_cgr.cgrid, priv->mac_dev->addr);
3243 
3244 	priv->use_ingress_cgr = true;
3245 
3246 out_error:
3247 	return err;
3248 }
3249 
3250 static u16 dpaa_get_headroom(struct dpaa_buffer_layout *bl,
3251 			     enum port_type port)
3252 {
3253 	u16 headroom;
3254 
3255 	/* The frame headroom must accommodate:
3256 	 * - the driver private data area
3257 	 * - parse results, hash results, timestamp if selected
3258 	 * If either hash results or time stamp are selected, both will
3259 	 * be copied to/from the frame headroom, as TS is located between PR and
3260 	 * HR in the IC and IC copy size has a granularity of 16bytes
3261 	 * (see description of FMBM_RICP and FMBM_TICP registers in DPAARM)
3262 	 *
3263 	 * Also make sure the headroom is a multiple of data_align bytes
3264 	 */
3265 	headroom = (u16)(bl[port].priv_data_size + DPAA_HWA_SIZE);
3266 
3267 	if (port == RX) {
3268 #ifdef CONFIG_DPAA_ERRATUM_A050385
3269 		if (unlikely(fman_has_errata_a050385()))
3270 			headroom = XDP_PACKET_HEADROOM;
3271 #endif
3272 
3273 		return ALIGN(headroom, DPAA_FD_RX_DATA_ALIGNMENT);
3274 	} else {
3275 		return ALIGN(headroom, DPAA_FD_DATA_ALIGNMENT);
3276 	}
3277 }
3278 
3279 static int dpaa_eth_probe(struct platform_device *pdev)
3280 {
3281 	struct net_device *net_dev = NULL;
3282 	struct dpaa_bp *dpaa_bp = NULL;
3283 	struct dpaa_fq *dpaa_fq, *tmp;
3284 	struct dpaa_priv *priv = NULL;
3285 	struct fm_port_fqs port_fqs;
3286 	struct mac_device *mac_dev;
3287 	int err = 0, channel;
3288 	struct device *dev;
3289 
3290 	dev = &pdev->dev;
3291 
3292 	err = bman_is_probed();
3293 	if (!err)
3294 		return -EPROBE_DEFER;
3295 	if (err < 0) {
3296 		dev_err(dev, "failing probe due to bman probe error\n");
3297 		return -ENODEV;
3298 	}
3299 	err = qman_is_probed();
3300 	if (!err)
3301 		return -EPROBE_DEFER;
3302 	if (err < 0) {
3303 		dev_err(dev, "failing probe due to qman probe error\n");
3304 		return -ENODEV;
3305 	}
3306 	err = bman_portals_probed();
3307 	if (!err)
3308 		return -EPROBE_DEFER;
3309 	if (err < 0) {
3310 		dev_err(dev,
3311 			"failing probe due to bman portals probe error\n");
3312 		return -ENODEV;
3313 	}
3314 	err = qman_portals_probed();
3315 	if (!err)
3316 		return -EPROBE_DEFER;
3317 	if (err < 0) {
3318 		dev_err(dev,
3319 			"failing probe due to qman portals probe error\n");
3320 		return -ENODEV;
3321 	}
3322 
3323 	/* Allocate this early, so we can store relevant information in
3324 	 * the private area
3325 	 */
3326 	net_dev = alloc_etherdev_mq(sizeof(*priv), DPAA_ETH_TXQ_NUM);
3327 	if (!net_dev) {
3328 		dev_err(dev, "alloc_etherdev_mq() failed\n");
3329 		return -ENOMEM;
3330 	}
3331 
3332 	/* Do this here, so we can be verbose early */
3333 	SET_NETDEV_DEV(net_dev, dev->parent);
3334 	dev_set_drvdata(dev, net_dev);
3335 
3336 	priv = netdev_priv(net_dev);
3337 	priv->net_dev = net_dev;
3338 
3339 	priv->msg_enable = netif_msg_init(debug, DPAA_MSG_DEFAULT);
3340 
3341 	mac_dev = dpaa_mac_dev_get(pdev);
3342 	if (IS_ERR(mac_dev)) {
3343 		netdev_err(net_dev, "dpaa_mac_dev_get() failed\n");
3344 		err = PTR_ERR(mac_dev);
3345 		goto free_netdev;
3346 	}
3347 
3348 	/* Devices used for DMA mapping */
3349 	priv->rx_dma_dev = fman_port_get_device(mac_dev->port[RX]);
3350 	priv->tx_dma_dev = fman_port_get_device(mac_dev->port[TX]);
3351 	err = dma_coerce_mask_and_coherent(priv->rx_dma_dev, DMA_BIT_MASK(40));
3352 	if (!err)
3353 		err = dma_coerce_mask_and_coherent(priv->tx_dma_dev,
3354 						   DMA_BIT_MASK(40));
3355 	if (err) {
3356 		netdev_err(net_dev, "dma_coerce_mask_and_coherent() failed\n");
3357 		goto free_netdev;
3358 	}
3359 
3360 	/* If fsl_fm_max_frm is set to a higher value than the all-common 1500,
3361 	 * we choose conservatively and let the user explicitly set a higher
3362 	 * MTU via ifconfig. Otherwise, the user may end up with different MTUs
3363 	 * in the same LAN.
3364 	 * If on the other hand fsl_fm_max_frm has been chosen below 1500,
3365 	 * start with the maximum allowed.
3366 	 */
3367 	net_dev->mtu = min(dpaa_get_max_mtu(), ETH_DATA_LEN);
3368 
3369 	netdev_dbg(net_dev, "Setting initial MTU on net device: %d\n",
3370 		   net_dev->mtu);
3371 
3372 	priv->buf_layout[RX].priv_data_size = DPAA_RX_PRIV_DATA_SIZE; /* Rx */
3373 	priv->buf_layout[TX].priv_data_size = DPAA_TX_PRIV_DATA_SIZE; /* Tx */
3374 
3375 	/* bp init */
3376 	dpaa_bp = dpaa_bp_alloc(dev);
3377 	if (IS_ERR(dpaa_bp)) {
3378 		err = PTR_ERR(dpaa_bp);
3379 		goto free_dpaa_bps;
3380 	}
3381 	/* the raw size of the buffers used for reception */
3382 	dpaa_bp->raw_size = DPAA_BP_RAW_SIZE;
3383 	/* avoid runtime computations by keeping the usable size here */
3384 	dpaa_bp->size = dpaa_bp_size(dpaa_bp->raw_size);
3385 	dpaa_bp->priv = priv;
3386 
3387 	err = dpaa_bp_alloc_pool(dpaa_bp);
3388 	if (err < 0)
3389 		goto free_dpaa_bps;
3390 	priv->dpaa_bp = dpaa_bp;
3391 
3392 	INIT_LIST_HEAD(&priv->dpaa_fq_list);
3393 
3394 	memset(&port_fqs, 0, sizeof(port_fqs));
3395 
3396 	err = dpaa_alloc_all_fqs(dev, &priv->dpaa_fq_list, &port_fqs);
3397 	if (err < 0) {
3398 		dev_err(dev, "dpaa_alloc_all_fqs() failed\n");
3399 		goto free_dpaa_bps;
3400 	}
3401 
3402 	priv->mac_dev = mac_dev;
3403 
3404 	channel = dpaa_get_channel();
3405 	if (channel < 0) {
3406 		dev_err(dev, "dpaa_get_channel() failed\n");
3407 		err = channel;
3408 		goto free_dpaa_bps;
3409 	}
3410 
3411 	priv->channel = (u16)channel;
3412 
3413 	/* Walk the CPUs with affine portals
3414 	 * and add this pool channel to each's dequeue mask.
3415 	 */
3416 	dpaa_eth_add_channel(priv->channel, &pdev->dev);
3417 
3418 	dpaa_fq_setup(priv, &dpaa_fq_cbs, priv->mac_dev->port[TX]);
3419 
3420 	/* Create a congestion group for this netdev, with
3421 	 * dynamically-allocated CGR ID.
3422 	 * Must be executed after probing the MAC, but before
3423 	 * assigning the egress FQs to the CGRs.
3424 	 */
3425 	err = dpaa_eth_cgr_init(priv);
3426 	if (err < 0) {
3427 		dev_err(dev, "Error initializing CGR\n");
3428 		goto free_dpaa_bps;
3429 	}
3430 
3431 	err = dpaa_ingress_cgr_init(priv);
3432 	if (err < 0) {
3433 		dev_err(dev, "Error initializing ingress CGR\n");
3434 		goto delete_egress_cgr;
3435 	}
3436 
3437 	/* Add the FQs to the interface, and make them active */
3438 	list_for_each_entry_safe(dpaa_fq, tmp, &priv->dpaa_fq_list, list) {
3439 		err = dpaa_fq_init(dpaa_fq, false);
3440 		if (err < 0)
3441 			goto free_dpaa_fqs;
3442 	}
3443 
3444 	priv->tx_headroom = dpaa_get_headroom(priv->buf_layout, TX);
3445 	priv->rx_headroom = dpaa_get_headroom(priv->buf_layout, RX);
3446 
3447 	/* All real interfaces need their ports initialized */
3448 	err = dpaa_eth_init_ports(mac_dev, dpaa_bp, &port_fqs,
3449 				  &priv->buf_layout[0], dev);
3450 	if (err)
3451 		goto free_dpaa_fqs;
3452 
3453 	/* Rx traffic distribution based on keygen hashing defaults to on */
3454 	priv->keygen_in_use = true;
3455 
3456 	priv->percpu_priv = devm_alloc_percpu(dev, *priv->percpu_priv);
3457 	if (!priv->percpu_priv) {
3458 		dev_err(dev, "devm_alloc_percpu() failed\n");
3459 		err = -ENOMEM;
3460 		goto free_dpaa_fqs;
3461 	}
3462 
3463 	priv->num_tc = 1;
3464 	netif_set_real_num_tx_queues(net_dev, priv->num_tc * DPAA_TC_TXQ_NUM);
3465 
3466 	/* Initialize NAPI */
3467 	err = dpaa_napi_add(net_dev);
3468 	if (err < 0)
3469 		goto delete_dpaa_napi;
3470 
3471 	err = dpaa_netdev_init(net_dev, &dpaa_ops, tx_timeout);
3472 	if (err < 0)
3473 		goto delete_dpaa_napi;
3474 
3475 	dpaa_eth_sysfs_init(&net_dev->dev);
3476 
3477 	netif_info(priv, probe, net_dev, "Probed interface %s\n",
3478 		   net_dev->name);
3479 
3480 	return 0;
3481 
3482 delete_dpaa_napi:
3483 	dpaa_napi_del(net_dev);
3484 free_dpaa_fqs:
3485 	dpaa_fq_free(dev, &priv->dpaa_fq_list);
3486 	qman_delete_cgr_safe(&priv->ingress_cgr);
3487 	qman_release_cgrid(priv->ingress_cgr.cgrid);
3488 delete_egress_cgr:
3489 	qman_delete_cgr_safe(&priv->cgr_data.cgr);
3490 	qman_release_cgrid(priv->cgr_data.cgr.cgrid);
3491 free_dpaa_bps:
3492 	dpaa_bps_free(priv);
3493 free_netdev:
3494 	dev_set_drvdata(dev, NULL);
3495 	free_netdev(net_dev);
3496 
3497 	return err;
3498 }
3499 
3500 static int dpaa_remove(struct platform_device *pdev)
3501 {
3502 	struct net_device *net_dev;
3503 	struct dpaa_priv *priv;
3504 	struct device *dev;
3505 	int err;
3506 
3507 	dev = &pdev->dev;
3508 	net_dev = dev_get_drvdata(dev);
3509 
3510 	priv = netdev_priv(net_dev);
3511 
3512 	dpaa_eth_sysfs_remove(dev);
3513 
3514 	dev_set_drvdata(dev, NULL);
3515 	unregister_netdev(net_dev);
3516 	phylink_destroy(priv->mac_dev->phylink);
3517 
3518 	err = dpaa_fq_free(dev, &priv->dpaa_fq_list);
3519 
3520 	qman_delete_cgr_safe(&priv->ingress_cgr);
3521 	qman_release_cgrid(priv->ingress_cgr.cgrid);
3522 	qman_delete_cgr_safe(&priv->cgr_data.cgr);
3523 	qman_release_cgrid(priv->cgr_data.cgr.cgrid);
3524 
3525 	dpaa_napi_del(net_dev);
3526 
3527 	dpaa_bps_free(priv);
3528 
3529 	free_netdev(net_dev);
3530 
3531 	return err;
3532 }
3533 
3534 static const struct platform_device_id dpaa_devtype[] = {
3535 	{
3536 		.name = "dpaa-ethernet",
3537 		.driver_data = 0,
3538 	}, {
3539 	}
3540 };
3541 MODULE_DEVICE_TABLE(platform, dpaa_devtype);
3542 
3543 static struct platform_driver dpaa_driver = {
3544 	.driver = {
3545 		.name = KBUILD_MODNAME,
3546 	},
3547 	.id_table = dpaa_devtype,
3548 	.probe = dpaa_eth_probe,
3549 	.remove = dpaa_remove
3550 };
3551 
3552 static int __init dpaa_load(void)
3553 {
3554 	int err;
3555 
3556 	pr_debug("FSL DPAA Ethernet driver\n");
3557 
3558 	/* initialize dpaa_eth mirror values */
3559 	dpaa_rx_extra_headroom = fman_get_rx_extra_headroom();
3560 	dpaa_max_frm = fman_get_max_frm();
3561 
3562 	err = platform_driver_register(&dpaa_driver);
3563 	if (err < 0)
3564 		pr_err("Error, platform_driver_register() = %d\n", err);
3565 
3566 	return err;
3567 }
3568 module_init(dpaa_load);
3569 
3570 static void __exit dpaa_unload(void)
3571 {
3572 	platform_driver_unregister(&dpaa_driver);
3573 
3574 	/* Only one channel is used and needs to be released after all
3575 	 * interfaces are removed
3576 	 */
3577 	dpaa_release_channel();
3578 }
3579 module_exit(dpaa_unload);
3580 
3581 MODULE_LICENSE("Dual BSD/GPL");
3582 MODULE_DESCRIPTION("FSL DPAA Ethernet driver");
3583