drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c

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