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
tc_fill_all_pass_entry(struct stmmac_tc_entry * entry)14 static 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
tc_find_entry(struct stmmac_priv * priv,struct tc_cls_u32_offload * cls,bool free)28 static 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
tc_fill_actions(struct stmmac_tc_entry * entry,struct stmmac_tc_entry * frag,struct tc_cls_u32_offload * cls)57 static 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(act)) {
75 action_entry->val.af = 1;
76 break;
77 }
78 /* Drop */
79 if (is_tcf_gact_shot(act)) {
80 action_entry->val.rf = 1;
81 break;
82 }
83
84 /* Unsupported */
85 return -EINVAL;
86 }
87
88 return 0;
89 }
90
tc_fill_entry(struct stmmac_priv * priv,struct tc_cls_u32_offload * cls)91 static 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, 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
164 err_unuse:
165 if (frag)
166 frag->in_use = false;
167 entry->in_use = false;
168 return ret;
169 }
170
tc_unfill_entry(struct stmmac_priv * priv,struct tc_cls_u32_offload * cls)171 static 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
tc_config_knode(struct stmmac_priv * priv,struct tc_cls_u32_offload * cls)188 static 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
204 err_unfill:
205 tc_unfill_entry(priv, cls);
206 return ret;
207 }
208
tc_delete_knode(struct stmmac_priv * priv,struct tc_cls_u32_offload * cls)209 static 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
tc_setup_cls_u32(struct stmmac_priv * priv,struct tc_cls_u32_offload * cls)219 static 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
tc_rfs_init(struct stmmac_priv * priv)235 static int tc_rfs_init(struct stmmac_priv *priv)
236 {
237 int i;
238
239 priv->rfs_entries_max[STMMAC_RFS_T_VLAN] = 8;
240
241 for (i = 0; i < STMMAC_RFS_T_MAX; i++)
242 priv->rfs_entries_total += priv->rfs_entries_max[i];
243
244 priv->rfs_entries = devm_kcalloc(priv->device,
245 priv->rfs_entries_total,
246 sizeof(*priv->rfs_entries),
247 GFP_KERNEL);
248 if (!priv->rfs_entries)
249 return -ENOMEM;
250
251 dev_info(priv->device, "Enabled RFS Flow TC (entries=%d)\n",
252 priv->rfs_entries_total);
253
254 return 0;
255 }
256
tc_init(struct stmmac_priv * priv)257 static int tc_init(struct stmmac_priv *priv)
258 {
259 struct dma_features *dma_cap = &priv->dma_cap;
260 unsigned int count;
261 int ret, i;
262
263 if (dma_cap->l3l4fnum) {
264 priv->flow_entries_max = dma_cap->l3l4fnum;
265 priv->flow_entries = devm_kcalloc(priv->device,
266 dma_cap->l3l4fnum,
267 sizeof(*priv->flow_entries),
268 GFP_KERNEL);
269 if (!priv->flow_entries)
270 return -ENOMEM;
271
272 for (i = 0; i < priv->flow_entries_max; i++)
273 priv->flow_entries[i].idx = i;
274
275 dev_info(priv->device, "Enabled L3L4 Flow TC (entries=%d)\n",
276 priv->flow_entries_max);
277 }
278
279 ret = tc_rfs_init(priv);
280 if (ret)
281 return -ENOMEM;
282
283 if (!priv->plat->fpe_cfg) {
284 priv->plat->fpe_cfg = devm_kzalloc(priv->device,
285 sizeof(*priv->plat->fpe_cfg),
286 GFP_KERNEL);
287 if (!priv->plat->fpe_cfg)
288 return -ENOMEM;
289 } else {
290 memset(priv->plat->fpe_cfg, 0, sizeof(*priv->plat->fpe_cfg));
291 }
292
293 /* Fail silently as we can still use remaining features, e.g. CBS */
294 if (!dma_cap->frpsel)
295 return 0;
296
297 switch (dma_cap->frpbs) {
298 case 0x0:
299 priv->tc_off_max = 64;
300 break;
301 case 0x1:
302 priv->tc_off_max = 128;
303 break;
304 case 0x2:
305 priv->tc_off_max = 256;
306 break;
307 default:
308 return -EINVAL;
309 }
310
311 switch (dma_cap->frpes) {
312 case 0x0:
313 count = 64;
314 break;
315 case 0x1:
316 count = 128;
317 break;
318 case 0x2:
319 count = 256;
320 break;
321 default:
322 return -EINVAL;
323 }
324
325 /* Reserve one last filter which lets all pass */
326 priv->tc_entries_max = count;
327 priv->tc_entries = devm_kcalloc(priv->device,
328 count, sizeof(*priv->tc_entries), GFP_KERNEL);
329 if (!priv->tc_entries)
330 return -ENOMEM;
331
332 tc_fill_all_pass_entry(&priv->tc_entries[count - 1]);
333
334 dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n",
335 priv->tc_entries_max, priv->tc_off_max);
336
337 return 0;
338 }
339
tc_setup_cbs(struct stmmac_priv * priv,struct tc_cbs_qopt_offload * qopt)340 static int tc_setup_cbs(struct stmmac_priv *priv,
341 struct tc_cbs_qopt_offload *qopt)
342 {
343 u32 tx_queues_count = priv->plat->tx_queues_to_use;
344 u32 queue = qopt->queue;
345 u32 ptr, speed_div;
346 u32 mode_to_use;
347 u64 value;
348 int ret;
349
350 /* Queue 0 is not AVB capable */
351 if (queue <= 0 || queue >= tx_queues_count)
352 return -EINVAL;
353 if (!priv->dma_cap.av)
354 return -EOPNOTSUPP;
355
356 /* Port Transmit Rate and Speed Divider */
357 switch (priv->speed) {
358 case SPEED_10000:
359 ptr = 32;
360 speed_div = 10000000;
361 break;
362 case SPEED_5000:
363 ptr = 32;
364 speed_div = 5000000;
365 break;
366 case SPEED_2500:
367 ptr = 8;
368 speed_div = 2500000;
369 break;
370 case SPEED_1000:
371 ptr = 8;
372 speed_div = 1000000;
373 break;
374 case SPEED_100:
375 ptr = 4;
376 speed_div = 100000;
377 break;
378 default:
379 return -EOPNOTSUPP;
380 }
381
382 mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
383 if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
384 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
385 if (ret)
386 return ret;
387
388 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
389 } else if (!qopt->enable) {
390 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue,
391 MTL_QUEUE_DCB);
392 if (ret)
393 return ret;
394
395 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB;
396 }
397
398 /* Final adjustments for HW */
399 value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div);
400 priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
401
402 value = div_s64(-qopt->sendslope * 1024ll * ptr, speed_div);
403 priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
404
405 value = qopt->hicredit * 1024ll * 8;
406 priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
407
408 value = qopt->locredit * 1024ll * 8;
409 priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
410
411 ret = stmmac_config_cbs(priv, priv->hw,
412 priv->plat->tx_queues_cfg[queue].send_slope,
413 priv->plat->tx_queues_cfg[queue].idle_slope,
414 priv->plat->tx_queues_cfg[queue].high_credit,
415 priv->plat->tx_queues_cfg[queue].low_credit,
416 queue);
417 if (ret)
418 return ret;
419
420 dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
421 queue, qopt->sendslope, qopt->idleslope,
422 qopt->hicredit, qopt->locredit);
423 return 0;
424 }
425
tc_parse_flow_actions(struct stmmac_priv * priv,struct flow_action * action,struct stmmac_flow_entry * entry,struct netlink_ext_ack * extack)426 static int tc_parse_flow_actions(struct stmmac_priv *priv,
427 struct flow_action *action,
428 struct stmmac_flow_entry *entry,
429 struct netlink_ext_ack *extack)
430 {
431 struct flow_action_entry *act;
432 int i;
433
434 if (!flow_action_has_entries(action))
435 return -EINVAL;
436
437 if (!flow_action_basic_hw_stats_check(action, extack))
438 return -EOPNOTSUPP;
439
440 flow_action_for_each(i, act, action) {
441 switch (act->id) {
442 case FLOW_ACTION_DROP:
443 entry->action |= STMMAC_FLOW_ACTION_DROP;
444 return 0;
445 default:
446 break;
447 }
448 }
449
450 /* Nothing to do, maybe inverse filter ? */
451 return 0;
452 }
453
tc_add_basic_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls,struct stmmac_flow_entry * entry)454 static int tc_add_basic_flow(struct stmmac_priv *priv,
455 struct flow_cls_offload *cls,
456 struct stmmac_flow_entry *entry)
457 {
458 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
459 struct flow_dissector *dissector = rule->match.dissector;
460 struct flow_match_basic match;
461
462 /* Nothing to do here */
463 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
464 return -EINVAL;
465
466 flow_rule_match_basic(rule, &match);
467 entry->ip_proto = match.key->ip_proto;
468 return 0;
469 }
470
tc_add_ip4_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls,struct stmmac_flow_entry * entry)471 static int tc_add_ip4_flow(struct stmmac_priv *priv,
472 struct flow_cls_offload *cls,
473 struct stmmac_flow_entry *entry)
474 {
475 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
476 struct flow_dissector *dissector = rule->match.dissector;
477 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
478 struct flow_match_ipv4_addrs match;
479 u32 hw_match;
480 int ret;
481
482 /* Nothing to do here */
483 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS))
484 return -EINVAL;
485
486 flow_rule_match_ipv4_addrs(rule, &match);
487 hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
488 if (hw_match) {
489 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
490 false, true, inv, hw_match);
491 if (ret)
492 return ret;
493 }
494
495 hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
496 if (hw_match) {
497 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
498 false, false, inv, hw_match);
499 if (ret)
500 return ret;
501 }
502
503 return 0;
504 }
505
tc_add_ports_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls,struct stmmac_flow_entry * entry)506 static int tc_add_ports_flow(struct stmmac_priv *priv,
507 struct flow_cls_offload *cls,
508 struct stmmac_flow_entry *entry)
509 {
510 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
511 struct flow_dissector *dissector = rule->match.dissector;
512 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
513 struct flow_match_ports match;
514 u32 hw_match;
515 bool is_udp;
516 int ret;
517
518 /* Nothing to do here */
519 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS))
520 return -EINVAL;
521
522 switch (entry->ip_proto) {
523 case IPPROTO_TCP:
524 is_udp = false;
525 break;
526 case IPPROTO_UDP:
527 is_udp = true;
528 break;
529 default:
530 return -EINVAL;
531 }
532
533 flow_rule_match_ports(rule, &match);
534
535 hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
536 if (hw_match) {
537 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
538 is_udp, true, inv, hw_match);
539 if (ret)
540 return ret;
541 }
542
543 hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
544 if (hw_match) {
545 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
546 is_udp, false, inv, hw_match);
547 if (ret)
548 return ret;
549 }
550
551 entry->is_l4 = true;
552 return 0;
553 }
554
tc_find_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls,bool get_free)555 static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv,
556 struct flow_cls_offload *cls,
557 bool get_free)
558 {
559 int i;
560
561 for (i = 0; i < priv->flow_entries_max; i++) {
562 struct stmmac_flow_entry *entry = &priv->flow_entries[i];
563
564 if (entry->cookie == cls->cookie)
565 return entry;
566 if (get_free && (entry->in_use == false))
567 return entry;
568 }
569
570 return NULL;
571 }
572
573 static struct {
574 int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls,
575 struct stmmac_flow_entry *entry);
576 } tc_flow_parsers[] = {
577 { .fn = tc_add_basic_flow },
578 { .fn = tc_add_ip4_flow },
579 { .fn = tc_add_ports_flow },
580 };
581
tc_add_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)582 static int tc_add_flow(struct stmmac_priv *priv,
583 struct flow_cls_offload *cls)
584 {
585 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
586 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
587 int i, ret;
588
589 if (!entry) {
590 entry = tc_find_flow(priv, cls, true);
591 if (!entry)
592 return -ENOENT;
593 }
594
595 ret = tc_parse_flow_actions(priv, &rule->action, entry,
596 cls->common.extack);
597 if (ret)
598 return ret;
599
600 for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
601 ret = tc_flow_parsers[i].fn(priv, cls, entry);
602 if (!ret)
603 entry->in_use = true;
604 }
605
606 if (!entry->in_use)
607 return -EINVAL;
608
609 entry->cookie = cls->cookie;
610 return 0;
611 }
612
tc_del_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)613 static int tc_del_flow(struct stmmac_priv *priv,
614 struct flow_cls_offload *cls)
615 {
616 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
617 int ret;
618
619 if (!entry || !entry->in_use)
620 return -ENOENT;
621
622 if (entry->is_l4) {
623 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false,
624 false, false, false, 0);
625 } else {
626 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
627 false, false, false, 0);
628 }
629
630 entry->in_use = false;
631 entry->cookie = 0;
632 entry->is_l4 = false;
633 return ret;
634 }
635
tc_find_rfs(struct stmmac_priv * priv,struct flow_cls_offload * cls,bool get_free)636 static struct stmmac_rfs_entry *tc_find_rfs(struct stmmac_priv *priv,
637 struct flow_cls_offload *cls,
638 bool get_free)
639 {
640 int i;
641
642 for (i = 0; i < priv->rfs_entries_total; i++) {
643 struct stmmac_rfs_entry *entry = &priv->rfs_entries[i];
644
645 if (entry->cookie == cls->cookie)
646 return entry;
647 if (get_free && entry->in_use == false)
648 return entry;
649 }
650
651 return NULL;
652 }
653
654 #define VLAN_PRIO_FULL_MASK (0x07)
655
tc_add_vlan_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)656 static int tc_add_vlan_flow(struct stmmac_priv *priv,
657 struct flow_cls_offload *cls)
658 {
659 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
660 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
661 struct flow_dissector *dissector = rule->match.dissector;
662 int tc = tc_classid_to_hwtc(priv->dev, cls->classid);
663 struct flow_match_vlan match;
664
665 if (!entry) {
666 entry = tc_find_rfs(priv, cls, true);
667 if (!entry)
668 return -ENOENT;
669 }
670
671 if (priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN] >=
672 priv->rfs_entries_max[STMMAC_RFS_T_VLAN])
673 return -ENOENT;
674
675 /* Nothing to do here */
676 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_VLAN))
677 return -EINVAL;
678
679 if (tc < 0) {
680 netdev_err(priv->dev, "Invalid traffic class\n");
681 return -EINVAL;
682 }
683
684 flow_rule_match_vlan(rule, &match);
685
686 if (match.mask->vlan_priority) {
687 u32 prio;
688
689 if (match.mask->vlan_priority != VLAN_PRIO_FULL_MASK) {
690 netdev_err(priv->dev, "Only full mask is supported for VLAN priority");
691 return -EINVAL;
692 }
693
694 prio = BIT(match.key->vlan_priority);
695 stmmac_rx_queue_prio(priv, priv->hw, prio, tc);
696
697 entry->in_use = true;
698 entry->cookie = cls->cookie;
699 entry->tc = tc;
700 entry->type = STMMAC_RFS_T_VLAN;
701 priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]++;
702 }
703
704 return 0;
705 }
706
tc_del_vlan_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)707 static int tc_del_vlan_flow(struct stmmac_priv *priv,
708 struct flow_cls_offload *cls)
709 {
710 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
711
712 if (!entry || !entry->in_use || entry->type != STMMAC_RFS_T_VLAN)
713 return -ENOENT;
714
715 stmmac_rx_queue_prio(priv, priv->hw, 0, entry->tc);
716
717 entry->in_use = false;
718 entry->cookie = 0;
719 entry->tc = 0;
720 entry->type = 0;
721
722 priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]--;
723
724 return 0;
725 }
726
tc_add_flow_cls(struct stmmac_priv * priv,struct flow_cls_offload * cls)727 static int tc_add_flow_cls(struct stmmac_priv *priv,
728 struct flow_cls_offload *cls)
729 {
730 int ret;
731
732 ret = tc_add_flow(priv, cls);
733 if (!ret)
734 return ret;
735
736 return tc_add_vlan_flow(priv, cls);
737 }
738
tc_del_flow_cls(struct stmmac_priv * priv,struct flow_cls_offload * cls)739 static int tc_del_flow_cls(struct stmmac_priv *priv,
740 struct flow_cls_offload *cls)
741 {
742 int ret;
743
744 ret = tc_del_flow(priv, cls);
745 if (!ret)
746 return ret;
747
748 return tc_del_vlan_flow(priv, cls);
749 }
750
tc_setup_cls(struct stmmac_priv * priv,struct flow_cls_offload * cls)751 static int tc_setup_cls(struct stmmac_priv *priv,
752 struct flow_cls_offload *cls)
753 {
754 int ret = 0;
755
756 /* When RSS is enabled, the filtering will be bypassed */
757 if (priv->rss.enable)
758 return -EBUSY;
759
760 switch (cls->command) {
761 case FLOW_CLS_REPLACE:
762 ret = tc_add_flow_cls(priv, cls);
763 break;
764 case FLOW_CLS_DESTROY:
765 ret = tc_del_flow_cls(priv, cls);
766 break;
767 default:
768 return -EOPNOTSUPP;
769 }
770
771 return ret;
772 }
773
stmmac_calc_tas_basetime(ktime_t old_base_time,ktime_t current_time,u64 cycle_time)774 struct timespec64 stmmac_calc_tas_basetime(ktime_t old_base_time,
775 ktime_t current_time,
776 u64 cycle_time)
777 {
778 struct timespec64 time;
779
780 if (ktime_after(old_base_time, current_time)) {
781 time = ktime_to_timespec64(old_base_time);
782 } else {
783 s64 n;
784 ktime_t base_time;
785
786 n = div64_s64(ktime_sub_ns(current_time, old_base_time),
787 cycle_time);
788 base_time = ktime_add_ns(old_base_time,
789 (n + 1) * cycle_time);
790
791 time = ktime_to_timespec64(base_time);
792 }
793
794 return time;
795 }
796
tc_setup_taprio(struct stmmac_priv * priv,struct tc_taprio_qopt_offload * qopt)797 static int tc_setup_taprio(struct stmmac_priv *priv,
798 struct tc_taprio_qopt_offload *qopt)
799 {
800 u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
801 struct plat_stmmacenet_data *plat = priv->plat;
802 struct timespec64 time, current_time, qopt_time;
803 ktime_t current_time_ns;
804 bool fpe = false;
805 int i, ret = 0;
806 u64 ctr;
807
808 if (!priv->dma_cap.estsel)
809 return -EOPNOTSUPP;
810
811 switch (wid) {
812 case 0x1:
813 wid = 16;
814 break;
815 case 0x2:
816 wid = 20;
817 break;
818 case 0x3:
819 wid = 24;
820 break;
821 default:
822 return -EOPNOTSUPP;
823 }
824
825 switch (dep) {
826 case 0x1:
827 dep = 64;
828 break;
829 case 0x2:
830 dep = 128;
831 break;
832 case 0x3:
833 dep = 256;
834 break;
835 case 0x4:
836 dep = 512;
837 break;
838 case 0x5:
839 dep = 1024;
840 break;
841 default:
842 return -EOPNOTSUPP;
843 }
844
845 if (!qopt->enable)
846 goto disable;
847 if (qopt->num_entries >= dep)
848 return -EINVAL;
849 if (!qopt->cycle_time)
850 return -ERANGE;
851
852 if (!plat->est) {
853 plat->est = devm_kzalloc(priv->device, sizeof(*plat->est),
854 GFP_KERNEL);
855 if (!plat->est)
856 return -ENOMEM;
857
858 mutex_init(&priv->plat->est->lock);
859 } else {
860 memset(plat->est, 0, sizeof(*plat->est));
861 }
862
863 size = qopt->num_entries;
864
865 mutex_lock(&priv->plat->est->lock);
866 priv->plat->est->gcl_size = size;
867 priv->plat->est->enable = qopt->enable;
868 mutex_unlock(&priv->plat->est->lock);
869
870 for (i = 0; i < size; i++) {
871 s64 delta_ns = qopt->entries[i].interval;
872 u32 gates = qopt->entries[i].gate_mask;
873
874 if (delta_ns > GENMASK(wid, 0))
875 return -ERANGE;
876 if (gates > GENMASK(31 - wid, 0))
877 return -ERANGE;
878
879 switch (qopt->entries[i].command) {
880 case TC_TAPRIO_CMD_SET_GATES:
881 if (fpe)
882 return -EINVAL;
883 break;
884 case TC_TAPRIO_CMD_SET_AND_HOLD:
885 gates |= BIT(0);
886 fpe = true;
887 break;
888 case TC_TAPRIO_CMD_SET_AND_RELEASE:
889 gates &= ~BIT(0);
890 fpe = true;
891 break;
892 default:
893 return -EOPNOTSUPP;
894 }
895
896 priv->plat->est->gcl[i] = delta_ns | (gates << wid);
897 }
898
899 mutex_lock(&priv->plat->est->lock);
900 /* Adjust for real system time */
901 priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, ¤t_time);
902 current_time_ns = timespec64_to_ktime(current_time);
903 time = stmmac_calc_tas_basetime(qopt->base_time, current_time_ns,
904 qopt->cycle_time);
905
906 priv->plat->est->btr[0] = (u32)time.tv_nsec;
907 priv->plat->est->btr[1] = (u32)time.tv_sec;
908
909 qopt_time = ktime_to_timespec64(qopt->base_time);
910 priv->plat->est->btr_reserve[0] = (u32)qopt_time.tv_nsec;
911 priv->plat->est->btr_reserve[1] = (u32)qopt_time.tv_sec;
912
913 ctr = qopt->cycle_time;
914 priv->plat->est->ctr[0] = do_div(ctr, NSEC_PER_SEC);
915 priv->plat->est->ctr[1] = (u32)ctr;
916
917 if (fpe && !priv->dma_cap.fpesel) {
918 mutex_unlock(&priv->plat->est->lock);
919 return -EOPNOTSUPP;
920 }
921
922 /* Actual FPE register configuration will be done after FPE handshake
923 * is success.
924 */
925 priv->plat->fpe_cfg->enable = fpe;
926
927 ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
928 priv->plat->clk_ptp_rate);
929 mutex_unlock(&priv->plat->est->lock);
930 if (ret) {
931 netdev_err(priv->dev, "failed to configure EST\n");
932 goto disable;
933 }
934
935 netdev_info(priv->dev, "configured EST\n");
936
937 if (fpe) {
938 stmmac_fpe_handshake(priv, true);
939 netdev_info(priv->dev, "start FPE handshake\n");
940 }
941
942 return 0;
943
944 disable:
945 if (priv->plat->est) {
946 mutex_lock(&priv->plat->est->lock);
947 priv->plat->est->enable = false;
948 stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
949 priv->plat->clk_ptp_rate);
950 mutex_unlock(&priv->plat->est->lock);
951 }
952
953 priv->plat->fpe_cfg->enable = false;
954 stmmac_fpe_configure(priv, priv->ioaddr,
955 priv->plat->tx_queues_to_use,
956 priv->plat->rx_queues_to_use,
957 false);
958 netdev_info(priv->dev, "disabled FPE\n");
959
960 stmmac_fpe_handshake(priv, false);
961 netdev_info(priv->dev, "stop FPE handshake\n");
962
963 return ret;
964 }
965
tc_setup_etf(struct stmmac_priv * priv,struct tc_etf_qopt_offload * qopt)966 static int tc_setup_etf(struct stmmac_priv *priv,
967 struct tc_etf_qopt_offload *qopt)
968 {
969 if (!priv->dma_cap.tbssel)
970 return -EOPNOTSUPP;
971 if (qopt->queue >= priv->plat->tx_queues_to_use)
972 return -EINVAL;
973 if (!(priv->tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL))
974 return -EINVAL;
975
976 if (qopt->enable)
977 priv->tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN;
978 else
979 priv->tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN;
980
981 netdev_info(priv->dev, "%s ETF for Queue %d\n",
982 qopt->enable ? "enabled" : "disabled", qopt->queue);
983 return 0;
984 }
985
986 const struct stmmac_tc_ops dwmac510_tc_ops = {
987 .init = tc_init,
988 .setup_cls_u32 = tc_setup_cls_u32,
989 .setup_cbs = tc_setup_cbs,
990 .setup_cls = tc_setup_cls,
991 .setup_taprio = tc_setup_taprio,
992 .setup_etf = tc_setup_etf,
993 };
994