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 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 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 #define IP_PROTO_FULL_MASK 0xFF 450 451 static int tc_add_basic_flow(struct stmmac_priv *priv, 452 struct flow_cls_offload *cls, 453 struct stmmac_flow_entry *entry) 454 { 455 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 456 struct flow_dissector *dissector = rule->match.dissector; 457 struct flow_match_basic match; 458 459 /* Nothing to do here */ 460 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC)) 461 return -EINVAL; 462 463 flow_rule_match_basic(rule, &match); 464 465 /* Both network proto and transport proto not present in the key */ 466 if (!match.mask || !(match.mask->n_proto || match.mask->ip_proto)) { 467 NL_SET_ERR_MSG_MOD(cls->common.extack, 468 "filter must specify network or transport protocol"); 469 return -EOPNOTSUPP; 470 } 471 472 /* If the proto is present in the key and is not full mask */ 473 if ((match.mask->n_proto && match.mask->n_proto != ETHER_TYPE_FULL_MASK) || 474 (match.mask->ip_proto && match.mask->ip_proto != IP_PROTO_FULL_MASK)) { 475 NL_SET_ERR_MSG_MOD(cls->common.extack, 476 "only full protocol mask is supported"); 477 return -EOPNOTSUPP; 478 } 479 480 /* Network proto is present in the key and is not IPv4 */ 481 if (match.mask->n_proto && match.key->n_proto != cpu_to_be16(ETH_P_IP)) { 482 NL_SET_ERR_MSG_MOD(cls->common.extack, 483 "only IPv4 network protocol is supported"); 484 return -EOPNOTSUPP; 485 } 486 487 /* Transport proto is present in the key and is not TCP or UDP */ 488 if (match.mask->ip_proto && 489 match.key->ip_proto != IPPROTO_TCP && 490 match.key->ip_proto != IPPROTO_UDP) { 491 NL_SET_ERR_MSG_MOD(cls->common.extack, 492 "only TCP and UDP transport protocols are supported"); 493 return -EOPNOTSUPP; 494 } 495 496 entry->ip_proto = match.key->ip_proto; 497 return 0; 498 } 499 500 static int tc_add_ip4_flow(struct stmmac_priv *priv, 501 struct flow_cls_offload *cls, 502 struct stmmac_flow_entry *entry) 503 { 504 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 505 struct flow_dissector *dissector = rule->match.dissector; 506 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP; 507 struct flow_match_ipv4_addrs match; 508 u32 hw_match; 509 int ret; 510 511 /* Nothing to do here */ 512 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS)) 513 return -EINVAL; 514 515 flow_rule_match_ipv4_addrs(rule, &match); 516 hw_match = ntohl(match.key->src) & ntohl(match.mask->src); 517 if (hw_match) { 518 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true, 519 false, true, inv, hw_match); 520 if (ret) 521 return ret; 522 } 523 524 hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst); 525 if (hw_match) { 526 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true, 527 false, false, inv, hw_match); 528 if (ret) 529 return ret; 530 } 531 532 return 0; 533 } 534 535 static int tc_add_ports_flow(struct stmmac_priv *priv, 536 struct flow_cls_offload *cls, 537 struct stmmac_flow_entry *entry) 538 { 539 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 540 struct flow_dissector *dissector = rule->match.dissector; 541 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP; 542 struct flow_match_ports match; 543 u32 hw_match; 544 bool is_udp; 545 int ret; 546 547 /* Nothing to do here */ 548 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS)) 549 return -EINVAL; 550 551 switch (entry->ip_proto) { 552 case IPPROTO_TCP: 553 is_udp = false; 554 break; 555 case IPPROTO_UDP: 556 is_udp = true; 557 break; 558 default: 559 return -EINVAL; 560 } 561 562 flow_rule_match_ports(rule, &match); 563 564 hw_match = ntohs(match.key->src) & ntohs(match.mask->src); 565 if (hw_match) { 566 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true, 567 is_udp, true, inv, hw_match); 568 if (ret) 569 return ret; 570 } 571 572 hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst); 573 if (hw_match) { 574 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true, 575 is_udp, false, inv, hw_match); 576 if (ret) 577 return ret; 578 } 579 580 entry->is_l4 = true; 581 return 0; 582 } 583 584 static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv, 585 struct flow_cls_offload *cls, 586 bool get_free) 587 { 588 int i; 589 590 for (i = 0; i < priv->flow_entries_max; i++) { 591 struct stmmac_flow_entry *entry = &priv->flow_entries[i]; 592 593 if (entry->cookie == cls->cookie) 594 return entry; 595 if (get_free && (entry->in_use == false)) 596 return entry; 597 } 598 599 return NULL; 600 } 601 602 static struct { 603 int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls, 604 struct stmmac_flow_entry *entry); 605 } tc_flow_parsers[] = { 606 { .fn = tc_add_basic_flow }, 607 { .fn = tc_add_ip4_flow }, 608 { .fn = tc_add_ports_flow }, 609 }; 610 611 static int tc_add_flow(struct stmmac_priv *priv, 612 struct flow_cls_offload *cls) 613 { 614 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false); 615 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 616 int i, ret; 617 618 if (!entry) { 619 entry = tc_find_flow(priv, cls, true); 620 if (!entry) 621 return -ENOENT; 622 } 623 624 ret = tc_parse_flow_actions(priv, &rule->action, entry, 625 cls->common.extack); 626 if (ret) 627 return ret; 628 629 for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) { 630 ret = tc_flow_parsers[i].fn(priv, cls, entry); 631 if (!ret) 632 entry->in_use = true; 633 else if (ret == -EOPNOTSUPP) 634 return ret; 635 } 636 637 if (!entry->in_use) 638 return -EINVAL; 639 640 entry->cookie = cls->cookie; 641 return 0; 642 } 643 644 static int tc_del_flow(struct stmmac_priv *priv, 645 struct flow_cls_offload *cls) 646 { 647 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false); 648 int ret; 649 650 if (!entry || !entry->in_use) 651 return -ENOENT; 652 653 if (entry->is_l4) { 654 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false, 655 false, false, false, 0); 656 } else { 657 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false, 658 false, false, false, 0); 659 } 660 661 entry->in_use = false; 662 entry->cookie = 0; 663 entry->is_l4 = false; 664 entry->action = 0; 665 return ret; 666 } 667 668 static struct stmmac_rfs_entry *tc_find_rfs(struct stmmac_priv *priv, 669 struct flow_cls_offload *cls, 670 bool get_free) 671 { 672 int i; 673 674 for (i = 0; i < priv->rfs_entries_total; i++) { 675 struct stmmac_rfs_entry *entry = &priv->rfs_entries[i]; 676 677 if (entry->cookie == cls->cookie) 678 return entry; 679 if (get_free && entry->in_use == false) 680 return entry; 681 } 682 683 return NULL; 684 } 685 686 #define VLAN_PRIO_FULL_MASK (0x07) 687 688 static int tc_add_vlan_flow(struct stmmac_priv *priv, 689 struct flow_cls_offload *cls) 690 { 691 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false); 692 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 693 struct flow_dissector *dissector = rule->match.dissector; 694 int tc = tc_classid_to_hwtc(priv->dev, cls->classid); 695 struct flow_match_vlan match; 696 697 if (!entry) { 698 entry = tc_find_rfs(priv, cls, true); 699 if (!entry) 700 return -ENOENT; 701 } 702 703 if (priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN] >= 704 priv->rfs_entries_max[STMMAC_RFS_T_VLAN]) 705 return -ENOENT; 706 707 /* Nothing to do here */ 708 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_VLAN)) 709 return -EINVAL; 710 711 if (tc < 0) { 712 netdev_err(priv->dev, "Invalid traffic class\n"); 713 return -EINVAL; 714 } 715 716 flow_rule_match_vlan(rule, &match); 717 718 if (match.mask->vlan_priority) { 719 u32 prio; 720 721 if (match.mask->vlan_priority != VLAN_PRIO_FULL_MASK) { 722 netdev_err(priv->dev, "Only full mask is supported for VLAN priority"); 723 return -EINVAL; 724 } 725 726 prio = BIT(match.key->vlan_priority); 727 stmmac_rx_queue_prio(priv, priv->hw, prio, tc); 728 729 entry->in_use = true; 730 entry->cookie = cls->cookie; 731 entry->tc = tc; 732 entry->type = STMMAC_RFS_T_VLAN; 733 priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]++; 734 } 735 736 return 0; 737 } 738 739 static int tc_del_vlan_flow(struct stmmac_priv *priv, 740 struct flow_cls_offload *cls) 741 { 742 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false); 743 744 if (!entry || !entry->in_use || entry->type != STMMAC_RFS_T_VLAN) 745 return -ENOENT; 746 747 stmmac_rx_queue_prio(priv, priv->hw, 0, entry->tc); 748 749 entry->in_use = false; 750 entry->cookie = 0; 751 entry->tc = 0; 752 entry->type = 0; 753 754 priv->rfs_entries_cnt[STMMAC_RFS_T_VLAN]--; 755 756 return 0; 757 } 758 759 static int tc_add_ethtype_flow(struct stmmac_priv *priv, 760 struct flow_cls_offload *cls) 761 { 762 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false); 763 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 764 struct flow_dissector *dissector = rule->match.dissector; 765 int tc = tc_classid_to_hwtc(priv->dev, cls->classid); 766 struct flow_match_basic match; 767 768 if (!entry) { 769 entry = tc_find_rfs(priv, cls, true); 770 if (!entry) 771 return -ENOENT; 772 } 773 774 /* Nothing to do here */ 775 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC)) 776 return -EINVAL; 777 778 if (tc < 0) { 779 netdev_err(priv->dev, "Invalid traffic class\n"); 780 return -EINVAL; 781 } 782 783 flow_rule_match_basic(rule, &match); 784 785 if (match.mask->n_proto) { 786 u16 etype = ntohs(match.key->n_proto); 787 788 if (match.mask->n_proto != ETHER_TYPE_FULL_MASK) { 789 netdev_err(priv->dev, "Only full mask is supported for EthType filter"); 790 return -EINVAL; 791 } 792 switch (etype) { 793 case ETH_P_LLDP: 794 if (priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP] >= 795 priv->rfs_entries_max[STMMAC_RFS_T_LLDP]) 796 return -ENOENT; 797 798 entry->type = STMMAC_RFS_T_LLDP; 799 priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP]++; 800 801 stmmac_rx_queue_routing(priv, priv->hw, 802 PACKET_DCBCPQ, tc); 803 break; 804 case ETH_P_1588: 805 if (priv->rfs_entries_cnt[STMMAC_RFS_T_1588] >= 806 priv->rfs_entries_max[STMMAC_RFS_T_1588]) 807 return -ENOENT; 808 809 entry->type = STMMAC_RFS_T_1588; 810 priv->rfs_entries_cnt[STMMAC_RFS_T_1588]++; 811 812 stmmac_rx_queue_routing(priv, priv->hw, 813 PACKET_PTPQ, tc); 814 break; 815 default: 816 netdev_err(priv->dev, "EthType(0x%x) is not supported", etype); 817 return -EINVAL; 818 } 819 820 entry->in_use = true; 821 entry->cookie = cls->cookie; 822 entry->tc = tc; 823 entry->etype = etype; 824 825 return 0; 826 } 827 828 return -EINVAL; 829 } 830 831 static int tc_del_ethtype_flow(struct stmmac_priv *priv, 832 struct flow_cls_offload *cls) 833 { 834 struct stmmac_rfs_entry *entry = tc_find_rfs(priv, cls, false); 835 836 if (!entry || !entry->in_use || 837 entry->type < STMMAC_RFS_T_LLDP || 838 entry->type > STMMAC_RFS_T_1588) 839 return -ENOENT; 840 841 switch (entry->etype) { 842 case ETH_P_LLDP: 843 stmmac_rx_queue_routing(priv, priv->hw, 844 PACKET_DCBCPQ, 0); 845 priv->rfs_entries_cnt[STMMAC_RFS_T_LLDP]--; 846 break; 847 case ETH_P_1588: 848 stmmac_rx_queue_routing(priv, priv->hw, 849 PACKET_PTPQ, 0); 850 priv->rfs_entries_cnt[STMMAC_RFS_T_1588]--; 851 break; 852 default: 853 netdev_err(priv->dev, "EthType(0x%x) is not supported", 854 entry->etype); 855 return -EINVAL; 856 } 857 858 entry->in_use = false; 859 entry->cookie = 0; 860 entry->tc = 0; 861 entry->etype = 0; 862 entry->type = 0; 863 864 return 0; 865 } 866 867 static int tc_add_flow_cls(struct stmmac_priv *priv, 868 struct flow_cls_offload *cls) 869 { 870 int ret; 871 872 ret = tc_add_flow(priv, cls); 873 if (!ret) 874 return ret; 875 876 ret = tc_add_ethtype_flow(priv, cls); 877 if (!ret) 878 return ret; 879 880 return tc_add_vlan_flow(priv, cls); 881 } 882 883 static int tc_del_flow_cls(struct stmmac_priv *priv, 884 struct flow_cls_offload *cls) 885 { 886 int ret; 887 888 ret = tc_del_flow(priv, cls); 889 if (!ret) 890 return ret; 891 892 ret = tc_del_ethtype_flow(priv, cls); 893 if (!ret) 894 return ret; 895 896 return tc_del_vlan_flow(priv, cls); 897 } 898 899 static int tc_setup_cls(struct stmmac_priv *priv, 900 struct flow_cls_offload *cls) 901 { 902 int ret = 0; 903 904 /* When RSS is enabled, the filtering will be bypassed */ 905 if (priv->rss.enable) 906 return -EBUSY; 907 908 switch (cls->command) { 909 case FLOW_CLS_REPLACE: 910 ret = tc_add_flow_cls(priv, cls); 911 break; 912 case FLOW_CLS_DESTROY: 913 ret = tc_del_flow_cls(priv, cls); 914 break; 915 default: 916 return -EOPNOTSUPP; 917 } 918 919 return ret; 920 } 921 922 struct timespec64 stmmac_calc_tas_basetime(ktime_t old_base_time, 923 ktime_t current_time, 924 u64 cycle_time) 925 { 926 struct timespec64 time; 927 928 if (ktime_after(old_base_time, current_time)) { 929 time = ktime_to_timespec64(old_base_time); 930 } else { 931 s64 n; 932 ktime_t base_time; 933 934 n = div64_s64(ktime_sub_ns(current_time, old_base_time), 935 cycle_time); 936 base_time = ktime_add_ns(old_base_time, 937 (n + 1) * cycle_time); 938 939 time = ktime_to_timespec64(base_time); 940 } 941 942 return time; 943 } 944 945 static void tc_taprio_map_maxsdu_txq(struct stmmac_priv *priv, 946 struct tc_taprio_qopt_offload *qopt) 947 { 948 u32 num_tc = qopt->mqprio.qopt.num_tc; 949 u32 offset, count, i, j; 950 951 /* QueueMaxSDU received from the driver corresponds to the Linux traffic 952 * class. Map queueMaxSDU per Linux traffic class to DWMAC Tx queues. 953 */ 954 for (i = 0; i < num_tc; i++) { 955 if (!qopt->max_sdu[i]) 956 continue; 957 958 offset = qopt->mqprio.qopt.offset[i]; 959 count = qopt->mqprio.qopt.count[i]; 960 961 for (j = offset; j < offset + count; j++) 962 priv->est->max_sdu[j] = qopt->max_sdu[i] + ETH_HLEN - ETH_TLEN; 963 } 964 } 965 966 static int tc_taprio_configure(struct stmmac_priv *priv, 967 struct tc_taprio_qopt_offload *qopt) 968 { 969 u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep; 970 struct netlink_ext_ack *extack = qopt->mqprio.extack; 971 struct timespec64 time, current_time, qopt_time; 972 ktime_t current_time_ns; 973 int err, i, ret = 0; 974 u64 ctr; 975 976 if (qopt->base_time < 0) 977 return -ERANGE; 978 979 if (!priv->dma_cap.estsel) 980 return -EOPNOTSUPP; 981 982 switch (wid) { 983 case 0x1: 984 wid = 16; 985 break; 986 case 0x2: 987 wid = 20; 988 break; 989 case 0x3: 990 wid = 24; 991 break; 992 default: 993 return -EOPNOTSUPP; 994 } 995 996 switch (dep) { 997 case 0x1: 998 dep = 64; 999 break; 1000 case 0x2: 1001 dep = 128; 1002 break; 1003 case 0x3: 1004 dep = 256; 1005 break; 1006 case 0x4: 1007 dep = 512; 1008 break; 1009 case 0x5: 1010 dep = 1024; 1011 break; 1012 default: 1013 return -EOPNOTSUPP; 1014 } 1015 1016 if (qopt->cmd == TAPRIO_CMD_DESTROY) 1017 goto disable; 1018 1019 if (qopt->num_entries > dep) 1020 return -EINVAL; 1021 if (!qopt->cycle_time) 1022 return -ERANGE; 1023 if (qopt->cycle_time_extension >= BIT(wid + 7)) 1024 return -ERANGE; 1025 1026 if (!priv->est) { 1027 priv->est = devm_kzalloc(priv->device, sizeof(*priv->est), 1028 GFP_KERNEL); 1029 if (!priv->est) 1030 return -ENOMEM; 1031 1032 mutex_init(&priv->est_lock); 1033 } else { 1034 mutex_lock(&priv->est_lock); 1035 memset(priv->est, 0, sizeof(*priv->est)); 1036 mutex_unlock(&priv->est_lock); 1037 } 1038 1039 size = qopt->num_entries; 1040 1041 mutex_lock(&priv->est_lock); 1042 priv->est->gcl_size = size; 1043 priv->est->enable = qopt->cmd == TAPRIO_CMD_REPLACE; 1044 mutex_unlock(&priv->est_lock); 1045 1046 for (i = 0; i < size; i++) { 1047 s64 delta_ns = qopt->entries[i].interval; 1048 u32 gates = qopt->entries[i].gate_mask; 1049 1050 if (delta_ns > GENMASK(wid - 1, 0)) 1051 return -ERANGE; 1052 if (gates > GENMASK(31 - wid, 0)) 1053 return -ERANGE; 1054 1055 switch (qopt->entries[i].command) { 1056 case TC_TAPRIO_CMD_SET_GATES: 1057 break; 1058 case TC_TAPRIO_CMD_SET_AND_HOLD: 1059 gates |= BIT(0); 1060 break; 1061 case TC_TAPRIO_CMD_SET_AND_RELEASE: 1062 gates &= ~BIT(0); 1063 break; 1064 default: 1065 return -EOPNOTSUPP; 1066 } 1067 1068 priv->est->gcl[i] = delta_ns | (gates << wid); 1069 } 1070 1071 mutex_lock(&priv->est_lock); 1072 /* Adjust for real system time */ 1073 priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, ¤t_time); 1074 current_time_ns = timespec64_to_ktime(current_time); 1075 time = stmmac_calc_tas_basetime(qopt->base_time, current_time_ns, 1076 qopt->cycle_time); 1077 1078 priv->est->btr[0] = (u32)time.tv_nsec; 1079 priv->est->btr[1] = (u32)time.tv_sec; 1080 1081 qopt_time = ktime_to_timespec64(qopt->base_time); 1082 priv->est->btr_reserve[0] = (u32)qopt_time.tv_nsec; 1083 priv->est->btr_reserve[1] = (u32)qopt_time.tv_sec; 1084 1085 ctr = qopt->cycle_time; 1086 priv->est->ctr[0] = do_div(ctr, NSEC_PER_SEC); 1087 priv->est->ctr[1] = (u32)ctr; 1088 1089 priv->est->ter = qopt->cycle_time_extension; 1090 1091 tc_taprio_map_maxsdu_txq(priv, qopt); 1092 1093 ret = stmmac_est_configure(priv, priv, priv->est, 1094 priv->plat->clk_ptp_rate); 1095 mutex_unlock(&priv->est_lock); 1096 if (ret) { 1097 netdev_err(priv->dev, "failed to configure EST\n"); 1098 goto disable; 1099 } 1100 1101 ret = stmmac_fpe_map_preemption_class(priv, priv->dev, extack, 1102 qopt->mqprio.preemptible_tcs); 1103 if (ret) 1104 goto disable; 1105 1106 return 0; 1107 1108 disable: 1109 if (priv->est) { 1110 mutex_lock(&priv->est_lock); 1111 priv->est->enable = false; 1112 stmmac_est_configure(priv, priv, priv->est, 1113 priv->plat->clk_ptp_rate); 1114 /* Reset taprio status */ 1115 for (i = 0; i < priv->plat->tx_queues_to_use; i++) { 1116 priv->xstats.max_sdu_txq_drop[i] = 0; 1117 priv->xstats.mtl_est_txq_hlbf[i] = 0; 1118 priv->xstats.mtl_est_txq_hlbs[i] = 0; 1119 } 1120 mutex_unlock(&priv->est_lock); 1121 } 1122 1123 err = stmmac_fpe_map_preemption_class(priv, priv->dev, extack, 0); 1124 1125 return qopt->cmd == TAPRIO_CMD_DESTROY ? err : ret; 1126 } 1127 1128 static void tc_taprio_stats(struct stmmac_priv *priv, 1129 struct tc_taprio_qopt_offload *qopt) 1130 { 1131 u64 window_drops = 0; 1132 int i = 0; 1133 1134 for (i = 0; i < priv->plat->tx_queues_to_use; i++) 1135 window_drops += priv->xstats.max_sdu_txq_drop[i] + 1136 priv->xstats.mtl_est_txq_hlbf[i] + 1137 priv->xstats.mtl_est_txq_hlbs[i]; 1138 qopt->stats.window_drops = window_drops; 1139 1140 /* Transmission overrun doesn't happen for stmmac, hence always 0 */ 1141 qopt->stats.tx_overruns = 0; 1142 } 1143 1144 static void tc_taprio_queue_stats(struct stmmac_priv *priv, 1145 struct tc_taprio_qopt_offload *qopt) 1146 { 1147 struct tc_taprio_qopt_queue_stats *q_stats = &qopt->queue_stats; 1148 int queue = qopt->queue_stats.queue; 1149 1150 q_stats->stats.window_drops = priv->xstats.max_sdu_txq_drop[queue] + 1151 priv->xstats.mtl_est_txq_hlbf[queue] + 1152 priv->xstats.mtl_est_txq_hlbs[queue]; 1153 1154 /* Transmission overrun doesn't happen for stmmac, hence always 0 */ 1155 q_stats->stats.tx_overruns = 0; 1156 } 1157 1158 static int tc_setup_taprio(struct stmmac_priv *priv, 1159 struct tc_taprio_qopt_offload *qopt) 1160 { 1161 int err = 0; 1162 1163 switch (qopt->cmd) { 1164 case TAPRIO_CMD_REPLACE: 1165 case TAPRIO_CMD_DESTROY: 1166 err = tc_taprio_configure(priv, qopt); 1167 break; 1168 case TAPRIO_CMD_STATS: 1169 tc_taprio_stats(priv, qopt); 1170 break; 1171 case TAPRIO_CMD_QUEUE_STATS: 1172 tc_taprio_queue_stats(priv, qopt); 1173 break; 1174 default: 1175 err = -EOPNOTSUPP; 1176 } 1177 1178 return err; 1179 } 1180 1181 static int tc_setup_taprio_without_fpe(struct stmmac_priv *priv, 1182 struct tc_taprio_qopt_offload *qopt) 1183 { 1184 if (!qopt->mqprio.preemptible_tcs) 1185 return tc_setup_taprio(priv, qopt); 1186 1187 NL_SET_ERR_MSG_MOD(qopt->mqprio.extack, 1188 "taprio with FPE is not implemented for this MAC"); 1189 1190 return -EOPNOTSUPP; 1191 } 1192 1193 static int tc_setup_etf(struct stmmac_priv *priv, 1194 struct tc_etf_qopt_offload *qopt) 1195 { 1196 if (!priv->dma_cap.tbssel) 1197 return -EOPNOTSUPP; 1198 if (qopt->queue >= priv->plat->tx_queues_to_use) 1199 return -EINVAL; 1200 if (!(priv->dma_conf.tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL)) 1201 return -EINVAL; 1202 1203 if (qopt->enable) 1204 priv->dma_conf.tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN; 1205 else 1206 priv->dma_conf.tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN; 1207 1208 netdev_info(priv->dev, "%s ETF for Queue %d\n", 1209 qopt->enable ? "enabled" : "disabled", qopt->queue); 1210 return 0; 1211 } 1212 1213 static int tc_query_caps(struct stmmac_priv *priv, 1214 struct tc_query_caps_base *base) 1215 { 1216 switch (base->type) { 1217 case TC_SETUP_QDISC_MQPRIO: { 1218 struct tc_mqprio_caps *caps = base->caps; 1219 1220 caps->validate_queue_counts = true; 1221 1222 return 0; 1223 } 1224 case TC_SETUP_QDISC_TAPRIO: { 1225 struct tc_taprio_caps *caps = base->caps; 1226 1227 if (!priv->dma_cap.estsel) 1228 return -EOPNOTSUPP; 1229 1230 caps->gate_mask_per_txq = true; 1231 caps->supports_queue_max_sdu = true; 1232 1233 return 0; 1234 } 1235 default: 1236 return -EOPNOTSUPP; 1237 } 1238 } 1239 1240 static int stmmac_set_ndev_tcs(struct net_device *ndev, u8 ntc, 1241 struct netdev_tc_txq *tc_to_txq) 1242 { 1243 int i, err; 1244 1245 netdev_reset_tc(ndev); 1246 if (!ntc) 1247 return 0; 1248 1249 err = netdev_set_num_tc(ndev, ntc); 1250 if (err) 1251 return err; 1252 1253 for (i = 0; i < ntc; i++) { 1254 u16 count, offset; 1255 1256 count = tc_to_txq[i].count; 1257 offset = tc_to_txq[i].offset; 1258 netdev_set_tc_queue(ndev, i, count, offset); 1259 } 1260 1261 return 0; 1262 } 1263 1264 static int stmmac_reset_tc_mqprio(struct net_device *ndev, 1265 struct netlink_ext_ack *extack) 1266 { 1267 struct stmmac_priv *priv = netdev_priv(ndev); 1268 1269 netdev_reset_tc(ndev); 1270 netif_set_real_num_tx_queues(ndev, priv->plat->tx_queues_to_use); 1271 1272 return stmmac_fpe_map_preemption_class(priv, ndev, extack, 0); 1273 } 1274 1275 static int tc_setup_dwmac510_mqprio(struct stmmac_priv *priv, 1276 struct tc_mqprio_qopt_offload *mqprio) 1277 { 1278 unsigned int ndev_num_tx_queues, num_tx_queues = 0; 1279 struct netdev_tc_txq ndev_tc_to_txq[TC_MAX_QUEUE]; 1280 struct netdev_tc_txq tc_to_txq[TC_MAX_QUEUE] = {}; 1281 struct netlink_ext_ack *extack = mqprio->extack; 1282 struct tc_mqprio_qopt *qopt = &mqprio->qopt; 1283 struct net_device *ndev = priv->dev; 1284 u8 ndev_prio_tc_map[TC_BITMASK + 1]; 1285 int i, err, ndev_ntc; 1286 1287 if (!qopt->num_tc) 1288 return stmmac_reset_tc_mqprio(ndev, extack); 1289 1290 if (qopt->num_tc > ARRAY_SIZE(tc_to_txq)) 1291 return -EINVAL; 1292 1293 /* save current tc values for reset */ 1294 ndev_ntc = netdev_get_num_tc(ndev); 1295 for (i = 0; i < ARRAY_SIZE(ndev->tc_to_txq); i++) 1296 ndev_tc_to_txq[i].combined = 1297 READ_ONCE(ndev->tc_to_txq[i].combined); 1298 for (i = 0; i < ARRAY_SIZE(ndev_prio_tc_map); i++) 1299 ndev_prio_tc_map[i] = READ_ONCE(ndev->prio_tc_map[i]); 1300 1301 for (i = 0; i < qopt->num_tc; i++) { 1302 tc_to_txq[i] = (struct netdev_tc_txq) { 1303 .count = qopt->count[i], 1304 .offset = qopt->offset[i], 1305 }; 1306 num_tx_queues += qopt->count[i]; 1307 } 1308 1309 err = stmmac_set_ndev_tcs(ndev, qopt->num_tc, tc_to_txq); 1310 if (err) 1311 goto error_reset_tc; 1312 1313 ndev_num_tx_queues = ndev->real_num_tx_queues; 1314 err = netif_set_real_num_tx_queues(ndev, num_tx_queues); 1315 if (err) 1316 goto error_reset_tc; 1317 1318 err = stmmac_fpe_map_preemption_class(priv, ndev, extack, 1319 mqprio->preemptible_tcs); 1320 if (err) 1321 goto error_reset_num_tx_queues; 1322 1323 return 0; 1324 1325 error_reset_num_tx_queues: 1326 if (netif_set_real_num_tx_queues(ndev, ndev_num_tx_queues)) 1327 netdev_warn(ndev, "Failed to restore %u TX queues\n", 1328 ndev_num_tx_queues); 1329 error_reset_tc: 1330 stmmac_set_ndev_tcs(ndev, ndev_ntc, ndev_tc_to_txq); 1331 for (i = 0; i < ARRAY_SIZE(ndev_prio_tc_map); i++) 1332 netdev_set_prio_tc_map(ndev, i, ndev_prio_tc_map[i]); 1333 1334 return err; 1335 } 1336 1337 static int tc_setup_mqprio_unimplemented(struct stmmac_priv *priv, 1338 struct tc_mqprio_qopt_offload *mqprio) 1339 { 1340 NL_SET_ERR_MSG_MOD(mqprio->extack, 1341 "mqprio HW offload is not implemented for this MAC"); 1342 return -EOPNOTSUPP; 1343 } 1344 1345 const struct stmmac_tc_ops dwmac4_tc_ops = { 1346 .init = tc_init, 1347 .setup_cls_u32 = tc_setup_cls_u32, 1348 .setup_cbs = tc_setup_cbs, 1349 .setup_cls = tc_setup_cls, 1350 .setup_taprio = tc_setup_taprio_without_fpe, 1351 .setup_etf = tc_setup_etf, 1352 .query_caps = tc_query_caps, 1353 .setup_mqprio = tc_setup_mqprio_unimplemented, 1354 }; 1355 1356 const struct stmmac_tc_ops dwmac510_tc_ops = { 1357 .init = tc_init, 1358 .setup_cls_u32 = tc_setup_cls_u32, 1359 .setup_cbs = tc_setup_cbs, 1360 .setup_cls = tc_setup_cls, 1361 .setup_taprio = tc_setup_taprio, 1362 .setup_etf = tc_setup_etf, 1363 .query_caps = tc_query_caps, 1364 .setup_mqprio = tc_setup_dwmac510_mqprio, 1365 }; 1366