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