1
2
3
4
5
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
33int (*cpsw_slave_index)(struct cpsw_common *cpsw, struct cpsw_priv *priv);
34
35void 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
43void 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
51void 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
75
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
85irqreturn_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
101irqreturn_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
117irqreturn_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
129int 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
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
161int 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
179int 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
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
211int 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
229void 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
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
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
251 if (!vid)
252 return;
253
254
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
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
271void 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
277void 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
289void 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
308static 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
319int cpsw_need_resplit(struct cpsw_common *cpsw)
320{
321 int i, rlim_ch_num;
322 int speed, ch_rate;
323
324
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
340 if (!rlim_ch_num || rlim_ch_num == cpsw->tx_ch_num)
341 return 0;
342
343 return 1;
344}
345
346void 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
374
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
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
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
431int 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
553static 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
577static 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
616static 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
676static 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
694static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
695{
696 return -EOPNOTSUPP;
697}
698
699static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
700{
701 return -EOPNOTSUPP;
702}
703#endif
704
705int 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
732int 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
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
781static 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
789bool 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
805static 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
821static 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
832
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;
876err:
877 dev_err(priv->dev, "Bandwidth doesn't fit in tc configuration");
878 return -EINVAL;
879}
880
881static 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
902 val &= ~(0xf << CPSW_FIFO_RATE_EN_SHIFT);
903
904
905 val &= ~(3 << CPSW_FIFO_QUEUE_TYPE_SHIFT);
906
907
908 if (bw)
909 val |= 2 << CPSW_FIFO_QUEUE_TYPE_SHIFT;
910 else
911 priv->shp_cfg_speed = 0;
912 }
913
914
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
922 cpsw_fifo_shp_on(priv, fifo, bw);
923 return 0;
924}
925
926
927
928
929
930
931static 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
944
945
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
954 if (!qopt->enable && !priv->fifo_bw[fifo])
955 return 0;
956
957
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
991static 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
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
1046static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f);
1047
1048int 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
1066void 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
1079void 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
1101int 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
1146static 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
1166static 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
1181static 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
1202static 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
1212void 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
1231int 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
1242
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
1257err_cleanup:
1258 cpsw_destroy_xdp_rxqs(cpsw);
1259
1260 return ret;
1261}
1262
1263static 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
1277int 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
1290int 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
1323int 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
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
1358
1359
1360
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;
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++;
1378out:
1379 return ret;
1380drop:
1381 page_pool_recycle_direct(cpsw->page_pool[ch], page);
1382 return ret;
1383}
1384
1385static 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
1445static 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
1479static 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
1503static 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
1522static 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
1534static 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
1560static LIST_HEAD(cpsw_qos_block_cb_list);
1561
1562static 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
1571void 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
