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 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(priv->device,
247 priv->rfs_entries_total,
248 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
tc_init(struct stmmac_priv * priv)259 static 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(priv->device,
268 dma_cap->l3l4fnum,
269 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(priv->device,
287 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(priv->device,
330 count, sizeof(*priv->tc_entries), GFP_KERNEL);
331 if (!priv->tc_entries)
332 return -ENOMEM;
333
334 tc_fill_all_pass_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
tc_setup_cbs(struct stmmac_priv * priv,struct tc_cbs_qopt_offload * qopt)342 static 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 s64 port_transmit_rate_kbps;
347 u32 queue = qopt->queue;
348 u32 mode_to_use;
349 u64 value;
350 u32 ptr;
351 int ret;
352
353 /* Queue 0 is not AVB capable */
354 if (queue <= 0 || queue >= tx_queues_count)
355 return -EINVAL;
356 if (!priv->dma_cap.av)
357 return -EOPNOTSUPP;
358
359 port_transmit_rate_kbps = qopt->idleslope - qopt->sendslope;
360
361 if (qopt->enable) {
362 /* Port Transmit Rate and Speed Divider */
363 switch (div_s64(port_transmit_rate_kbps, 1000)) {
364 case SPEED_10000:
365 case SPEED_5000:
366 ptr = 32;
367 break;
368 case SPEED_2500:
369 case SPEED_1000:
370 ptr = 8;
371 break;
372 case SPEED_100:
373 ptr = 4;
374 break;
375 default:
376 netdev_err(priv->dev,
377 "Invalid portTransmitRate %lld (idleSlope - sendSlope)\n",
378 port_transmit_rate_kbps);
379 return -EINVAL;
380 }
381 } else {
382 ptr = 0;
383 }
384
385 mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
386 if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
387 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
388 if (ret)
389 return ret;
390
391 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
392 } else if (!qopt->enable) {
393 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue,
394 MTL_QUEUE_DCB);
395 if (ret)
396 return ret;
397
398 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB;
399 return 0;
400 }
401
402 /* Final adjustments for HW */
403 value = div_s64(qopt->idleslope * 1024ll * ptr, port_transmit_rate_kbps);
404 priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
405
406 value = div_s64(-qopt->sendslope * 1024ll * ptr, port_transmit_rate_kbps);
407 priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
408
409 value = qopt->hicredit * 1024ll * 8;
410 priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
411
412 value = qopt->locredit * 1024ll * 8;
413 priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
414
415 ret = stmmac_config_cbs(priv, priv->hw,
416 priv->plat->tx_queues_cfg[queue].send_slope,
417 priv->plat->tx_queues_cfg[queue].idle_slope,
418 priv->plat->tx_queues_cfg[queue].high_credit,
419 priv->plat->tx_queues_cfg[queue].low_credit,
420 queue);
421 if (ret)
422 return ret;
423
424 dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
425 queue, qopt->sendslope, qopt->idleslope,
426 qopt->hicredit, qopt->locredit);
427 return 0;
428 }
429
tc_parse_flow_actions(struct stmmac_priv * priv,struct flow_action * action,struct stmmac_flow_entry * entry,struct netlink_ext_ack * extack)430 static int tc_parse_flow_actions(struct stmmac_priv *priv,
431 struct flow_action *action,
432 struct stmmac_flow_entry *entry,
433 struct netlink_ext_ack *extack)
434 {
435 struct flow_action_entry *act;
436 int i;
437
438 if (!flow_action_has_entries(action))
439 return -EINVAL;
440
441 if (!flow_action_basic_hw_stats_check(action, extack))
442 return -EOPNOTSUPP;
443
444 flow_action_for_each(i, act, action) {
445 switch (act->id) {
446 case FLOW_ACTION_DROP:
447 entry->action |= STMMAC_FLOW_ACTION_DROP;
448 return 0;
449 default:
450 break;
451 }
452 }
453
454 /* Nothing to do, maybe inverse filter ? */
455 return 0;
456 }
457
458 #define ETHER_TYPE_FULL_MASK cpu_to_be16(~0)
459
tc_add_basic_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls,struct stmmac_flow_entry * entry)460 static int tc_add_basic_flow(struct stmmac_priv *priv,
461 struct flow_cls_offload *cls,
462 struct stmmac_flow_entry *entry)
463 {
464 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
465 struct flow_dissector *dissector = rule->match.dissector;
466 struct flow_match_basic match;
467
468 /* Nothing to do here */
469 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
470 return -EINVAL;
471
472 flow_rule_match_basic(rule, &match);
473
474 entry->ip_proto = match.key->ip_proto;
475 return 0;
476 }
477
tc_add_ip4_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls,struct stmmac_flow_entry * entry)478 static int tc_add_ip4_flow(struct stmmac_priv *priv,
479 struct flow_cls_offload *cls,
480 struct stmmac_flow_entry *entry)
481 {
482 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
483 struct flow_dissector *dissector = rule->match.dissector;
484 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
485 struct flow_match_ipv4_addrs match;
486 u32 hw_match;
487 int ret;
488
489 /* Nothing to do here */
490 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS))
491 return -EINVAL;
492
493 flow_rule_match_ipv4_addrs(rule, &match);
494 hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
495 if (hw_match) {
496 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
497 false, true, inv, hw_match);
498 if (ret)
499 return ret;
500 }
501
502 hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
503 if (hw_match) {
504 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
505 false, false, inv, hw_match);
506 if (ret)
507 return ret;
508 }
509
510 return 0;
511 }
512
tc_add_ports_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls,struct stmmac_flow_entry * entry)513 static int tc_add_ports_flow(struct stmmac_priv *priv,
514 struct flow_cls_offload *cls,
515 struct stmmac_flow_entry *entry)
516 {
517 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
518 struct flow_dissector *dissector = rule->match.dissector;
519 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
520 struct flow_match_ports match;
521 u32 hw_match;
522 bool is_udp;
523 int ret;
524
525 /* Nothing to do here */
526 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS))
527 return -EINVAL;
528
529 switch (entry->ip_proto) {
530 case IPPROTO_TCP:
531 is_udp = false;
532 break;
533 case IPPROTO_UDP:
534 is_udp = true;
535 break;
536 default:
537 return -EINVAL;
538 }
539
540 flow_rule_match_ports(rule, &match);
541
542 hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
543 if (hw_match) {
544 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
545 is_udp, true, inv, hw_match);
546 if (ret)
547 return ret;
548 }
549
550 hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
551 if (hw_match) {
552 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
553 is_udp, false, inv, hw_match);
554 if (ret)
555 return ret;
556 }
557
558 entry->is_l4 = true;
559 return 0;
560 }
561
tc_find_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls,bool get_free)562 static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv,
563 struct flow_cls_offload *cls,
564 bool get_free)
565 {
566 int i;
567
568 for (i = 0; i < priv->flow_entries_max; i++) {
569 struct stmmac_flow_entry *entry = &priv->flow_entries[i];
570
571 if (entry->cookie == cls->cookie)
572 return entry;
573 if (get_free && (entry->in_use == false))
574 return entry;
575 }
576
577 return NULL;
578 }
579
580 static struct {
581 int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls,
582 struct stmmac_flow_entry *entry);
583 } tc_flow_parsers[] = {
584 { .fn = tc_add_basic_flow },
585 { .fn = tc_add_ip4_flow },
586 { .fn = tc_add_ports_flow },
587 };
588
tc_add_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)589 static int tc_add_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 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
594 int i, ret;
595
596 if (!entry) {
597 entry = tc_find_flow(priv, cls, true);
598 if (!entry)
599 return -ENOENT;
600 }
601
602 ret = tc_parse_flow_actions(priv, &rule->action, entry,
603 cls->common.extack);
604 if (ret)
605 return ret;
606
607 for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
608 ret = tc_flow_parsers[i].fn(priv, cls, entry);
609 if (!ret)
610 entry->in_use = true;
611 }
612
613 if (!entry->in_use)
614 return -EINVAL;
615
616 entry->cookie = cls->cookie;
617 return 0;
618 }
619
tc_del_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)620 static int tc_del_flow(struct stmmac_priv *priv,
621 struct flow_cls_offload *cls)
622 {
623 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
624 int ret;
625
626 if (!entry || !entry->in_use)
627 return -ENOENT;
628
629 if (entry->is_l4) {
630 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false,
631 false, false, false, 0);
632 } else {
633 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
634 false, false, false, 0);
635 }
636
637 entry->in_use = false;
638 entry->cookie = 0;
639 entry->is_l4 = false;
640 return ret;
641 }
642
tc_find_rfs(struct stmmac_priv * priv,struct flow_cls_offload * cls,bool get_free)643 static struct stmmac_rfs_entry *tc_find_rfs(struct stmmac_priv *priv,
644 struct flow_cls_offload *cls,
645 bool get_free)
646 {
647 int i;
648
649 for (i = 0; i < priv->rfs_entries_total; i++) {
650 struct stmmac_rfs_entry *entry = &priv->rfs_entries[i];
651
652 if (entry->cookie == cls->cookie)
653 return entry;
654 if (get_free && entry->in_use == false)
655 return entry;
656 }
657
658 return NULL;
659 }
660
661 #define VLAN_PRIO_FULL_MASK (0x07)
662
tc_add_vlan_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)663 static int tc_add_vlan_flow(struct stmmac_priv *priv,
664 struct flow_cls_offload *cls)
665 {
666 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
667 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
668 struct flow_dissector *dissector = rule->match.dissector;
669 int tc = tc_classid_to_hwtc(priv->dev, cls->classid);
670 struct flow_match_vlan match;
671
672 if (!entry) {
673 entry = tc_find_rfs(priv, cls, true);
674 if (!entry)
675 return -ENOENT;
676 }
677
678 if (priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN] >=
679 priv->rfs_entries_max[STMMAC_RFS_T_VLAN])
680 return -ENOENT;
681
682 /* Nothing to do here */
683 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_VLAN))
684 return -EINVAL;
685
686 if (tc < 0) {
687 netdev_err(priv->dev, "Invalid traffic class\n");
688 return -EINVAL;
689 }
690
691 flow_rule_match_vlan(rule, &match);
692
693 if (match.mask->vlan_priority) {
694 u32 prio;
695
696 if (match.mask->vlan_priority != VLAN_PRIO_FULL_MASK) {
697 netdev_err(priv->dev, "Only full mask is supported for VLAN priority");
698 return -EINVAL;
699 }
700
701 prio = BIT(match.key->vlan_priority);
702 stmmac_rx_queue_prio(priv, priv->hw, prio, tc);
703
704 entry->in_use = true;
705 entry->cookie = cls->cookie;
706 entry->tc = tc;
707 entry->type = STMMAC_RFS_T_VLAN;
708 priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]++;
709 }
710
711 return 0;
712 }
713
tc_del_vlan_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)714 static int tc_del_vlan_flow(struct stmmac_priv *priv,
715 struct flow_cls_offload *cls)
716 {
717 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
718
719 if (!entry || !entry->in_use || entry->type != STMMAC_RFS_T_VLAN)
720 return -ENOENT;
721
722 stmmac_rx_queue_prio(priv, priv->hw, 0, entry->tc);
723
724 entry->in_use = false;
725 entry->cookie = 0;
726 entry->tc = 0;
727 entry->type = 0;
728
729 priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]--;
730
731 return 0;
732 }
733
tc_add_ethtype_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)734 static int tc_add_ethtype_flow(struct stmmac_priv *priv,
735 struct flow_cls_offload *cls)
736 {
737 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
738 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
739 struct flow_dissector *dissector = rule->match.dissector;
740 int tc = tc_classid_to_hwtc(priv->dev, cls->classid);
741 struct flow_match_basic match;
742
743 if (!entry) {
744 entry = tc_find_rfs(priv, cls, true);
745 if (!entry)
746 return -ENOENT;
747 }
748
749 /* Nothing to do here */
750 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
751 return -EINVAL;
752
753 if (tc < 0) {
754 netdev_err(priv->dev, "Invalid traffic class\n");
755 return -EINVAL;
756 }
757
758 flow_rule_match_basic(rule, &match);
759
760 if (match.mask->n_proto) {
761 u16 etype = ntohs(match.key->n_proto);
762
763 if (match.mask->n_proto != ETHER_TYPE_FULL_MASK) {
764 netdev_err(priv->dev, "Only full mask is supported for EthType filter");
765 return -EINVAL;
766 }
767 switch (etype) {
768 case ETH_P_LLDP:
769 if (priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP] >=
770 priv->rfs_entries_max[STMMAC_RFS_T_LLDP])
771 return -ENOENT;
772
773 entry->type = STMMAC_RFS_T_LLDP;
774 priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP]++;
775
776 stmmac_rx_queue_routing(priv, priv->hw,
777 PACKET_DCBCPQ, tc);
778 break;
779 case ETH_P_1588:
780 if (priv->rfs_entries_cnt[STMMAC_RFS_T_1588] >=
781 priv->rfs_entries_max[STMMAC_RFS_T_1588])
782 return -ENOENT;
783
784 entry->type = STMMAC_RFS_T_1588;
785 priv->rfs_entries_cnt[STMMAC_RFS_T_1588]++;
786
787 stmmac_rx_queue_routing(priv, priv->hw,
788 PACKET_PTPQ, tc);
789 break;
790 default:
791 netdev_err(priv->dev, "EthType(0x%x) is not supported", etype);
792 return -EINVAL;
793 }
794
795 entry->in_use = true;
796 entry->cookie = cls->cookie;
797 entry->tc = tc;
798 entry->etype = etype;
799
800 return 0;
801 }
802
803 return -EINVAL;
804 }
805
tc_del_ethtype_flow(struct stmmac_priv * priv,struct flow_cls_offload * cls)806 static int tc_del_ethtype_flow(struct stmmac_priv *priv,
807 struct flow_cls_offload *cls)
808 {
809 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
810
811 if (!entry || !entry->in_use ||
812 entry->type < STMMAC_RFS_T_LLDP ||
813 entry->type > STMMAC_RFS_T_1588)
814 return -ENOENT;
815
816 switch (entry->etype) {
817 case ETH_P_LLDP:
818 stmmac_rx_queue_routing(priv, priv->hw,
819 PACKET_DCBCPQ, 0);
820 priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP]--;
821 break;
822 case ETH_P_1588:
823 stmmac_rx_queue_routing(priv, priv->hw,
824 PACKET_PTPQ, 0);
825 priv->rfs_entries_cnt[STMMAC_RFS_T_1588]--;
826 break;
827 default:
828 netdev_err(priv->dev, "EthType(0x%x) is not supported",
829 entry->etype);
830 return -EINVAL;
831 }
832
833 entry->in_use = false;
834 entry->cookie = 0;
835 entry->tc = 0;
836 entry->etype = 0;
837 entry->type = 0;
838
839 return 0;
840 }
841
tc_add_flow_cls(struct stmmac_priv * priv,struct flow_cls_offload * cls)842 static int tc_add_flow_cls(struct stmmac_priv *priv,
843 struct flow_cls_offload *cls)
844 {
845 int ret;
846
847 ret = tc_add_flow(priv, cls);
848 if (!ret)
849 return ret;
850
851 ret = tc_add_ethtype_flow(priv, cls);
852 if (!ret)
853 return ret;
854
855 return tc_add_vlan_flow(priv, cls);
856 }
857
tc_del_flow_cls(struct stmmac_priv * priv,struct flow_cls_offload * cls)858 static int tc_del_flow_cls(struct stmmac_priv *priv,
859 struct flow_cls_offload *cls)
860 {
861 int ret;
862
863 ret = tc_del_flow(priv, cls);
864 if (!ret)
865 return ret;
866
867 ret = tc_del_ethtype_flow(priv, cls);
868 if (!ret)
869 return ret;
870
871 return tc_del_vlan_flow(priv, cls);
872 }
873
tc_setup_cls(struct stmmac_priv * priv,struct flow_cls_offload * cls)874 static int tc_setup_cls(struct stmmac_priv *priv,
875 struct flow_cls_offload *cls)
876 {
877 int ret = 0;
878
879 /* When RSS is enabled, the filtering will be bypassed */
880 if (priv->rss.enable)
881 return -EBUSY;
882
883 switch (cls->command) {
884 case FLOW_CLS_REPLACE:
885 ret = tc_add_flow_cls(priv, cls);
886 break;
887 case FLOW_CLS_DESTROY:
888 ret = tc_del_flow_cls(priv, cls);
889 break;
890 default:
891 return -EOPNOTSUPP;
892 }
893
894 return ret;
895 }
896
stmmac_calc_tas_basetime(ktime_t old_base_time,ktime_t current_time,u64 cycle_time)897 struct timespec64 stmmac_calc_tas_basetime(ktime_t old_base_time,
898 ktime_t current_time,
899 u64 cycle_time)
900 {
901 struct timespec64 time;
902
903 if (ktime_after(old_base_time, current_time)) {
904 time = ktime_to_timespec64(old_base_time);
905 } else {
906 s64 n;
907 ktime_t base_time;
908
909 n = div64_s64(ktime_sub_ns(current_time, old_base_time),
910 cycle_time);
911 base_time = ktime_add_ns(old_base_time,
912 (n + 1) * cycle_time);
913
914 time = ktime_to_timespec64(base_time);
915 }
916
917 return time;
918 }
919
tc_setup_taprio(struct stmmac_priv * priv,struct tc_taprio_qopt_offload * qopt)920 static int tc_setup_taprio(struct stmmac_priv *priv,
921 struct tc_taprio_qopt_offload *qopt)
922 {
923 u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
924 struct plat_stmmacenet_data *plat = priv->plat;
925 struct timespec64 time, current_time, qopt_time;
926 ktime_t current_time_ns;
927 bool fpe = false;
928 int i, ret = 0;
929 u64 ctr;
930
931 if (qopt->base_time < 0)
932 return -ERANGE;
933
934 if (!priv->dma_cap.estsel)
935 return -EOPNOTSUPP;
936
937 switch (wid) {
938 case 0x1:
939 wid = 16;
940 break;
941 case 0x2:
942 wid = 20;
943 break;
944 case 0x3:
945 wid = 24;
946 break;
947 default:
948 return -EOPNOTSUPP;
949 }
950
951 switch (dep) {
952 case 0x1:
953 dep = 64;
954 break;
955 case 0x2:
956 dep = 128;
957 break;
958 case 0x3:
959 dep = 256;
960 break;
961 case 0x4:
962 dep = 512;
963 break;
964 case 0x5:
965 dep = 1024;
966 break;
967 default:
968 return -EOPNOTSUPP;
969 }
970
971 if (qopt->cmd == TAPRIO_CMD_DESTROY)
972 goto disable;
973 else if (qopt->cmd != TAPRIO_CMD_REPLACE)
974 return -EOPNOTSUPP;
975
976 if (qopt->num_entries >= dep)
977 return -EINVAL;
978 if (!qopt->cycle_time)
979 return -ERANGE;
980
981 if (!plat->est) {
982 plat->est = devm_kzalloc(priv->device, sizeof(*plat->est),
983 GFP_KERNEL);
984 if (!plat->est)
985 return -ENOMEM;
986
987 mutex_init(&priv->est_lock);
988 } else {
989 mutex_lock(&priv->est_lock);
990 memset(plat->est, 0, sizeof(*plat->est));
991 mutex_unlock(&priv->est_lock);
992 }
993
994 size = qopt->num_entries;
995
996 mutex_lock(&priv->est_lock);
997 priv->plat->est->gcl_size = size;
998 priv->plat->est->enable = qopt->cmd == TAPRIO_CMD_REPLACE;
999 mutex_unlock(&priv->est_lock);
1000
1001 for (i = 0; i < size; i++) {
1002 s64 delta_ns = qopt->entries[i].interval;
1003 u32 gates = qopt->entries[i].gate_mask;
1004
1005 if (delta_ns > GENMASK(wid, 0))
1006 return -ERANGE;
1007 if (gates > GENMASK(31 - wid, 0))
1008 return -ERANGE;
1009
1010 switch (qopt->entries[i].command) {
1011 case TC_TAPRIO_CMD_SET_GATES:
1012 if (fpe)
1013 return -EINVAL;
1014 break;
1015 case TC_TAPRIO_CMD_SET_AND_HOLD:
1016 gates |= BIT(0);
1017 fpe = true;
1018 break;
1019 case TC_TAPRIO_CMD_SET_AND_RELEASE:
1020 gates &= ~BIT(0);
1021 fpe = true;
1022 break;
1023 default:
1024 return -EOPNOTSUPP;
1025 }
1026
1027 priv->plat->est->gcl[i] = delta_ns | (gates << wid);
1028 }
1029
1030 mutex_lock(&priv->est_lock);
1031 /* Adjust for real system time */
1032 priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, ¤t_time);
1033 current_time_ns = timespec64_to_ktime(current_time);
1034 time = stmmac_calc_tas_basetime(qopt->base_time, current_time_ns,
1035 qopt->cycle_time);
1036
1037 priv->plat->est->btr[0] = (u32)time.tv_nsec;
1038 priv->plat->est->btr[1] = (u32)time.tv_sec;
1039
1040 qopt_time = ktime_to_timespec64(qopt->base_time);
1041 priv->plat->est->btr_reserve[0] = (u32)qopt_time.tv_nsec;
1042 priv->plat->est->btr_reserve[1] = (u32)qopt_time.tv_sec;
1043
1044 ctr = qopt->cycle_time;
1045 priv->plat->est->ctr[0] = do_div(ctr, NSEC_PER_SEC);
1046 priv->plat->est->ctr[1] = (u32)ctr;
1047
1048 if (fpe && !priv->dma_cap.fpesel) {
1049 mutex_unlock(&priv->est_lock);
1050 return -EOPNOTSUPP;
1051 }
1052
1053 /* Actual FPE register configuration will be done after FPE handshake
1054 * is success.
1055 */
1056 priv->plat->fpe_cfg->enable = fpe;
1057
1058 ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
1059 priv->plat->clk_ptp_rate);
1060 mutex_unlock(&priv->est_lock);
1061 if (ret) {
1062 netdev_err(priv->dev, "failed to configure EST\n");
1063 goto disable;
1064 }
1065
1066 netdev_info(priv->dev, "configured EST\n");
1067
1068 if (fpe) {
1069 stmmac_fpe_handshake(priv, true);
1070 netdev_info(priv->dev, "start FPE handshake\n");
1071 }
1072
1073 return 0;
1074
1075 disable:
1076 if (priv->plat->est) {
1077 mutex_lock(&priv->est_lock);
1078 priv->plat->est->enable = false;
1079 stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
1080 priv->plat->clk_ptp_rate);
1081 mutex_unlock(&priv->est_lock);
1082 }
1083
1084 priv->plat->fpe_cfg->enable = false;
1085 stmmac_fpe_configure(priv, priv->ioaddr,
1086 priv->plat->fpe_cfg,
1087 priv->plat->tx_queues_to_use,
1088 priv->plat->rx_queues_to_use,
1089 false);
1090 netdev_info(priv->dev, "disabled FPE\n");
1091
1092 stmmac_fpe_handshake(priv, false);
1093 netdev_info(priv->dev, "stop FPE handshake\n");
1094
1095 return ret;
1096 }
1097
tc_setup_etf(struct stmmac_priv * priv,struct tc_etf_qopt_offload * qopt)1098 static int tc_setup_etf(struct stmmac_priv *priv,
1099 struct tc_etf_qopt_offload *qopt)
1100 {
1101 if (!priv->dma_cap.tbssel)
1102 return -EOPNOTSUPP;
1103 if (qopt->queue >= priv->plat->tx_queues_to_use)
1104 return -EINVAL;
1105 if (!(priv->dma_conf.tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL))
1106 return -EINVAL;
1107
1108 if (qopt->enable)
1109 priv->dma_conf.tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN;
1110 else
1111 priv->dma_conf.tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN;
1112
1113 netdev_info(priv->dev, "%s ETF for Queue %d\n",
1114 qopt->enable ? "enabled" : "disabled", qopt->queue);
1115 return 0;
1116 }
1117
tc_query_caps(struct stmmac_priv * priv,struct tc_query_caps_base * base)1118 static int tc_query_caps(struct stmmac_priv *priv,
1119 struct tc_query_caps_base *base)
1120 {
1121 switch (base->type) {
1122 case TC_SETUP_QDISC_TAPRIO: {
1123 struct tc_taprio_caps *caps = base->caps;
1124
1125 if (!priv->dma_cap.estsel)
1126 return -EOPNOTSUPP;
1127
1128 caps->gate_mask_per_txq = true;
1129
1130 return 0;
1131 }
1132 default:
1133 return -EOPNOTSUPP;
1134 }
1135 }
1136
1137 const struct stmmac_tc_ops dwmac510_tc_ops = {
1138 .init = tc_init,
1139 .setup_cls_u32 = tc_setup_cls_u32,
1140 .setup_cbs = tc_setup_cbs,
1141 .setup_cls = tc_setup_cls,
1142 .setup_taprio = tc_setup_taprio,
1143 .setup_etf = tc_setup_etf,
1144 .query_caps = tc_query_caps,
1145 };
1146