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
34int (*cpsw_slave_index)(struct cpsw_common *cpsw, struct cpsw_priv *priv);
35
36void 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(ctlr: cpsw->dma, enable: true);
42}
43
44void 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(ctlr: cpsw->dma, enable: false);
50}
51
52void 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(handle: token)) {
62 xdpf = cpsw_handle_to_xdpf(handle: 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(dev: ndev, index: ch);
79 if (unlikely(netif_tx_queue_stopped(txq)))
80 netif_tx_wake_queue(dev_queue: txq);
81
82 ndev->stats.tx_packets++;
83 ndev->stats.tx_bytes += len;
84}
85
86irqreturn_t cpsw_tx_interrupt(int irq, void *dev_id)
87{
88 struct cpsw_common *cpsw = dev_id;
89
90 writel(val: 0, addr: &cpsw->wr_regs->tx_en);
91 cpdma_ctlr_eoi(ctlr: cpsw->dma, CPDMA_EOI_TX);
92
93 if (cpsw->quirk_irq) {
94 disable_irq_nosync(irq: cpsw->irqs_table[1]);
95 cpsw->tx_irq_disabled = true;
96 }
97
98 napi_schedule(n: &cpsw->napi_tx);
99 return IRQ_HANDLED;
100}
101
102irqreturn_t cpsw_rx_interrupt(int irq, void *dev_id)
103{
104 struct cpsw_common *cpsw = dev_id;
105
106 writel(val: 0, addr: &cpsw->wr_regs->rx_en);
107 cpdma_ctlr_eoi(ctlr: cpsw->dma, CPDMA_EOI_RX);
108
109 if (cpsw->quirk_irq) {
110 disable_irq_nosync(irq: cpsw->irqs_table[0]);
111 cpsw->rx_irq_disabled = true;
112 }
113
114 napi_schedule(n: &cpsw->napi_rx);
115 return IRQ_HANDLED;
116}
117
118irqreturn_t cpsw_misc_interrupt(int irq, void *dev_id)
119{
120 struct cpsw_common *cpsw = dev_id;
121
122 writel(val: 0, addr: &cpsw->wr_regs->misc_en);
123 cpdma_ctlr_eoi(ctlr: cpsw->dma, CPDMA_EOI_MISC);
124 cpts_misc_interrupt(cpts: cpsw->cpts);
125 writel(val: 0x10, addr: &cpsw->wr_regs->misc_en);
126
127 return IRQ_HANDLED;
128}
129
130int 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(ctlr: 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(chan: txv->ch, quota: cur_budget);
150 if (num_tx >= budget)
151 break;
152 }
153
154 if (num_tx < budget) {
155 napi_complete(n: napi_tx);
156 writel(val: 0xff, addr: &cpsw->wr_regs->tx_en);
157 }
158
159 return num_tx;
160}
161
162int 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(chan: cpsw->txv[0].ch, quota: budget);
168 if (num_tx < budget) {
169 napi_complete(n: napi_tx);
170 writel(val: 0xff, addr: &cpsw->wr_regs->tx_en);
171 if (cpsw->tx_irq_disabled) {
172 cpsw->tx_irq_disabled = false;
173 enable_irq(irq: cpsw->irqs_table[1]);
174 }
175 }
176
177 return num_tx;
178}
179
180int 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(ctlr: 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(chan: rxv->ch, quota: cur_budget);
200 if (num_rx >= budget)
201 break;
202 }
203
204 if (num_rx < budget) {
205 napi_complete_done(n: napi_rx, work_done: num_rx);
206 writel(val: 0xff, addr: &cpsw->wr_regs->rx_en);
207 }
208
209 return num_rx;
210}
211
212int 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(chan: cpsw->rxv[0].ch, quota: budget);
218 if (num_rx < budget) {
219 napi_complete_done(n: napi_rx, work_done: num_rx);
220 writel(val: 0xff, addr: &cpsw->wr_regs->rx_en);
221 if (cpsw->rx_irq_disabled) {
222 cpsw->rx_irq_disabled = false;
223 enable_irq(irq: cpsw->irqs_table[0]);
224 }
225 }
226
227 return num_rx;
228}
229
230void cpsw_rx_vlan_encap(struct sk_buff *skb)
231{
232 struct cpsw_priv *priv = netdev_priv(dev: 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(ale: 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), vlan_tci: 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
272void 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
278void 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
290void cpsw_ndo_tx_timeout(struct net_device *ndev, unsigned int txqueue)
291{
292 struct cpsw_priv *priv = netdev_priv(dev: 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(chan: cpsw->txv[ch].ch);
301 cpdma_chan_start(chan: cpsw->txv[ch].ch);
302 }
303
304 cpsw_intr_enable(cpsw);
305 netif_trans_update(dev: ndev);
306 netif_tx_wake_all_queues(dev: ndev);
307}
308
309static 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
320int 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(ch: 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
347void 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(ch: 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(ch: 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(ch: cpsw->txv[i].ch, weight: 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(ch: cpsw->txv[i].ch, weight: 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
432int 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(addr: &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(device_id: "cpsw", dev, sl_base: regs + sliver_offset);
495 if (IS_ERR(ptr: slave->mac_sl))
496 return PTR_ERR(ptr: 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(params: &ale_params);
509 if (IS_ERR(ptr: cpsw->ale)) {
510 dev_err(dev, "error initializing ale engine\n");
511 return PTR_ERR(ptr: 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(params: &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(node: cpsw->dev->of_node, name: "cpts");
538 if (!cpts_node)
539 cpts_node = cpsw->dev->of_node;
540
541 cpsw->cpts = cpts_create(dev: cpsw->dev, regs: cpts_regs, node: cpts_node,
542 CPTS_N_ETX_TS);
543 if (IS_ERR(ptr: cpsw->cpts)) {
544 ret = PTR_ERR(ptr: cpsw->cpts);
545 cpdma_ctlr_destroy(ctlr: cpsw->dma);
546 }
547 of_node_put(node: cpts_node);
548
549 return ret;
550}
551
552#if IS_ENABLED(CONFIG_TI_CPTS)
553
554static 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, val: 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, val: ts_en, CPSW1_TS_CTL);
575 slave_write(slave, val: seq_id, CPSW1_TS_SEQ_LTYPE);
576}
577
578static 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, val: mtype, CPSW2_TS_SEQ_MTYPE);
612 slave_write(slave, val: 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
617static 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(to: &cfg, from: ifr->ifr_data, n: 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(to: ifr->ifr_data, from: &cfg, n: sizeof(cfg)) ? -EFAULT : 0;
675}
676
677static 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(to: ifr->ifr_data, from: &cfg, n: sizeof(cfg)) ? -EFAULT : 0;
693}
694#else
695static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
696{
697 return -EOPNOTSUPP;
698}
699
700static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
701{
702 return -EOPNOTSUPP;
703}
704#endif /*CONFIG_TI_CPTS*/
705
706int 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(phydev: phy)) {
719 switch (cmd) {
720 case SIOCSHWTSTAMP:
721 return cpsw_hwtstamp_set(dev, ifr: req);
722 case SIOCGHWTSTAMP:
723 return cpsw_hwtstamp_get(dev, ifr: req);
724 }
725 }
726
727 if (phy)
728 return phy_mii_ioctl(phydev: phy, ifr: req, cmd);
729
730 return -EOPNOTSUPP;
731}
732
733int cpsw_ndo_set_tx_maxrate(struct net_device *ndev, int queue, u32 rate)
734{
735 struct cpsw_priv *priv = netdev_priv(dev: 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(dev: ndev, index: 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(ctlr: 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(dev: cpsw->dev);
760 if (ret < 0)
761 return ret;
762
763 ret = cpdma_chan_set_rate(ch: cpsw->txv[queue].ch, rate: ch_rate);
764 pm_runtime_put(dev: 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(dev: slave->ndev, index: queue)->tx_maxrate = rate;
776 }
777
778 cpsw_split_res(cpsw);
779 return ret;
780}
781
782static 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
790bool 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
806static 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
822static 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, val: 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;
877err:
878 dev_err(priv->dev, "Bandwidth doesn't fit in tc configuration");
879 return -EINVAL;
880}
881
882static 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, on: bw);
899
900 val = slave_read(slave, offset: 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, offset: tx_in_ctl_rg);
921
922 /* FIFO transmit shape enable */
923 cpsw_fifo_shp_on(priv, fifo, on: 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 */
932static int cpsw_set_cbs(struct net_device *ndev,
933 struct tc_cbs_qopt_offload *qopt)
934{
935 struct cpsw_priv *priv = netdev_priv(dev: 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(dev: priv->ndev, txq: 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, num_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(dev: 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(dev: cpsw->dev);
989 return ret;
990}
991
992static 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(dev: 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(dev: 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(dev: 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(dev: ndev, tc: i, count, offset);
1025 }
1026 }
1027
1028 if (!mqprio->qopt.hw) {
1029 /* restore default configuration */
1030 netdev_reset_tc(dev: 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, val: tx_prio_map, offset);
1041
1042 pm_runtime_put_sync(dev: cpsw->dev);
1043
1044 return 0;
1045}
1046
1047static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f);
1048
1049int 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, qopt: 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, f: type_data);
1061
1062 default:
1063 return -EOPNOTSUPP;
1064 }
1065}
1066
1067void 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
1080void 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(dev: priv->ndev, prio: 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, val: tx_prio_map, offset: tx_prio_rg);
1100}
1101
1102int 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(chan: 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(chan: cpsw->rxv[ch].ch,
1128 token: page, data: dma,
1129 len: cpsw->rx_packet_max,
1130 directed: 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
1147static 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(params: &pp_params);
1161 if (IS_ERR(ptr: pool))
1162 dev_err(cpsw->dev, "cannot create rx page pool\n");
1163
1164 return pool;
1165}
1166
1167static 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(chan: cpsw->rxv[ch].ch);
1173 pool = cpsw_create_page_pool(cpsw, size: pool_size);
1174 if (IS_ERR(ptr: pool))
1175 ret = PTR_ERR(ptr: pool);
1176 else
1177 cpsw->page_pool[ch] = pool;
1178
1179 return ret;
1180}
1181
1182static 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(xdp_rxq: rxq, dev: priv->ndev, queue_index: ch, napi_id: 0);
1193 if (ret)
1194 return ret;
1195
1196 ret = xdp_rxq_info_reg_mem_model(xdp_rxq: rxq, type: MEM_TYPE_PAGE_POOL, allocator: pool);
1197 if (ret)
1198 xdp_rxq_info_unreg(xdp_rxq: rxq);
1199
1200 return ret;
1201}
1202
1203static 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(xdp_rxq: rxq))
1208 return;
1209
1210 xdp_rxq_info_unreg(xdp_rxq: rxq);
1211}
1212
1213void 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(priv: netdev_priv(dev: ndev), ch);
1225 }
1226
1227 page_pool_destroy(pool: cpsw->page_pool[ch]);
1228 cpsw->page_pool[ch] = NULL;
1229 }
1230}
1231
1232int 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(priv: netdev_priv(dev: ndev), ch);
1251 if (ret)
1252 goto err_cleanup;
1253 }
1254 }
1255
1256 return 0;
1257
1258err_cleanup:
1259 cpsw_destroy_xdp_rxqs(cpsw);
1260
1261 return ret;
1262}
1263
1264static 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(info: &priv->xdpi, bpf);
1274
1275 return 0;
1276}
1277
1278int cpsw_ndo_bpf(struct net_device *ndev, struct netdev_bpf *bpf)
1279{
1280 struct cpsw_priv *priv = netdev_priv(dev: 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
1291int 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(chan: txch, token: cpsw_xdpf_to_handle(xdpf),
1309 data: dma, len: xdpf->len, directed: port);
1310 } else {
1311 if (sizeof(*xmeta) > xdpf->headroom)
1312 return -EINVAL;
1313
1314 ret = cpdma_chan_submit(chan: txch, token: cpsw_xdpf_to_handle(xdpf),
1315 data: xdpf->data, len: xdpf->len, directed: port);
1316 }
1317
1318 if (ret)
1319 priv->ndev->stats.tx_dropped++;
1320
1321 return ret;
1322}
1323
1324int 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(dev: 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();
1364 break;
1365 default:
1366 bpf_warn_invalid_xdp_action(dev: ndev, prog, act);
1367 fallthrough;
1368 case XDP_ABORTED:
1369 trace_xdp_exception(dev: ndev, xdp: 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++;
1379out:
1380 return ret;
1381drop:
1382 page_pool_recycle_direct(pool: cpsw->page_pool[ch], page);
1383 return ret;
1384}
1385
1386static 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(flow_cmd: 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, key: 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, out: &match);
1413
1414 if (!is_zero_ether_addr(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(addr: match.key->dst) &&
1423 is_broadcast_ether_addr(addr: match.mask->dst)) {
1424 ret = cpsw_ale_rx_ratelimit_bc(ale: priv->cpsw->ale, port: port_id, ratelimit_pps: 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(addr1: match.key->dst, addr2: mc_mac) &&
1431 ether_addr_equal_unaligned(addr1: match.mask->dst, addr2: mc_mac)) {
1432 ret = cpsw_ale_rx_ratelimit_mc(ale: priv->cpsw->ale, port: port_id, ratelimit_pps: 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
1446static 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, entry: 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
1480static 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(flow_cmd: 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(action: &rule->action, act, extack);
1491 if (ret)
1492 return ret;
1493
1494 return cpsw_qos_clsflower_add_policer(priv, extack, cls,
1495 rate_pkt_ps: 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
1504static 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(ale: priv->cpsw->ale, port: port_id, ratelimit_pps: 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(ale: priv->cpsw->ale, port: port_id, ratelimit_pps: 0);
1518 }
1519
1520 return 0;
1521}
1522
1523static 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: cls_flower);
1528 case FLOW_CLS_DESTROY:
1529 return cpsw_qos_delete_clsflower(priv, cls: cls_flower);
1530 default:
1531 return -EOPNOTSUPP;
1532 }
1533}
1534
1535static 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(dev: priv->ndev, common: type_data))
1541 return -EOPNOTSUPP;
1542
1543 ret = pm_runtime_get_sync(dev: priv->dev);
1544 if (ret < 0) {
1545 pm_runtime_put_noidle(dev: priv->dev);
1546 return ret;
1547 }
1548
1549 switch (type) {
1550 case TC_SETUP_CLSFLOWER:
1551 ret = cpsw_qos_setup_tc_clsflower(priv, cls_flower: type_data);
1552 break;
1553 default:
1554 ret = -EOPNOTSUPP;
1555 }
1556
1557 pm_runtime_put(dev: priv->dev);
1558 return ret;
1559}
1560
1561static LIST_HEAD(cpsw_qos_block_cb_list);
1562
1563static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f)
1564{
1565 struct cpsw_priv *priv = netdev_priv(dev: ndev);
1566
1567 return flow_block_cb_setup_simple(f, driver_list: &cpsw_qos_block_cb_list,
1568 cb: cpsw_qos_setup_tc_block_cb,
1569 cb_ident: priv, cb_priv: priv, ingress_only: true);
1570}
1571
1572void 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(ale: priv->cpsw->ale, port: port_id,
1578 ratelimit_pps: priv->ale_bc_ratelimit.rate_packet_ps);
1579
1580 if (priv->ale_mc_ratelimit.cookie)
1581 cpsw_ale_rx_ratelimit_mc(ale: priv->cpsw->ale, port: port_id,
1582 ratelimit_pps: priv->ale_mc_ratelimit.rate_packet_ps);
1583}
1584

source code of linux/drivers/net/ethernet/ti/cpsw_priv.c