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