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 
cpsw_intr_enable(struct cpsw_common * cpsw)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 
cpsw_intr_disable(struct cpsw_common * cpsw)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 
cpsw_tx_handler(void * token,int len,int status)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 
cpsw_tx_interrupt(int irq,void * dev_id)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 
cpsw_rx_interrupt(int irq,void * dev_id)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 
cpsw_misc_interrupt(int irq,void * dev_id)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 
cpsw_tx_mq_poll(struct napi_struct * napi_tx,int budget)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 
cpsw_tx_poll(struct napi_struct * napi_tx,int budget)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 
cpsw_rx_mq_poll(struct napi_struct * napi_rx,int budget)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 
cpsw_rx_poll(struct napi_struct * napi_rx,int budget)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 
cpsw_rx_vlan_encap(struct sk_buff * skb)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 
cpsw_set_slave_mac(struct cpsw_slave * slave,struct cpsw_priv * priv)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 
soft_reset(const char * module,void __iomem * reg)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 
cpsw_ndo_tx_timeout(struct net_device * ndev,unsigned int txqueue)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 
cpsw_get_common_speed(struct cpsw_common * cpsw)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 
cpsw_need_resplit(struct cpsw_common * cpsw)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 
cpsw_split_res(struct cpsw_common * cpsw)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 = NAPI_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 * NAPI_POLL_WEIGHT) / max_rate;
383 		ch_budget = (NAPI_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 = NAPI_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 * NAPI_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 = NAPI_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 
cpsw_init_common(struct cpsw_common * cpsw,void __iomem * ss_regs,int ale_ageout,phys_addr_t desc_mem_phys,int descs_pool_size)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 	ale_params.bus_freq		= cpsw->bus_freq_mhz * 1000000;
506 
507 	cpsw->ale = cpsw_ale_create(&ale_params);
508 	if (IS_ERR(cpsw->ale)) {
509 		dev_err(dev, "error initializing ale engine\n");
510 		return PTR_ERR(cpsw->ale);
511 	}
512 
513 	dma_params.dev		= dev;
514 	dma_params.rxthresh	= dma_params.dmaregs + CPDMA_RXTHRESH;
515 	dma_params.rxfree	= dma_params.dmaregs + CPDMA_RXFREE;
516 	dma_params.rxhdp	= dma_params.txhdp + CPDMA_RXHDP;
517 	dma_params.txcp		= dma_params.txhdp + CPDMA_TXCP;
518 	dma_params.rxcp		= dma_params.txhdp + CPDMA_RXCP;
519 
520 	dma_params.num_chan		= data->channels;
521 	dma_params.has_soft_reset	= true;
522 	dma_params.min_packet_size	= CPSW_MIN_PACKET_SIZE;
523 	dma_params.desc_mem_size	= data->bd_ram_size;
524 	dma_params.desc_align		= 16;
525 	dma_params.has_ext_regs		= true;
526 	dma_params.desc_hw_addr         = dma_params.desc_mem_phys;
527 	dma_params.bus_freq_mhz		= cpsw->bus_freq_mhz;
528 	dma_params.descs_pool_size	= descs_pool_size;
529 
530 	cpsw->dma = cpdma_ctlr_create(&dma_params);
531 	if (!cpsw->dma) {
532 		dev_err(dev, "error initializing dma\n");
533 		return -ENOMEM;
534 	}
535 
536 	cpts_node = of_get_child_by_name(cpsw->dev->of_node, "cpts");
537 	if (!cpts_node)
538 		cpts_node = cpsw->dev->of_node;
539 
540 	cpsw->cpts = cpts_create(cpsw->dev, cpts_regs, cpts_node,
541 				 CPTS_N_ETX_TS);
542 	if (IS_ERR(cpsw->cpts)) {
543 		ret = PTR_ERR(cpsw->cpts);
544 		cpdma_ctlr_destroy(cpsw->dma);
545 	}
546 	of_node_put(cpts_node);
547 
548 	return ret;
549 }
550 
551 #if IS_ENABLED(CONFIG_TI_CPTS)
552 
cpsw_hwtstamp_v1(struct cpsw_priv * priv)553 static void cpsw_hwtstamp_v1(struct cpsw_priv *priv)
554 {
555 	struct cpsw_common *cpsw = priv->cpsw;
556 	struct cpsw_slave *slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
557 	u32 ts_en, seq_id;
558 
559 	if (!priv->tx_ts_enabled && !priv->rx_ts_enabled) {
560 		slave_write(slave, 0, CPSW1_TS_CTL);
561 		return;
562 	}
563 
564 	seq_id = (30 << CPSW_V1_SEQ_ID_OFS_SHIFT) | ETH_P_1588;
565 	ts_en = EVENT_MSG_BITS << CPSW_V1_MSG_TYPE_OFS;
566 
567 	if (priv->tx_ts_enabled)
568 		ts_en |= CPSW_V1_TS_TX_EN;
569 
570 	if (priv->rx_ts_enabled)
571 		ts_en |= CPSW_V1_TS_RX_EN;
572 
573 	slave_write(slave, ts_en, CPSW1_TS_CTL);
574 	slave_write(slave, seq_id, CPSW1_TS_SEQ_LTYPE);
575 }
576 
cpsw_hwtstamp_v2(struct cpsw_priv * priv)577 static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)
578 {
579 	struct cpsw_common *cpsw = priv->cpsw;
580 	struct cpsw_slave *slave;
581 	u32 ctrl, mtype;
582 
583 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
584 
585 	ctrl = slave_read(slave, CPSW2_CONTROL);
586 	switch (cpsw->version) {
587 	case CPSW_VERSION_2:
588 		ctrl &= ~CTRL_V2_ALL_TS_MASK;
589 
590 		if (priv->tx_ts_enabled)
591 			ctrl |= CTRL_V2_TX_TS_BITS;
592 
593 		if (priv->rx_ts_enabled)
594 			ctrl |= CTRL_V2_RX_TS_BITS;
595 		break;
596 	case CPSW_VERSION_3:
597 	default:
598 		ctrl &= ~CTRL_V3_ALL_TS_MASK;
599 
600 		if (priv->tx_ts_enabled)
601 			ctrl |= CTRL_V3_TX_TS_BITS;
602 
603 		if (priv->rx_ts_enabled)
604 			ctrl |= CTRL_V3_RX_TS_BITS;
605 		break;
606 	}
607 
608 	mtype = (30 << TS_SEQ_ID_OFFSET_SHIFT) | EVENT_MSG_BITS;
609 
610 	slave_write(slave, mtype, CPSW2_TS_SEQ_MTYPE);
611 	slave_write(slave, ctrl, CPSW2_CONTROL);
612 	writel_relaxed(ETH_P_1588, &cpsw->regs->ts_ltype);
613 	writel_relaxed(ETH_P_8021Q, &cpsw->regs->vlan_ltype);
614 }
615 
cpsw_hwtstamp_set(struct net_device * dev,struct ifreq * ifr)616 static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
617 {
618 	struct cpsw_priv *priv = netdev_priv(dev);
619 	struct cpsw_common *cpsw = priv->cpsw;
620 	struct hwtstamp_config cfg;
621 
622 	if (cpsw->version != CPSW_VERSION_1 &&
623 	    cpsw->version != CPSW_VERSION_2 &&
624 	    cpsw->version != CPSW_VERSION_3)
625 		return -EOPNOTSUPP;
626 
627 	if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
628 		return -EFAULT;
629 
630 	if (cfg.tx_type != HWTSTAMP_TX_OFF && cfg.tx_type != HWTSTAMP_TX_ON)
631 		return -ERANGE;
632 
633 	switch (cfg.rx_filter) {
634 	case HWTSTAMP_FILTER_NONE:
635 		priv->rx_ts_enabled = 0;
636 		break;
637 	case HWTSTAMP_FILTER_ALL:
638 	case HWTSTAMP_FILTER_NTP_ALL:
639 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
640 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
641 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
642 		return -ERANGE;
643 	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
644 	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
645 	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
646 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
647 	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
648 	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
649 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
650 	case HWTSTAMP_FILTER_PTP_V2_SYNC:
651 	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
652 		priv->rx_ts_enabled = HWTSTAMP_FILTER_PTP_V2_EVENT;
653 		cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
654 		break;
655 	default:
656 		return -ERANGE;
657 	}
658 
659 	priv->tx_ts_enabled = cfg.tx_type == HWTSTAMP_TX_ON;
660 
661 	switch (cpsw->version) {
662 	case CPSW_VERSION_1:
663 		cpsw_hwtstamp_v1(priv);
664 		break;
665 	case CPSW_VERSION_2:
666 	case CPSW_VERSION_3:
667 		cpsw_hwtstamp_v2(priv);
668 		break;
669 	default:
670 		WARN_ON(1);
671 	}
672 
673 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
674 }
675 
cpsw_hwtstamp_get(struct net_device * dev,struct ifreq * ifr)676 static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
677 {
678 	struct cpsw_common *cpsw = ndev_to_cpsw(dev);
679 	struct cpsw_priv *priv = netdev_priv(dev);
680 	struct hwtstamp_config cfg;
681 
682 	if (cpsw->version != CPSW_VERSION_1 &&
683 	    cpsw->version != CPSW_VERSION_2 &&
684 	    cpsw->version != CPSW_VERSION_3)
685 		return -EOPNOTSUPP;
686 
687 	cfg.flags = 0;
688 	cfg.tx_type = priv->tx_ts_enabled ? HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
689 	cfg.rx_filter = priv->rx_ts_enabled;
690 
691 	return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
692 }
693 #else
cpsw_hwtstamp_get(struct net_device * dev,struct ifreq * ifr)694 static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
695 {
696 	return -EOPNOTSUPP;
697 }
698 
cpsw_hwtstamp_set(struct net_device * dev,struct ifreq * ifr)699 static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
700 {
701 	return -EOPNOTSUPP;
702 }
703 #endif /*CONFIG_TI_CPTS*/
704 
cpsw_ndo_ioctl(struct net_device * dev,struct ifreq * req,int cmd)705 int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
706 {
707 	struct cpsw_priv *priv = netdev_priv(dev);
708 	struct cpsw_common *cpsw = priv->cpsw;
709 	int slave_no = cpsw_slave_index(cpsw, priv);
710 	struct phy_device *phy;
711 
712 	if (!netif_running(dev))
713 		return -EINVAL;
714 
715 	phy = cpsw->slaves[slave_no].phy;
716 
717 	if (!phy_has_hwtstamp(phy)) {
718 		switch (cmd) {
719 		case SIOCSHWTSTAMP:
720 			return cpsw_hwtstamp_set(dev, req);
721 		case SIOCGHWTSTAMP:
722 			return cpsw_hwtstamp_get(dev, req);
723 		}
724 	}
725 
726 	if (phy)
727 		return phy_mii_ioctl(phy, req, cmd);
728 
729 	return -EOPNOTSUPP;
730 }
731 
cpsw_ndo_set_tx_maxrate(struct net_device * ndev,int queue,u32 rate)732 int cpsw_ndo_set_tx_maxrate(struct net_device *ndev, int queue, u32 rate)
733 {
734 	struct cpsw_priv *priv = netdev_priv(ndev);
735 	struct cpsw_common *cpsw = priv->cpsw;
736 	struct cpsw_slave *slave;
737 	u32 min_rate;
738 	u32 ch_rate;
739 	int i, ret;
740 
741 	ch_rate = netdev_get_tx_queue(ndev, queue)->tx_maxrate;
742 	if (ch_rate == rate)
743 		return 0;
744 
745 	ch_rate = rate * 1000;
746 	min_rate = cpdma_chan_get_min_rate(cpsw->dma);
747 	if ((ch_rate < min_rate && ch_rate)) {
748 		dev_err(priv->dev, "The channel rate cannot be less than %dMbps",
749 			min_rate);
750 		return -EINVAL;
751 	}
752 
753 	if (rate > cpsw->speed) {
754 		dev_err(priv->dev, "The channel rate cannot be more than 2Gbps");
755 		return -EINVAL;
756 	}
757 
758 	ret = pm_runtime_resume_and_get(cpsw->dev);
759 	if (ret < 0)
760 		return ret;
761 
762 	ret = cpdma_chan_set_rate(cpsw->txv[queue].ch, ch_rate);
763 	pm_runtime_put(cpsw->dev);
764 
765 	if (ret)
766 		return ret;
767 
768 	/* update rates for slaves tx queues */
769 	for (i = 0; i < cpsw->data.slaves; i++) {
770 		slave = &cpsw->slaves[i];
771 		if (!slave->ndev)
772 			continue;
773 
774 		netdev_get_tx_queue(slave->ndev, queue)->tx_maxrate = rate;
775 	}
776 
777 	cpsw_split_res(cpsw);
778 	return ret;
779 }
780 
cpsw_tc_to_fifo(int tc,int num_tc)781 static int cpsw_tc_to_fifo(int tc, int num_tc)
782 {
783 	if (tc == num_tc - 1)
784 		return 0;
785 
786 	return CPSW_FIFO_SHAPERS_NUM - tc;
787 }
788 
cpsw_shp_is_off(struct cpsw_priv * priv)789 bool cpsw_shp_is_off(struct cpsw_priv *priv)
790 {
791 	struct cpsw_common *cpsw = priv->cpsw;
792 	struct cpsw_slave *slave;
793 	u32 shift, mask, val;
794 
795 	val = readl_relaxed(&cpsw->regs->ptype);
796 
797 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
798 	shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num;
799 	mask = 7 << shift;
800 	val = val & mask;
801 
802 	return !val;
803 }
804 
cpsw_fifo_shp_on(struct cpsw_priv * priv,int fifo,int on)805 static void cpsw_fifo_shp_on(struct cpsw_priv *priv, int fifo, int on)
806 {
807 	struct cpsw_common *cpsw = priv->cpsw;
808 	struct cpsw_slave *slave;
809 	u32 shift, mask, val;
810 
811 	val = readl_relaxed(&cpsw->regs->ptype);
812 
813 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
814 	shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num;
815 	mask = (1 << --fifo) << shift;
816 	val = on ? val | mask : val & ~mask;
817 
818 	writel_relaxed(val, &cpsw->regs->ptype);
819 }
820 
cpsw_set_fifo_bw(struct cpsw_priv * priv,int fifo,int bw)821 static int cpsw_set_fifo_bw(struct cpsw_priv *priv, int fifo, int bw)
822 {
823 	struct cpsw_common *cpsw = priv->cpsw;
824 	u32 val = 0, send_pct, shift;
825 	struct cpsw_slave *slave;
826 	int pct = 0, i;
827 
828 	if (bw > priv->shp_cfg_speed * 1000)
829 		goto err;
830 
831 	/* shaping has to stay enabled for highest fifos linearly
832 	 * and fifo bw no more then interface can allow
833 	 */
834 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
835 	send_pct = slave_read(slave, SEND_PERCENT);
836 	for (i = CPSW_FIFO_SHAPERS_NUM; i > 0; i--) {
837 		if (!bw) {
838 			if (i >= fifo || !priv->fifo_bw[i])
839 				continue;
840 
841 			dev_warn(priv->dev, "Prev FIFO%d is shaped", i);
842 			continue;
843 		}
844 
845 		if (!priv->fifo_bw[i] && i > fifo) {
846 			dev_err(priv->dev, "Upper FIFO%d is not shaped", i);
847 			return -EINVAL;
848 		}
849 
850 		shift = (i - 1) * 8;
851 		if (i == fifo) {
852 			send_pct &= ~(CPSW_PCT_MASK << shift);
853 			val = DIV_ROUND_UP(bw, priv->shp_cfg_speed * 10);
854 			if (!val)
855 				val = 1;
856 
857 			send_pct |= val << shift;
858 			pct += val;
859 			continue;
860 		}
861 
862 		if (priv->fifo_bw[i])
863 			pct += (send_pct >> shift) & CPSW_PCT_MASK;
864 	}
865 
866 	if (pct >= 100)
867 		goto err;
868 
869 	slave_write(slave, send_pct, SEND_PERCENT);
870 	priv->fifo_bw[fifo] = bw;
871 
872 	dev_warn(priv->dev, "set FIFO%d bw = %d\n", fifo,
873 		 DIV_ROUND_CLOSEST(val * priv->shp_cfg_speed, 100));
874 
875 	return 0;
876 err:
877 	dev_err(priv->dev, "Bandwidth doesn't fit in tc configuration");
878 	return -EINVAL;
879 }
880 
cpsw_set_fifo_rlimit(struct cpsw_priv * priv,int fifo,int bw)881 static int cpsw_set_fifo_rlimit(struct cpsw_priv *priv, int fifo, int bw)
882 {
883 	struct cpsw_common *cpsw = priv->cpsw;
884 	struct cpsw_slave *slave;
885 	u32 tx_in_ctl_rg, val;
886 	int ret;
887 
888 	ret = cpsw_set_fifo_bw(priv, fifo, bw);
889 	if (ret)
890 		return ret;
891 
892 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
893 	tx_in_ctl_rg = cpsw->version == CPSW_VERSION_1 ?
894 		       CPSW1_TX_IN_CTL : CPSW2_TX_IN_CTL;
895 
896 	if (!bw)
897 		cpsw_fifo_shp_on(priv, fifo, bw);
898 
899 	val = slave_read(slave, tx_in_ctl_rg);
900 	if (cpsw_shp_is_off(priv)) {
901 		/* disable FIFOs rate limited queues */
902 		val &= ~(0xf << CPSW_FIFO_RATE_EN_SHIFT);
903 
904 		/* set type of FIFO queues to normal priority mode */
905 		val &= ~(3 << CPSW_FIFO_QUEUE_TYPE_SHIFT);
906 
907 		/* set type of FIFO queues to be rate limited */
908 		if (bw)
909 			val |= 2 << CPSW_FIFO_QUEUE_TYPE_SHIFT;
910 		else
911 			priv->shp_cfg_speed = 0;
912 	}
913 
914 	/* toggle a FIFO rate limited queue */
915 	if (bw)
916 		val |= BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT);
917 	else
918 		val &= ~BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT);
919 	slave_write(slave, val, tx_in_ctl_rg);
920 
921 	/* FIFO transmit shape enable */
922 	cpsw_fifo_shp_on(priv, fifo, bw);
923 	return 0;
924 }
925 
926 /* Defaults:
927  * class A - prio 3
928  * class B - prio 2
929  * shaping for class A should be set first
930  */
cpsw_set_cbs(struct net_device * ndev,struct tc_cbs_qopt_offload * qopt)931 static int cpsw_set_cbs(struct net_device *ndev,
932 			struct tc_cbs_qopt_offload *qopt)
933 {
934 	struct cpsw_priv *priv = netdev_priv(ndev);
935 	struct cpsw_common *cpsw = priv->cpsw;
936 	struct cpsw_slave *slave;
937 	int prev_speed = 0;
938 	int tc, ret, fifo;
939 	u32 bw = 0;
940 
941 	tc = netdev_txq_to_tc(priv->ndev, qopt->queue);
942 
943 	/* enable channels in backward order, as highest FIFOs must be rate
944 	 * limited first and for compliance with CPDMA rate limited channels
945 	 * that also used in bacward order. FIFO0 cannot be rate limited.
946 	 */
947 	fifo = cpsw_tc_to_fifo(tc, ndev->num_tc);
948 	if (!fifo) {
949 		dev_err(priv->dev, "Last tc%d can't be rate limited", tc);
950 		return -EINVAL;
951 	}
952 
953 	/* do nothing, it's disabled anyway */
954 	if (!qopt->enable && !priv->fifo_bw[fifo])
955 		return 0;
956 
957 	/* shapers can be set if link speed is known */
958 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
959 	if (slave->phy && slave->phy->link) {
960 		if (priv->shp_cfg_speed &&
961 		    priv->shp_cfg_speed != slave->phy->speed)
962 			prev_speed = priv->shp_cfg_speed;
963 
964 		priv->shp_cfg_speed = slave->phy->speed;
965 	}
966 
967 	if (!priv->shp_cfg_speed) {
968 		dev_err(priv->dev, "Link speed is not known");
969 		return -1;
970 	}
971 
972 	ret = pm_runtime_resume_and_get(cpsw->dev);
973 	if (ret < 0)
974 		return ret;
975 
976 	bw = qopt->enable ? qopt->idleslope : 0;
977 	ret = cpsw_set_fifo_rlimit(priv, fifo, bw);
978 	if (ret) {
979 		priv->shp_cfg_speed = prev_speed;
980 		prev_speed = 0;
981 	}
982 
983 	if (bw && prev_speed)
984 		dev_warn(priv->dev,
985 			 "Speed was changed, CBS shaper speeds are changed!");
986 
987 	pm_runtime_put_sync(cpsw->dev);
988 	return ret;
989 }
990 
cpsw_set_mqprio(struct net_device * ndev,void * type_data)991 static int cpsw_set_mqprio(struct net_device *ndev, void *type_data)
992 {
993 	struct tc_mqprio_qopt_offload *mqprio = type_data;
994 	struct cpsw_priv *priv = netdev_priv(ndev);
995 	struct cpsw_common *cpsw = priv->cpsw;
996 	int fifo, num_tc, count, offset;
997 	struct cpsw_slave *slave;
998 	u32 tx_prio_map = 0;
999 	int i, tc, ret;
1000 
1001 	num_tc = mqprio->qopt.num_tc;
1002 	if (num_tc > CPSW_TC_NUM)
1003 		return -EINVAL;
1004 
1005 	if (mqprio->mode != TC_MQPRIO_MODE_DCB)
1006 		return -EINVAL;
1007 
1008 	ret = pm_runtime_resume_and_get(cpsw->dev);
1009 	if (ret < 0)
1010 		return ret;
1011 
1012 	if (num_tc) {
1013 		for (i = 0; i < 8; i++) {
1014 			tc = mqprio->qopt.prio_tc_map[i];
1015 			fifo = cpsw_tc_to_fifo(tc, num_tc);
1016 			tx_prio_map |= fifo << (4 * i);
1017 		}
1018 
1019 		netdev_set_num_tc(ndev, num_tc);
1020 		for (i = 0; i < num_tc; i++) {
1021 			count = mqprio->qopt.count[i];
1022 			offset = mqprio->qopt.offset[i];
1023 			netdev_set_tc_queue(ndev, i, count, offset);
1024 		}
1025 	}
1026 
1027 	if (!mqprio->qopt.hw) {
1028 		/* restore default configuration */
1029 		netdev_reset_tc(ndev);
1030 		tx_prio_map = TX_PRIORITY_MAPPING;
1031 	}
1032 
1033 	priv->mqprio_hw = mqprio->qopt.hw;
1034 
1035 	offset = cpsw->version == CPSW_VERSION_1 ?
1036 		 CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;
1037 
1038 	slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1039 	slave_write(slave, tx_prio_map, offset);
1040 
1041 	pm_runtime_put_sync(cpsw->dev);
1042 
1043 	return 0;
1044 }
1045 
1046 static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f);
1047 
cpsw_ndo_setup_tc(struct net_device * ndev,enum tc_setup_type type,void * type_data)1048 int cpsw_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type,
1049 		      void *type_data)
1050 {
1051 	switch (type) {
1052 	case TC_SETUP_QDISC_CBS:
1053 		return cpsw_set_cbs(ndev, type_data);
1054 
1055 	case TC_SETUP_QDISC_MQPRIO:
1056 		return cpsw_set_mqprio(ndev, type_data);
1057 
1058 	case TC_SETUP_BLOCK:
1059 		return cpsw_qos_setup_tc_block(ndev, type_data);
1060 
1061 	default:
1062 		return -EOPNOTSUPP;
1063 	}
1064 }
1065 
cpsw_cbs_resume(struct cpsw_slave * slave,struct cpsw_priv * priv)1066 void cpsw_cbs_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)
1067 {
1068 	int fifo, bw;
1069 
1070 	for (fifo = CPSW_FIFO_SHAPERS_NUM; fifo > 0; fifo--) {
1071 		bw = priv->fifo_bw[fifo];
1072 		if (!bw)
1073 			continue;
1074 
1075 		cpsw_set_fifo_rlimit(priv, fifo, bw);
1076 	}
1077 }
1078 
cpsw_mqprio_resume(struct cpsw_slave * slave,struct cpsw_priv * priv)1079 void cpsw_mqprio_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)
1080 {
1081 	struct cpsw_common *cpsw = priv->cpsw;
1082 	u32 tx_prio_map = 0;
1083 	int i, tc, fifo;
1084 	u32 tx_prio_rg;
1085 
1086 	if (!priv->mqprio_hw)
1087 		return;
1088 
1089 	for (i = 0; i < 8; i++) {
1090 		tc = netdev_get_prio_tc_map(priv->ndev, i);
1091 		fifo = CPSW_FIFO_SHAPERS_NUM - tc;
1092 		tx_prio_map |= fifo << (4 * i);
1093 	}
1094 
1095 	tx_prio_rg = cpsw->version == CPSW_VERSION_1 ?
1096 		     CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;
1097 
1098 	slave_write(slave, tx_prio_map, tx_prio_rg);
1099 }
1100 
cpsw_fill_rx_channels(struct cpsw_priv * priv)1101 int cpsw_fill_rx_channels(struct cpsw_priv *priv)
1102 {
1103 	struct cpsw_common *cpsw = priv->cpsw;
1104 	struct cpsw_meta_xdp *xmeta;
1105 	struct page_pool *pool;
1106 	struct page *page;
1107 	int ch_buf_num;
1108 	int ch, i, ret;
1109 	dma_addr_t dma;
1110 
1111 	for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1112 		pool = cpsw->page_pool[ch];
1113 		ch_buf_num = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);
1114 		for (i = 0; i < ch_buf_num; i++) {
1115 			page = page_pool_dev_alloc_pages(pool);
1116 			if (!page) {
1117 				cpsw_err(priv, ifup, "allocate rx page err\n");
1118 				return -ENOMEM;
1119 			}
1120 
1121 			xmeta = page_address(page) + CPSW_XMETA_OFFSET;
1122 			xmeta->ndev = priv->ndev;
1123 			xmeta->ch = ch;
1124 
1125 			dma = page_pool_get_dma_addr(page) + CPSW_HEADROOM_NA;
1126 			ret = cpdma_chan_idle_submit_mapped(cpsw->rxv[ch].ch,
1127 							    page, dma,
1128 							    cpsw->rx_packet_max,
1129 							    0);
1130 			if (ret < 0) {
1131 				cpsw_err(priv, ifup,
1132 					 "cannot submit page to channel %d rx, error %d\n",
1133 					 ch, ret);
1134 				page_pool_recycle_direct(pool, page);
1135 				return ret;
1136 			}
1137 		}
1138 
1139 		cpsw_info(priv, ifup, "ch %d rx, submitted %d descriptors\n",
1140 			  ch, ch_buf_num);
1141 	}
1142 
1143 	return 0;
1144 }
1145 
cpsw_create_page_pool(struct cpsw_common * cpsw,int size)1146 static struct page_pool *cpsw_create_page_pool(struct cpsw_common *cpsw,
1147 					       int size)
1148 {
1149 	struct page_pool_params pp_params = {};
1150 	struct page_pool *pool;
1151 
1152 	pp_params.order = 0;
1153 	pp_params.flags = PP_FLAG_DMA_MAP;
1154 	pp_params.pool_size = size;
1155 	pp_params.nid = NUMA_NO_NODE;
1156 	pp_params.dma_dir = DMA_BIDIRECTIONAL;
1157 	pp_params.dev = cpsw->dev;
1158 
1159 	pool = page_pool_create(&pp_params);
1160 	if (IS_ERR(pool))
1161 		dev_err(cpsw->dev, "cannot create rx page pool\n");
1162 
1163 	return pool;
1164 }
1165 
cpsw_create_rx_pool(struct cpsw_common * cpsw,int ch)1166 static int cpsw_create_rx_pool(struct cpsw_common *cpsw, int ch)
1167 {
1168 	struct page_pool *pool;
1169 	int ret = 0, pool_size;
1170 
1171 	pool_size = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);
1172 	pool = cpsw_create_page_pool(cpsw, pool_size);
1173 	if (IS_ERR(pool))
1174 		ret = PTR_ERR(pool);
1175 	else
1176 		cpsw->page_pool[ch] = pool;
1177 
1178 	return ret;
1179 }
1180 
cpsw_ndev_create_xdp_rxq(struct cpsw_priv * priv,int ch)1181 static int cpsw_ndev_create_xdp_rxq(struct cpsw_priv *priv, int ch)
1182 {
1183 	struct cpsw_common *cpsw = priv->cpsw;
1184 	struct xdp_rxq_info *rxq;
1185 	struct page_pool *pool;
1186 	int ret;
1187 
1188 	pool = cpsw->page_pool[ch];
1189 	rxq = &priv->xdp_rxq[ch];
1190 
1191 	ret = xdp_rxq_info_reg(rxq, priv->ndev, ch, 0);
1192 	if (ret)
1193 		return ret;
1194 
1195 	ret = xdp_rxq_info_reg_mem_model(rxq, MEM_TYPE_PAGE_POOL, pool);
1196 	if (ret)
1197 		xdp_rxq_info_unreg(rxq);
1198 
1199 	return ret;
1200 }
1201 
cpsw_ndev_destroy_xdp_rxq(struct cpsw_priv * priv,int ch)1202 static void cpsw_ndev_destroy_xdp_rxq(struct cpsw_priv *priv, int ch)
1203 {
1204 	struct xdp_rxq_info *rxq = &priv->xdp_rxq[ch];
1205 
1206 	if (!xdp_rxq_info_is_reg(rxq))
1207 		return;
1208 
1209 	xdp_rxq_info_unreg(rxq);
1210 }
1211 
cpsw_destroy_xdp_rxqs(struct cpsw_common * cpsw)1212 void cpsw_destroy_xdp_rxqs(struct cpsw_common *cpsw)
1213 {
1214 	struct net_device *ndev;
1215 	int i, ch;
1216 
1217 	for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1218 		for (i = 0; i < cpsw->data.slaves; i++) {
1219 			ndev = cpsw->slaves[i].ndev;
1220 			if (!ndev)
1221 				continue;
1222 
1223 			cpsw_ndev_destroy_xdp_rxq(netdev_priv(ndev), ch);
1224 		}
1225 
1226 		page_pool_destroy(cpsw->page_pool[ch]);
1227 		cpsw->page_pool[ch] = NULL;
1228 	}
1229 }
1230 
cpsw_create_xdp_rxqs(struct cpsw_common * cpsw)1231 int cpsw_create_xdp_rxqs(struct cpsw_common *cpsw)
1232 {
1233 	struct net_device *ndev;
1234 	int i, ch, ret;
1235 
1236 	for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1237 		ret = cpsw_create_rx_pool(cpsw, ch);
1238 		if (ret)
1239 			goto err_cleanup;
1240 
1241 		/* using same page pool is allowed as no running rx handlers
1242 		 * simultaneously for both ndevs
1243 		 */
1244 		for (i = 0; i < cpsw->data.slaves; i++) {
1245 			ndev = cpsw->slaves[i].ndev;
1246 			if (!ndev)
1247 				continue;
1248 
1249 			ret = cpsw_ndev_create_xdp_rxq(netdev_priv(ndev), ch);
1250 			if (ret)
1251 				goto err_cleanup;
1252 		}
1253 	}
1254 
1255 	return 0;
1256 
1257 err_cleanup:
1258 	cpsw_destroy_xdp_rxqs(cpsw);
1259 
1260 	return ret;
1261 }
1262 
cpsw_xdp_prog_setup(struct cpsw_priv * priv,struct netdev_bpf * bpf)1263 static int cpsw_xdp_prog_setup(struct cpsw_priv *priv, struct netdev_bpf *bpf)
1264 {
1265 	struct bpf_prog *prog = bpf->prog;
1266 
1267 	if (!priv->xdpi.prog && !prog)
1268 		return 0;
1269 
1270 	WRITE_ONCE(priv->xdp_prog, prog);
1271 
1272 	xdp_attachment_setup(&priv->xdpi, bpf);
1273 
1274 	return 0;
1275 }
1276 
cpsw_ndo_bpf(struct net_device * ndev,struct netdev_bpf * bpf)1277 int cpsw_ndo_bpf(struct net_device *ndev, struct netdev_bpf *bpf)
1278 {
1279 	struct cpsw_priv *priv = netdev_priv(ndev);
1280 
1281 	switch (bpf->command) {
1282 	case XDP_SETUP_PROG:
1283 		return cpsw_xdp_prog_setup(priv, bpf);
1284 
1285 	default:
1286 		return -EINVAL;
1287 	}
1288 }
1289 
cpsw_xdp_tx_frame(struct cpsw_priv * priv,struct xdp_frame * xdpf,struct page * page,int port)1290 int cpsw_xdp_tx_frame(struct cpsw_priv *priv, struct xdp_frame *xdpf,
1291 		      struct page *page, int port)
1292 {
1293 	struct cpsw_common *cpsw = priv->cpsw;
1294 	struct cpsw_meta_xdp *xmeta;
1295 	struct cpdma_chan *txch;
1296 	dma_addr_t dma;
1297 	int ret;
1298 
1299 	xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;
1300 	xmeta->ndev = priv->ndev;
1301 	xmeta->ch = 0;
1302 	txch = cpsw->txv[0].ch;
1303 
1304 	if (page) {
1305 		dma = page_pool_get_dma_addr(page);
1306 		dma += xdpf->headroom + sizeof(struct xdp_frame);
1307 		ret = cpdma_chan_submit_mapped(txch, cpsw_xdpf_to_handle(xdpf),
1308 					       dma, xdpf->len, port);
1309 	} else {
1310 		if (sizeof(*xmeta) > xdpf->headroom)
1311 			return -EINVAL;
1312 
1313 		ret = cpdma_chan_submit(txch, cpsw_xdpf_to_handle(xdpf),
1314 					xdpf->data, xdpf->len, port);
1315 	}
1316 
1317 	if (ret)
1318 		priv->ndev->stats.tx_dropped++;
1319 
1320 	return ret;
1321 }
1322 
cpsw_run_xdp(struct cpsw_priv * priv,int ch,struct xdp_buff * xdp,struct page * page,int port,int * len)1323 int cpsw_run_xdp(struct cpsw_priv *priv, int ch, struct xdp_buff *xdp,
1324 		 struct page *page, int port, int *len)
1325 {
1326 	struct cpsw_common *cpsw = priv->cpsw;
1327 	struct net_device *ndev = priv->ndev;
1328 	int ret = CPSW_XDP_CONSUMED;
1329 	struct xdp_frame *xdpf;
1330 	struct bpf_prog *prog;
1331 	u32 act;
1332 
1333 	prog = READ_ONCE(priv->xdp_prog);
1334 	if (!prog)
1335 		return CPSW_XDP_PASS;
1336 
1337 	act = bpf_prog_run_xdp(prog, xdp);
1338 	/* XDP prog might have changed packet data and boundaries */
1339 	*len = xdp->data_end - xdp->data;
1340 
1341 	switch (act) {
1342 	case XDP_PASS:
1343 		ret = CPSW_XDP_PASS;
1344 		goto out;
1345 	case XDP_TX:
1346 		xdpf = xdp_convert_buff_to_frame(xdp);
1347 		if (unlikely(!xdpf))
1348 			goto drop;
1349 
1350 		if (cpsw_xdp_tx_frame(priv, xdpf, page, port))
1351 			xdp_return_frame_rx_napi(xdpf);
1352 		break;
1353 	case XDP_REDIRECT:
1354 		if (xdp_do_redirect(ndev, xdp, prog))
1355 			goto drop;
1356 
1357 		/*  Have to flush here, per packet, instead of doing it in bulk
1358 		 *  at the end of the napi handler. The RX devices on this
1359 		 *  particular hardware is sharing a common queue, so the
1360 		 *  incoming device might change per packet.
1361 		 */
1362 		xdp_do_flush_map();
1363 		break;
1364 	default:
1365 		bpf_warn_invalid_xdp_action(ndev, prog, act);
1366 		fallthrough;
1367 	case XDP_ABORTED:
1368 		trace_xdp_exception(ndev, prog, act);
1369 		fallthrough;	/* handle aborts by dropping packet */
1370 	case XDP_DROP:
1371 		ndev->stats.rx_bytes += *len;
1372 		ndev->stats.rx_packets++;
1373 		goto drop;
1374 	}
1375 
1376 	ndev->stats.rx_bytes += *len;
1377 	ndev->stats.rx_packets++;
1378 out:
1379 	return ret;
1380 drop:
1381 	page_pool_recycle_direct(cpsw->page_pool[ch], page);
1382 	return ret;
1383 }
1384 
cpsw_qos_clsflower_add_policer(struct cpsw_priv * priv,struct netlink_ext_ack * extack,struct flow_cls_offload * cls,u64 rate_pkt_ps)1385 static int cpsw_qos_clsflower_add_policer(struct cpsw_priv *priv,
1386 					  struct netlink_ext_ack *extack,
1387 					  struct flow_cls_offload *cls,
1388 					  u64 rate_pkt_ps)
1389 {
1390 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
1391 	struct flow_dissector *dissector = rule->match.dissector;
1392 	static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00};
1393 	struct flow_match_eth_addrs match;
1394 	u32 port_id;
1395 	int ret;
1396 
1397 	if (dissector->used_keys &
1398 	    ~(BIT(FLOW_DISSECTOR_KEY_BASIC) |
1399 	      BIT(FLOW_DISSECTOR_KEY_CONTROL) |
1400 	      BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS))) {
1401 		NL_SET_ERR_MSG_MOD(extack,
1402 				   "Unsupported keys used");
1403 		return -EOPNOTSUPP;
1404 	}
1405 
1406 	if (!flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
1407 		NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address");
1408 		return -EOPNOTSUPP;
1409 	}
1410 
1411 	flow_rule_match_eth_addrs(rule, &match);
1412 
1413 	if (!is_zero_ether_addr(match.mask->src)) {
1414 		NL_SET_ERR_MSG_MOD(extack,
1415 				   "Matching on source MAC not supported");
1416 		return -EOPNOTSUPP;
1417 	}
1418 
1419 	port_id = cpsw_slave_index(priv->cpsw, priv) + 1;
1420 
1421 	if (is_broadcast_ether_addr(match.key->dst) &&
1422 	    is_broadcast_ether_addr(match.mask->dst)) {
1423 		ret = cpsw_ale_rx_ratelimit_bc(priv->cpsw->ale, port_id, rate_pkt_ps);
1424 		if (ret)
1425 			return ret;
1426 
1427 		priv->ale_bc_ratelimit.cookie = cls->cookie;
1428 		priv->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps;
1429 	} else if (ether_addr_equal_unaligned(match.key->dst, mc_mac) &&
1430 		   ether_addr_equal_unaligned(match.mask->dst, mc_mac)) {
1431 		ret = cpsw_ale_rx_ratelimit_mc(priv->cpsw->ale, port_id, rate_pkt_ps);
1432 		if (ret)
1433 			return ret;
1434 
1435 		priv->ale_mc_ratelimit.cookie = cls->cookie;
1436 		priv->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps;
1437 	} else {
1438 		NL_SET_ERR_MSG_MOD(extack, "Not supported matching key");
1439 		return -EOPNOTSUPP;
1440 	}
1441 
1442 	return 0;
1443 }
1444 
cpsw_qos_clsflower_policer_validate(const struct flow_action * action,const struct flow_action_entry * act,struct netlink_ext_ack * extack)1445 static int cpsw_qos_clsflower_policer_validate(const struct flow_action *action,
1446 					       const struct flow_action_entry *act,
1447 					       struct netlink_ext_ack *extack)
1448 {
1449 	if (act->police.exceed.act_id != FLOW_ACTION_DROP) {
1450 		NL_SET_ERR_MSG_MOD(extack,
1451 				   "Offload not supported when exceed action is not drop");
1452 		return -EOPNOTSUPP;
1453 	}
1454 
1455 	if (act->police.notexceed.act_id != FLOW_ACTION_PIPE &&
1456 	    act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) {
1457 		NL_SET_ERR_MSG_MOD(extack,
1458 				   "Offload not supported when conform action is not pipe or ok");
1459 		return -EOPNOTSUPP;
1460 	}
1461 
1462 	if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT &&
1463 	    !flow_action_is_last_entry(action, act)) {
1464 		NL_SET_ERR_MSG_MOD(extack,
1465 				   "Offload not supported when conform action is ok, but action is not last");
1466 		return -EOPNOTSUPP;
1467 	}
1468 
1469 	if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps ||
1470 	    act->police.avrate || act->police.overhead) {
1471 		NL_SET_ERR_MSG_MOD(extack,
1472 				   "Offload not supported when bytes per second/peakrate/avrate/overhead is configured");
1473 		return -EOPNOTSUPP;
1474 	}
1475 
1476 	return 0;
1477 }
1478 
cpsw_qos_configure_clsflower(struct cpsw_priv * priv,struct flow_cls_offload * cls)1479 static int cpsw_qos_configure_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls)
1480 {
1481 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
1482 	struct netlink_ext_ack *extack = cls->common.extack;
1483 	const struct flow_action_entry *act;
1484 	int i, ret;
1485 
1486 	flow_action_for_each(i, act, &rule->action) {
1487 		switch (act->id) {
1488 		case FLOW_ACTION_POLICE:
1489 			ret = cpsw_qos_clsflower_policer_validate(&rule->action, act, extack);
1490 			if (ret)
1491 				return ret;
1492 
1493 			return cpsw_qos_clsflower_add_policer(priv, extack, cls,
1494 							      act->police.rate_pkt_ps);
1495 		default:
1496 			NL_SET_ERR_MSG_MOD(extack, "Action not supported");
1497 			return -EOPNOTSUPP;
1498 		}
1499 	}
1500 	return -EOPNOTSUPP;
1501 }
1502 
cpsw_qos_delete_clsflower(struct cpsw_priv * priv,struct flow_cls_offload * cls)1503 static int cpsw_qos_delete_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls)
1504 {
1505 	u32 port_id = cpsw_slave_index(priv->cpsw, priv) + 1;
1506 
1507 	if (cls->cookie == priv->ale_bc_ratelimit.cookie) {
1508 		priv->ale_bc_ratelimit.cookie = 0;
1509 		priv->ale_bc_ratelimit.rate_packet_ps = 0;
1510 		cpsw_ale_rx_ratelimit_bc(priv->cpsw->ale, port_id, 0);
1511 	}
1512 
1513 	if (cls->cookie == priv->ale_mc_ratelimit.cookie) {
1514 		priv->ale_mc_ratelimit.cookie = 0;
1515 		priv->ale_mc_ratelimit.rate_packet_ps = 0;
1516 		cpsw_ale_rx_ratelimit_mc(priv->cpsw->ale, port_id, 0);
1517 	}
1518 
1519 	return 0;
1520 }
1521 
cpsw_qos_setup_tc_clsflower(struct cpsw_priv * priv,struct flow_cls_offload * cls_flower)1522 static int cpsw_qos_setup_tc_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls_flower)
1523 {
1524 	switch (cls_flower->command) {
1525 	case FLOW_CLS_REPLACE:
1526 		return cpsw_qos_configure_clsflower(priv, cls_flower);
1527 	case FLOW_CLS_DESTROY:
1528 		return cpsw_qos_delete_clsflower(priv, cls_flower);
1529 	default:
1530 		return -EOPNOTSUPP;
1531 	}
1532 }
1533 
cpsw_qos_setup_tc_block_cb(enum tc_setup_type type,void * type_data,void * cb_priv)1534 static int cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
1535 {
1536 	struct cpsw_priv *priv = cb_priv;
1537 	int ret;
1538 
1539 	if (!tc_cls_can_offload_and_chain0(priv->ndev, type_data))
1540 		return -EOPNOTSUPP;
1541 
1542 	ret = pm_runtime_get_sync(priv->dev);
1543 	if (ret < 0) {
1544 		pm_runtime_put_noidle(priv->dev);
1545 		return ret;
1546 	}
1547 
1548 	switch (type) {
1549 	case TC_SETUP_CLSFLOWER:
1550 		ret = cpsw_qos_setup_tc_clsflower(priv, type_data);
1551 		break;
1552 	default:
1553 		ret = -EOPNOTSUPP;
1554 	}
1555 
1556 	pm_runtime_put(priv->dev);
1557 	return ret;
1558 }
1559 
1560 static LIST_HEAD(cpsw_qos_block_cb_list);
1561 
cpsw_qos_setup_tc_block(struct net_device * ndev,struct flow_block_offload * f)1562 static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f)
1563 {
1564 	struct cpsw_priv *priv = netdev_priv(ndev);
1565 
1566 	return flow_block_cb_setup_simple(f, &cpsw_qos_block_cb_list,
1567 					  cpsw_qos_setup_tc_block_cb,
1568 					  priv, priv, true);
1569 }
1570 
cpsw_qos_clsflower_resume(struct cpsw_priv * priv)1571 void cpsw_qos_clsflower_resume(struct cpsw_priv *priv)
1572 {
1573 	u32 port_id = cpsw_slave_index(priv->cpsw, priv) + 1;
1574 
1575 	if (priv->ale_bc_ratelimit.cookie)
1576 		cpsw_ale_rx_ratelimit_bc(priv->cpsw->ale, port_id,
1577 					 priv->ale_bc_ratelimit.rate_packet_ps);
1578 
1579 	if (priv->ale_mc_ratelimit.cookie)
1580 		cpsw_ale_rx_ratelimit_mc(priv->cpsw->ale, port_id,
1581 					 priv->ale_mc_ratelimit.rate_packet_ps);
1582 }
1583