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