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