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