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