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_init(struct stmmac_priv *priv) 236 { 237 struct dma_features *dma_cap = &priv->dma_cap; 238 unsigned int count; 239 int i; 240 241 if (dma_cap->l3l4fnum) { 242 priv->flow_entries_max = dma_cap->l3l4fnum; 243 priv->flow_entries = devm_kcalloc(priv->device, 244 dma_cap->l3l4fnum, 245 sizeof(*priv->flow_entries), 246 GFP_KERNEL); 247 if (!priv->flow_entries) 248 return -ENOMEM; 249 250 for (i = 0; i < priv->flow_entries_max; i++) 251 priv->flow_entries[i].idx = i; 252 253 dev_info(priv->device, "Enabled Flow TC (entries=%d)\n", 254 priv->flow_entries_max); 255 } 256 257 /* Fail silently as we can still use remaining features, e.g. CBS */ 258 if (!dma_cap->frpsel) 259 return 0; 260 261 switch (dma_cap->frpbs) { 262 case 0x0: 263 priv->tc_off_max = 64; 264 break; 265 case 0x1: 266 priv->tc_off_max = 128; 267 break; 268 case 0x2: 269 priv->tc_off_max = 256; 270 break; 271 default: 272 return -EINVAL; 273 } 274 275 switch (dma_cap->frpes) { 276 case 0x0: 277 count = 64; 278 break; 279 case 0x1: 280 count = 128; 281 break; 282 case 0x2: 283 count = 256; 284 break; 285 default: 286 return -EINVAL; 287 } 288 289 /* Reserve one last filter which lets all pass */ 290 priv->tc_entries_max = count; 291 priv->tc_entries = devm_kcalloc(priv->device, 292 count, sizeof(*priv->tc_entries), GFP_KERNEL); 293 if (!priv->tc_entries) 294 return -ENOMEM; 295 296 tc_fill_all_pass_entry(&priv->tc_entries[count - 1]); 297 298 dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n", 299 priv->tc_entries_max, priv->tc_off_max); 300 return 0; 301 } 302 303 static int tc_setup_cbs(struct stmmac_priv *priv, 304 struct tc_cbs_qopt_offload *qopt) 305 { 306 u32 tx_queues_count = priv->plat->tx_queues_to_use; 307 u32 queue = qopt->queue; 308 u32 ptr, speed_div; 309 u32 mode_to_use; 310 u64 value; 311 int ret; 312 313 /* Queue 0 is not AVB capable */ 314 if (queue <= 0 || queue >= tx_queues_count) 315 return -EINVAL; 316 if (!priv->dma_cap.av) 317 return -EOPNOTSUPP; 318 319 /* Port Transmit Rate and Speed Divider */ 320 switch (priv->speed) { 321 case SPEED_10000: 322 ptr = 32; 323 speed_div = 10000000; 324 break; 325 case SPEED_5000: 326 ptr = 32; 327 speed_div = 5000000; 328 break; 329 case SPEED_2500: 330 ptr = 8; 331 speed_div = 2500000; 332 break; 333 case SPEED_1000: 334 ptr = 8; 335 speed_div = 1000000; 336 break; 337 case SPEED_100: 338 ptr = 4; 339 speed_div = 100000; 340 break; 341 default: 342 return -EOPNOTSUPP; 343 } 344 345 mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use; 346 if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) { 347 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB); 348 if (ret) 349 return ret; 350 351 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB; 352 } else if (!qopt->enable) { 353 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, 354 MTL_QUEUE_DCB); 355 if (ret) 356 return ret; 357 358 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_DCB; 359 } 360 361 /* Final adjustments for HW */ 362 value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div); 363 priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0); 364 365 value = div_s64(-qopt->sendslope * 1024ll * ptr, speed_div); 366 priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0); 367 368 value = qopt->hicredit * 1024ll * 8; 369 priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0); 370 371 value = qopt->locredit * 1024ll * 8; 372 priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0); 373 374 ret = stmmac_config_cbs(priv, priv->hw, 375 priv->plat->tx_queues_cfg[queue].send_slope, 376 priv->plat->tx_queues_cfg[queue].idle_slope, 377 priv->plat->tx_queues_cfg[queue].high_credit, 378 priv->plat->tx_queues_cfg[queue].low_credit, 379 queue); 380 if (ret) 381 return ret; 382 383 dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n", 384 queue, qopt->sendslope, qopt->idleslope, 385 qopt->hicredit, qopt->locredit); 386 return 0; 387 } 388 389 static int tc_parse_flow_actions(struct stmmac_priv *priv, 390 struct flow_action *action, 391 struct stmmac_flow_entry *entry, 392 struct netlink_ext_ack *extack) 393 { 394 struct flow_action_entry *act; 395 int i; 396 397 if (!flow_action_has_entries(action)) 398 return -EINVAL; 399 400 if (!flow_action_basic_hw_stats_check(action, extack)) 401 return -EOPNOTSUPP; 402 403 flow_action_for_each(i, act, action) { 404 switch (act->id) { 405 case FLOW_ACTION_DROP: 406 entry->action |= STMMAC_FLOW_ACTION_DROP; 407 return 0; 408 default: 409 break; 410 } 411 } 412 413 /* Nothing to do, maybe inverse filter ? */ 414 return 0; 415 } 416 417 static int tc_add_basic_flow(struct stmmac_priv *priv, 418 struct flow_cls_offload *cls, 419 struct stmmac_flow_entry *entry) 420 { 421 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 422 struct flow_dissector *dissector = rule->match.dissector; 423 struct flow_match_basic match; 424 425 /* Nothing to do here */ 426 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC)) 427 return -EINVAL; 428 429 flow_rule_match_basic(rule, &match); 430 entry->ip_proto = match.key->ip_proto; 431 return 0; 432 } 433 434 static int tc_add_ip4_flow(struct stmmac_priv *priv, 435 struct flow_cls_offload *cls, 436 struct stmmac_flow_entry *entry) 437 { 438 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 439 struct flow_dissector *dissector = rule->match.dissector; 440 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP; 441 struct flow_match_ipv4_addrs match; 442 u32 hw_match; 443 int ret; 444 445 /* Nothing to do here */ 446 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS)) 447 return -EINVAL; 448 449 flow_rule_match_ipv4_addrs(rule, &match); 450 hw_match = ntohl(match.key->src) & ntohl(match.mask->src); 451 if (hw_match) { 452 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true, 453 false, true, inv, hw_match); 454 if (ret) 455 return ret; 456 } 457 458 hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst); 459 if (hw_match) { 460 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true, 461 false, false, inv, hw_match); 462 if (ret) 463 return ret; 464 } 465 466 return 0; 467 } 468 469 static int tc_add_ports_flow(struct stmmac_priv *priv, 470 struct flow_cls_offload *cls, 471 struct stmmac_flow_entry *entry) 472 { 473 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 474 struct flow_dissector *dissector = rule->match.dissector; 475 bool inv = entry->action & STMMAC_FLOW_ACTION_DROP; 476 struct flow_match_ports match; 477 u32 hw_match; 478 bool is_udp; 479 int ret; 480 481 /* Nothing to do here */ 482 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS)) 483 return -EINVAL; 484 485 switch (entry->ip_proto) { 486 case IPPROTO_TCP: 487 is_udp = false; 488 break; 489 case IPPROTO_UDP: 490 is_udp = true; 491 break; 492 default: 493 return -EINVAL; 494 } 495 496 flow_rule_match_ports(rule, &match); 497 498 hw_match = ntohs(match.key->src) & ntohs(match.mask->src); 499 if (hw_match) { 500 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true, 501 is_udp, true, inv, hw_match); 502 if (ret) 503 return ret; 504 } 505 506 hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst); 507 if (hw_match) { 508 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true, 509 is_udp, false, inv, hw_match); 510 if (ret) 511 return ret; 512 } 513 514 entry->is_l4 = true; 515 return 0; 516 } 517 518 static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv, 519 struct flow_cls_offload *cls, 520 bool get_free) 521 { 522 int i; 523 524 for (i = 0; i < priv->flow_entries_max; i++) { 525 struct stmmac_flow_entry *entry = &priv->flow_entries[i]; 526 527 if (entry->cookie == cls->cookie) 528 return entry; 529 if (get_free && (entry->in_use == false)) 530 return entry; 531 } 532 533 return NULL; 534 } 535 536 static struct { 537 int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls, 538 struct stmmac_flow_entry *entry); 539 } tc_flow_parsers[] = { 540 { .fn = tc_add_basic_flow }, 541 { .fn = tc_add_ip4_flow }, 542 { .fn = tc_add_ports_flow }, 543 }; 544 545 static int tc_add_flow(struct stmmac_priv *priv, 546 struct flow_cls_offload *cls) 547 { 548 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false); 549 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 550 int i, ret; 551 552 if (!entry) { 553 entry = tc_find_flow(priv, cls, true); 554 if (!entry) 555 return -ENOENT; 556 } 557 558 ret = tc_parse_flow_actions(priv, &rule->action, entry, 559 cls->common.extack); 560 if (ret) 561 return ret; 562 563 for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) { 564 ret = tc_flow_parsers[i].fn(priv, cls, entry); 565 if (!ret) { 566 entry->in_use = true; 567 continue; 568 } 569 } 570 571 if (!entry->in_use) 572 return -EINVAL; 573 574 entry->cookie = cls->cookie; 575 return 0; 576 } 577 578 static int tc_del_flow(struct stmmac_priv *priv, 579 struct flow_cls_offload *cls) 580 { 581 struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false); 582 int ret; 583 584 if (!entry || !entry->in_use) 585 return -ENOENT; 586 587 if (entry->is_l4) { 588 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false, 589 false, false, false, 0); 590 } else { 591 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false, 592 false, false, false, 0); 593 } 594 595 entry->in_use = false; 596 entry->cookie = 0; 597 entry->is_l4 = false; 598 return ret; 599 } 600 601 #define VLAN_PRIO_FULL_MASK (0x07) 602 603 static int tc_add_vlan_flow(struct stmmac_priv *priv, 604 struct flow_cls_offload *cls) 605 { 606 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 607 struct flow_dissector *dissector = rule->match.dissector; 608 int tc = tc_classid_to_hwtc(priv->dev, cls->classid); 609 struct flow_match_vlan match; 610 611 /* Nothing to do here */ 612 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_VLAN)) 613 return -EINVAL; 614 615 if (tc < 0) { 616 netdev_err(priv->dev, "Invalid traffic class\n"); 617 return -EINVAL; 618 } 619 620 flow_rule_match_vlan(rule, &match); 621 622 if (match.mask->vlan_priority) { 623 u32 prio; 624 625 if (match.mask->vlan_priority != VLAN_PRIO_FULL_MASK) { 626 netdev_err(priv->dev, "Only full mask is supported for VLAN priority"); 627 return -EINVAL; 628 } 629 630 prio = BIT(match.key->vlan_priority); 631 stmmac_rx_queue_prio(priv, priv->hw, prio, tc); 632 } 633 634 return 0; 635 } 636 637 static int tc_del_vlan_flow(struct stmmac_priv *priv, 638 struct flow_cls_offload *cls) 639 { 640 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 641 struct flow_dissector *dissector = rule->match.dissector; 642 int tc = tc_classid_to_hwtc(priv->dev, cls->classid); 643 644 /* Nothing to do here */ 645 if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_VLAN)) 646 return -EINVAL; 647 648 if (tc < 0) { 649 netdev_err(priv->dev, "Invalid traffic class\n"); 650 return -EINVAL; 651 } 652 653 stmmac_rx_queue_prio(priv, priv->hw, 0, tc); 654 655 return 0; 656 } 657 658 static int tc_add_flow_cls(struct stmmac_priv *priv, 659 struct flow_cls_offload *cls) 660 { 661 int ret; 662 663 ret = tc_add_flow(priv, cls); 664 if (!ret) 665 return ret; 666 667 return tc_add_vlan_flow(priv, cls); 668 } 669 670 static int tc_del_flow_cls(struct stmmac_priv *priv, 671 struct flow_cls_offload *cls) 672 { 673 int ret; 674 675 ret = tc_del_flow(priv, cls); 676 if (!ret) 677 return ret; 678 679 return tc_del_vlan_flow(priv, cls); 680 } 681 682 static int tc_setup_cls(struct stmmac_priv *priv, 683 struct flow_cls_offload *cls) 684 { 685 int ret = 0; 686 687 /* When RSS is enabled, the filtering will be bypassed */ 688 if (priv->rss.enable) 689 return -EBUSY; 690 691 switch (cls->command) { 692 case FLOW_CLS_REPLACE: 693 ret = tc_add_flow_cls(priv, cls); 694 break; 695 case FLOW_CLS_DESTROY: 696 ret = tc_del_flow_cls(priv, cls); 697 break; 698 default: 699 return -EOPNOTSUPP; 700 } 701 702 return ret; 703 } 704 705 static int tc_setup_taprio(struct stmmac_priv *priv, 706 struct tc_taprio_qopt_offload *qopt) 707 { 708 u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep; 709 struct plat_stmmacenet_data *plat = priv->plat; 710 struct timespec64 time, current_time; 711 ktime_t current_time_ns; 712 bool fpe = false; 713 int i, ret = 0; 714 u64 ctr; 715 716 if (!priv->dma_cap.estsel) 717 return -EOPNOTSUPP; 718 719 switch (wid) { 720 case 0x1: 721 wid = 16; 722 break; 723 case 0x2: 724 wid = 20; 725 break; 726 case 0x3: 727 wid = 24; 728 break; 729 default: 730 return -EOPNOTSUPP; 731 } 732 733 switch (dep) { 734 case 0x1: 735 dep = 64; 736 break; 737 case 0x2: 738 dep = 128; 739 break; 740 case 0x3: 741 dep = 256; 742 break; 743 case 0x4: 744 dep = 512; 745 break; 746 case 0x5: 747 dep = 1024; 748 break; 749 default: 750 return -EOPNOTSUPP; 751 } 752 753 if (!qopt->enable) 754 goto disable; 755 if (qopt->num_entries >= dep) 756 return -EINVAL; 757 if (!qopt->base_time) 758 return -ERANGE; 759 if (!qopt->cycle_time) 760 return -ERANGE; 761 762 if (!plat->est) { 763 plat->est = devm_kzalloc(priv->device, sizeof(*plat->est), 764 GFP_KERNEL); 765 if (!plat->est) 766 return -ENOMEM; 767 } else { 768 memset(plat->est, 0, sizeof(*plat->est)); 769 } 770 771 size = qopt->num_entries; 772 773 priv->plat->est->gcl_size = size; 774 priv->plat->est->enable = qopt->enable; 775 776 for (i = 0; i < size; i++) { 777 s64 delta_ns = qopt->entries[i].interval; 778 u32 gates = qopt->entries[i].gate_mask; 779 780 if (delta_ns > GENMASK(wid, 0)) 781 return -ERANGE; 782 if (gates > GENMASK(31 - wid, 0)) 783 return -ERANGE; 784 785 switch (qopt->entries[i].command) { 786 case TC_TAPRIO_CMD_SET_GATES: 787 if (fpe) 788 return -EINVAL; 789 break; 790 case TC_TAPRIO_CMD_SET_AND_HOLD: 791 gates |= BIT(0); 792 fpe = true; 793 break; 794 case TC_TAPRIO_CMD_SET_AND_RELEASE: 795 gates &= ~BIT(0); 796 fpe = true; 797 break; 798 default: 799 return -EOPNOTSUPP; 800 } 801 802 priv->plat->est->gcl[i] = delta_ns | (gates << wid); 803 } 804 805 /* Adjust for real system time */ 806 priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, ¤t_time); 807 current_time_ns = timespec64_to_ktime(current_time); 808 if (ktime_after(qopt->base_time, current_time_ns)) { 809 time = ktime_to_timespec64(qopt->base_time); 810 } else { 811 ktime_t base_time; 812 s64 n; 813 814 n = div64_s64(ktime_sub_ns(current_time_ns, qopt->base_time), 815 qopt->cycle_time); 816 base_time = ktime_add_ns(qopt->base_time, 817 (n + 1) * qopt->cycle_time); 818 819 time = ktime_to_timespec64(base_time); 820 } 821 822 priv->plat->est->btr[0] = (u32)time.tv_nsec; 823 priv->plat->est->btr[1] = (u32)time.tv_sec; 824 825 ctr = qopt->cycle_time; 826 priv->plat->est->ctr[0] = do_div(ctr, NSEC_PER_SEC); 827 priv->plat->est->ctr[1] = (u32)ctr; 828 829 if (fpe && !priv->dma_cap.fpesel) 830 return -EOPNOTSUPP; 831 832 ret = stmmac_fpe_configure(priv, priv->ioaddr, 833 priv->plat->tx_queues_to_use, 834 priv->plat->rx_queues_to_use, fpe); 835 if (ret && fpe) { 836 netdev_err(priv->dev, "failed to enable Frame Preemption\n"); 837 return ret; 838 } 839 840 ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est, 841 priv->plat->clk_ptp_rate); 842 if (ret) { 843 netdev_err(priv->dev, "failed to configure EST\n"); 844 goto disable; 845 } 846 847 netdev_info(priv->dev, "configured EST\n"); 848 return 0; 849 850 disable: 851 priv->plat->est->enable = false; 852 stmmac_est_configure(priv, priv->ioaddr, priv->plat->est, 853 priv->plat->clk_ptp_rate); 854 return ret; 855 } 856 857 static int tc_setup_etf(struct stmmac_priv *priv, 858 struct tc_etf_qopt_offload *qopt) 859 { 860 if (!priv->dma_cap.tbssel) 861 return -EOPNOTSUPP; 862 if (qopt->queue >= priv->plat->tx_queues_to_use) 863 return -EINVAL; 864 if (!(priv->tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL)) 865 return -EINVAL; 866 867 if (qopt->enable) 868 priv->tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN; 869 else 870 priv->tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN; 871 872 netdev_info(priv->dev, "%s ETF for Queue %d\n", 873 qopt->enable ? "enabled" : "disabled", qopt->queue); 874 return 0; 875 } 876 877 const struct stmmac_tc_ops dwmac510_tc_ops = { 878 .init = tc_init, 879 .setup_cls_u32 = tc_setup_cls_u32, 880 .setup_cbs = tc_setup_cbs, 881 .setup_cls = tc_setup_cls, 882 .setup_taprio = tc_setup_taprio, 883 .setup_etf = tc_setup_etf, 884 }; 885