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 	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 
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 		/* Fall through */
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 
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 
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 	mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
326 	if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
327 		ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
328 		if (ret)
329 			return ret;
330 
331 		priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
332 	} else if (!qopt->enable) {
333 		return stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_DCB);
334 	}
335 
336 	/* Port Transmit Rate and Speed Divider */
337 	ptr = (priv->speed == SPEED_100) ? 4 : 8;
338 	speed_div = (priv->speed == SPEED_100) ? 100000 : 1000000;
339 
340 	/* Final adjustments for HW */
341 	value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div);
342 	priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
343 
344 	value = div_s64(-qopt->sendslope * 1024ll * ptr, speed_div);
345 	priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
346 
347 	value = qopt->hicredit * 1024ll * 8;
348 	priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
349 
350 	value = qopt->locredit * 1024ll * 8;
351 	priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
352 
353 	ret = stmmac_config_cbs(priv, priv->hw,
354 				priv->plat->tx_queues_cfg[queue].send_slope,
355 				priv->plat->tx_queues_cfg[queue].idle_slope,
356 				priv->plat->tx_queues_cfg[queue].high_credit,
357 				priv->plat->tx_queues_cfg[queue].low_credit,
358 				queue);
359 	if (ret)
360 		return ret;
361 
362 	dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
363 			queue, qopt->sendslope, qopt->idleslope,
364 			qopt->hicredit, qopt->locredit);
365 	return 0;
366 }
367 
368 static int tc_parse_flow_actions(struct stmmac_priv *priv,
369 				 struct flow_action *action,
370 				 struct stmmac_flow_entry *entry)
371 {
372 	struct flow_action_entry *act;
373 	int i;
374 
375 	if (!flow_action_has_entries(action))
376 		return -EINVAL;
377 
378 	flow_action_for_each(i, act, action) {
379 		switch (act->id) {
380 		case FLOW_ACTION_DROP:
381 			entry->action |= STMMAC_FLOW_ACTION_DROP;
382 			return 0;
383 		default:
384 			break;
385 		}
386 	}
387 
388 	/* Nothing to do, maybe inverse filter ? */
389 	return 0;
390 }
391 
392 static int tc_add_basic_flow(struct stmmac_priv *priv,
393 			     struct flow_cls_offload *cls,
394 			     struct stmmac_flow_entry *entry)
395 {
396 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
397 	struct flow_dissector *dissector = rule->match.dissector;
398 	struct flow_match_basic match;
399 
400 	/* Nothing to do here */
401 	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
402 		return -EINVAL;
403 
404 	flow_rule_match_basic(rule, &match);
405 	entry->ip_proto = match.key->ip_proto;
406 	return 0;
407 }
408 
409 static int tc_add_ip4_flow(struct stmmac_priv *priv,
410 			   struct flow_cls_offload *cls,
411 			   struct stmmac_flow_entry *entry)
412 {
413 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
414 	struct flow_dissector *dissector = rule->match.dissector;
415 	bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
416 	struct flow_match_ipv4_addrs match;
417 	u32 hw_match;
418 	int ret;
419 
420 	/* Nothing to do here */
421 	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS))
422 		return -EINVAL;
423 
424 	flow_rule_match_ipv4_addrs(rule, &match);
425 	hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
426 	if (hw_match) {
427 		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
428 					      false, true, inv, hw_match);
429 		if (ret)
430 			return ret;
431 	}
432 
433 	hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
434 	if (hw_match) {
435 		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
436 					      false, false, inv, hw_match);
437 		if (ret)
438 			return ret;
439 	}
440 
441 	return 0;
442 }
443 
444 static int tc_add_ports_flow(struct stmmac_priv *priv,
445 			     struct flow_cls_offload *cls,
446 			     struct stmmac_flow_entry *entry)
447 {
448 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
449 	struct flow_dissector *dissector = rule->match.dissector;
450 	bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
451 	struct flow_match_ports match;
452 	u32 hw_match;
453 	bool is_udp;
454 	int ret;
455 
456 	/* Nothing to do here */
457 	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS))
458 		return -EINVAL;
459 
460 	switch (entry->ip_proto) {
461 	case IPPROTO_TCP:
462 		is_udp = false;
463 		break;
464 	case IPPROTO_UDP:
465 		is_udp = true;
466 		break;
467 	default:
468 		return -EINVAL;
469 	}
470 
471 	flow_rule_match_ports(rule, &match);
472 
473 	hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
474 	if (hw_match) {
475 		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
476 					      is_udp, true, inv, hw_match);
477 		if (ret)
478 			return ret;
479 	}
480 
481 	hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
482 	if (hw_match) {
483 		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
484 					      is_udp, false, inv, hw_match);
485 		if (ret)
486 			return ret;
487 	}
488 
489 	entry->is_l4 = true;
490 	return 0;
491 }
492 
493 static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv,
494 					      struct flow_cls_offload *cls,
495 					      bool get_free)
496 {
497 	int i;
498 
499 	for (i = 0; i < priv->flow_entries_max; i++) {
500 		struct stmmac_flow_entry *entry = &priv->flow_entries[i];
501 
502 		if (entry->cookie == cls->cookie)
503 			return entry;
504 		if (get_free && (entry->in_use == false))
505 			return entry;
506 	}
507 
508 	return NULL;
509 }
510 
511 static struct {
512 	int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls,
513 		  struct stmmac_flow_entry *entry);
514 } tc_flow_parsers[] = {
515 	{ .fn = tc_add_basic_flow },
516 	{ .fn = tc_add_ip4_flow },
517 	{ .fn = tc_add_ports_flow },
518 };
519 
520 static int tc_add_flow(struct stmmac_priv *priv,
521 		       struct flow_cls_offload *cls)
522 {
523 	struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
524 	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
525 	int i, ret;
526 
527 	if (!entry) {
528 		entry = tc_find_flow(priv, cls, true);
529 		if (!entry)
530 			return -ENOENT;
531 	}
532 
533 	ret = tc_parse_flow_actions(priv, &rule->action, entry);
534 	if (ret)
535 		return ret;
536 
537 	for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
538 		ret = tc_flow_parsers[i].fn(priv, cls, entry);
539 		if (!ret) {
540 			entry->in_use = true;
541 			continue;
542 		}
543 	}
544 
545 	if (!entry->in_use)
546 		return -EINVAL;
547 
548 	entry->cookie = cls->cookie;
549 	return 0;
550 }
551 
552 static int tc_del_flow(struct stmmac_priv *priv,
553 		       struct flow_cls_offload *cls)
554 {
555 	struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
556 	int ret;
557 
558 	if (!entry || !entry->in_use)
559 		return -ENOENT;
560 
561 	if (entry->is_l4) {
562 		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false,
563 					      false, false, false, 0);
564 	} else {
565 		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
566 					      false, false, false, 0);
567 	}
568 
569 	entry->in_use = false;
570 	entry->cookie = 0;
571 	entry->is_l4 = false;
572 	return ret;
573 }
574 
575 static int tc_setup_cls(struct stmmac_priv *priv,
576 			struct flow_cls_offload *cls)
577 {
578 	int ret = 0;
579 
580 	switch (cls->command) {
581 	case FLOW_CLS_REPLACE:
582 		ret = tc_add_flow(priv, cls);
583 		break;
584 	case FLOW_CLS_DESTROY:
585 		ret = tc_del_flow(priv, cls);
586 		break;
587 	default:
588 		return -EOPNOTSUPP;
589 	}
590 
591 	return ret;
592 }
593 
594 const struct stmmac_tc_ops dwmac510_tc_ops = {
595 	.init = tc_init,
596 	.setup_cls_u32 = tc_setup_cls_u32,
597 	.setup_cbs = tc_setup_cbs,
598 	.setup_cls = tc_setup_cls,
599 };
600