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 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 342static 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 428static 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 458static 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 476static 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 511static 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 560static 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 578static 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 587static 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 618static 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 641static 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 661static 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 712static 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 732static 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 804static 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 840static 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 856static 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 872static 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 895struct 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 918static void tc_taprio_map_maxsdu_txq(struct stmmac_priv *priv, 919 struct tc_taprio_qopt_offload *qopt) 920{ 921 struct plat_stmmacenet_data *plat = priv->plat; 922 u32 num_tc = qopt->mqprio.qopt.num_tc; 923 u32 offset, count, i, j; 924 925 /* QueueMaxSDU received from the driver corresponds to the Linux traffic 926 * class. Map queueMaxSDU per Linux traffic class to DWMAC Tx queues. 927 */ 928 for (i = 0; i < num_tc; i++) { 929 if (!qopt->max_sdu[i]) 930 continue; 931 932 offset = qopt->mqprio.qopt.offset[i]; 933 count = qopt->mqprio.qopt.count[i]; 934 935 for (j = offset; j < offset + count; j++) 936 plat->est->max_sdu[j] = qopt->max_sdu[i] + ETH_HLEN - ETH_TLEN; 937 } 938} 939 940static int tc_taprio_configure(struct stmmac_priv *priv, 941 struct tc_taprio_qopt_offload *qopt) 942{ 943 u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep; 944 struct plat_stmmacenet_data *plat = priv->plat; 945 struct timespec64 time, current_time, qopt_time; 946 ktime_t current_time_ns; 947 bool fpe = false; 948 int i, ret = 0; 949 u64 ctr; 950 951 if (qopt->base_time < 0) 952 return -ERANGE; 953 954 if (!priv->dma_cap.estsel) 955 return -EOPNOTSUPP; 956 957 switch (wid) { 958 case 0x1: 959 wid = 16; 960 break; 961 case 0x2: 962 wid = 20; 963 break; 964 case 0x3: 965 wid = 24; 966 break; 967 default: 968 return -EOPNOTSUPP; 969 } 970 971 switch (dep) { 972 case 0x1: 973 dep = 64; 974 break; 975 case 0x2: 976 dep = 128; 977 break; 978 case 0x3: 979 dep = 256; 980 break; 981 case 0x4: 982 dep = 512; 983 break; 984 case 0x5: 985 dep = 1024; 986 break; 987 default: 988 return -EOPNOTSUPP; 989 } 990 991 if (qopt->cmd == TAPRIO_CMD_DESTROY) 992 goto disable; 993 994 if (qopt->num_entries >= dep) 995 return -EINVAL; 996 if (!qopt->cycle_time) 997 return -ERANGE; 998 if (qopt->cycle_time_extension >= BIT(wid + 7)) 999 return -ERANGE; 1000 1001 if (!plat->est) { 1002 plat->est = devm_kzalloc(priv->device, sizeof(*plat->est), 1003 GFP_KERNEL); 1004 if (!plat->est) 1005 return -ENOMEM; 1006 1007 mutex_init(&priv->plat->est->lock); 1008 } else { 1009 memset(plat->est, 0, sizeof(*plat->est)); 1010 } 1011 1012 size = qopt->num_entries; 1013 1014 mutex_lock(&priv->plat->est->lock); 1015 priv->plat->est->gcl_size = size; 1016 priv->plat->est->enable = qopt->cmd == TAPRIO_CMD_REPLACE; 1017 mutex_unlock(&priv->plat->est->lock); 1018 1019 for (i = 0; i < size; i++) { 1020 s64 delta_ns = qopt->entries[i].interval; 1021 u32 gates = qopt->entries[i].gate_mask; 1022 1023 if (delta_ns > GENMASK(wid, 0)) 1024 return -ERANGE; 1025 if (gates > GENMASK(31 - wid, 0)) 1026 return -ERANGE; 1027 1028 switch (qopt->entries[i].command) { 1029 case TC_TAPRIO_CMD_SET_GATES: 1030 if (fpe) 1031 return -EINVAL; 1032 break; 1033 case TC_TAPRIO_CMD_SET_AND_HOLD: 1034 gates |= BIT(0); 1035 fpe = true; 1036 break; 1037 case TC_TAPRIO_CMD_SET_AND_RELEASE: 1038 gates &= ~BIT(0); 1039 fpe = true; 1040 break; 1041 default: 1042 return -EOPNOTSUPP; 1043 } 1044 1045 priv->plat->est->gcl[i] = delta_ns | (gates << wid); 1046 } 1047 1048 mutex_lock(&priv->plat->est->lock); 1049 /* Adjust for real system time */ 1050 priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, ¤t_time); 1051 current_time_ns = timespec64_to_ktime(current_time); 1052 time = stmmac_calc_tas_basetime(qopt->base_time, current_time_ns, 1053 qopt->cycle_time); 1054 1055 priv->plat->est->btr[0] = (u32)time.tv_nsec; 1056 priv->plat->est->btr[1] = (u32)time.tv_sec; 1057 1058 qopt_time = ktime_to_timespec64(qopt->base_time); 1059 priv->plat->est->btr_reserve[0] = (u32)qopt_time.tv_nsec; 1060 priv->plat->est->btr_reserve[1] = (u32)qopt_time.tv_sec; 1061 1062 ctr = qopt->cycle_time; 1063 priv->plat->est->ctr[0] = do_div(ctr, NSEC_PER_SEC); 1064 priv->plat->est->ctr[1] = (u32)ctr; 1065 1066 priv->plat->est->ter = qopt->cycle_time_extension; 1067 1068 tc_taprio_map_maxsdu_txq(priv, qopt); 1069 1070 if (fpe && !priv->dma_cap.fpesel) { 1071 mutex_unlock(&priv->plat->est->lock); 1072 return -EOPNOTSUPP; 1073 } 1074 1075 /* Actual FPE register configuration will be done after FPE handshake 1076 * is success. 1077 */ 1078 priv->plat->fpe_cfg->enable = fpe; 1079 1080 ret = stmmac_est_configure(priv, priv, priv->plat->est, 1081 priv->plat->clk_ptp_rate); 1082 mutex_unlock(&priv->plat->est->lock); 1083 if (ret) { 1084 netdev_err(priv->dev, "failed to configure EST\n"); 1085 goto disable; 1086 } 1087 1088 netdev_info(priv->dev, "configured EST\n"); 1089 1090 if (fpe) { 1091 stmmac_fpe_handshake(priv, true); 1092 netdev_info(priv->dev, "start FPE handshake\n"); 1093 } 1094 1095 return 0; 1096 1097disable: 1098 if (priv->plat->est) { 1099 mutex_lock(&priv->plat->est->lock); 1100 priv->plat->est->enable = false; 1101 stmmac_est_configure(priv, priv, priv->plat->est, 1102 priv->plat->clk_ptp_rate); 1103 /* Reset taprio status */ 1104 for (i = 0; i < priv->plat->tx_queues_to_use; i++) { 1105 priv->xstats.max_sdu_txq_drop[i] = 0; 1106 priv->xstats.mtl_est_txq_hlbf[i] = 0; 1107 } 1108 mutex_unlock(&priv->plat->est->lock); 1109 } 1110 1111 priv->plat->fpe_cfg->enable = false; 1112 stmmac_fpe_configure(priv, priv->ioaddr, 1113 priv->plat->fpe_cfg, 1114 priv->plat->tx_queues_to_use, 1115 priv->plat->rx_queues_to_use, 1116 false); 1117 netdev_info(priv->dev, "disabled FPE\n"); 1118 1119 stmmac_fpe_handshake(priv, false); 1120 netdev_info(priv->dev, "stop FPE handshake\n"); 1121 1122 return ret; 1123} 1124 1125static void tc_taprio_stats(struct stmmac_priv *priv, 1126 struct tc_taprio_qopt_offload *qopt) 1127{ 1128 u64 window_drops = 0; 1129 int i = 0; 1130 1131 for (i = 0; i < priv->plat->tx_queues_to_use; i++) 1132 window_drops += priv->xstats.max_sdu_txq_drop[i] + 1133 priv->xstats.mtl_est_txq_hlbf[i]; 1134 qopt->stats.window_drops = window_drops; 1135 1136 /* Transmission overrun doesn't happen for stmmac, hence always 0 */ 1137 qopt->stats.tx_overruns = 0; 1138} 1139 1140static void tc_taprio_queue_stats(struct stmmac_priv *priv, 1141 struct tc_taprio_qopt_offload *qopt) 1142{ 1143 struct tc_taprio_qopt_queue_stats *q_stats = &qopt->queue_stats; 1144 int queue = qopt->queue_stats.queue; 1145 1146 q_stats->stats.window_drops = priv->xstats.max_sdu_txq_drop[queue] + 1147 priv->xstats.mtl_est_txq_hlbf[queue]; 1148 1149 /* Transmission overrun doesn't happen for stmmac, hence always 0 */ 1150 q_stats->stats.tx_overruns = 0; 1151} 1152 1153static int tc_setup_taprio(struct stmmac_priv *priv, 1154 struct tc_taprio_qopt_offload *qopt) 1155{ 1156 int err = 0; 1157 1158 switch (qopt->cmd) { 1159 case TAPRIO_CMD_REPLACE: 1160 case TAPRIO_CMD_DESTROY: 1161 err = tc_taprio_configure(priv, qopt); 1162 break; 1163 case TAPRIO_CMD_STATS: 1164 tc_taprio_stats(priv, qopt); 1165 break; 1166 case TAPRIO_CMD_QUEUE_STATS: 1167 tc_taprio_queue_stats(priv, qopt); 1168 break; 1169 default: 1170 err = -EOPNOTSUPP; 1171 } 1172 1173 return err; 1174} 1175 1176static int tc_setup_etf(struct stmmac_priv *priv, 1177 struct tc_etf_qopt_offload *qopt) 1178{ 1179 if (!priv->dma_cap.tbssel) 1180 return -EOPNOTSUPP; 1181 if (qopt->queue >= priv->plat->tx_queues_to_use) 1182 return -EINVAL; 1183 if (!(priv->dma_conf.tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL)) 1184 return -EINVAL; 1185 1186 if (qopt->enable) 1187 priv->dma_conf.tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN; 1188 else 1189 priv->dma_conf.tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN; 1190 1191 netdev_info(priv->dev, "%s ETF for Queue %d\n", 1192 qopt->enable ? "enabled" : "disabled", qopt->queue); 1193 return 0; 1194} 1195 1196static int tc_query_caps(struct stmmac_priv *priv, 1197 struct tc_query_caps_base *base) 1198{ 1199 switch (base->type) { 1200 case TC_SETUP_QDISC_TAPRIO: { 1201 struct tc_taprio_caps *caps = base->caps; 1202 1203 if (!priv->dma_cap.estsel) 1204 return -EOPNOTSUPP; 1205 1206 caps->gate_mask_per_txq = true; 1207 caps->supports_queue_max_sdu = true; 1208 1209 return 0; 1210 } 1211 default: 1212 return -EOPNOTSUPP; 1213 } 1214} 1215 1216const struct stmmac_tc_ops dwmac510_tc_ops = { 1217 .init = tc_init, 1218 .setup_cls_u32 = tc_setup_cls_u32, 1219 .setup_cbs = tc_setup_cbs, 1220 .setup_cls = tc_setup_cls, 1221 .setup_taprio = tc_setup_taprio, 1222 .setup_etf = tc_setup_etf, 1223 .query_caps = tc_query_caps, 1224}; 1225