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