1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Texas Instruments Ethernet Switch Driver
4  *
5  * Copyright (C) 2019 Texas Instruments
6  */
7 
8 #include <linux/bpf.h>
9 #include <linux/bpf_trace.h>
10 #include <linux/if_ether.h>
11 #include <linux/if_vlan.h>
12 #include <linux/kmemleak.h>
13 #include <linux/module.h>
14 #include <linux/netdevice.h>
15 #include <linux/net_tstamp.h>
16 #include <linux/of.h>
17 #include <linux/phy.h>
18 #include <linux/platform_device.h>
19 #include <linux/pm_runtime.h>
20 #include <linux/skbuff.h>
21 #include <net/page_pool.h>
22 #include <net/pkt_cls.h>
23 
24 #include "cpsw.h"
25 #include "cpts.h"
26 #include "cpsw_ale.h"
27 #include "cpsw_priv.h"
28 #include "cpsw_sl.h"
29 #include "davinci_cpdma.h"
30 
31 #define CPTS_N_ETX_TS 4
32 
33 int (*cpsw_slave_index)(struct cpsw_common *cpsw, struct cpsw_priv *priv);
34 
35 void cpsw_intr_enable(struct cpsw_common *cpsw)
36 {
37 	writel_relaxed(0xFF, &cpsw->wr_regs->tx_en);
38 	writel_relaxed(0xFF, &cpsw->wr_regs->rx_en);
39 
40 	cpdma_ctlr_int_ctrl(cpsw->dma, true);
41 }
42 
43 void cpsw_intr_disable(struct cpsw_common *cpsw)
44 {
45 	writel_relaxed(0, &cpsw->wr_regs->tx_en);
46 	writel_relaxed(0, &cpsw->wr_regs->rx_en);
47 
48 	cpdma_ctlr_int_ctrl(cpsw->dma, false);
49 }
50 
51 void cpsw_tx_handler(void *token, int len, int status)
52 {
53 	struct cpsw_meta_xdp	*xmeta;
54 	struct xdp_frame	*xdpf;
55 	struct net_device	*ndev;
56 	struct netdev_queue	*txq;
57 	struct sk_buff		*skb;
58 	int			ch;
59 
60 	if (cpsw_is_xdpf_handle(token)) {
61 		xdpf = cpsw_handle_to_xdpf(token);
62 		xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;
63 		ndev = xmeta->ndev;
64 		ch = xmeta->ch;
65 		xdp_return_frame(xdpf);
66 	} else {
67 		skb = token;
68 		ndev = skb->dev;
69 		ch = skb_get_queue_mapping(skb);
70 		cpts_tx_timestamp(ndev_to_cpsw(ndev)->cpts, skb);
71 		dev_kfree_skb_any(skb);
72 	}
73 
74 	/* Check whether the queue is stopped due to stalled tx dma, if the
75 	 * queue is stopped then start the queue as we have free desc for tx
76 	 */
77 	txq = netdev_get_tx_queue(ndev, ch);
78 	if (unlikely(netif_tx_queue_stopped(txq)))
79 		netif_tx_wake_queue(txq);
80 
81 	ndev->stats.tx_packets++;
82 	ndev->stats.tx_bytes += len;
83 }
84 
85 irqreturn_t cpsw_tx_interrupt(int irq, void *dev_id)
86 {
87 	struct cpsw_common *cpsw = dev_id;
88 
89 	writel(0, &cpsw->wr_regs->tx_en);
90 	cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_TX);
91 
92 	if (cpsw->quirk_irq) {
93 		disable_irq_nosync(cpsw->irqs_table[1]);
94 		cpsw->tx_irq_disabled = true;
95 	}
96 
97 	napi_schedule(&cpsw->napi_tx);
98 	return IRQ_HANDLED;
99 }
100 
101 irqreturn_t cpsw_rx_interrupt(int irq, void *dev_id)
102 {
103 	struct cpsw_common *cpsw = dev_id;
104 
105 	writel(0, &cpsw->wr_regs->rx_en);
106 	cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_RX);
107 
108 	if (cpsw->quirk_irq) {
109 		disable_irq_nosync(cpsw->irqs_table[0]);
110 		cpsw->rx_irq_disabled = true;
111 	}
112 
113 	napi_schedule(&cpsw->napi_rx);
114 	return IRQ_HANDLED;
115 }
116 
117 irqreturn_t cpsw_misc_interrupt(int irq, void *dev_id)
118 {
119 	struct cpsw_common *cpsw = dev_id;
120 
121 	writel(0, &cpsw->wr_regs->misc_en);
122 	cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_MISC);
123 	cpts_misc_interrupt(cpsw->cpts);
124 	writel(0x10, &cpsw->wr_regs->misc_en);
125 
126 	return IRQ_HANDLED;
127 }
128 
129 int cpsw_tx_mq_poll(struct napi_struct *napi_tx, int budget)
130 {
131 	struct cpsw_common	*cpsw = napi_to_cpsw(napi_tx);
132 	int			num_tx, cur_budget, ch;
133 	u32			ch_map;
134 	struct cpsw_vector	*txv;
135 
136 	/* process every unprocessed channel */
137 	ch_map = cpdma_ctrl_txchs_state(cpsw->dma);
138 	for (ch = 0, num_tx = 0; ch_map & 0xff; ch_map <<= 1, ch++) {
139 		if (!(ch_map & 0x80))
140 			continue;
141 
142 		txv = &cpsw->txv[ch];
143 		if (unlikely(txv->budget > budget - num_tx))
144 			cur_budget = budget - num_tx;
145 		else
146 			cur_budget = txv->budget;
147 
148 		num_tx += cpdma_chan_process(txv->ch, cur_budget);
149 		if (num_tx >= budget)
150 			break;
151 	}
152 
153 	if (num_tx < budget) {
154 		napi_complete(napi_tx);
155 		writel(0xff, &cpsw->wr_regs->tx_en);
156 	}
157 
158 	return num_tx;
159 }
160 
161 int cpsw_tx_poll(struct napi_struct *napi_tx, int budget)
162 {
163 	struct cpsw_common *cpsw = napi_to_cpsw(napi_tx);
164 	int num_tx;
165 
166 	num_tx = cpdma_chan_process(cpsw->txv[0].ch, budget);
167 	if (num_tx < budget) {
168 		napi_complete(napi_tx);
169 		writel(0xff, &cpsw->wr_regs->tx_en);
170 		if (cpsw->tx_irq_disabled) {
171 			cpsw->tx_irq_disabled = false;
172 			enable_irq(cpsw->irqs_table[1]);
173 		}
174 	}
175 
176 	return num_tx;
177 }
178 
179 int cpsw_rx_mq_poll(struct napi_struct *napi_rx, int budget)
180 {
181 	struct cpsw_common	*cpsw = napi_to_cpsw(napi_rx);
182 	int			num_rx, cur_budget, ch;
183 	u32			ch_map;
184 	struct cpsw_vector	*rxv;
185 
186 	/* process every unprocessed channel */
187 	ch_map = cpdma_ctrl_rxchs_state(cpsw->dma);
188 	for (ch = 0, num_rx = 0; ch_map; ch_map >>= 1, ch++) {
189 		if (!(ch_map & 0x01))
190 			continue;
191 
192 		rxv = &cpsw->rxv[ch];
193 		if (unlikely(rxv->budget > budget - num_rx))
194 			cur_budget = budget - num_rx;
195 		else
196 			cur_budget = rxv->budget;
197 
198 		num_rx += cpdma_chan_process(rxv->ch, cur_budget);
199 		if (num_rx >= budget)
200 			break;
201 	}
202 
203 	if (num_rx < budget) {
204 		napi_complete_done(napi_rx, num_rx);
205 		writel(0xff, &cpsw->wr_regs->rx_en);
206 	}
207 
208 	return num_rx;
209 }
210 
211 int cpsw_rx_poll(struct napi_struct *napi_rx, int budget)
212 {
213 	struct cpsw_common *cpsw = napi_to_cpsw(napi_rx);
214 	int num_rx;
215 
216 	num_rx = cpdma_chan_process(cpsw->rxv[0].ch, budget);
217 	if (num_rx < budget) {
218 		napi_complete_done(napi_rx, num_rx);
219 		writel(0xff, &cpsw->wr_regs->rx_en);
220 		if (cpsw->rx_irq_disabled) {
221 			cpsw->rx_irq_disabled = false;
222 			enable_irq(cpsw->irqs_table[0]);
223 		}
224 	}
225 
226 	return num_rx;
227 }
228 
229 void cpsw_rx_vlan_encap(struct sk_buff *skb)
230 {
231 	struct cpsw_priv *priv = netdev_priv(skb->dev);
232 	u32 rx_vlan_encap_hdr = *((u32 *)skb->data);
233 	struct cpsw_common *cpsw = priv->cpsw;
234 	u16 vtag, vid, prio, pkt_type;
235 
236 	/* Remove VLAN header encapsulation word */
237 	skb_pull(skb, CPSW_RX_VLAN_ENCAP_HDR_SIZE);
238 
239 	pkt_type = (rx_vlan_encap_hdr >>
240 		    CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_SHIFT) &
241 		    CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_MSK;
242 	/* Ignore unknown & Priority-tagged packets*/
243 	if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_RESERV ||
244 	    pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_PRIO_TAG)
245 		return;
246 
247 	vid = (rx_vlan_encap_hdr >>
248 	       CPSW_RX_VLAN_ENCAP_HDR_VID_SHIFT) &
249 	       VLAN_VID_MASK;
250 	/* Ignore vid 0 and pass packet as is */
251 	if (!vid)
252 		return;
253 
254 	/* Untag P0 packets if set for vlan */
255 	if (!cpsw_ale_get_vlan_p0_untag(cpsw->ale, vid)) {
256 		prio = (rx_vlan_encap_hdr >>
257 			CPSW_RX_VLAN_ENCAP_HDR_PRIO_SHIFT) &
258 			CPSW_RX_VLAN_ENCAP_HDR_PRIO_MSK;
259 
260 		vtag = (prio << VLAN_PRIO_SHIFT) | vid;
261 		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vtag);
262 	}
263 
264 	/* strip vlan tag for VLAN-tagged packet */
265 	if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_VLAN_TAG) {
266 		memmove(skb->data + VLAN_HLEN, skb->data, 2 * ETH_ALEN);
267 		skb_pull(skb, VLAN_HLEN);
268 	}
269 }
270 
271 void cpsw_set_slave_mac(struct cpsw_slave *slave, struct cpsw_priv *priv)
272 {
273 	slave_write(slave, mac_hi(priv->mac_addr), SA_HI);
274 	slave_write(slave, mac_lo(priv->mac_addr), SA_LO);
275 }
276 
277 void soft_reset(const char *module, void __iomem *reg)
278 {
279 	unsigned long timeout = jiffies + HZ;
280 
281 	writel_relaxed(1, reg);
282 	do {
283 		cpu_relax();
284 	} while ((readl_relaxed(reg) & 1) && time_after(timeout, jiffies));
285 
286 	WARN(readl_relaxed(reg) & 1, "failed to soft-reset %s\n", module);
287 }
288 
289 void cpsw_ndo_tx_timeout(struct net_device *ndev, unsigned int txqueue)
290 {
291 	struct cpsw_priv *priv = netdev_priv(ndev);
292 	struct cpsw_common *cpsw = priv->cpsw;
293 	int ch;
294 
295 	cpsw_err(priv, tx_err, "transmit timeout, restarting dma\n");
296 	ndev->stats.tx_errors++;
297 	cpsw_intr_disable(cpsw);
298 	for (ch = 0; ch < cpsw->tx_ch_num; ch++) {
299 		cpdma_chan_stop(cpsw->txv[ch].ch);
300 		cpdma_chan_start(cpsw->txv[ch].ch);
301 	}
302 
303 	cpsw_intr_enable(cpsw);
304 	netif_trans_update(ndev);
305 	netif_tx_wake_all_queues(ndev);
306 }
307 
308 static int cpsw_get_common_speed(struct cpsw_common *cpsw)
309 {
310 	int i, speed;
311 
312 	for (i = 0, speed = 0; i < cpsw->data.slaves; i++)
313 		if (cpsw->slaves[i].phy && cpsw->slaves[i].phy->link)
314 			speed += cpsw->slaves[i].phy->speed;
315 
316 	return speed;
317 }
318 
319 int cpsw_need_resplit(struct cpsw_common *cpsw)
320 {
321 	int i, rlim_ch_num;
322 	int speed, ch_rate;
323 
324 	/* re-split resources only in case speed was changed */
325 	speed = cpsw_get_common_speed(cpsw);
326 	if (speed == cpsw->speed || !speed)
327 		return 0;
328 
329 	cpsw->speed = speed;
330 
331 	for (i = 0, rlim_ch_num = 0; i < cpsw->tx_ch_num; i++) {
332 		ch_rate = cpdma_chan_get_rate(cpsw->txv[i].ch);
333 		if (!ch_rate)
334 			break;
335 
336 		rlim_ch_num++;
337 	}
338 
339 	/* cases not dependent on speed */
340 	if (!rlim_ch_num || rlim_ch_num == cpsw->tx_ch_num)
341 		return 0;
342 
343 	return 1;
344 }
345 
346 void cpsw_split_res(struct cpsw_common *cpsw)
347 {
348 	u32 consumed_rate = 0, bigest_rate = 0;
349 	struct cpsw_vector *txv = cpsw->txv;
350 	int i, ch_weight, rlim_ch_num = 0;
351 	int budget, bigest_rate_ch = 0;
352 	u32 ch_rate, max_rate;
353 	int ch_budget = 0;
354 
355 	for (i = 0; i < cpsw->tx_ch_num; i++) {
356 		ch_rate = cpdma_chan_get_rate(txv[i].ch);
357 		if (!ch_rate)
358 			continue;
359 
360 		rlim_ch_num++;
361 		consumed_rate += ch_rate;
362 	}
363 
364 	if (cpsw->tx_ch_num == rlim_ch_num) {
365 		max_rate = consumed_rate;
366 	} else if (!rlim_ch_num) {
367 		ch_budget = CPSW_POLL_WEIGHT / cpsw->tx_ch_num;
368 		bigest_rate = 0;
369 		max_rate = consumed_rate;
370 	} else {
371 		max_rate = cpsw->speed * 1000;
372 
373 		/* if max_rate is less then expected due to reduced link speed,
374 		 * split proportionally according next potential max speed
375 		 */
376 		if (max_rate < consumed_rate)
377 			max_rate *= 10;
378 
379 		if (max_rate < consumed_rate)
380 			max_rate *= 10;
381 
382 		ch_budget = (consumed_rate * CPSW_POLL_WEIGHT) / max_rate;
383 		ch_budget = (CPSW_POLL_WEIGHT - ch_budget) /
384 			    (cpsw->tx_ch_num - rlim_ch_num);
385 		bigest_rate = (max_rate - consumed_rate) /
386 			      (cpsw->tx_ch_num - rlim_ch_num);
387 	}
388 
389 	/* split tx weight/budget */
390 	budget = CPSW_POLL_WEIGHT;
391 	for (i = 0; i < cpsw->tx_ch_num; i++) {
392 		ch_rate = cpdma_chan_get_rate(txv[i].ch);
393 		if (ch_rate) {
394 			txv[i].budget = (ch_rate * CPSW_POLL_WEIGHT) / max_rate;
395 			if (!txv[i].budget)
396 				txv[i].budget++;
397 			if (ch_rate > bigest_rate) {
398 				bigest_rate_ch = i;
399 				bigest_rate = ch_rate;
400 			}
401 
402 			ch_weight = (ch_rate * 100) / max_rate;
403 			if (!ch_weight)
404 				ch_weight++;
405 			cpdma_chan_set_weight(cpsw->txv[i].ch, ch_weight);
406 		} else {
407 			txv[i].budget = ch_budget;
408 			if (!bigest_rate_ch)
409 				bigest_rate_ch = i;
410 			cpdma_chan_set_weight(cpsw->txv[i].ch, 0);
411 		}
412 
413 		budget -= txv[i].budget;
414 	}
415 
416 	if (budget)
417 		txv[bigest_rate_ch].budget += budget;
418 
419 	/* split rx budget */
420 	budget = CPSW_POLL_WEIGHT;
421 	ch_budget = budget / cpsw->rx_ch_num;
422 	for (i = 0; i < cpsw->rx_ch_num; i++) {
423 		cpsw->rxv[i].budget = ch_budget;
424 		budget -= ch_budget;
425 	}
426 
427 	if (budget)
428 		cpsw->rxv[0].budget += budget;
429 }
430 
431 int cpsw_init_common(struct cpsw_common *cpsw, void __iomem *ss_regs,
432 		     int ale_ageout, phys_addr_t desc_mem_phys,
433 		     int descs_pool_size)
434 {
435 	u32 slave_offset, sliver_offset, slave_size;
436 	struct cpsw_ale_params ale_params;
437 	struct cpsw_platform_data *data;
438 	struct cpdma_params dma_params;
439 	struct device *dev = cpsw->dev;
440 	struct device_node *cpts_node;
441 	void __iomem *cpts_regs;
442 	int ret = 0, i;
443 
444 	data = &cpsw->data;
445 	cpsw->rx_ch_num = 1;
446 	cpsw->tx_ch_num = 1;
447 
448 	cpsw->version = readl(&cpsw->regs->id_ver);
449 
450 	memset(&dma_params, 0, sizeof(dma_params));
451 	memset(&ale_params, 0, sizeof(ale_params));
452 
453 	switch (cpsw->version) {
454 	case CPSW_VERSION_1:
455 		cpsw->host_port_regs = ss_regs + CPSW1_HOST_PORT_OFFSET;
456 		cpts_regs	     = ss_regs + CPSW1_CPTS_OFFSET;
457 		cpsw->hw_stats	     = ss_regs + CPSW1_HW_STATS;
458 		dma_params.dmaregs   = ss_regs + CPSW1_CPDMA_OFFSET;
459 		dma_params.txhdp     = ss_regs + CPSW1_STATERAM_OFFSET;
460 		ale_params.ale_regs  = ss_regs + CPSW1_ALE_OFFSET;
461 		slave_offset         = CPSW1_SLAVE_OFFSET;
462 		slave_size           = CPSW1_SLAVE_SIZE;
463 		sliver_offset        = CPSW1_SLIVER_OFFSET;
464 		dma_params.desc_mem_phys = 0;
465 		break;
466 	case CPSW_VERSION_2:
467 	case CPSW_VERSION_3:
468 	case CPSW_VERSION_4:
469 		cpsw->host_port_regs = ss_regs + CPSW2_HOST_PORT_OFFSET;
470 		cpts_regs	     = ss_regs + CPSW2_CPTS_OFFSET;
471 		cpsw->hw_stats	     = ss_regs + CPSW2_HW_STATS;
472 		dma_params.dmaregs   = ss_regs + CPSW2_CPDMA_OFFSET;
473 		dma_params.txhdp     = ss_regs + CPSW2_STATERAM_OFFSET;
474 		ale_params.ale_regs  = ss_regs + CPSW2_ALE_OFFSET;
475 		slave_offset         = CPSW2_SLAVE_OFFSET;
476 		slave_size           = CPSW2_SLAVE_SIZE;
477 		sliver_offset        = CPSW2_SLIVER_OFFSET;
478 		dma_params.desc_mem_phys = desc_mem_phys;
479 		break;
480 	default:
481 		dev_err(dev, "unknown version 0x%08x\n", cpsw->version);
482 		return -ENODEV;
483 	}
484 
485 	for (i = 0; i < cpsw->data.slaves; i++) {
486 		struct cpsw_slave *slave = &cpsw->slaves[i];
487 		void __iomem		*regs = cpsw->regs;
488 
489 		slave->slave_num = i;
490 		slave->data	= &cpsw->data.slave_data[i];
491 		slave->regs	= regs + slave_offset;
492 		slave->port_vlan = slave->data->dual_emac_res_vlan;
493 		slave->mac_sl = cpsw_sl_get("cpsw", dev, regs + sliver_offset);
494 		if (IS_ERR(slave->mac_sl))
495 			return PTR_ERR(slave->mac_sl);
496 
497 		slave_offset  += slave_size;
498 		sliver_offset += SLIVER_SIZE;
499 	}
500 
501 	ale_params.dev			= dev;
502 	ale_params.ale_ageout		= ale_ageout;
503 	ale_params.ale_ports		= CPSW_ALE_PORTS_NUM;
504 	ale_params.dev_id		= "cpsw";
505 
506 	cpsw->ale = cpsw_ale_create(&ale_params);
507 	if (IS_ERR(cpsw->ale)) {
508 		dev_err(dev, "error initializing ale engine\n");
509 		return PTR_ERR(cpsw->ale);
510 	}
511 
512 	dma_params.dev		= dev;
513 	dma_params.rxthresh	= dma_params.dmaregs + CPDMA_RXTHRESH;
514 	dma_params.rxfree	= dma_params.dmaregs + CPDMA_RXFREE;
515 	dma_params.rxhdp	= dma_params.txhdp + CPDMA_RXHDP;
516 	dma_params.txcp		= dma_params.txhdp + CPDMA_TXCP;
517 	dma_params.rxcp		= dma_params.txhdp + CPDMA_RXCP;
518 
519 	dma_params.num_chan		= data->channels;
520 	dma_params.has_soft_reset	= true;
521 	dma_params.min_packet_size	= CPSW_MIN_PACKET_SIZE;
522 	dma_params.desc_mem_size	= data->bd_ram_size;
523 	dma_params.desc_align		= 16;
524 	dma_params.has_ext_regs		= true;
525 	dma_params.desc_hw_addr         = dma_params.desc_mem_phys;
526 	dma_params.bus_freq_mhz		= cpsw->bus_freq_mhz;
527 	dma_params.descs_pool_size	= descs_pool_size;
528 
529 	cpsw->dma = cpdma_ctlr_create(&dma_params);
530 	if (!cpsw->dma) {
531 		dev_err(dev, "error initializing dma\n");
532 		return -ENOMEM;
533 	}
534 
535 	cpts_node = of_get_child_by_name(cpsw->dev->of_node, "cpts");
536 	if (!cpts_node)
537 		cpts_node = cpsw->dev->of_node;
538 
539 	cpsw->cpts = cpts_create(cpsw->dev, cpts_regs, cpts_node,
540 				 CPTS_N_ETX_TS);
541 	if (IS_ERR(cpsw->cpts)) {
542 		ret = PTR_ERR(cpsw->cpts);
543 		cpdma_ctlr_destroy(cpsw->dma);
544 	}
545 	of_node_put(cpts_node);
546 
547 	return ret;
548 }
549 
550 #if IS_ENABLED(CONFIG_TI_CPTS)
551 
552 static void cpsw_hwtstamp_v1(struct cpsw_priv *priv)
553 {
554 	struct cpsw_common *cpsw = priv->cpsw;
555 	struct cpsw_slave *slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
556 	u32 ts_en, seq_id;
557 
558 	if (!priv->tx_ts_enabled && !priv->rx_ts_enabled) {
559 		slave_write(slave, 0, CPSW1_TS_CTL);
560 		return;
561 	}
562 
563 	seq_id = (30 << CPSW_V1_SEQ_ID_OFS_SHIFT) | ETH_P_1588;
564 	ts_en = EVENT_MSG_BITS << CPSW_V1_MSG_TYPE_OFS;
565 
566 	if (priv->tx_ts_enabled)
567 		ts_en |= CPSW_V1_TS_TX_EN;
568 
569 	if (priv->rx_ts_enabled)
570 		ts_en |= CPSW_V1_TS_RX_EN;
571 
572 	slave_write(slave, ts_en, CPSW1_TS_CTL);
573 	slave_write(slave, seq_id, CPSW1_TS_SEQ_LTYPE);
574 }
575 
576 static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)
577 {
578 	struct cpsw_common *cpsw = priv->cpsw;
579 	struct cpsw_slave *slave;
580 	u32 ctrl, mtype;
581 
582 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
583 
584 	ctrl = slave_read(slave, CPSW2_CONTROL);
585 	switch (cpsw->version) {
586 	case CPSW_VERSION_2:
587 		ctrl &= ~CTRL_V2_ALL_TS_MASK;
588 
589 		if (priv->tx_ts_enabled)
590 			ctrl |= CTRL_V2_TX_TS_BITS;
591 
592 		if (priv->rx_ts_enabled)
593 			ctrl |= CTRL_V2_RX_TS_BITS;
594 		break;
595 	case CPSW_VERSION_3:
596 	default:
597 		ctrl &= ~CTRL_V3_ALL_TS_MASK;
598 
599 		if (priv->tx_ts_enabled)
600 			ctrl |= CTRL_V3_TX_TS_BITS;
601 
602 		if (priv->rx_ts_enabled)
603 			ctrl |= CTRL_V3_RX_TS_BITS;
604 		break;
605 	}
606 
607 	mtype = (30 << TS_SEQ_ID_OFFSET_SHIFT) | EVENT_MSG_BITS;
608 
609 	slave_write(slave, mtype, CPSW2_TS_SEQ_MTYPE);
610 	slave_write(slave, ctrl, CPSW2_CONTROL);
611 	writel_relaxed(ETH_P_1588, &cpsw->regs->ts_ltype);
612 	writel_relaxed(ETH_P_8021Q, &cpsw->regs->vlan_ltype);
613 }
614 
615 static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
616 {
617 	struct cpsw_priv *priv = netdev_priv(dev);
618 	struct cpsw_common *cpsw = priv->cpsw;
619 	struct hwtstamp_config cfg;
620 
621 	if (cpsw->version != CPSW_VERSION_1 &&
622 	    cpsw->version != CPSW_VERSION_2 &&
623 	    cpsw->version != CPSW_VERSION_3)
624 		return -EOPNOTSUPP;
625 
626 	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
627 		return -EFAULT;
628 
629 	/* reserved for future extensions */
630 	if (cfg.flags)
631 		return -EINVAL;
632 
633 	if (cfg.tx_type != HWTSTAMP_TX_OFF && cfg.tx_type != HWTSTAMP_TX_ON)
634 		return -ERANGE;
635 
636 	switch (cfg.rx_filter) {
637 	case HWTSTAMP_FILTER_NONE:
638 		priv->rx_ts_enabled = 0;
639 		break;
640 	case HWTSTAMP_FILTER_ALL:
641 	case HWTSTAMP_FILTER_NTP_ALL:
642 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
643 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
644 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
645 		return -ERANGE;
646 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
647 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
648 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
649 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
650 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
651 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
652 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
653 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
654 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
655 		priv->rx_ts_enabled = HWTSTAMP_FILTER_PTP_V2_EVENT;
656 		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
657 		break;
658 	default:
659 		return -ERANGE;
660 	}
661 
662 	priv->tx_ts_enabled = cfg.tx_type == HWTSTAMP_TX_ON;
663 
664 	switch (cpsw->version) {
665 	case CPSW_VERSION_1:
666 		cpsw_hwtstamp_v1(priv);
667 		break;
668 	case CPSW_VERSION_2:
669 	case CPSW_VERSION_3:
670 		cpsw_hwtstamp_v2(priv);
671 		break;
672 	default:
673 		WARN_ON(1);
674 	}
675 
676 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
677 }
678 
679 static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
680 {
681 	struct cpsw_common *cpsw = ndev_to_cpsw(dev);
682 	struct cpsw_priv *priv = netdev_priv(dev);
683 	struct hwtstamp_config cfg;
684 
685 	if (cpsw->version != CPSW_VERSION_1 &&
686 	    cpsw->version != CPSW_VERSION_2 &&
687 	    cpsw->version != CPSW_VERSION_3)
688 		return -EOPNOTSUPP;
689 
690 	cfg.flags = 0;
691 	cfg.tx_type = priv->tx_ts_enabled ? HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
692 	cfg.rx_filter = priv->rx_ts_enabled;
693 
694 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
695 }
696 #else
697 static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
698 {
699 	return -EOPNOTSUPP;
700 }
701 
702 static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
703 {
704 	return -EOPNOTSUPP;
705 }
706 #endif /*CONFIG_TI_CPTS*/
707 
708 int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
709 {
710 	struct cpsw_priv *priv = netdev_priv(dev);
711 	struct cpsw_common *cpsw = priv->cpsw;
712 	int slave_no = cpsw_slave_index(cpsw, priv);
713 
714 	if (!netif_running(dev))
715 		return -EINVAL;
716 
717 	switch (cmd) {
718 	case SIOCSHWTSTAMP:
719 		return cpsw_hwtstamp_set(dev, req);
720 	case SIOCGHWTSTAMP:
721 		return cpsw_hwtstamp_get(dev, req);
722 	}
723 
724 	if (!cpsw->slaves[slave_no].phy)
725 		return -EOPNOTSUPP;
726 	return phy_mii_ioctl(cpsw->slaves[slave_no].phy, req, cmd);
727 }
728 
729 int cpsw_ndo_set_tx_maxrate(struct net_device *ndev, int queue, u32 rate)
730 {
731 	struct cpsw_priv *priv = netdev_priv(ndev);
732 	struct cpsw_common *cpsw = priv->cpsw;
733 	struct cpsw_slave *slave;
734 	u32 min_rate;
735 	u32 ch_rate;
736 	int i, ret;
737 
738 	ch_rate = netdev_get_tx_queue(ndev, queue)->tx_maxrate;
739 	if (ch_rate == rate)
740 		return 0;
741 
742 	ch_rate = rate * 1000;
743 	min_rate = cpdma_chan_get_min_rate(cpsw->dma);
744 	if ((ch_rate < min_rate && ch_rate)) {
745 		dev_err(priv->dev, "The channel rate cannot be less than %dMbps",
746 			min_rate);
747 		return -EINVAL;
748 	}
749 
750 	if (rate > cpsw->speed) {
751 		dev_err(priv->dev, "The channel rate cannot be more than 2Gbps");
752 		return -EINVAL;
753 	}
754 
755 	ret = pm_runtime_get_sync(cpsw->dev);
756 	if (ret < 0) {
757 		pm_runtime_put_noidle(cpsw->dev);
758 		return ret;
759 	}
760 
761 	ret = cpdma_chan_set_rate(cpsw->txv[queue].ch, ch_rate);
762 	pm_runtime_put(cpsw->dev);
763 
764 	if (ret)
765 		return ret;
766 
767 	/* update rates for slaves tx queues */
768 	for (i = 0; i < cpsw->data.slaves; i++) {
769 		slave = &cpsw->slaves[i];
770 		if (!slave->ndev)
771 			continue;
772 
773 		netdev_get_tx_queue(slave->ndev, queue)->tx_maxrate = rate;
774 	}
775 
776 	cpsw_split_res(cpsw);
777 	return ret;
778 }
779 
780 static int cpsw_tc_to_fifo(int tc, int num_tc)
781 {
782 	if (tc == num_tc - 1)
783 		return 0;
784 
785 	return CPSW_FIFO_SHAPERS_NUM - tc;
786 }
787 
788 bool cpsw_shp_is_off(struct cpsw_priv *priv)
789 {
790 	struct cpsw_common *cpsw = priv->cpsw;
791 	struct cpsw_slave *slave;
792 	u32 shift, mask, val;
793 
794 	val = readl_relaxed(&cpsw->regs->ptype);
795 
796 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
797 	shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num;
798 	mask = 7 << shift;
799 	val = val & mask;
800 
801 	return !val;
802 }
803 
804 static void cpsw_fifo_shp_on(struct cpsw_priv *priv, int fifo, int on)
805 {
806 	struct cpsw_common *cpsw = priv->cpsw;
807 	struct cpsw_slave *slave;
808 	u32 shift, mask, val;
809 
810 	val = readl_relaxed(&cpsw->regs->ptype);
811 
812 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
813 	shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num;
814 	mask = (1 << --fifo) << shift;
815 	val = on ? val | mask : val & ~mask;
816 
817 	writel_relaxed(val, &cpsw->regs->ptype);
818 }
819 
820 static int cpsw_set_fifo_bw(struct cpsw_priv *priv, int fifo, int bw)
821 {
822 	struct cpsw_common *cpsw = priv->cpsw;
823 	u32 val = 0, send_pct, shift;
824 	struct cpsw_slave *slave;
825 	int pct = 0, i;
826 
827 	if (bw > priv->shp_cfg_speed * 1000)
828 		goto err;
829 
830 	/* shaping has to stay enabled for highest fifos linearly
831 	 * and fifo bw no more then interface can allow
832 	 */
833 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
834 	send_pct = slave_read(slave, SEND_PERCENT);
835 	for (i = CPSW_FIFO_SHAPERS_NUM; i > 0; i--) {
836 		if (!bw) {
837 			if (i >= fifo || !priv->fifo_bw[i])
838 				continue;
839 
840 			dev_warn(priv->dev, "Prev FIFO%d is shaped", i);
841 			continue;
842 		}
843 
844 		if (!priv->fifo_bw[i] && i > fifo) {
845 			dev_err(priv->dev, "Upper FIFO%d is not shaped", i);
846 			return -EINVAL;
847 		}
848 
849 		shift = (i - 1) * 8;
850 		if (i == fifo) {
851 			send_pct &= ~(CPSW_PCT_MASK << shift);
852 			val = DIV_ROUND_UP(bw, priv->shp_cfg_speed * 10);
853 			if (!val)
854 				val = 1;
855 
856 			send_pct |= val << shift;
857 			pct += val;
858 			continue;
859 		}
860 
861 		if (priv->fifo_bw[i])
862 			pct += (send_pct >> shift) & CPSW_PCT_MASK;
863 	}
864 
865 	if (pct >= 100)
866 		goto err;
867 
868 	slave_write(slave, send_pct, SEND_PERCENT);
869 	priv->fifo_bw[fifo] = bw;
870 
871 	dev_warn(priv->dev, "set FIFO%d bw = %d\n", fifo,
872 		 DIV_ROUND_CLOSEST(val * priv->shp_cfg_speed, 100));
873 
874 	return 0;
875 err:
876 	dev_err(priv->dev, "Bandwidth doesn't fit in tc configuration");
877 	return -EINVAL;
878 }
879 
880 static int cpsw_set_fifo_rlimit(struct cpsw_priv *priv, int fifo, int bw)
881 {
882 	struct cpsw_common *cpsw = priv->cpsw;
883 	struct cpsw_slave *slave;
884 	u32 tx_in_ctl_rg, val;
885 	int ret;
886 
887 	ret = cpsw_set_fifo_bw(priv, fifo, bw);
888 	if (ret)
889 		return ret;
890 
891 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
892 	tx_in_ctl_rg = cpsw->version == CPSW_VERSION_1 ?
893 		       CPSW1_TX_IN_CTL : CPSW2_TX_IN_CTL;
894 
895 	if (!bw)
896 		cpsw_fifo_shp_on(priv, fifo, bw);
897 
898 	val = slave_read(slave, tx_in_ctl_rg);
899 	if (cpsw_shp_is_off(priv)) {
900 		/* disable FIFOs rate limited queues */
901 		val &= ~(0xf << CPSW_FIFO_RATE_EN_SHIFT);
902 
903 		/* set type of FIFO queues to normal priority mode */
904 		val &= ~(3 << CPSW_FIFO_QUEUE_TYPE_SHIFT);
905 
906 		/* set type of FIFO queues to be rate limited */
907 		if (bw)
908 			val |= 2 << CPSW_FIFO_QUEUE_TYPE_SHIFT;
909 		else
910 			priv->shp_cfg_speed = 0;
911 	}
912 
913 	/* toggle a FIFO rate limited queue */
914 	if (bw)
915 		val |= BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT);
916 	else
917 		val &= ~BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT);
918 	slave_write(slave, val, tx_in_ctl_rg);
919 
920 	/* FIFO transmit shape enable */
921 	cpsw_fifo_shp_on(priv, fifo, bw);
922 	return 0;
923 }
924 
925 /* Defaults:
926  * class A - prio 3
927  * class B - prio 2
928  * shaping for class A should be set first
929  */
930 static int cpsw_set_cbs(struct net_device *ndev,
931 			struct tc_cbs_qopt_offload *qopt)
932 {
933 	struct cpsw_priv *priv = netdev_priv(ndev);
934 	struct cpsw_common *cpsw = priv->cpsw;
935 	struct cpsw_slave *slave;
936 	int prev_speed = 0;
937 	int tc, ret, fifo;
938 	u32 bw = 0;
939 
940 	tc = netdev_txq_to_tc(priv->ndev, qopt->queue);
941 
942 	/* enable channels in backward order, as highest FIFOs must be rate
943 	 * limited first and for compliance with CPDMA rate limited channels
944 	 * that also used in bacward order. FIFO0 cannot be rate limited.
945 	 */
946 	fifo = cpsw_tc_to_fifo(tc, ndev->num_tc);
947 	if (!fifo) {
948 		dev_err(priv->dev, "Last tc%d can't be rate limited", tc);
949 		return -EINVAL;
950 	}
951 
952 	/* do nothing, it's disabled anyway */
953 	if (!qopt->enable && !priv->fifo_bw[fifo])
954 		return 0;
955 
956 	/* shapers can be set if link speed is known */
957 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
958 	if (slave->phy && slave->phy->link) {
959 		if (priv->shp_cfg_speed &&
960 		    priv->shp_cfg_speed != slave->phy->speed)
961 			prev_speed = priv->shp_cfg_speed;
962 
963 		priv->shp_cfg_speed = slave->phy->speed;
964 	}
965 
966 	if (!priv->shp_cfg_speed) {
967 		dev_err(priv->dev, "Link speed is not known");
968 		return -1;
969 	}
970 
971 	ret = pm_runtime_get_sync(cpsw->dev);
972 	if (ret < 0) {
973 		pm_runtime_put_noidle(cpsw->dev);
974 		return ret;
975 	}
976 
977 	bw = qopt->enable ? qopt->idleslope : 0;
978 	ret = cpsw_set_fifo_rlimit(priv, fifo, bw);
979 	if (ret) {
980 		priv->shp_cfg_speed = prev_speed;
981 		prev_speed = 0;
982 	}
983 
984 	if (bw && prev_speed)
985 		dev_warn(priv->dev,
986 			 "Speed was changed, CBS shaper speeds are changed!");
987 
988 	pm_runtime_put_sync(cpsw->dev);
989 	return ret;
990 }
991 
992 static int cpsw_set_mqprio(struct net_device *ndev, void *type_data)
993 {
994 	struct tc_mqprio_qopt_offload *mqprio = type_data;
995 	struct cpsw_priv *priv = netdev_priv(ndev);
996 	struct cpsw_common *cpsw = priv->cpsw;
997 	int fifo, num_tc, count, offset;
998 	struct cpsw_slave *slave;
999 	u32 tx_prio_map = 0;
1000 	int i, tc, ret;
1001 
1002 	num_tc = mqprio->qopt.num_tc;
1003 	if (num_tc > CPSW_TC_NUM)
1004 		return -EINVAL;
1005 
1006 	if (mqprio->mode != TC_MQPRIO_MODE_DCB)
1007 		return -EINVAL;
1008 
1009 	ret = pm_runtime_get_sync(cpsw->dev);
1010 	if (ret < 0) {
1011 		pm_runtime_put_noidle(cpsw->dev);
1012 		return ret;
1013 	}
1014 
1015 	if (num_tc) {
1016 		for (i = 0; i < 8; i++) {
1017 			tc = mqprio->qopt.prio_tc_map[i];
1018 			fifo = cpsw_tc_to_fifo(tc, num_tc);
1019 			tx_prio_map |= fifo << (4 * i);
1020 		}
1021 
1022 		netdev_set_num_tc(ndev, num_tc);
1023 		for (i = 0; i < num_tc; i++) {
1024 			count = mqprio->qopt.count[i];
1025 			offset = mqprio->qopt.offset[i];
1026 			netdev_set_tc_queue(ndev, i, count, offset);
1027 		}
1028 	}
1029 
1030 	if (!mqprio->qopt.hw) {
1031 		/* restore default configuration */
1032 		netdev_reset_tc(ndev);
1033 		tx_prio_map = TX_PRIORITY_MAPPING;
1034 	}
1035 
1036 	priv->mqprio_hw = mqprio->qopt.hw;
1037 
1038 	offset = cpsw->version == CPSW_VERSION_1 ?
1039 		 CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;
1040 
1041 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1042 	slave_write(slave, tx_prio_map, offset);
1043 
1044 	pm_runtime_put_sync(cpsw->dev);
1045 
1046 	return 0;
1047 }
1048 
1049 int cpsw_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type,
1050 		      void *type_data)
1051 {
1052 	switch (type) {
1053 	case TC_SETUP_QDISC_CBS:
1054 		return cpsw_set_cbs(ndev, type_data);
1055 
1056 	case TC_SETUP_QDISC_MQPRIO:
1057 		return cpsw_set_mqprio(ndev, type_data);
1058 
1059 	default:
1060 		return -EOPNOTSUPP;
1061 	}
1062 }
1063 
1064 void cpsw_cbs_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)
1065 {
1066 	int fifo, bw;
1067 
1068 	for (fifo = CPSW_FIFO_SHAPERS_NUM; fifo > 0; fifo--) {
1069 		bw = priv->fifo_bw[fifo];
1070 		if (!bw)
1071 			continue;
1072 
1073 		cpsw_set_fifo_rlimit(priv, fifo, bw);
1074 	}
1075 }
1076 
1077 void cpsw_mqprio_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)
1078 {
1079 	struct cpsw_common *cpsw = priv->cpsw;
1080 	u32 tx_prio_map = 0;
1081 	int i, tc, fifo;
1082 	u32 tx_prio_rg;
1083 
1084 	if (!priv->mqprio_hw)
1085 		return;
1086 
1087 	for (i = 0; i < 8; i++) {
1088 		tc = netdev_get_prio_tc_map(priv->ndev, i);
1089 		fifo = CPSW_FIFO_SHAPERS_NUM - tc;
1090 		tx_prio_map |= fifo << (4 * i);
1091 	}
1092 
1093 	tx_prio_rg = cpsw->version == CPSW_VERSION_1 ?
1094 		     CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;
1095 
1096 	slave_write(slave, tx_prio_map, tx_prio_rg);
1097 }
1098 
1099 int cpsw_fill_rx_channels(struct cpsw_priv *priv)
1100 {
1101 	struct cpsw_common *cpsw = priv->cpsw;
1102 	struct cpsw_meta_xdp *xmeta;
1103 	struct page_pool *pool;
1104 	struct page *page;
1105 	int ch_buf_num;
1106 	int ch, i, ret;
1107 	dma_addr_t dma;
1108 
1109 	for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1110 		pool = cpsw->page_pool[ch];
1111 		ch_buf_num = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);
1112 		for (i = 0; i < ch_buf_num; i++) {
1113 			page = page_pool_dev_alloc_pages(pool);
1114 			if (!page) {
1115 				cpsw_err(priv, ifup, "allocate rx page err\n");
1116 				return -ENOMEM;
1117 			}
1118 
1119 			xmeta = page_address(page) + CPSW_XMETA_OFFSET;
1120 			xmeta->ndev = priv->ndev;
1121 			xmeta->ch = ch;
1122 
1123 			dma = page_pool_get_dma_addr(page) + CPSW_HEADROOM;
1124 			ret = cpdma_chan_idle_submit_mapped(cpsw->rxv[ch].ch,
1125 							    page, dma,
1126 							    cpsw->rx_packet_max,
1127 							    0);
1128 			if (ret < 0) {
1129 				cpsw_err(priv, ifup,
1130 					 "cannot submit page to channel %d rx, error %d\n",
1131 					 ch, ret);
1132 				page_pool_recycle_direct(pool, page);
1133 				return ret;
1134 			}
1135 		}
1136 
1137 		cpsw_info(priv, ifup, "ch %d rx, submitted %d descriptors\n",
1138 			  ch, ch_buf_num);
1139 	}
1140 
1141 	return 0;
1142 }
1143 
1144 static struct page_pool *cpsw_create_page_pool(struct cpsw_common *cpsw,
1145 					       int size)
1146 {
1147 	struct page_pool_params pp_params;
1148 	struct page_pool *pool;
1149 
1150 	pp_params.order = 0;
1151 	pp_params.flags = PP_FLAG_DMA_MAP;
1152 	pp_params.pool_size = size;
1153 	pp_params.nid = NUMA_NO_NODE;
1154 	pp_params.dma_dir = DMA_BIDIRECTIONAL;
1155 	pp_params.dev = cpsw->dev;
1156 
1157 	pool = page_pool_create(&pp_params);
1158 	if (IS_ERR(pool))
1159 		dev_err(cpsw->dev, "cannot create rx page pool\n");
1160 
1161 	return pool;
1162 }
1163 
1164 static int cpsw_create_rx_pool(struct cpsw_common *cpsw, int ch)
1165 {
1166 	struct page_pool *pool;
1167 	int ret = 0, pool_size;
1168 
1169 	pool_size = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);
1170 	pool = cpsw_create_page_pool(cpsw, pool_size);
1171 	if (IS_ERR(pool))
1172 		ret = PTR_ERR(pool);
1173 	else
1174 		cpsw->page_pool[ch] = pool;
1175 
1176 	return ret;
1177 }
1178 
1179 static int cpsw_ndev_create_xdp_rxq(struct cpsw_priv *priv, int ch)
1180 {
1181 	struct cpsw_common *cpsw = priv->cpsw;
1182 	struct xdp_rxq_info *rxq;
1183 	struct page_pool *pool;
1184 	int ret;
1185 
1186 	pool = cpsw->page_pool[ch];
1187 	rxq = &priv->xdp_rxq[ch];
1188 
1189 	ret = xdp_rxq_info_reg(rxq, priv->ndev, ch, 0);
1190 	if (ret)
1191 		return ret;
1192 
1193 	ret = xdp_rxq_info_reg_mem_model(rxq, MEM_TYPE_PAGE_POOL, pool);
1194 	if (ret)
1195 		xdp_rxq_info_unreg(rxq);
1196 
1197 	return ret;
1198 }
1199 
1200 static void cpsw_ndev_destroy_xdp_rxq(struct cpsw_priv *priv, int ch)
1201 {
1202 	struct xdp_rxq_info *rxq = &priv->xdp_rxq[ch];
1203 
1204 	if (!xdp_rxq_info_is_reg(rxq))
1205 		return;
1206 
1207 	xdp_rxq_info_unreg(rxq);
1208 }
1209 
1210 void cpsw_destroy_xdp_rxqs(struct cpsw_common *cpsw)
1211 {
1212 	struct net_device *ndev;
1213 	int i, ch;
1214 
1215 	for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1216 		for (i = 0; i < cpsw->data.slaves; i++) {
1217 			ndev = cpsw->slaves[i].ndev;
1218 			if (!ndev)
1219 				continue;
1220 
1221 			cpsw_ndev_destroy_xdp_rxq(netdev_priv(ndev), ch);
1222 		}
1223 
1224 		page_pool_destroy(cpsw->page_pool[ch]);
1225 		cpsw->page_pool[ch] = NULL;
1226 	}
1227 }
1228 
1229 int cpsw_create_xdp_rxqs(struct cpsw_common *cpsw)
1230 {
1231 	struct net_device *ndev;
1232 	int i, ch, ret;
1233 
1234 	for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1235 		ret = cpsw_create_rx_pool(cpsw, ch);
1236 		if (ret)
1237 			goto err_cleanup;
1238 
1239 		/* using same page pool is allowed as no running rx handlers
1240 		 * simultaneously for both ndevs
1241 		 */
1242 		for (i = 0; i < cpsw->data.slaves; i++) {
1243 			ndev = cpsw->slaves[i].ndev;
1244 			if (!ndev)
1245 				continue;
1246 
1247 			ret = cpsw_ndev_create_xdp_rxq(netdev_priv(ndev), ch);
1248 			if (ret)
1249 				goto err_cleanup;
1250 		}
1251 	}
1252 
1253 	return 0;
1254 
1255 err_cleanup:
1256 	cpsw_destroy_xdp_rxqs(cpsw);
1257 
1258 	return ret;
1259 }
1260 
1261 static int cpsw_xdp_prog_setup(struct cpsw_priv *priv, struct netdev_bpf *bpf)
1262 {
1263 	struct bpf_prog *prog = bpf->prog;
1264 
1265 	if (!priv->xdpi.prog && !prog)
1266 		return 0;
1267 
1268 	WRITE_ONCE(priv->xdp_prog, prog);
1269 
1270 	xdp_attachment_setup(&priv->xdpi, bpf);
1271 
1272 	return 0;
1273 }
1274 
1275 int cpsw_ndo_bpf(struct net_device *ndev, struct netdev_bpf *bpf)
1276 {
1277 	struct cpsw_priv *priv = netdev_priv(ndev);
1278 
1279 	switch (bpf->command) {
1280 	case XDP_SETUP_PROG:
1281 		return cpsw_xdp_prog_setup(priv, bpf);
1282 
1283 	default:
1284 		return -EINVAL;
1285 	}
1286 }
1287 
1288 int cpsw_xdp_tx_frame(struct cpsw_priv *priv, struct xdp_frame *xdpf,
1289 		      struct page *page, int port)
1290 {
1291 	struct cpsw_common *cpsw = priv->cpsw;
1292 	struct cpsw_meta_xdp *xmeta;
1293 	struct cpdma_chan *txch;
1294 	dma_addr_t dma;
1295 	int ret;
1296 
1297 	xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;
1298 	xmeta->ndev = priv->ndev;
1299 	xmeta->ch = 0;
1300 	txch = cpsw->txv[0].ch;
1301 
1302 	if (page) {
1303 		dma = page_pool_get_dma_addr(page);
1304 		dma += xdpf->headroom + sizeof(struct xdp_frame);
1305 		ret = cpdma_chan_submit_mapped(txch, cpsw_xdpf_to_handle(xdpf),
1306 					       dma, xdpf->len, port);
1307 	} else {
1308 		if (sizeof(*xmeta) > xdpf->headroom)
1309 			return -EINVAL;
1310 
1311 		ret = cpdma_chan_submit(txch, cpsw_xdpf_to_handle(xdpf),
1312 					xdpf->data, xdpf->len, port);
1313 	}
1314 
1315 	if (ret)
1316 		priv->ndev->stats.tx_dropped++;
1317 
1318 	return ret;
1319 }
1320 
1321 int cpsw_run_xdp(struct cpsw_priv *priv, int ch, struct xdp_buff *xdp,
1322 		 struct page *page, int port, int *len)
1323 {
1324 	struct cpsw_common *cpsw = priv->cpsw;
1325 	struct net_device *ndev = priv->ndev;
1326 	int ret = CPSW_XDP_CONSUMED;
1327 	struct xdp_frame *xdpf;
1328 	struct bpf_prog *prog;
1329 	u32 act;
1330 
1331 	prog = READ_ONCE(priv->xdp_prog);
1332 	if (!prog)
1333 		return CPSW_XDP_PASS;
1334 
1335 	act = bpf_prog_run_xdp(prog, xdp);
1336 	/* XDP prog might have changed packet data and boundaries */
1337 	*len = xdp->data_end - xdp->data;
1338 
1339 	switch (act) {
1340 	case XDP_PASS:
1341 		ret = CPSW_XDP_PASS;
1342 		goto out;
1343 	case XDP_TX:
1344 		xdpf = xdp_convert_buff_to_frame(xdp);
1345 		if (unlikely(!xdpf))
1346 			goto drop;
1347 
1348 		if (cpsw_xdp_tx_frame(priv, xdpf, page, port))
1349 			xdp_return_frame_rx_napi(xdpf);
1350 		break;
1351 	case XDP_REDIRECT:
1352 		if (xdp_do_redirect(ndev, xdp, prog))
1353 			goto drop;
1354 
1355 		/*  Have to flush here, per packet, instead of doing it in bulk
1356 		 *  at the end of the napi handler. The RX devices on this
1357 		 *  particular hardware is sharing a common queue, so the
1358 		 *  incoming device might change per packet.
1359 		 */
1360 		xdp_do_flush_map();
1361 		break;
1362 	default:
1363 		bpf_warn_invalid_xdp_action(act);
1364 		fallthrough;
1365 	case XDP_ABORTED:
1366 		trace_xdp_exception(ndev, prog, act);
1367 		fallthrough;	/* handle aborts by dropping packet */
1368 	case XDP_DROP:
1369 		ndev->stats.rx_bytes += *len;
1370 		ndev->stats.rx_packets++;
1371 		goto drop;
1372 	}
1373 
1374 	ndev->stats.rx_bytes += *len;
1375 	ndev->stats.rx_packets++;
1376 out:
1377 	return ret;
1378 drop:
1379 	page_pool_recycle_direct(cpsw->page_pool[ch], page);
1380 	return ret;
1381 }
1382