stmmac_tc.c

   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
  14static 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
  28static 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
  57static 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
  91static 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
 164err_unuse:
 165	if (frag)
 166		frag->in_use = false;
 167	entry->in_use = false;
 168	return ret;
 169}
 170
 171static 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
 188static 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
 204err_unfill:
 205	tc_unfill_entry(priv, cls);
 206	return ret;
 207}
 208
 209static 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
 219static 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
 235static 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
 259static 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	priv->flow_entries_max = dma_cap->l3l4fnum;
 266	if (priv->flow_entries_max) {
 267		priv->flow_entries = devm_kcalloc(priv->device,
 268						  priv->flow_entries_max,
 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	/* Fail silently as we can still use remaining features, e.g. CBS */
 286	if (!dma_cap->frpsel)
 287		return 0;
 288
 289	switch (dma_cap->frpbs) {
 290	case 0x0:
 291		priv->tc_off_max = 64;
 292		break;
 293	case 0x1:
 294		priv->tc_off_max = 128;
 295		break;
 296	case 0x2:
 297		priv->tc_off_max = 256;
 298		break;
 299	default:
 300		return -EINVAL;
 301	}
 302
 303	switch (dma_cap->frpes) {
 304	case 0x0:
 305		count = 64;
 306		break;
 307	case 0x1:
 308		count = 128;
 309		break;
 310	case 0x2:
 311		count = 256;
 312		break;
 313	default:
 314		return -EINVAL;
 315	}
 316
 317	/* Reserve one last filter which lets all pass */
 318	priv->tc_entries_max = count;
 319	priv->tc_entries = devm_kcalloc(priv->device,
 320			count, sizeof(*priv->tc_entries), GFP_KERNEL);
 321	if (!priv->tc_entries)
 322		return -ENOMEM;
 323
 324	tc_fill_all_pass_entry(&priv->tc_entries[count - 1]);
 325
 326	dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n",
 327			priv->tc_entries_max, priv->tc_off_max);
 328
 329	return 0;
 330}
 331
 332static int tc_setup_cbs(struct stmmac_priv *priv,
 333			struct tc_cbs_qopt_offload *qopt)
 334{
 335	u32 tx_queues_count = priv->plat->tx_queues_to_use;
 336	s64 port_transmit_rate_kbps;
 337	u32 queue = qopt->queue;
 338	u32 mode_to_use;
 339	u64 value;
 340	u32 ptr;
 341	int ret;
 342
 343	/* Queue 0 is not AVB capable */
 344	if (queue <= 0 || queue >= tx_queues_count)
 345		return -EINVAL;
 346	if (!priv->dma_cap.av)
 347		return -EOPNOTSUPP;
 348
 349	port_transmit_rate_kbps = qopt->idleslope - qopt->sendslope;
 350
 351	if (qopt->enable) {
 352		/* Port Transmit Rate and Speed Divider */
 353		switch (div_s64(port_transmit_rate_kbps, 1000)) {
 354		case SPEED_10000:
 355		case SPEED_5000:
 356			ptr = 32;
 357			break;
 358		case SPEED_2500:
 359		case SPEED_1000:
 360			ptr = 8;
 361			break;
 362		case SPEED_100:
 363			ptr = 4;
 364			break;
 365		default:
 366			netdev_err(priv->dev,
 367				   "Invalid portTransmitRate %lld (idleSlope - sendSlope)\n",
 368				   port_transmit_rate_kbps);
 369			return -EINVAL;
 370		}
 371	} else {
 372		ptr = 0;
 373	}
 374
 375	mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
 376	if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
 377		ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
 378		if (ret)
 379			return ret;
 380
 381		priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
 382	} else if (!qopt->enable) {
 383		ret = stmmac_dma_qmode(priv, priv->ioaddr, queue,
 384				       MTL_QUEUE_DCB);
 385		if (ret)
 386			return ret;
 387
 388		priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB;
 389		return 0;
 390	}
 391
 392	/* Final adjustments for HW */
 393	value = div_s64(qopt->idleslope * 1024ll * ptr, port_transmit_rate_kbps);
 394	priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
 395
 396	value = div_s64(-qopt->sendslope * 1024ll * ptr, port_transmit_rate_kbps);
 397	priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
 398
 399	value = qopt->hicredit * 1024ll * 8;
 400	priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
 401
 402	value = qopt->locredit * 1024ll * 8;
 403	priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
 404
 405	ret = stmmac_config_cbs(priv, priv->hw,
 406				priv->plat->tx_queues_cfg[queue].send_slope,
 407				priv->plat->tx_queues_cfg[queue].idle_slope,
 408				priv->plat->tx_queues_cfg[queue].high_credit,
 409				priv->plat->tx_queues_cfg[queue].low_credit,
 410				queue);
 411	if (ret)
 412		return ret;
 413
 414	dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
 415			queue, qopt->sendslope, qopt->idleslope,
 416			qopt->hicredit, qopt->locredit);
 417	return 0;
 418}
 419
 420static int tc_parse_flow_actions(struct stmmac_priv *priv,
 421				 struct flow_action *action,
 422				 struct stmmac_flow_entry *entry,
 423				 struct netlink_ext_ack *extack)
 424{
 425	struct flow_action_entry *act;
 426	int i;
 427
 428	if (!flow_action_has_entries(action))
 429		return -EINVAL;
 430
 431	if (!flow_action_basic_hw_stats_check(action, extack))
 432		return -EOPNOTSUPP;
 433
 434	flow_action_for_each(i, act, action) {
 435		switch (act->id) {
 436		case FLOW_ACTION_DROP:
 437			entry->action |= STMMAC_FLOW_ACTION_DROP;
 438			return 0;
 439		default:
 440			break;
 441		}
 442	}
 443
 444	/* Nothing to do, maybe inverse filter ? */
 445	return 0;
 446}
 447
 448#define ETHER_TYPE_FULL_MASK	cpu_to_be16(~0)
 449
 450static int tc_add_basic_flow(struct stmmac_priv *priv,
 451			     struct flow_cls_offload *cls,
 452			     struct stmmac_flow_entry *entry)
 453{
 454	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 455	struct flow_dissector *dissector = rule->match.dissector;
 456	struct flow_match_basic match;
 457
 458	/* Nothing to do here */
 459	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
 460		return -EINVAL;
 461
 462	flow_rule_match_basic(rule, &match);
 463
 464	entry->ip_proto = match.key->ip_proto;
 465	return 0;
 466}
 467
 468static int tc_add_ip4_flow(struct stmmac_priv *priv,
 469			   struct flow_cls_offload *cls,
 470			   struct stmmac_flow_entry *entry)
 471{
 472	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 473	struct flow_dissector *dissector = rule->match.dissector;
 474	bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
 475	struct flow_match_ipv4_addrs match;
 476	u32 hw_match;
 477	int ret;
 478
 479	/* Nothing to do here */
 480	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS))
 481		return -EINVAL;
 482
 483	flow_rule_match_ipv4_addrs(rule, &match);
 484	hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
 485	if (hw_match) {
 486		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
 487					      false, true, inv, hw_match);
 488		if (ret)
 489			return ret;
 490	}
 491
 492	hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
 493	if (hw_match) {
 494		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
 495					      false, false, inv, hw_match);
 496		if (ret)
 497			return ret;
 498	}
 499
 500	return 0;
 501}
 502
 503static int tc_add_ports_flow(struct stmmac_priv *priv,
 504			     struct flow_cls_offload *cls,
 505			     struct stmmac_flow_entry *entry)
 506{
 507	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 508	struct flow_dissector *dissector = rule->match.dissector;
 509	bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
 510	struct flow_match_ports match;
 511	u32 hw_match;
 512	bool is_udp;
 513	int ret;
 514
 515	/* Nothing to do here */
 516	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS))
 517		return -EINVAL;
 518
 519	switch (entry->ip_proto) {
 520	case IPPROTO_TCP:
 521		is_udp = false;
 522		break;
 523	case IPPROTO_UDP:
 524		is_udp = true;
 525		break;
 526	default:
 527		return -EINVAL;
 528	}
 529
 530	flow_rule_match_ports(rule, &match);
 531
 532	hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
 533	if (hw_match) {
 534		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
 535					      is_udp, true, inv, hw_match);
 536		if (ret)
 537			return ret;
 538	}
 539
 540	hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
 541	if (hw_match) {
 542		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
 543					      is_udp, false, inv, hw_match);
 544		if (ret)
 545			return ret;
 546	}
 547
 548	entry->is_l4 = true;
 549	return 0;
 550}
 551
 552static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv,
 553					      struct flow_cls_offload *cls,
 554					      bool get_free)
 555{
 556	int i;
 557
 558	for (i = 0; i < priv->flow_entries_max; i++) {
 559		struct stmmac_flow_entry *entry = &priv->flow_entries[i];
 560
 561		if (entry->cookie == cls->cookie)
 562			return entry;
 563		if (get_free && (entry->in_use == false))
 564			return entry;
 565	}
 566
 567	return NULL;
 568}
 569
 570static struct {
 571	int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls,
 572		  struct stmmac_flow_entry *entry);
 573} tc_flow_parsers[] = {
 574	{ .fn = tc_add_basic_flow },
 575	{ .fn = tc_add_ip4_flow },
 576	{ .fn = tc_add_ports_flow },
 577};
 578
 579static int tc_add_flow(struct stmmac_priv *priv,
 580		       struct flow_cls_offload *cls)
 581{
 582	struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
 583	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 584	int i, ret;
 585
 586	if (!entry) {
 587		entry = tc_find_flow(priv, cls, true);
 588		if (!entry)
 589			return -ENOENT;
 590	}
 591
 592	ret = tc_parse_flow_actions(priv, &rule->action, entry,
 593				    cls->common.extack);
 594	if (ret)
 595		return ret;
 596
 597	for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
 598		ret = tc_flow_parsers[i].fn(priv, cls, entry);
 599		if (!ret)
 600			entry->in_use = true;
 601	}
 602
 603	if (!entry->in_use)
 604		return -EINVAL;
 605
 606	entry->cookie = cls->cookie;
 607	return 0;
 608}
 609
 610static int tc_del_flow(struct stmmac_priv *priv,
 611		       struct flow_cls_offload *cls)
 612{
 613	struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
 614	int ret;
 615
 616	if (!entry || !entry->in_use)
 617		return -ENOENT;
 618
 619	if (entry->is_l4) {
 620		ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false,
 621					      false, false, false, 0);
 622	} else {
 623		ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
 624					      false, false, false, 0);
 625	}
 626
 627	entry->in_use = false;
 628	entry->cookie = 0;
 629	entry->is_l4 = false;
 630	return ret;
 631}
 632
 633static struct stmmac_rfs_entry *tc_find_rfs(struct stmmac_priv *priv,
 634					    struct flow_cls_offload *cls,
 635					    bool get_free)
 636{
 637	int i;
 638
 639	for (i = 0; i < priv->rfs_entries_total; i++) {
 640		struct stmmac_rfs_entry *entry = &priv->rfs_entries[i];
 641
 642		if (entry->cookie == cls->cookie)
 643			return entry;
 644		if (get_free && entry->in_use == false)
 645			return entry;
 646	}
 647
 648	return NULL;
 649}
 650
 651#define VLAN_PRIO_FULL_MASK (0x07)
 652
 653static int tc_add_vlan_flow(struct stmmac_priv *priv,
 654			    struct flow_cls_offload *cls)
 655{
 656	struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
 657	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 658	struct flow_dissector *dissector = rule->match.dissector;
 659	int tc = tc_classid_to_hwtc(priv->dev, cls->classid);
 660	struct flow_match_vlan match;
 661
 662	if (!entry) {
 663		entry = tc_find_rfs(priv, cls, true);
 664		if (!entry)
 665			return -ENOENT;
 666	}
 667
 668	if (priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN] >=
 669	    priv->rfs_entries_max[STMMAC_RFS_T_VLAN])
 670		return -ENOENT;
 671
 672	/* Nothing to do here */
 673	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_VLAN))
 674		return -EINVAL;
 675
 676	if (tc < 0) {
 677		netdev_err(priv->dev, "Invalid traffic class\n");
 678		return -EINVAL;
 679	}
 680
 681	flow_rule_match_vlan(rule, &match);
 682
 683	if (match.mask->vlan_priority) {
 684		u32 prio;
 685
 686		if (match.mask->vlan_priority != VLAN_PRIO_FULL_MASK) {
 687			netdev_err(priv->dev, "Only full mask is supported for VLAN priority");
 688			return -EINVAL;
 689		}
 690
 691		prio = BIT(match.key->vlan_priority);
 692		stmmac_rx_queue_prio(priv, priv->hw, prio, tc);
 693
 694		entry->in_use = true;
 695		entry->cookie = cls->cookie;
 696		entry->tc = tc;
 697		entry->type = STMMAC_RFS_T_VLAN;
 698		priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]++;
 699	}
 700
 701	return 0;
 702}
 703
 704static int tc_del_vlan_flow(struct stmmac_priv *priv,
 705			    struct flow_cls_offload *cls)
 706{
 707	struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
 708
 709	if (!entry || !entry->in_use || entry->type != STMMAC_RFS_T_VLAN)
 710		return -ENOENT;
 711
 712	stmmac_rx_queue_prio(priv, priv->hw, 0, entry->tc);
 713
 714	entry->in_use = false;
 715	entry->cookie = 0;
 716	entry->tc = 0;
 717	entry->type = 0;
 718
 719	priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]--;
 720
 721	return 0;
 722}
 723
 724static int tc_add_ethtype_flow(struct stmmac_priv *priv,
 725			       struct flow_cls_offload *cls)
 726{
 727	struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
 728	struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 729	struct flow_dissector *dissector = rule->match.dissector;
 730	int tc = tc_classid_to_hwtc(priv->dev, cls->classid);
 731	struct flow_match_basic match;
 732
 733	if (!entry) {
 734		entry = tc_find_rfs(priv, cls, true);
 735		if (!entry)
 736			return -ENOENT;
 737	}
 738
 739	/* Nothing to do here */
 740	if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
 741		return -EINVAL;
 742
 743	if (tc < 0) {
 744		netdev_err(priv->dev, "Invalid traffic class\n");
 745		return -EINVAL;
 746	}
 747
 748	flow_rule_match_basic(rule, &match);
 749
 750	if (match.mask->n_proto) {
 751		u16 etype = ntohs(match.key->n_proto);
 752
 753		if (match.mask->n_proto != ETHER_TYPE_FULL_MASK) {
 754			netdev_err(priv->dev, "Only full mask is supported for EthType filter");
 755			return -EINVAL;
 756		}
 757		switch (etype) {
 758		case ETH_P_LLDP:
 759			if (priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP] >=
 760			    priv->rfs_entries_max[STMMAC_RFS_T_LLDP])
 761				return -ENOENT;
 762
 763			entry->type = STMMAC_RFS_T_LLDP;
 764			priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP]++;
 765
 766			stmmac_rx_queue_routing(priv, priv->hw,
 767						PACKET_DCBCPQ, tc);
 768			break;
 769		case ETH_P_1588:
 770			if (priv->rfs_entries_cnt[STMMAC_RFS_T_1588] >=
 771			    priv->rfs_entries_max[STMMAC_RFS_T_1588])
 772				return -ENOENT;
 773
 774			entry->type = STMMAC_RFS_T_1588;
 775			priv->rfs_entries_cnt[STMMAC_RFS_T_1588]++;
 776
 777			stmmac_rx_queue_routing(priv, priv->hw,
 778						PACKET_PTPQ, tc);
 779			break;
 780		default:
 781			netdev_err(priv->dev, "EthType(0x%x) is not supported", etype);
 782			return -EINVAL;
 783		}
 784
 785		entry->in_use = true;
 786		entry->cookie = cls->cookie;
 787		entry->tc = tc;
 788		entry->etype = etype;
 789
 790		return 0;
 791	}
 792
 793	return -EINVAL;
 794}
 795
 796static int tc_del_ethtype_flow(struct stmmac_priv *priv,
 797			       struct flow_cls_offload *cls)
 798{
 799	struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false);
 800
 801	if (!entry || !entry->in_use ||
 802	    entry->type < STMMAC_RFS_T_LLDP ||
 803	    entry->type > STMMAC_RFS_T_1588)
 804		return -ENOENT;
 805
 806	switch (entry->etype) {
 807	case ETH_P_LLDP:
 808		stmmac_rx_queue_routing(priv, priv->hw,
 809					PACKET_DCBCPQ, 0);
 810		priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP]--;
 811		break;
 812	case ETH_P_1588:
 813		stmmac_rx_queue_routing(priv, priv->hw,
 814					PACKET_PTPQ, 0);
 815		priv->rfs_entries_cnt[STMMAC_RFS_T_1588]--;
 816		break;
 817	default:
 818		netdev_err(priv->dev, "EthType(0x%x) is not supported",
 819			   entry->etype);
 820		return -EINVAL;
 821	}
 822
 823	entry->in_use = false;
 824	entry->cookie = 0;
 825	entry->tc = 0;
 826	entry->etype = 0;
 827	entry->type = 0;
 828
 829	return 0;
 830}
 831
 832static int tc_add_flow_cls(struct stmmac_priv *priv,
 833			   struct flow_cls_offload *cls)
 834{
 835	int ret;
 836
 837	ret = tc_add_flow(priv, cls);
 838	if (!ret)
 839		return ret;
 840
 841	ret = tc_add_ethtype_flow(priv, cls);
 842	if (!ret)
 843		return ret;
 844
 845	return tc_add_vlan_flow(priv, cls);
 846}
 847
 848static int tc_del_flow_cls(struct stmmac_priv *priv,
 849			   struct flow_cls_offload *cls)
 850{
 851	int ret;
 852
 853	ret = tc_del_flow(priv, cls);
 854	if (!ret)
 855		return ret;
 856
 857	ret = tc_del_ethtype_flow(priv, cls);
 858	if (!ret)
 859		return ret;
 860
 861	return tc_del_vlan_flow(priv, cls);
 862}
 863
 864static int tc_setup_cls(struct stmmac_priv *priv,
 865			struct flow_cls_offload *cls)
 866{
 867	int ret = 0;
 868
 869	/* When RSS is enabled, the filtering will be bypassed */
 870	if (priv->rss.enable)
 871		return -EBUSY;
 872
 873	switch (cls->command) {
 874	case FLOW_CLS_REPLACE:
 875		ret = tc_add_flow_cls(priv, cls);
 876		break;
 877	case FLOW_CLS_DESTROY:
 878		ret = tc_del_flow_cls(priv, cls);
 879		break;
 880	default:
 881		return -EOPNOTSUPP;
 882	}
 883
 884	return ret;
 885}
 886
 887struct timespec64 stmmac_calc_tas_basetime(ktime_t old_base_time,
 888					   ktime_t current_time,
 889					   u64 cycle_time)
 890{
 891	struct timespec64 time;
 892
 893	if (ktime_after(old_base_time, current_time)) {
 894		time = ktime_to_timespec64(old_base_time);
 895	} else {
 896		s64 n;
 897		ktime_t base_time;
 898
 899		n = div64_s64(ktime_sub_ns(current_time, old_base_time),
 900			      cycle_time);
 901		base_time = ktime_add_ns(old_base_time,
 902					 (n + 1) * cycle_time);
 903
 904		time = ktime_to_timespec64(base_time);
 905	}
 906
 907	return time;
 908}
 909
 910static void tc_taprio_map_maxsdu_txq(struct stmmac_priv *priv,
 911				     struct tc_taprio_qopt_offload *qopt)
 912{
 913	u32 num_tc = qopt->mqprio.qopt.num_tc;
 914	u32 offset, count, i, j;
 915
 916	/* QueueMaxSDU received from the driver corresponds to the Linux traffic
 917	 * class. Map queueMaxSDU per Linux traffic class to DWMAC Tx queues.
 918	 */
 919	for (i = 0; i < num_tc; i++) {
 920		if (!qopt->max_sdu[i])
 921			continue;
 922
 923		offset = qopt->mqprio.qopt.offset[i];
 924		count = qopt->mqprio.qopt.count[i];
 925
 926		for (j = offset; j < offset + count; j++)
 927			priv->est->max_sdu[j] = qopt->max_sdu[i] + ETH_HLEN - ETH_TLEN;
 928	}
 929}
 930
 931static int tc_taprio_configure(struct stmmac_priv *priv,
 932			       struct tc_taprio_qopt_offload *qopt)
 933{
 934	u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
 935	struct netlink_ext_ack *extack = qopt->mqprio.extack;
 936	struct timespec64 time, current_time, qopt_time;
 937	ktime_t current_time_ns;
 938	int i, ret = 0;
 939	u64 ctr;
 940
 941	if (qopt->base_time < 0)
 942		return -ERANGE;
 943
 944	if (!priv->dma_cap.estsel)
 945		return -EOPNOTSUPP;
 946
 947	switch (wid) {
 948	case 0x1:
 949		wid = 16;
 950		break;
 951	case 0x2:
 952		wid = 20;
 953		break;
 954	case 0x3:
 955		wid = 24;
 956		break;
 957	default:
 958		return -EOPNOTSUPP;
 959	}
 960
 961	switch (dep) {
 962	case 0x1:
 963		dep = 64;
 964		break;
 965	case 0x2:
 966		dep = 128;
 967		break;
 968	case 0x3:
 969		dep = 256;
 970		break;
 971	case 0x4:
 972		dep = 512;
 973		break;
 974	case 0x5:
 975		dep = 1024;
 976		break;
 977	default:
 978		return -EOPNOTSUPP;
 979	}
 980
 981	if (qopt->cmd == TAPRIO_CMD_DESTROY)
 982		goto disable;
 983
 984	if (qopt->num_entries > dep)
 985		return -EINVAL;
 986	if (!qopt->cycle_time)
 987		return -ERANGE;
 988	if (qopt->cycle_time_extension >= BIT(wid + 7))
 989		return -ERANGE;
 990
 991	if (!priv->est) {
 992		priv->est = devm_kzalloc(priv->device, sizeof(*priv->est),
 993					 GFP_KERNEL);
 994		if (!priv->est)
 995			return -ENOMEM;
 996
 997		mutex_init(&priv->est_lock);
 998	} else {
 999		mutex_lock(&priv->est_lock);
1000		memset(priv->est, 0, sizeof(*priv->est));
1001		mutex_unlock(&priv->est_lock);
1002	}
1003
1004	size = qopt->num_entries;
1005
1006	mutex_lock(&priv->est_lock);
1007	priv->est->gcl_size = size;
1008	priv->est->enable = qopt->cmd == TAPRIO_CMD_REPLACE;
1009	mutex_unlock(&priv->est_lock);
1010
1011	for (i = 0; i < size; i++) {
1012		s64 delta_ns = qopt->entries[i].interval;
1013		u32 gates = qopt->entries[i].gate_mask;
1014
1015		if (delta_ns > GENMASK(wid - 1, 0))
1016			return -ERANGE;
1017		if (gates > GENMASK(31 - wid, 0))
1018			return -ERANGE;
1019
1020		switch (qopt->entries[i].command) {
1021		case TC_TAPRIO_CMD_SET_GATES:
1022			break;
1023		case TC_TAPRIO_CMD_SET_AND_HOLD:
1024			gates |= BIT(0);
1025			break;
1026		case TC_TAPRIO_CMD_SET_AND_RELEASE:
1027			gates &= ~BIT(0);
1028			break;
1029		default:
1030			return -EOPNOTSUPP;
1031		}
1032
1033		priv->est->gcl[i] = delta_ns | (gates << wid);
1034	}
1035
1036	mutex_lock(&priv->est_lock);
1037	/* Adjust for real system time */
1038	priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, &current_time);
1039	current_time_ns = timespec64_to_ktime(current_time);
1040	time = stmmac_calc_tas_basetime(qopt->base_time, current_time_ns,
1041					qopt->cycle_time);
1042
1043	priv->est->btr[0] = (u32)time.tv_nsec;
1044	priv->est->btr[1] = (u32)time.tv_sec;
1045
1046	qopt_time = ktime_to_timespec64(qopt->base_time);
1047	priv->est->btr_reserve[0] = (u32)qopt_time.tv_nsec;
1048	priv->est->btr_reserve[1] = (u32)qopt_time.tv_sec;
1049
1050	ctr = qopt->cycle_time;
1051	priv->est->ctr[0] = do_div(ctr, NSEC_PER_SEC);
1052	priv->est->ctr[1] = (u32)ctr;
1053
1054	priv->est->ter = qopt->cycle_time_extension;
1055
1056	tc_taprio_map_maxsdu_txq(priv, qopt);
1057
1058	ret = stmmac_est_configure(priv, priv, priv->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	ret = stmmac_fpe_map_preemption_class(priv, priv->dev, extack,
1067					      qopt->mqprio.preemptible_tcs);
1068	if (ret)
1069		goto disable;
1070
1071	return 0;
1072
1073disable:
1074	if (priv->est) {
1075		mutex_lock(&priv->est_lock);
1076		priv->est->enable = false;
1077		stmmac_est_configure(priv, priv, priv->est,
1078				     priv->plat->clk_ptp_rate);
1079		/* Reset taprio status */
1080		for (i = 0; i < priv->plat->tx_queues_to_use; i++) {
1081			priv->xstats.max_sdu_txq_drop[i] = 0;
1082			priv->xstats.mtl_est_txq_hlbf[i] = 0;
1083			priv->xstats.mtl_est_txq_hlbs[i] = 0;
1084		}
1085		mutex_unlock(&priv->est_lock);
1086	}
1087
1088	stmmac_fpe_map_preemption_class(priv, priv->dev, extack, 0);
1089
1090	return ret;
1091}
1092
1093static void tc_taprio_stats(struct stmmac_priv *priv,
1094			    struct tc_taprio_qopt_offload *qopt)
1095{
1096	u64 window_drops = 0;
1097	int i = 0;
1098
1099	for (i = 0; i < priv->plat->tx_queues_to_use; i++)
1100		window_drops += priv->xstats.max_sdu_txq_drop[i] +
1101				priv->xstats.mtl_est_txq_hlbf[i] +
1102				priv->xstats.mtl_est_txq_hlbs[i];
1103	qopt->stats.window_drops = window_drops;
1104
1105	/* Transmission overrun doesn't happen for stmmac, hence always 0 */
1106	qopt->stats.tx_overruns = 0;
1107}
1108
1109static void tc_taprio_queue_stats(struct stmmac_priv *priv,
1110				  struct tc_taprio_qopt_offload *qopt)
1111{
1112	struct tc_taprio_qopt_queue_stats *q_stats = &qopt->queue_stats;
1113	int queue = qopt->queue_stats.queue;
1114
1115	q_stats->stats.window_drops = priv->xstats.max_sdu_txq_drop[queue] +
1116				      priv->xstats.mtl_est_txq_hlbf[queue] +
1117				      priv->xstats.mtl_est_txq_hlbs[queue];
1118
1119	/* Transmission overrun doesn't happen for stmmac, hence always 0 */
1120	q_stats->stats.tx_overruns = 0;
1121}
1122
1123static int tc_setup_taprio(struct stmmac_priv *priv,
1124			   struct tc_taprio_qopt_offload *qopt)
1125{
1126	int err = 0;
1127
1128	switch (qopt->cmd) {
1129	case TAPRIO_CMD_REPLACE:
1130	case TAPRIO_CMD_DESTROY:
1131		err = tc_taprio_configure(priv, qopt);
1132		break;
1133	case TAPRIO_CMD_STATS:
1134		tc_taprio_stats(priv, qopt);
1135		break;
1136	case TAPRIO_CMD_QUEUE_STATS:
1137		tc_taprio_queue_stats(priv, qopt);
1138		break;
1139	default:
1140		err = -EOPNOTSUPP;
1141	}
1142
1143	return err;
1144}
1145
1146static int tc_setup_taprio_without_fpe(struct stmmac_priv *priv,
1147				       struct tc_taprio_qopt_offload *qopt)
1148{
1149	if (!qopt->mqprio.preemptible_tcs)
1150		return tc_setup_taprio(priv, qopt);
1151
1152	NL_SET_ERR_MSG_MOD(qopt->mqprio.extack,
1153			   "taprio with FPE is not implemented for this MAC");
1154
1155	return -EOPNOTSUPP;
1156}
1157
1158static int tc_setup_etf(struct stmmac_priv *priv,
1159			struct tc_etf_qopt_offload *qopt)
1160{
1161	if (!priv->dma_cap.tbssel)
1162		return -EOPNOTSUPP;
1163	if (qopt->queue >= priv->plat->tx_queues_to_use)
1164		return -EINVAL;
1165	if (!(priv->dma_conf.tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL))
1166		return -EINVAL;
1167
1168	if (qopt->enable)
1169		priv->dma_conf.tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN;
1170	else
1171		priv->dma_conf.tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN;
1172
1173	netdev_info(priv->dev, "%s ETF for Queue %d\n",
1174		    qopt->enable ? "enabled" : "disabled", qopt->queue);
1175	return 0;
1176}
1177
1178static int tc_query_caps(struct stmmac_priv *priv,
1179			 struct tc_query_caps_base *base)
1180{
1181	switch (base->type) {
1182	case TC_SETUP_QDISC_MQPRIO: {
1183		struct tc_mqprio_caps *caps = base->caps;
1184
1185		caps->validate_queue_counts = true;
1186
1187		return 0;
1188	}
1189	case TC_SETUP_QDISC_TAPRIO: {
1190		struct tc_taprio_caps *caps = base->caps;
1191
1192		if (!priv->dma_cap.estsel)
1193			return -EOPNOTSUPP;
1194
1195		caps->gate_mask_per_txq = true;
1196		caps->supports_queue_max_sdu = true;
1197
1198		return 0;
1199	}
1200	default:
1201		return -EOPNOTSUPP;
1202	}
1203}
1204
1205static void stmmac_reset_tc_mqprio(struct net_device *ndev,
1206				   struct netlink_ext_ack *extack)
1207{
1208	struct stmmac_priv *priv = netdev_priv(ndev);
1209
1210	netdev_reset_tc(ndev);
1211	netif_set_real_num_tx_queues(ndev, priv->plat->tx_queues_to_use);
1212	stmmac_fpe_map_preemption_class(priv, ndev, extack, 0);
1213}
1214
1215static int tc_setup_dwmac510_mqprio(struct stmmac_priv *priv,
1216				    struct tc_mqprio_qopt_offload *mqprio)
1217{
1218	struct netlink_ext_ack *extack = mqprio->extack;
1219	struct tc_mqprio_qopt *qopt = &mqprio->qopt;
1220	u32 offset, count, num_stack_tx_queues = 0;
1221	struct net_device *ndev = priv->dev;
1222	u32 num_tc = qopt->num_tc;
1223	int err;
1224
1225	if (!num_tc) {
1226		stmmac_reset_tc_mqprio(ndev, extack);
1227		return 0;
1228	}
1229
1230	err = netdev_set_num_tc(ndev, num_tc);
1231	if (err)
1232		return err;
1233
1234	for (u32 tc = 0; tc < num_tc; tc++) {
1235		offset = qopt->offset[tc];
1236		count = qopt->count[tc];
1237		num_stack_tx_queues += count;
1238
1239		err = netdev_set_tc_queue(ndev, tc, count, offset);
1240		if (err)
1241			goto err_reset_tc;
1242	}
1243
1244	err = netif_set_real_num_tx_queues(ndev, num_stack_tx_queues);
1245	if (err)
1246		goto err_reset_tc;
1247
1248	err = stmmac_fpe_map_preemption_class(priv, ndev, extack,
1249					      mqprio->preemptible_tcs);
1250	if (err)
1251		goto err_reset_tc;
1252
1253	return 0;
1254
1255err_reset_tc:
1256	stmmac_reset_tc_mqprio(ndev, extack);
1257
1258	return err;
1259}
1260
1261static int tc_setup_mqprio_unimplemented(struct stmmac_priv *priv,
1262					 struct tc_mqprio_qopt_offload *mqprio)
1263{
1264	NL_SET_ERR_MSG_MOD(mqprio->extack,
1265			   "mqprio HW offload is not implemented for this MAC");
1266	return -EOPNOTSUPP;
1267}
1268
1269const struct stmmac_tc_ops dwmac4_tc_ops = {
1270	.init = tc_init,
1271	.setup_cls_u32 = tc_setup_cls_u32,
1272	.setup_cbs = tc_setup_cbs,
1273	.setup_cls = tc_setup_cls,
1274	.setup_taprio = tc_setup_taprio_without_fpe,
1275	.setup_etf = tc_setup_etf,
1276	.query_caps = tc_query_caps,
1277	.setup_mqprio = tc_setup_mqprio_unimplemented,
1278};
1279
1280const struct stmmac_tc_ops dwmac510_tc_ops = {
1281	.init = tc_init,
1282	.setup_cls_u32 = tc_setup_cls_u32,
1283	.setup_cbs = tc_setup_cbs,
1284	.setup_cls = tc_setup_cls,
1285	.setup_taprio = tc_setup_taprio,
1286	.setup_etf = tc_setup_etf,
1287	.query_caps = tc_query_caps,
1288	.setup_mqprio = tc_setup_dwmac510_mqprio,
1289};