1
2
3
4
5
6
7#include <net/pkt_cls.h>
8#include <net/tc_act/tc_gact.h>
9#include "common.h"
10#include "dwmac4.h"
11#include "dwmac5.h"
12#include "stmmac.h"
13
14static void tc_fill_all_pass_entry(struct stmmac_tc_entry *entry)
15{
16 memset(entry, 0, sizeof(*entry));
17 entry->in_use = true;
18 entry->is_last = true;
19 entry->is_frag = false;
20 entry->prio = ~0x0;
21 entry->handle = 0;
22 entry->val.match_data = 0x0;
23 entry->val.match_en = 0x0;
24 entry->val.af = 1;
25 entry->val.dma_ch_no = 0x0;
26}
27
28static struct stmmac_tc_entry *tc_find_entry(struct stmmac_priv *priv,
29 struct tc_cls_u32_offload *cls,
30 bool free)
31{
32 struct stmmac_tc_entry *entry, *first = NULL, *dup = NULL;
33 u32 loc = cls->knode.handle;
34 int i;
35
36 for (i = 0; i < priv->tc_entries_max; i++) {
37 entry = &priv->tc_entries[i];
38 if (!entry->in_use && !first && free)
39 first = entry;
40 if ((entry->handle == loc) && !free && !entry->is_frag)
41 dup = entry;
42 }
43
44 if (dup)
45 return dup;
46 if (first) {
47 first->handle = loc;
48 first->in_use = true;
49
50
51 memset(&first->val, 0, sizeof(first->val));
52 }
53
54 return first;
55}
56
57static int tc_fill_actions(struct stmmac_tc_entry *entry,
58 struct stmmac_tc_entry *frag,
59 struct tc_cls_u32_offload *cls)
60{
61 struct stmmac_tc_entry *action_entry = entry;
62 const struct tc_action *act;
63 struct tcf_exts *exts;
64 int i;
65
66 exts = cls->knode.exts;
67 if (!tcf_exts_has_actions(exts))
68 return -EINVAL;
69 if (frag)
70 action_entry = frag;
71
72 tcf_exts_for_each_action(i, act, exts) {
73
74 if (is_tcf_gact_ok(act)) {
75 action_entry->val.af = 1;
76 break;
77 }
78
79 if (is_tcf_gact_shot(act)) {
80 action_entry->val.rf = 1;
81 break;
82 }
83
84
85 return -EINVAL;
86 }
87
88 return 0;
89}
90
91static int tc_fill_entry(struct stmmac_priv *priv,
92 struct tc_cls_u32_offload *cls)
93{
94 struct stmmac_tc_entry *entry, *frag = NULL;
95 struct tc_u32_sel *sel = cls->knode.sel;
96 u32 off, data, mask, real_off, rem;
97 u32 prio = cls->common.prio << 16;
98 int ret;
99
100
101 if (sel->nkeys <= 0 || sel->nkeys > 1)
102 return -EINVAL;
103
104 off = sel->keys[0].off << sel->offshift;
105 data = sel->keys[0].val;
106 mask = sel->keys[0].mask;
107
108 switch (ntohs(cls->common.protocol)) {
109 case ETH_P_ALL:
110 break;
111 case ETH_P_IP:
112 off += ETH_HLEN;
113 break;
114 default:
115 return -EINVAL;
116 }
117
118 if (off > priv->tc_off_max)
119 return -EINVAL;
120
121 real_off = off / 4;
122 rem = off % 4;
123
124 entry = tc_find_entry(priv, cls, true);
125 if (!entry)
126 return -EINVAL;
127
128 if (rem) {
129 frag = tc_find_entry(priv, cls, true);
130 if (!frag) {
131 ret = -EINVAL;
132 goto err_unuse;
133 }
134
135 entry->frag_ptr = frag;
136 entry->val.match_en = (mask << (rem * 8)) &
137 GENMASK(31, rem * 8);
138 entry->val.match_data = (data << (rem * 8)) &
139 GENMASK(31, rem * 8);
140 entry->val.frame_offset = real_off;
141 entry->prio = prio;
142
143 frag->val.match_en = (mask >> (rem * 8)) &
144 GENMASK(rem * 8 - 1, 0);
145 frag->val.match_data = (data >> (rem * 8)) &
146 GENMASK(rem * 8 - 1, 0);
147 frag->val.frame_offset = real_off + 1;
148 frag->prio = prio;
149 frag->is_frag = true;
150 } else {
151 entry->frag_ptr = NULL;
152 entry->val.match_en = mask;
153 entry->val.match_data = data;
154 entry->val.frame_offset = real_off;
155 entry->prio = prio;
156 }
157
158 ret = tc_fill_actions(entry, frag, cls);
159 if (ret)
160 goto err_unuse;
161
162 return 0;
163
164err_unuse:
165 if (frag)
166 frag->in_use = false;
167 entry->in_use = false;
168 return ret;
169}
170
171static void tc_unfill_entry(struct stmmac_priv *priv,
172 struct tc_cls_u32_offload *cls)
173{
174 struct stmmac_tc_entry *entry;
175
176 entry = tc_find_entry(priv, cls, false);
177 if (!entry)
178 return;
179
180 entry->in_use = false;
181 if (entry->frag_ptr) {
182 entry = entry->frag_ptr;
183 entry->is_frag = false;
184 entry->in_use = false;
185 }
186}
187
188static int tc_config_knode(struct stmmac_priv *priv,
189 struct tc_cls_u32_offload *cls)
190{
191 int ret;
192
193 ret = tc_fill_entry(priv, cls);
194 if (ret)
195 return ret;
196
197 ret = stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries,
198 priv->tc_entries_max);
199 if (ret)
200 goto err_unfill;
201
202 return 0;
203
204err_unfill:
205 tc_unfill_entry(priv, cls);
206 return ret;
207}
208
209static int tc_delete_knode(struct stmmac_priv *priv,
210 struct tc_cls_u32_offload *cls)
211{
212 int ret;
213
214
215 tc_unfill_entry(priv, cls);
216
217 ret = stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries,
218 priv->tc_entries_max);
219 if (ret)
220 return ret;
221
222 return 0;
223}
224
225static int tc_setup_cls_u32(struct stmmac_priv *priv,
226 struct tc_cls_u32_offload *cls)
227{
228 switch (cls->command) {
229 case TC_CLSU32_REPLACE_KNODE:
230 tc_unfill_entry(priv, cls);
231 fallthrough;
232 case TC_CLSU32_NEW_KNODE:
233 return tc_config_knode(priv, cls);
234 case TC_CLSU32_DELETE_KNODE:
235 return tc_delete_knode(priv, cls);
236 default:
237 return -EOPNOTSUPP;
238 }
239}
240
241static int tc_init(struct stmmac_priv *priv)
242{
243 struct dma_features *dma_cap = &priv->dma_cap;
244 unsigned int count;
245 int i;
246
247 if (dma_cap->l3l4fnum) {
248 priv->flow_entries_max = dma_cap->l3l4fnum;
249 priv->flow_entries = devm_kcalloc(priv->device,
250 dma_cap->l3l4fnum,
251 sizeof(*priv->flow_entries),
252 GFP_KERNEL);
253 if (!priv->flow_entries)
254 return -ENOMEM;
255
256 for (i = 0; i < priv->flow_entries_max; i++)
257 priv->flow_entries[i].idx = i;
258
259 dev_info(priv->device, "Enabled Flow TC (entries=%d)\n",
260 priv->flow_entries_max);
261 }
262
263
264 if (!dma_cap->frpsel)
265 return 0;
266
267 switch (dma_cap->frpbs) {
268 case 0x0:
269 priv->tc_off_max = 64;
270 break;
271 case 0x1:
272 priv->tc_off_max = 128;
273 break;
274 case 0x2:
275 priv->tc_off_max = 256;
276 break;
277 default:
278 return -EINVAL;
279 }
280
281 switch (dma_cap->frpes) {
282 case 0x0:
283 count = 64;
284 break;
285 case 0x1:
286 count = 128;
287 break;
288 case 0x2:
289 count = 256;
290 break;
291 default:
292 return -EINVAL;
293 }
294
295
296 priv->tc_entries_max = count;
297 priv->tc_entries = devm_kcalloc(priv->device,
298 count, sizeof(*priv->tc_entries), GFP_KERNEL);
299 if (!priv->tc_entries)
300 return -ENOMEM;
301
302 tc_fill_all_pass_entry(&priv->tc_entries[count - 1]);
303
304 dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n",
305 priv->tc_entries_max, priv->tc_off_max);
306 return 0;
307}
308
309static int tc_setup_cbs(struct stmmac_priv *priv,
310 struct tc_cbs_qopt_offload *qopt)
311{
312 u32 tx_queues_count = priv->plat->tx_queues_to_use;
313 u32 queue = qopt->queue;
314 u32 ptr, speed_div;
315 u32 mode_to_use;
316 u64 value;
317 int ret;
318
319
320 if (queue <= 0 || queue >= tx_queues_count)
321 return -EINVAL;
322 if (!priv->dma_cap.av)
323 return -EOPNOTSUPP;
324
325 mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
326 if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
327 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
328 if (ret)
329 return ret;
330
331 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
332 } else if (!qopt->enable) {
333 return stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_DCB);
334 }
335
336
337 ptr = (priv->speed == SPEED_100) ? 4 : 8;
338 speed_div = (priv->speed == SPEED_100) ? 100000 : 1000000;
339
340
341 value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div);
342 priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
343
344 value = div_s64(-qopt->sendslope * 1024ll * ptr, speed_div);
345 priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
346
347 value = qopt->hicredit * 1024ll * 8;
348 priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
349
350 value = qopt->locredit * 1024ll * 8;
351 priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
352
353 ret = stmmac_config_cbs(priv, priv->hw,
354 priv->plat->tx_queues_cfg[queue].send_slope,
355 priv->plat->tx_queues_cfg[queue].idle_slope,
356 priv->plat->tx_queues_cfg[queue].high_credit,
357 priv->plat->tx_queues_cfg[queue].low_credit,
358 queue);
359 if (ret)
360 return ret;
361
362 dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
363 queue, qopt->sendslope, qopt->idleslope,
364 qopt->hicredit, qopt->locredit);
365 return 0;
366}
367
368static int tc_parse_flow_actions(struct stmmac_priv *priv,
369 struct flow_action *action,
370 struct stmmac_flow_entry *entry,
371 struct netlink_ext_ack *extack)
372{
373 struct flow_action_entry *act;
374 int i;
375
376 if (!flow_action_has_entries(action))
377 return -EINVAL;
378
379 if (!flow_action_basic_hw_stats_check(action, extack))
380 return -EOPNOTSUPP;
381
382 flow_action_for_each(i, act, action) {
383 switch (act->id) {
384 case FLOW_ACTION_DROP:
385 entry->action |= STMMAC_FLOW_ACTION_DROP;
386 return 0;
387 default:
388 break;
389 }
390 }
391
392
393 return 0;
394}
395
396static int tc_add_basic_flow(struct stmmac_priv *priv,
397 struct flow_cls_offload *cls,
398 struct stmmac_flow_entry *entry)
399{
400 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
401 struct flow_dissector *dissector = rule->match.dissector;
402 struct flow_match_basic match;
403
404
405 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
406 return -EINVAL;
407
408 flow_rule_match_basic(rule, &match);
409 entry->ip_proto = match.key->ip_proto;
410 return 0;
411}
412
413static int tc_add_ip4_flow(struct stmmac_priv *priv,
414 struct flow_cls_offload *cls,
415 struct stmmac_flow_entry *entry)
416{
417 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
418 struct flow_dissector *dissector = rule->match.dissector;
419 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
420 struct flow_match_ipv4_addrs match;
421 u32 hw_match;
422 int ret;
423
424
425 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS))
426 return -EINVAL;
427
428 flow_rule_match_ipv4_addrs(rule, &match);
429 hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
430 if (hw_match) {
431 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
432 false, true, inv, hw_match);
433 if (ret)
434 return ret;
435 }
436
437 hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
438 if (hw_match) {
439 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
440 false, false, inv, hw_match);
441 if (ret)
442 return ret;
443 }
444
445 return 0;
446}
447
448static int tc_add_ports_flow(struct stmmac_priv *priv,
449 struct flow_cls_offload *cls,
450 struct stmmac_flow_entry *entry)
451{
452 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
453 struct flow_dissector *dissector = rule->match.dissector;
454 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
455 struct flow_match_ports match;
456 u32 hw_match;
457 bool is_udp;
458 int ret;
459
460
461 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS))
462 return -EINVAL;
463
464 switch (entry->ip_proto) {
465 case IPPROTO_TCP:
466 is_udp = false;
467 break;
468 case IPPROTO_UDP:
469 is_udp = true;
470 break;
471 default:
472 return -EINVAL;
473 }
474
475 flow_rule_match_ports(rule, &match);
476
477 hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
478 if (hw_match) {
479 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
480 is_udp, true, inv, hw_match);
481 if (ret)
482 return ret;
483 }
484
485 hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
486 if (hw_match) {
487 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
488 is_udp, false, inv, hw_match);
489 if (ret)
490 return ret;
491 }
492
493 entry->is_l4 = true;
494 return 0;
495}
496
497static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv,
498 struct flow_cls_offload *cls,
499 bool get_free)
500{
501 int i;
502
503 for (i = 0; i < priv->flow_entries_max; i++) {
504 struct stmmac_flow_entry *entry = &priv->flow_entries[i];
505
506 if (entry->cookie == cls->cookie)
507 return entry;
508 if (get_free && (entry->in_use == false))
509 return entry;
510 }
511
512 return NULL;
513}
514
515static struct {
516 int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls,
517 struct stmmac_flow_entry *entry);
518} tc_flow_parsers[] = {
519 { .fn = tc_add_basic_flow },
520 { .fn = tc_add_ip4_flow },
521 { .fn = tc_add_ports_flow },
522};
523
524static int tc_add_flow(struct stmmac_priv *priv,
525 struct flow_cls_offload *cls)
526{
527 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
528 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
529 int i, ret;
530
531 if (!entry) {
532 entry = tc_find_flow(priv, cls, true);
533 if (!entry)
534 return -ENOENT;
535 }
536
537 ret = tc_parse_flow_actions(priv, &rule->action, entry,
538 cls->common.extack);
539 if (ret)
540 return ret;
541
542 for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
543 ret = tc_flow_parsers[i].fn(priv, cls, entry);
544 if (!ret) {
545 entry->in_use = true;
546 continue;
547 }
548 }
549
550 if (!entry->in_use)
551 return -EINVAL;
552
553 entry->cookie = cls->cookie;
554 return 0;
555}
556
557static int tc_del_flow(struct stmmac_priv *priv,
558 struct flow_cls_offload *cls)
559{
560 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
561 int ret;
562
563 if (!entry || !entry->in_use)
564 return -ENOENT;
565
566 if (entry->is_l4) {
567 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false,
568 false, false, false, 0);
569 } else {
570 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
571 false, false, false, 0);
572 }
573
574 entry->in_use = false;
575 entry->cookie = 0;
576 entry->is_l4 = false;
577 return ret;
578}
579
580static int tc_setup_cls(struct stmmac_priv *priv,
581 struct flow_cls_offload *cls)
582{
583 int ret = 0;
584
585
586 if (priv->rss.enable)
587 return -EBUSY;
588
589 switch (cls->command) {
590 case FLOW_CLS_REPLACE:
591 ret = tc_add_flow(priv, cls);
592 break;
593 case FLOW_CLS_DESTROY:
594 ret = tc_del_flow(priv, cls);
595 break;
596 default:
597 return -EOPNOTSUPP;
598 }
599
600 return ret;
601}
602
603static int tc_setup_taprio(struct stmmac_priv *priv,
604 struct tc_taprio_qopt_offload *qopt)
605{
606 u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
607 struct plat_stmmacenet_data *plat = priv->plat;
608 struct timespec64 time;
609 bool fpe = false;
610 int i, ret = 0;
611 u64 ctr;
612
613 if (!priv->dma_cap.estsel)
614 return -EOPNOTSUPP;
615
616 switch (wid) {
617 case 0x1:
618 wid = 16;
619 break;
620 case 0x2:
621 wid = 20;
622 break;
623 case 0x3:
624 wid = 24;
625 break;
626 default:
627 return -EOPNOTSUPP;
628 }
629
630 switch (dep) {
631 case 0x1:
632 dep = 64;
633 break;
634 case 0x2:
635 dep = 128;
636 break;
637 case 0x3:
638 dep = 256;
639 break;
640 case 0x4:
641 dep = 512;
642 break;
643 case 0x5:
644 dep = 1024;
645 break;
646 default:
647 return -EOPNOTSUPP;
648 }
649
650 if (!qopt->enable)
651 goto disable;
652 if (qopt->num_entries >= dep)
653 return -EINVAL;
654 if (!qopt->base_time)
655 return -ERANGE;
656 if (!qopt->cycle_time)
657 return -ERANGE;
658
659 if (!plat->est) {
660 plat->est = devm_kzalloc(priv->device, sizeof(*plat->est),
661 GFP_KERNEL);
662 if (!plat->est)
663 return -ENOMEM;
664 } else {
665 memset(plat->est, 0, sizeof(*plat->est));
666 }
667
668 size = qopt->num_entries;
669
670 priv->plat->est->gcl_size = size;
671 priv->plat->est->enable = qopt->enable;
672
673 for (i = 0; i < size; i++) {
674 s64 delta_ns = qopt->entries[i].interval;
675 u32 gates = qopt->entries[i].gate_mask;
676
677 if (delta_ns > GENMASK(wid, 0))
678 return -ERANGE;
679 if (gates > GENMASK(31 - wid, 0))
680 return -ERANGE;
681
682 switch (qopt->entries[i].command) {
683 case TC_TAPRIO_CMD_SET_GATES:
684 if (fpe)
685 return -EINVAL;
686 break;
687 case TC_TAPRIO_CMD_SET_AND_HOLD:
688 gates |= BIT(0);
689 fpe = true;
690 break;
691 case TC_TAPRIO_CMD_SET_AND_RELEASE:
692 gates &= ~BIT(0);
693 fpe = true;
694 break;
695 default:
696 return -EOPNOTSUPP;
697 }
698
699 priv->plat->est->gcl[i] = delta_ns | (gates << wid);
700 }
701
702
703 time = ktime_to_timespec64(qopt->base_time);
704 priv->plat->est->btr[0] = (u32)time.tv_nsec;
705 priv->plat->est->btr[1] = (u32)time.tv_sec;
706
707 ctr = qopt->cycle_time;
708 priv->plat->est->ctr[0] = do_div(ctr, NSEC_PER_SEC);
709 priv->plat->est->ctr[1] = (u32)ctr;
710
711 if (fpe && !priv->dma_cap.fpesel)
712 return -EOPNOTSUPP;
713
714 ret = stmmac_fpe_configure(priv, priv->ioaddr,
715 priv->plat->tx_queues_to_use,
716 priv->plat->rx_queues_to_use, fpe);
717 if (ret && fpe) {
718 netdev_err(priv->dev, "failed to enable Frame Preemption\n");
719 return ret;
720 }
721
722 ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
723 priv->plat->clk_ptp_rate);
724 if (ret) {
725 netdev_err(priv->dev, "failed to configure EST\n");
726 goto disable;
727 }
728
729 netdev_info(priv->dev, "configured EST\n");
730 return 0;
731
732disable:
733 priv->plat->est->enable = false;
734 stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
735 priv->plat->clk_ptp_rate);
736 return ret;
737}
738
739static int tc_setup_etf(struct stmmac_priv *priv,
740 struct tc_etf_qopt_offload *qopt)
741{
742 if (!priv->dma_cap.tbssel)
743 return -EOPNOTSUPP;
744 if (qopt->queue >= priv->plat->tx_queues_to_use)
745 return -EINVAL;
746 if (!(priv->tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL))
747 return -EINVAL;
748
749 if (qopt->enable)
750 priv->tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN;
751 else
752 priv->tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN;
753
754 netdev_info(priv->dev, "%s ETF for Queue %d\n",
755 qopt->enable ? "enabled" : "disabled", qopt->queue);
756 return 0;
757}
758
759const struct stmmac_tc_ops dwmac510_tc_ops = {
760 .init = tc_init,
761 .setup_cls_u32 = tc_setup_cls_u32,
762 .setup_cbs = tc_setup_cbs,
763 .setup_cls = tc_setup_cls,
764 .setup_taprio = tc_setup_taprio,
765 .setup_etf = tc_setup_etf,
766};
767
