1 // SPDX-License-Identifier: (GPL-2.0 OR MIT) 2 /* 3 * Copyright (c) 2018 Synopsys, Inc. and/or its affiliates. 4 * stmmac TC Handling (HW only) 5 */ 6 7 #include <net/pkt_cls.h> 8 #include <net/tc_act/tc_gact.h> 9 #include "common.h" 10 #include "dwmac4.h" 11 #include "dwmac5.h" 12 #include "stmmac.h" 13 14 static void tc_fill_all_pass_entry(struct stmmac_tc_entry *entry) 15 { 16 memset(entry, 0, sizeof(*entry)); 17 entry->in_use = true; 18 entry->is_last = true; 19 entry->is_frag = false; 20 entry->prio = ~0x0; 21 entry->handle = 0; 22 entry->val.match_data = 0x0; 23 entry->val.match_en = 0x0; 24 entry->val.af = 1; 25 entry->val.dma_ch_no = 0x0; 26 } 27 28 static struct stmmac_tc_entry *tc_find_entry(struct stmmac_priv *priv, 29 struct tc_cls_u32_offload *cls, 30 bool free) 31 { 32 struct stmmac_tc_entry *entry, *first = NULL, *dup = NULL; 33 u32 loc = cls->knode.handle; 34 int i; 35 36 for (i = 0; i < priv->tc_entries_max; i++) { 37 entry = &priv->tc_entries[i]; 38 if (!entry->in_use && !first && free) 39 first = entry; 40 if ((entry->handle == loc) && !free && !entry->is_frag) 41 dup = entry; 42 } 43 44 if (dup) 45 return dup; 46 if (first) { 47 first->handle = loc; 48 first->in_use = true; 49 50 /* Reset HW values */ 51 memset(&first->val, 0, sizeof(first->val)); 52 } 53 54 return first; 55 } 56 57 static int tc_fill_actions(struct stmmac_tc_entry *entry, 58 struct stmmac_tc_entry *frag, 59 struct tc_cls_u32_offload *cls) 60 { 61 struct stmmac_tc_entry *action_entry = entry; 62 const struct tc_action *act; 63 struct tcf_exts *exts; 64 int i; 65 66 exts = cls->knode.exts; 67 if (!tcf_exts_has_actions(exts)) 68 return -EINVAL; 69 if (frag) 70 action_entry = frag; 71 72 tcf_exts_for_each_action(i, act, exts) { 73 /* Accept */ 74 if (is_tcf_gact_ok(act)) { 75 action_entry->val.af = 1; 76 break; 77 } 78 /* Drop */ 79 if (is_tcf_gact_shot(act)) { 80 action_entry->val.rf = 1; 81 break; 82 } 83 84 /* Unsupported */ 85 return -EINVAL; 86 } 87 88 return 0; 89 } 90 91 static int tc_fill_entry(struct stmmac_priv *priv, 92 struct tc_cls_u32_offload *cls) 93 { 94 struct stmmac_tc_entry *entry, *frag = NULL; 95 struct tc_u32_sel *sel = cls->knode.sel; 96 u32 off, data, mask, real_off, rem; 97 u32 prio = cls->common.prio << 16; 98 int ret; 99 100 /* Only 1 match per entry */ 101 if (sel->nkeys <= 0 || sel->nkeys > 1) 102 return -EINVAL; 103 104 off = sel->keys[0].off << sel->offshift; 105 data = sel->keys[0].val; 106 mask = sel->keys[0].mask; 107 108 switch (ntohs(cls->common.protocol)) { 109 case ETH_P_ALL: 110 break; 111 case ETH_P_IP: 112 off += ETH_HLEN; 113 break; 114 default: 115 return -EINVAL; 116 } 117 118 if (off > priv->tc_off_max) 119 return -EINVAL; 120 121 real_off = off / 4; 122 rem = off % 4; 123 124 entry = tc_find_entry(priv, cls, true); 125 if (!entry) 126 return -EINVAL; 127 128 if (rem) { 129 frag = tc_find_entry(priv, cls, true); 130 if (!frag) { 131 ret = -EINVAL; 132 goto err_unuse; 133 } 134 135 entry->frag_ptr = frag; 136 entry->val.match_en = (mask << (rem * 8)) & 137 GENMASK(31, rem * 8); 138 entry->val.match_data = (data << (rem * 8)) & 139 GENMASK(31, rem * 8); 140 entry->val.frame_offset = real_off; 141 entry->prio = prio; 142 143 frag->val.match_en = (mask >> (rem * 8)) & 144 GENMASK(rem * 8 - 1, 0); 145 frag->val.match_data = (data >> (rem * 8)) & 146 GENMASK(rem * 8 - 1, 0); 147 frag->val.frame_offset = real_off + 1; 148 frag->prio = prio; 149 frag->is_frag = true; 150 } else { 151 entry->frag_ptr = NULL; 152 entry->val.match_en = mask; 153 entry->val.match_data = data; 154 entry->val.frame_offset = real_off; 155 entry->prio = prio; 156 } 157 158 ret = tc_fill_actions(entry, frag, cls); 159 if (ret) 160 goto err_unuse; 161 162 return 0; 163 164 err_unuse: 165 if (frag) 166 frag->in_use = false; 167 entry->in_use = false; 168 return ret; 169 } 170 171 static void tc_unfill_entry(struct stmmac_priv *priv, 172 struct tc_cls_u32_offload *cls) 173 { 174 struct stmmac_tc_entry *entry; 175 176 entry = tc_find_entry(priv, cls, false); 177 if (!entry) 178 return; 179 180 entry->in_use = false; 181 if (entry->frag_ptr) { 182 entry = entry->frag_ptr; 183 entry->is_frag = false; 184 entry->in_use = false; 185 } 186 } 187 188 static int tc_config_knode(struct stmmac_priv *priv, 189 struct tc_cls_u32_offload *cls) 190 { 191 int ret; 192 193 ret = tc_fill_entry(priv, cls); 194 if (ret) 195 return ret; 196 197 ret = stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries, 198 priv->tc_entries_max); 199 if (ret) 200 goto err_unfill; 201 202 return 0; 203 204 err_unfill: 205 tc_unfill_entry(priv, cls); 206 return ret; 207 } 208 209 static int tc_delete_knode(struct stmmac_priv *priv, 210 struct tc_cls_u32_offload *cls) 211 { 212 /* Set entry and fragments as not used */ 213 tc_unfill_entry(priv, cls); 214 215 return stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries, 216 priv->tc_entries_max); 217 } 218 219 static int tc_setup_cls_u32(struct stmmac_priv *priv, 220 struct tc_cls_u32_offload *cls) 221 { 222 switch (cls->command) { 223 case TC_CLSU32_REPLACE_KNODE: 224 tc_unfill_entry(priv, cls); 225 fallthrough; 226 case TC_CLSU32_NEW_KNODE: 227 return tc_config_knode(priv, cls); 228 case TC_CLSU32_DELETE_KNODE: 229 return tc_delete_knode(priv, cls); 230 default: 231 return -EOPNOTSUPP; 232 } 233 } 234 235 static int tc_rfs_init(struct stmmac_priv *priv) 236 { 237 int i; 238 239 priv->rfs_entries_max[STMMAC_RFS_T_VLAN] = 8; 240 priv->rfs_entries_max[STMMAC_RFS_T_LLDP] = 1; 241 priv->rfs_entries_max[STMMAC_RFS_T_1588] = 1; 242 243 for (i = 0; i < STMMAC_RFS_T_MAX; i++) 244 priv->rfs_entries_total += priv->rfs_entries_max[i]; 245 246 priv->rfs_entries = devm_kcalloc(priv->device, 247 priv->rfs_entries_total, 248 sizeof(*priv->rfs_entries), 249 GFP_KERNEL); 250 if (!priv->rfs_entries) 251 return -ENOMEM; 252 253 dev_info(priv->device, "Enabled RFS Flow TC (entries=%d)\n", 254 priv->rfs_entries_total); 255 256 return 0; 257 } 258 259 static int tc_init(struct stmmac_priv *priv) 260 { 261 struct dma_features *dma_cap = &priv->dma_cap; 262 unsigned int count; 263 int ret, i; 264 265 if (dma_cap->l3l4fnum) { 266 priv->flow_entries_max = dma_cap->l3l4fnum; 267 priv->flow_entries = devm_kcalloc(priv->device, 268 dma_cap->l3l4fnum, 269 sizeof(*priv->flow_entries), 270 GFP_KERNEL); 271 if (!priv->flow_entries) 272 return -ENOMEM; 273 274 for (i = 0; i < priv->flow_entries_max; i++) 275 priv->flow_entries[i].idx = i; 276 277 dev_info(priv->device, "Enabled L3L4 Flow TC (entries=%d)\n", 278 priv->flow_entries_max); 279 } 280 281 ret = tc_rfs_init(priv); 282 if (ret) 283 return -ENOMEM; 284 285 /* 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 332 static 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 420 static 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 450 static 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 468 static 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 503 static 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 552 static 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 570 static 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 579 static 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 610 static 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 633 static 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 653 static 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 704 static 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 724 static 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 796 static 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 832 static 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 848 static 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 864 static 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 887 struct 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 910 static 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 931 static 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, 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, ¤t_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 1073 disable: 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 } 1084 mutex_unlock(&priv->est_lock); 1085 } 1086 1087 stmmac_fpe_map_preemption_class(priv, priv->dev, extack, 0); 1088 1089 return ret; 1090 } 1091 1092 static void tc_taprio_stats(struct stmmac_priv *priv, 1093 struct tc_taprio_qopt_offload *qopt) 1094 { 1095 u64 window_drops = 0; 1096 int i = 0; 1097 1098 for (i = 0; i < priv->plat->tx_queues_to_use; i++) 1099 window_drops += priv->xstats.max_sdu_txq_drop[i] + 1100 priv->xstats.mtl_est_txq_hlbf[i]; 1101 qopt->stats.window_drops = window_drops; 1102 1103 /* Transmission overrun doesn't happen for stmmac, hence always 0 */ 1104 qopt->stats.tx_overruns = 0; 1105 } 1106 1107 static void tc_taprio_queue_stats(struct stmmac_priv *priv, 1108 struct tc_taprio_qopt_offload *qopt) 1109 { 1110 struct tc_taprio_qopt_queue_stats *q_stats = &qopt->queue_stats; 1111 int queue = qopt->queue_stats.queue; 1112 1113 q_stats->stats.window_drops = priv->xstats.max_sdu_txq_drop[queue] + 1114 priv->xstats.mtl_est_txq_hlbf[queue]; 1115 1116 /* Transmission overrun doesn't happen for stmmac, hence always 0 */ 1117 q_stats->stats.tx_overruns = 0; 1118 } 1119 1120 static int tc_setup_taprio(struct stmmac_priv *priv, 1121 struct tc_taprio_qopt_offload *qopt) 1122 { 1123 int err = 0; 1124 1125 switch (qopt->cmd) { 1126 case TAPRIO_CMD_REPLACE: 1127 case TAPRIO_CMD_DESTROY: 1128 err = tc_taprio_configure(priv, qopt); 1129 break; 1130 case TAPRIO_CMD_STATS: 1131 tc_taprio_stats(priv, qopt); 1132 break; 1133 case TAPRIO_CMD_QUEUE_STATS: 1134 tc_taprio_queue_stats(priv, qopt); 1135 break; 1136 default: 1137 err = -EOPNOTSUPP; 1138 } 1139 1140 return err; 1141 } 1142 1143 static int tc_setup_taprio_without_fpe(struct stmmac_priv *priv, 1144 struct tc_taprio_qopt_offload *qopt) 1145 { 1146 if (!qopt->mqprio.preemptible_tcs) 1147 return tc_setup_taprio(priv, qopt); 1148 1149 NL_SET_ERR_MSG_MOD(qopt->mqprio.extack, 1150 "taprio with FPE is not implemented for this MAC"); 1151 1152 return -EOPNOTSUPP; 1153 } 1154 1155 static int tc_setup_etf(struct stmmac_priv *priv, 1156 struct tc_etf_qopt_offload *qopt) 1157 { 1158 if (!priv->dma_cap.tbssel) 1159 return -EOPNOTSUPP; 1160 if (qopt->queue >= priv->plat->tx_queues_to_use) 1161 return -EINVAL; 1162 if (!(priv->dma_conf.tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL)) 1163 return -EINVAL; 1164 1165 if (qopt->enable) 1166 priv->dma_conf.tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN; 1167 else 1168 priv->dma_conf.tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN; 1169 1170 netdev_info(priv->dev, "%s ETF for Queue %d\n", 1171 qopt->enable ? "enabled" : "disabled", qopt->queue); 1172 return 0; 1173 } 1174 1175 static int tc_query_caps(struct stmmac_priv *priv, 1176 struct tc_query_caps_base *base) 1177 { 1178 switch (base->type) { 1179 case TC_SETUP_QDISC_MQPRIO: { 1180 struct tc_mqprio_caps *caps = base->caps; 1181 1182 caps->validate_queue_counts = true; 1183 1184 return 0; 1185 } 1186 case TC_SETUP_QDISC_TAPRIO: { 1187 struct tc_taprio_caps *caps = base->caps; 1188 1189 if (!priv->dma_cap.estsel) 1190 return -EOPNOTSUPP; 1191 1192 caps->gate_mask_per_txq = true; 1193 caps->supports_queue_max_sdu = true; 1194 1195 return 0; 1196 } 1197 default: 1198 return -EOPNOTSUPP; 1199 } 1200 } 1201 1202 static void stmmac_reset_tc_mqprio(struct net_device *ndev, 1203 struct netlink_ext_ack *extack) 1204 { 1205 struct stmmac_priv *priv = netdev_priv(ndev); 1206 1207 netdev_reset_tc(ndev); 1208 netif_set_real_num_tx_queues(ndev, priv->plat->tx_queues_to_use); 1209 stmmac_fpe_map_preemption_class(priv, ndev, extack, 0); 1210 } 1211 1212 static int tc_setup_dwmac510_mqprio(struct stmmac_priv *priv, 1213 struct tc_mqprio_qopt_offload *mqprio) 1214 { 1215 struct netlink_ext_ack *extack = mqprio->extack; 1216 struct tc_mqprio_qopt *qopt = &mqprio->qopt; 1217 u32 offset, count, num_stack_tx_queues = 0; 1218 struct net_device *ndev = priv->dev; 1219 u32 num_tc = qopt->num_tc; 1220 int err; 1221 1222 if (!num_tc) { 1223 stmmac_reset_tc_mqprio(ndev, extack); 1224 return 0; 1225 } 1226 1227 err = netdev_set_num_tc(ndev, num_tc); 1228 if (err) 1229 return err; 1230 1231 for (u32 tc = 0; tc < num_tc; tc++) { 1232 offset = qopt->offset[tc]; 1233 count = qopt->count[tc]; 1234 num_stack_tx_queues += count; 1235 1236 err = netdev_set_tc_queue(ndev, tc, count, offset); 1237 if (err) 1238 goto err_reset_tc; 1239 } 1240 1241 err = netif_set_real_num_tx_queues(ndev, num_stack_tx_queues); 1242 if (err) 1243 goto err_reset_tc; 1244 1245 err = stmmac_fpe_map_preemption_class(priv, ndev, extack, 1246 mqprio->preemptible_tcs); 1247 if (err) 1248 goto err_reset_tc; 1249 1250 return 0; 1251 1252 err_reset_tc: 1253 stmmac_reset_tc_mqprio(ndev, extack); 1254 1255 return err; 1256 } 1257 1258 static int tc_setup_mqprio_unimplemented(struct stmmac_priv *priv, 1259 struct tc_mqprio_qopt_offload *mqprio) 1260 { 1261 NL_SET_ERR_MSG_MOD(mqprio->extack, 1262 "mqprio HW offload is not implemented for this MAC"); 1263 return -EOPNOTSUPP; 1264 } 1265 1266 const struct stmmac_tc_ops dwmac4_tc_ops = { 1267 .init = tc_init, 1268 .setup_cls_u32 = tc_setup_cls_u32, 1269 .setup_cbs = tc_setup_cbs, 1270 .setup_cls = tc_setup_cls, 1271 .setup_taprio = tc_setup_taprio_without_fpe, 1272 .setup_etf = tc_setup_etf, 1273 .query_caps = tc_query_caps, 1274 .setup_mqprio = tc_setup_mqprio_unimplemented, 1275 }; 1276 1277 const struct stmmac_tc_ops dwmac510_tc_ops = { 1278 .init = tc_init, 1279 .setup_cls_u32 = tc_setup_cls_u32, 1280 .setup_cbs = tc_setup_cbs, 1281 .setup_cls = tc_setup_cls, 1282 .setup_taprio = tc_setup_taprio, 1283 .setup_etf = tc_setup_etf, 1284 .query_caps = tc_query_caps, 1285 .setup_mqprio = tc_setup_dwmac510_mqprio, 1286 }; 1287 1288 const struct stmmac_tc_ops dwxgmac_tc_ops = { 1289 .init = tc_init, 1290 .setup_cls_u32 = tc_setup_cls_u32, 1291 .setup_cbs = tc_setup_cbs, 1292 .setup_cls = tc_setup_cls, 1293 .setup_taprio = tc_setup_taprio_without_fpe, 1294 .setup_etf = tc_setup_etf, 1295 .query_caps = tc_query_caps, 1296 .setup_mqprio = tc_setup_mqprio_unimplemented, 1297 }; 1298