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