1// SPDX-License-Identifier: (GPL-2.0 OR MIT)
2/*
3 * Copyright (c) 2018 Synopsys, Inc. and/or its affiliates.
4 * stmmac TC Handling (HW only)
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 /* Reset HW values */
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 /* Accept */
74 if (is_tcf_gact_ok(a: act)) {
75 action_entry->val.af = 1;
76 break;
77 }
78 /* Drop */
79 if (is_tcf_gact_shot(a: act)) {
80 action_entry->val.rf = 1;
81 break;
82 }
83
84 /* Unsupported */
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 /* Only 1 match per entry */
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, free: true);
125 if (!entry)
126 return -EINVAL;
127
128 if (rem) {
129 frag = tc_find_entry(priv, cls, free: 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, free: 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 /* Set entry and fragments as not used */
213 tc_unfill_entry(priv, cls);
214
215 return stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries,
216 priv->tc_entries_max);
217}
218
219static int tc_setup_cls_u32(struct stmmac_priv *priv,
220 struct tc_cls_u32_offload *cls)
221{
222 switch (cls->command) {
223 case TC_CLSU32_REPLACE_KNODE:
224 tc_unfill_entry(priv, cls);
225 fallthrough;
226 case TC_CLSU32_NEW_KNODE:
227 return tc_config_knode(priv, cls);
228 case TC_CLSU32_DELETE_KNODE:
229 return tc_delete_knode(priv, cls);
230 default:
231 return -EOPNOTSUPP;
232 }
233}
234
235static int tc_rfs_init(struct stmmac_priv *priv)
236{
237 int i;
238
239 priv->rfs_entries_max[STMMAC_RFS_T_VLAN] = 8;
240 priv->rfs_entries_max[STMMAC_RFS_T_LLDP] = 1;
241 priv->rfs_entries_max[STMMAC_RFS_T_1588] = 1;
242
243 for (i = 0; i < STMMAC_RFS_T_MAX; i++)
244 priv->rfs_entries_total += priv->rfs_entries_max[i];
245
246 priv->rfs_entries = devm_kcalloc(dev: priv->device,
247 n: priv->rfs_entries_total,
248 size: sizeof(*priv->rfs_entries),
249 GFP_KERNEL);
250 if (!priv->rfs_entries)
251 return -ENOMEM;
252
253 dev_info(priv->device, "Enabled RFS Flow TC (entries=%d)\n",
254 priv->rfs_entries_total);
255
256 return 0;
257}
258
259static int tc_init(struct stmmac_priv *priv)
260{
261 struct dma_features *dma_cap = &priv->dma_cap;
262 unsigned int count;
263 int ret, i;
264
265 if (dma_cap->l3l4fnum) {
266 priv->flow_entries_max = dma_cap->l3l4fnum;
267 priv->flow_entries = devm_kcalloc(dev: priv->device,
268 n: dma_cap->l3l4fnum,
269 size: sizeof(*priv->flow_entries),
270 GFP_KERNEL);
271 if (!priv->flow_entries)
272 return -ENOMEM;
273
274 for (i = 0; i < priv->flow_entries_max; i++)
275 priv->flow_entries[i].idx = i;
276
277 dev_info(priv->device, "Enabled L3L4 Flow TC (entries=%d)\n",
278 priv->flow_entries_max);
279 }
280
281 ret = tc_rfs_init(priv);
282 if (ret)
283 return -ENOMEM;
284
285 if (!priv->plat->fpe_cfg) {
286 priv->plat->fpe_cfg = devm_kzalloc(dev: priv->device,
287 size: sizeof(*priv->plat->fpe_cfg),
288 GFP_KERNEL);
289 if (!priv->plat->fpe_cfg)
290 return -ENOMEM;
291 } else {
292 memset(priv->plat->fpe_cfg, 0, sizeof(*priv->plat->fpe_cfg));
293 }
294
295 /* Fail silently as we can still use remaining features, e.g. CBS */
296 if (!dma_cap->frpsel)
297 return 0;
298
299 switch (dma_cap->frpbs) {
300 case 0x0:
301 priv->tc_off_max = 64;
302 break;
303 case 0x1:
304 priv->tc_off_max = 128;
305 break;
306 case 0x2:
307 priv->tc_off_max = 256;
308 break;
309 default:
310 return -EINVAL;
311 }
312
313 switch (dma_cap->frpes) {
314 case 0x0:
315 count = 64;
316 break;
317 case 0x1:
318 count = 128;
319 break;
320 case 0x2:
321 count = 256;
322 break;
323 default:
324 return -EINVAL;
325 }
326
327 /* Reserve one last filter which lets all pass */
328 priv->tc_entries_max = count;
329 priv->tc_entries = devm_kcalloc(dev: priv->device,
330 n: count, size: sizeof(*priv->tc_entries), GFP_KERNEL);
331 if (!priv->tc_entries)
332 return -ENOMEM;
333
334 tc_fill_all_pass_entry(entry: &priv->tc_entries[count - 1]);
335
336 dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n",
337 priv->tc_entries_max, priv->tc_off_max);
338
339 return 0;
340}
341
342static int tc_setup_cbs(struct stmmac_priv *priv,
343 struct tc_cbs_qopt_offload *qopt)
344{
345 u32 tx_queues_count = priv->plat->tx_queues_to_use;
346 u32 queue = qopt->queue;
347 u32 ptr, speed_div;
348 u32 mode_to_use;
349 u64 value;
350 int ret;
351
352 /* Queue 0 is not AVB capable */
353 if (queue <= 0 || queue >= tx_queues_count)
354 return -EINVAL;
355 if (!priv->dma_cap.av)
356 return -EOPNOTSUPP;
357
358 /* Port Transmit Rate and Speed Divider */
359 switch (priv->speed) {
360 case SPEED_10000:
361 ptr = 32;
362 speed_div = 10000000;
363 break;
364 case SPEED_5000:
365 ptr = 32;
366 speed_div = 5000000;
367 break;
368 case SPEED_2500:
369 ptr = 8;
370 speed_div = 2500000;
371 break;
372 case SPEED_1000:
373 ptr = 8;
374 speed_div = 1000000;
375 break;
376 case SPEED_100:
377 ptr = 4;
378 speed_div = 100000;
379 break;
380 default:
381 return -EOPNOTSUPP;
382 }
383
384 mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
385 if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
386 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
387 if (ret)
388 return ret;
389
390 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
391 } else if (!qopt->enable) {
392 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue,
393 MTL_QUEUE_DCB);
394 if (ret)
395 return ret;
396
397 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB;
398 }
399
400 /* Final adjustments for HW */
401 value = div_s64(dividend: qopt->idleslope * 1024ll * ptr, divisor: speed_div);
402 priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
403
404 value = div_s64(dividend: -qopt->sendslope * 1024ll * ptr, divisor: speed_div);
405 priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
406
407 value = qopt->hicredit * 1024ll * 8;
408 priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
409
410 value = qopt->locredit * 1024ll * 8;
411 priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
412
413 ret = stmmac_config_cbs(priv, priv->hw,
414 priv->plat->tx_queues_cfg[queue].send_slope,
415 priv->plat->tx_queues_cfg[queue].idle_slope,
416 priv->plat->tx_queues_cfg[queue].high_credit,
417 priv->plat->tx_queues_cfg[queue].low_credit,
418 queue);
419 if (ret)
420 return ret;
421
422 dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
423 queue, qopt->sendslope, qopt->idleslope,
424 qopt->hicredit, qopt->locredit);
425 return 0;
426}
427
428static int tc_parse_flow_actions(struct stmmac_priv *priv,
429 struct flow_action *action,
430 struct stmmac_flow_entry *entry,
431 struct netlink_ext_ack *extack)
432{
433 struct flow_action_entry *act;
434 int i;
435
436 if (!flow_action_has_entries(action))
437 return -EINVAL;
438
439 if (!flow_action_basic_hw_stats_check(action, extack))
440 return -EOPNOTSUPP;
441
442 flow_action_for_each(i, act, action) {
443 switch (act->id) {
444 case FLOW_ACTION_DROP:
445 entry->action |= STMMAC_FLOW_ACTION_DROP;
446 return 0;
447 default:
448 break;
449 }
450 }
451
452 /* Nothing to do, maybe inverse filter ? */
453 return 0;
454}
455
456#define ETHER_TYPE_FULL_MASK cpu_to_be16(~0)
457
458static int tc_add_basic_flow(struct stmmac_priv *priv,
459 struct flow_cls_offload *cls,
460 struct stmmac_flow_entry *entry)
461{
462 struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls);
463 struct flow_dissector *dissector = rule->match.dissector;
464 struct flow_match_basic match;
465
466 /* Nothing to do here */
467 if (!dissector_uses_key(flow_dissector: dissector, key_id: FLOW_DISSECTOR_KEY_BASIC))
468 return -EINVAL;
469
470 flow_rule_match_basic(rule, out: &match);
471
472 entry->ip_proto = match.key->ip_proto;
473 return 0;
474}
475
476static int tc_add_ip4_flow(struct stmmac_priv *priv,
477 struct flow_cls_offload *cls,
478 struct stmmac_flow_entry *entry)
479{
480 struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls);
481 struct flow_dissector *dissector = rule->match.dissector;
482 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
483 struct flow_match_ipv4_addrs match;
484 u32 hw_match;
485 int ret;
486
487 /* Nothing to do here */
488 if (!dissector_uses_key(flow_dissector: dissector, key_id: FLOW_DISSECTOR_KEY_IPV4_ADDRS))
489 return -EINVAL;
490
491 flow_rule_match_ipv4_addrs(rule, out: &match);
492 hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
493 if (hw_match) {
494 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
495 false, true, inv, hw_match);
496 if (ret)
497 return ret;
498 }
499
500 hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
501 if (hw_match) {
502 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
503 false, false, inv, hw_match);
504 if (ret)
505 return ret;
506 }
507
508 return 0;
509}
510
511static int tc_add_ports_flow(struct stmmac_priv *priv,
512 struct flow_cls_offload *cls,
513 struct stmmac_flow_entry *entry)
514{
515 struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls);
516 struct flow_dissector *dissector = rule->match.dissector;
517 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
518 struct flow_match_ports match;
519 u32 hw_match;
520 bool is_udp;
521 int ret;
522
523 /* Nothing to do here */
524 if (!dissector_uses_key(flow_dissector: dissector, key_id: FLOW_DISSECTOR_KEY_PORTS))
525 return -EINVAL;
526
527 switch (entry->ip_proto) {
528 case IPPROTO_TCP:
529 is_udp = false;
530 break;
531 case IPPROTO_UDP:
532 is_udp = true;
533 break;
534 default:
535 return -EINVAL;
536 }
537
538 flow_rule_match_ports(rule, out: &match);
539
540 hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
541 if (hw_match) {
542 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
543 is_udp, true, inv, hw_match);
544 if (ret)
545 return ret;
546 }
547
548 hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
549 if (hw_match) {
550 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
551 is_udp, false, inv, hw_match);
552 if (ret)
553 return ret;
554 }
555
556 entry->is_l4 = true;
557 return 0;
558}
559
560static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv,
561 struct flow_cls_offload *cls,
562 bool get_free)
563{
564 int i;
565
566 for (i = 0; i < priv->flow_entries_max; i++) {
567 struct stmmac_flow_entry *entry = &priv->flow_entries[i];
568
569 if (entry->cookie == cls->cookie)
570 return entry;
571 if (get_free && (entry->in_use == false))
572 return entry;
573 }
574
575 return NULL;
576}
577
578static struct {
579 int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls,
580 struct stmmac_flow_entry *entry);
581} tc_flow_parsers[] = {
582 { .fn = tc_add_basic_flow },
583 { .fn = tc_add_ip4_flow },
584 { .fn = tc_add_ports_flow },
585};
586
587static int tc_add_flow(struct stmmac_priv *priv,
588 struct flow_cls_offload *cls)
589{
590 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, get_free: false);
591 struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls);
592 int i, ret;
593
594 if (!entry) {
595 entry = tc_find_flow(priv, cls, get_free: true);
596 if (!entry)
597 return -ENOENT;
598 }
599
600 ret = tc_parse_flow_actions(priv, action: &rule->action, entry,
601 extack: cls->common.extack);
602 if (ret)
603 return ret;
604
605 for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
606 ret = tc_flow_parsers[i].fn(priv, cls, entry);
607 if (!ret)
608 entry->in_use = true;
609 }
610
611 if (!entry->in_use)
612 return -EINVAL;
613
614 entry->cookie = cls->cookie;
615 return 0;
616}
617
618static int tc_del_flow(struct stmmac_priv *priv,
619 struct flow_cls_offload *cls)
620{
621 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, get_free: false);
622 int ret;
623
624 if (!entry || !entry->in_use)
625 return -ENOENT;
626
627 if (entry->is_l4) {
628 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false,
629 false, false, false, 0);
630 } else {
631 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
632 false, false, false, 0);
633 }
634
635 entry->in_use = false;
636 entry->cookie = 0;
637 entry->is_l4 = false;
638 return ret;
639}
640
641static struct stmmac_rfs_entry *tc_find_rfs(struct stmmac_priv *priv,
642 struct flow_cls_offload *cls,
643 bool get_free)
644{
645 int i;
646
647 for (i = 0; i < priv->rfs_entries_total; i++) {
648 struct stmmac_rfs_entry *entry = &priv->rfs_entries[i];
649
650 if (entry->cookie == cls->cookie)
651 return entry;
652 if (get_free && entry->in_use == false)
653 return entry;
654 }
655
656 return NULL;
657}
658
659#define VLAN_PRIO_FULL_MASK (0x07)
660
661static int tc_add_vlan_flow(struct stmmac_priv *priv,
662 struct flow_cls_offload *cls)
663{
664 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, get_free: false);
665 struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls);
666 struct flow_dissector *dissector = rule->match.dissector;
667 int tc = tc_classid_to_hwtc(dev: priv->dev, classid: cls->classid);
668 struct flow_match_vlan match;
669
670 if (!entry) {
671 entry = tc_find_rfs(priv, cls, get_free: true);
672 if (!entry)
673 return -ENOENT;
674 }
675
676 if (priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN] >=
677 priv->rfs_entries_max[STMMAC_RFS_T_VLAN])
678 return -ENOENT;
679
680 /* Nothing to do here */
681 if (!dissector_uses_key(flow_dissector: dissector, key_id: FLOW_DISSECTOR_KEY_VLAN))
682 return -EINVAL;
683
684 if (tc < 0) {
685 netdev_err(dev: priv->dev, format: "Invalid traffic class\n");
686 return -EINVAL;
687 }
688
689 flow_rule_match_vlan(rule, out: &match);
690
691 if (match.mask->vlan_priority) {
692 u32 prio;
693
694 if (match.mask->vlan_priority != VLAN_PRIO_FULL_MASK) {
695 netdev_err(dev: priv->dev, format: "Only full mask is supported for VLAN priority");
696 return -EINVAL;
697 }
698
699 prio = BIT(match.key->vlan_priority);
700 stmmac_rx_queue_prio(priv, priv->hw, prio, tc);
701
702 entry->in_use = true;
703 entry->cookie = cls->cookie;
704 entry->tc = tc;
705 entry->type = STMMAC_RFS_T_VLAN;
706 priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]++;
707 }
708
709 return 0;
710}
711
712static int tc_del_vlan_flow(struct stmmac_priv *priv,
713 struct flow_cls_offload *cls)
714{
715 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, get_free: false);
716
717 if (!entry || !entry->in_use || entry->type != STMMAC_RFS_T_VLAN)
718 return -ENOENT;
719
720 stmmac_rx_queue_prio(priv, priv->hw, 0, entry->tc);
721
722 entry->in_use = false;
723 entry->cookie = 0;
724 entry->tc = 0;
725 entry->type = 0;
726
727 priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]--;
728
729 return 0;
730}
731
732static int tc_add_ethtype_flow(struct stmmac_priv *priv,
733 struct flow_cls_offload *cls)
734{
735 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, get_free: false);
736 struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls);
737 struct flow_dissector *dissector = rule->match.dissector;
738 int tc = tc_classid_to_hwtc(dev: priv->dev, classid: cls->classid);
739 struct flow_match_basic match;
740
741 if (!entry) {
742 entry = tc_find_rfs(priv, cls, get_free: true);
743 if (!entry)
744 return -ENOENT;
745 }
746
747 /* Nothing to do here */
748 if (!dissector_uses_key(flow_dissector: dissector, key_id: FLOW_DISSECTOR_KEY_BASIC))
749 return -EINVAL;
750
751 if (tc < 0) {
752 netdev_err(dev: priv->dev, format: "Invalid traffic class\n");
753 return -EINVAL;
754 }
755
756 flow_rule_match_basic(rule, out: &match);
757
758 if (match.mask->n_proto) {
759 u16 etype = ntohs(match.key->n_proto);
760
761 if (match.mask->n_proto != ETHER_TYPE_FULL_MASK) {
762 netdev_err(dev: priv->dev, format: "Only full mask is supported for EthType filter");
763 return -EINVAL;
764 }
765 switch (etype) {
766 case ETH_P_LLDP:
767 if (priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP] >=
768 priv->rfs_entries_max[STMMAC_RFS_T_LLDP])
769 return -ENOENT;
770
771 entry->type = STMMAC_RFS_T_LLDP;
772 priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP]++;
773
774 stmmac_rx_queue_routing(priv, priv->hw,
775 PACKET_DCBCPQ, tc);
776 break;
777 case ETH_P_1588:
778 if (priv->rfs_entries_cnt[STMMAC_RFS_T_1588] >=
779 priv->rfs_entries_max[STMMAC_RFS_T_1588])
780 return -ENOENT;
781
782 entry->type = STMMAC_RFS_T_1588;
783 priv->rfs_entries_cnt[STMMAC_RFS_T_1588]++;
784
785 stmmac_rx_queue_routing(priv, priv->hw,
786 PACKET_PTPQ, tc);
787 break;
788 default:
789 netdev_err(dev: priv->dev, format: "EthType(0x%x) is not supported", etype);
790 return -EINVAL;
791 }
792
793 entry->in_use = true;
794 entry->cookie = cls->cookie;
795 entry->tc = tc;
796 entry->etype = etype;
797
798 return 0;
799 }
800
801 return -EINVAL;
802}
803
804static int tc_del_ethtype_flow(struct stmmac_priv *priv,
805 struct flow_cls_offload *cls)
806{
807 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, get_free: false);
808
809 if (!entry || !entry->in_use ||
810 entry->type < STMMAC_RFS_T_LLDP ||
811 entry->type > STMMAC_RFS_T_1588)
812 return -ENOENT;
813
814 switch (entry->etype) {
815 case ETH_P_LLDP:
816 stmmac_rx_queue_routing(priv, priv->hw,
817 PACKET_DCBCPQ, 0);
818 priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP]--;
819 break;
820 case ETH_P_1588:
821 stmmac_rx_queue_routing(priv, priv->hw,
822 PACKET_PTPQ, 0);
823 priv->rfs_entries_cnt[STMMAC_RFS_T_1588]--;
824 break;
825 default:
826 netdev_err(dev: priv->dev, format: "EthType(0x%x) is not supported",
827 entry->etype);
828 return -EINVAL;
829 }
830
831 entry->in_use = false;
832 entry->cookie = 0;
833 entry->tc = 0;
834 entry->etype = 0;
835 entry->type = 0;
836
837 return 0;
838}
839
840static int tc_add_flow_cls(struct stmmac_priv *priv,
841 struct flow_cls_offload *cls)
842{
843 int ret;
844
845 ret = tc_add_flow(priv, cls);
846 if (!ret)
847 return ret;
848
849 ret = tc_add_ethtype_flow(priv, cls);
850 if (!ret)
851 return ret;
852
853 return tc_add_vlan_flow(priv, cls);
854}
855
856static int tc_del_flow_cls(struct stmmac_priv *priv,
857 struct flow_cls_offload *cls)
858{
859 int ret;
860
861 ret = tc_del_flow(priv, cls);
862 if (!ret)
863 return ret;
864
865 ret = tc_del_ethtype_flow(priv, cls);
866 if (!ret)
867 return ret;
868
869 return tc_del_vlan_flow(priv, cls);
870}
871
872static int tc_setup_cls(struct stmmac_priv *priv,
873 struct flow_cls_offload *cls)
874{
875 int ret = 0;
876
877 /* When RSS is enabled, the filtering will be bypassed */
878 if (priv->rss.enable)
879 return -EBUSY;
880
881 switch (cls->command) {
882 case FLOW_CLS_REPLACE:
883 ret = tc_add_flow_cls(priv, cls);
884 break;
885 case FLOW_CLS_DESTROY:
886 ret = tc_del_flow_cls(priv, cls);
887 break;
888 default:
889 return -EOPNOTSUPP;
890 }
891
892 return ret;
893}
894
895struct timespec64 stmmac_calc_tas_basetime(ktime_t old_base_time,
896 ktime_t current_time,
897 u64 cycle_time)
898{
899 struct timespec64 time;
900
901 if (ktime_after(cmp1: old_base_time, cmp2: current_time)) {
902 time = ktime_to_timespec64(old_base_time);
903 } else {
904 s64 n;
905 ktime_t base_time;
906
907 n = div64_s64(ktime_sub_ns(current_time, old_base_time),
908 divisor: cycle_time);
909 base_time = ktime_add_ns(old_base_time,
910 (n + 1) * cycle_time);
911
912 time = ktime_to_timespec64(base_time);
913 }
914
915 return time;
916}
917
918static void tc_taprio_map_maxsdu_txq(struct stmmac_priv *priv,
919 struct tc_taprio_qopt_offload *qopt)
920{
921 struct plat_stmmacenet_data *plat = priv->plat;
922 u32 num_tc = qopt->mqprio.qopt.num_tc;
923 u32 offset, count, i, j;
924
925 /* QueueMaxSDU received from the driver corresponds to the Linux traffic
926 * class. Map queueMaxSDU per Linux traffic class to DWMAC Tx queues.
927 */
928 for (i = 0; i < num_tc; i++) {
929 if (!qopt->max_sdu[i])
930 continue;
931
932 offset = qopt->mqprio.qopt.offset[i];
933 count = qopt->mqprio.qopt.count[i];
934
935 for (j = offset; j < offset + count; j++)
936 plat->est->max_sdu[j] = qopt->max_sdu[i] + ETH_HLEN - ETH_TLEN;
937 }
938}
939
940static int tc_taprio_configure(struct stmmac_priv *priv,
941 struct tc_taprio_qopt_offload *qopt)
942{
943 u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
944 struct plat_stmmacenet_data *plat = priv->plat;
945 struct timespec64 time, current_time, qopt_time;
946 ktime_t current_time_ns;
947 bool fpe = false;
948 int i, ret = 0;
949 u64 ctr;
950
951 if (qopt->base_time < 0)
952 return -ERANGE;
953
954 if (!priv->dma_cap.estsel)
955 return -EOPNOTSUPP;
956
957 switch (wid) {
958 case 0x1:
959 wid = 16;
960 break;
961 case 0x2:
962 wid = 20;
963 break;
964 case 0x3:
965 wid = 24;
966 break;
967 default:
968 return -EOPNOTSUPP;
969 }
970
971 switch (dep) {
972 case 0x1:
973 dep = 64;
974 break;
975 case 0x2:
976 dep = 128;
977 break;
978 case 0x3:
979 dep = 256;
980 break;
981 case 0x4:
982 dep = 512;
983 break;
984 case 0x5:
985 dep = 1024;
986 break;
987 default:
988 return -EOPNOTSUPP;
989 }
990
991 if (qopt->cmd == TAPRIO_CMD_DESTROY)
992 goto disable;
993
994 if (qopt->num_entries >= dep)
995 return -EINVAL;
996 if (!qopt->cycle_time)
997 return -ERANGE;
998 if (qopt->cycle_time_extension >= BIT(wid + 7))
999 return -ERANGE;
1000
1001 if (!plat->est) {
1002 plat->est = devm_kzalloc(dev: priv->device, size: sizeof(*plat->est),
1003 GFP_KERNEL);
1004 if (!plat->est)
1005 return -ENOMEM;
1006
1007 mutex_init(&priv->plat->est->lock);
1008 } else {
1009 memset(plat->est, 0, sizeof(*plat->est));
1010 }
1011
1012 size = qopt->num_entries;
1013
1014 mutex_lock(&priv->plat->est->lock);
1015 priv->plat->est->gcl_size = size;
1016 priv->plat->est->enable = qopt->cmd == TAPRIO_CMD_REPLACE;
1017 mutex_unlock(lock: &priv->plat->est->lock);
1018
1019 for (i = 0; i < size; i++) {
1020 s64 delta_ns = qopt->entries[i].interval;
1021 u32 gates = qopt->entries[i].gate_mask;
1022
1023 if (delta_ns > GENMASK(wid, 0))
1024 return -ERANGE;
1025 if (gates > GENMASK(31 - wid, 0))
1026 return -ERANGE;
1027
1028 switch (qopt->entries[i].command) {
1029 case TC_TAPRIO_CMD_SET_GATES:
1030 if (fpe)
1031 return -EINVAL;
1032 break;
1033 case TC_TAPRIO_CMD_SET_AND_HOLD:
1034 gates |= BIT(0);
1035 fpe = true;
1036 break;
1037 case TC_TAPRIO_CMD_SET_AND_RELEASE:
1038 gates &= ~BIT(0);
1039 fpe = true;
1040 break;
1041 default:
1042 return -EOPNOTSUPP;
1043 }
1044
1045 priv->plat->est->gcl[i] = delta_ns | (gates << wid);
1046 }
1047
1048 mutex_lock(&priv->plat->est->lock);
1049 /* Adjust for real system time */
1050 priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, &current_time);
1051 current_time_ns = timespec64_to_ktime(ts: current_time);
1052 time = stmmac_calc_tas_basetime(old_base_time: qopt->base_time, current_time: current_time_ns,
1053 cycle_time: qopt->cycle_time);
1054
1055 priv->plat->est->btr[0] = (u32)time.tv_nsec;
1056 priv->plat->est->btr[1] = (u32)time.tv_sec;
1057
1058 qopt_time = ktime_to_timespec64(qopt->base_time);
1059 priv->plat->est->btr_reserve[0] = (u32)qopt_time.tv_nsec;
1060 priv->plat->est->btr_reserve[1] = (u32)qopt_time.tv_sec;
1061
1062 ctr = qopt->cycle_time;
1063 priv->plat->est->ctr[0] = do_div(ctr, NSEC_PER_SEC);
1064 priv->plat->est->ctr[1] = (u32)ctr;
1065
1066 priv->plat->est->ter = qopt->cycle_time_extension;
1067
1068 tc_taprio_map_maxsdu_txq(priv, qopt);
1069
1070 if (fpe && !priv->dma_cap.fpesel) {
1071 mutex_unlock(lock: &priv->plat->est->lock);
1072 return -EOPNOTSUPP;
1073 }
1074
1075 /* Actual FPE register configuration will be done after FPE handshake
1076 * is success.
1077 */
1078 priv->plat->fpe_cfg->enable = fpe;
1079
1080 ret = stmmac_est_configure(priv, priv, priv->plat->est,
1081 priv->plat->clk_ptp_rate);
1082 mutex_unlock(lock: &priv->plat->est->lock);
1083 if (ret) {
1084 netdev_err(dev: priv->dev, format: "failed to configure EST\n");
1085 goto disable;
1086 }
1087
1088 netdev_info(dev: priv->dev, format: "configured EST\n");
1089
1090 if (fpe) {
1091 stmmac_fpe_handshake(priv, enable: true);
1092 netdev_info(dev: priv->dev, format: "start FPE handshake\n");
1093 }
1094
1095 return 0;
1096
1097disable:
1098 if (priv->plat->est) {
1099 mutex_lock(&priv->plat->est->lock);
1100 priv->plat->est->enable = false;
1101 stmmac_est_configure(priv, priv, priv->plat->est,
1102 priv->plat->clk_ptp_rate);
1103 /* Reset taprio status */
1104 for (i = 0; i < priv->plat->tx_queues_to_use; i++) {
1105 priv->xstats.max_sdu_txq_drop[i] = 0;
1106 priv->xstats.mtl_est_txq_hlbf[i] = 0;
1107 }
1108 mutex_unlock(lock: &priv->plat->est->lock);
1109 }
1110
1111 priv->plat->fpe_cfg->enable = false;
1112 stmmac_fpe_configure(priv, priv->ioaddr,
1113 priv->plat->fpe_cfg,
1114 priv->plat->tx_queues_to_use,
1115 priv->plat->rx_queues_to_use,
1116 false);
1117 netdev_info(dev: priv->dev, format: "disabled FPE\n");
1118
1119 stmmac_fpe_handshake(priv, enable: false);
1120 netdev_info(dev: priv->dev, format: "stop FPE handshake\n");
1121
1122 return ret;
1123}
1124
1125static void tc_taprio_stats(struct stmmac_priv *priv,
1126 struct tc_taprio_qopt_offload *qopt)
1127{
1128 u64 window_drops = 0;
1129 int i = 0;
1130
1131 for (i = 0; i < priv->plat->tx_queues_to_use; i++)
1132 window_drops += priv->xstats.max_sdu_txq_drop[i] +
1133 priv->xstats.mtl_est_txq_hlbf[i];
1134 qopt->stats.window_drops = window_drops;
1135
1136 /* Transmission overrun doesn't happen for stmmac, hence always 0 */
1137 qopt->stats.tx_overruns = 0;
1138}
1139
1140static void tc_taprio_queue_stats(struct stmmac_priv *priv,
1141 struct tc_taprio_qopt_offload *qopt)
1142{
1143 struct tc_taprio_qopt_queue_stats *q_stats = &qopt->queue_stats;
1144 int queue = qopt->queue_stats.queue;
1145
1146 q_stats->stats.window_drops = priv->xstats.max_sdu_txq_drop[queue] +
1147 priv->xstats.mtl_est_txq_hlbf[queue];
1148
1149 /* Transmission overrun doesn't happen for stmmac, hence always 0 */
1150 q_stats->stats.tx_overruns = 0;
1151}
1152
1153static int tc_setup_taprio(struct stmmac_priv *priv,
1154 struct tc_taprio_qopt_offload *qopt)
1155{
1156 int err = 0;
1157
1158 switch (qopt->cmd) {
1159 case TAPRIO_CMD_REPLACE:
1160 case TAPRIO_CMD_DESTROY:
1161 err = tc_taprio_configure(priv, qopt);
1162 break;
1163 case TAPRIO_CMD_STATS:
1164 tc_taprio_stats(priv, qopt);
1165 break;
1166 case TAPRIO_CMD_QUEUE_STATS:
1167 tc_taprio_queue_stats(priv, qopt);
1168 break;
1169 default:
1170 err = -EOPNOTSUPP;
1171 }
1172
1173 return err;
1174}
1175
1176static int tc_setup_etf(struct stmmac_priv *priv,
1177 struct tc_etf_qopt_offload *qopt)
1178{
1179 if (!priv->dma_cap.tbssel)
1180 return -EOPNOTSUPP;
1181 if (qopt->queue >= priv->plat->tx_queues_to_use)
1182 return -EINVAL;
1183 if (!(priv->dma_conf.tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL))
1184 return -EINVAL;
1185
1186 if (qopt->enable)
1187 priv->dma_conf.tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN;
1188 else
1189 priv->dma_conf.tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN;
1190
1191 netdev_info(dev: priv->dev, format: "%s ETF for Queue %d\n",
1192 qopt->enable ? "enabled" : "disabled", qopt->queue);
1193 return 0;
1194}
1195
1196static int tc_query_caps(struct stmmac_priv *priv,
1197 struct tc_query_caps_base *base)
1198{
1199 switch (base->type) {
1200 case TC_SETUP_QDISC_TAPRIO: {
1201 struct tc_taprio_caps *caps = base->caps;
1202
1203 if (!priv->dma_cap.estsel)
1204 return -EOPNOTSUPP;
1205
1206 caps->gate_mask_per_txq = true;
1207 caps->supports_queue_max_sdu = true;
1208
1209 return 0;
1210 }
1211 default:
1212 return -EOPNOTSUPP;
1213 }
1214}
1215
1216const struct stmmac_tc_ops dwmac510_tc_ops = {
1217 .init = tc_init,
1218 .setup_cls_u32 = tc_setup_cls_u32,
1219 .setup_cbs = tc_setup_cbs,
1220 .setup_cls = tc_setup_cls,
1221 .setup_taprio = tc_setup_taprio,
1222 .setup_etf = tc_setup_etf,
1223 .query_caps = tc_query_caps,
1224};
1225

source code of linux/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c