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                 /* Fall through */
 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         mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
 326         if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
 327                 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
 328                 if (ret)
 329                         return ret;
 330
 331                 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
 332         } else if (!qopt->enable) {
 333                 return stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_DCB);
 334         }
 335
 336         /* Port Transmit Rate and Speed Divider */
 337         ptr = (priv->speed == SPEED_100) ? 4 : 8;
 338         speed_div = (priv->speed == SPEED_100) ? 100000 : 1000000;
 339
 340         /* Final adjustments for HW */
 341         value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div);
 342         priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
 343
 344         value = div_s64(-qopt->sendslope * 1024ll * ptr, speed_div);
 345         priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
 346
 347         value = qopt->hicredit * 1024ll * 8;
 348         priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
 349
 350         value = qopt->locredit * 1024ll * 8;
 351         priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
 352
 353         ret = stmmac_config_cbs(priv, priv->hw,
 354                                 priv->plat->tx_queues_cfg[queue].send_slope,
 355                                 priv->plat->tx_queues_cfg[queue].idle_slope,
 356                                 priv->plat->tx_queues_cfg[queue].high_credit,
 357                                 priv->plat->tx_queues_cfg[queue].low_credit,
 358                                 queue);
 359         if (ret)
 360                 return ret;
 361
 362         dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
 363                         queue, qopt->sendslope, qopt->idleslope,
 364                         qopt->hicredit, qopt->locredit);
 365         return 0;
 366 }
 367
 368 static int tc_parse_flow_actions(struct stmmac_priv *priv,
 369                                  struct flow_action *action,
 370                                  struct stmmac_flow_entry *entry,
 371                                  struct netlink_ext_ack *extack)
 372 {
 373         struct flow_action_entry *act;
 374         int i;
 375
 376         if (!flow_action_has_entries(action))
 377                 return -EINVAL;
 378
 379         if (!flow_action_basic_hw_stats_check(action, extack))
 380                 return -EOPNOTSUPP;
 381
 382         flow_action_for_each(i, act, action) {
 383                 switch (act->id) {
 384                 case FLOW_ACTION_DROP:
 385                         entry->action |= STMMAC_FLOW_ACTION_DROP;
 386                         return 0;
 387                 default:
 388                         break;
 389                 }
 390         }
 391
 392         /* Nothing to do, maybe inverse filter ? */
 393         return 0;
 394 }
 395
 396 static int tc_add_basic_flow(struct stmmac_priv *priv,
 397                              struct flow_cls_offload *cls,
 398                              struct stmmac_flow_entry *entry)
 399 {
 400         struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 401         struct flow_dissector *dissector = rule->match.dissector;
 402         struct flow_match_basic match;
 403
 404         /* Nothing to do here */
 405         if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_BASIC))
 406                 return -EINVAL;
 407
 408         flow_rule_match_basic(rule, &match);
 409         entry->ip_proto = match.key->ip_proto;
 410         return 0;
 411 }
 412
 413 static int tc_add_ip4_flow(struct stmmac_priv *priv,
 414                            struct flow_cls_offload *cls,
 415                            struct stmmac_flow_entry *entry)
 416 {
 417         struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 418         struct flow_dissector *dissector = rule->match.dissector;
 419         bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
 420         struct flow_match_ipv4_addrs match;
 421         u32 hw_match;
 422         int ret;
 423
 424         /* Nothing to do here */
 425         if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_IPV4_ADDRS))
 426                 return -EINVAL;
 427
 428         flow_rule_match_ipv4_addrs(rule, &match);
 429         hw_match = ntohl(match.key->src) & ntohl(match.mask->src);
 430         if (hw_match) {
 431                 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
 432                                               false, true, inv, hw_match);
 433                 if (ret)
 434                         return ret;
 435         }
 436
 437         hw_match = ntohl(match.key->dst) & ntohl(match.mask->dst);
 438         if (hw_match) {
 439                 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, true,
 440                                               false, false, inv, hw_match);
 441                 if (ret)
 442                         return ret;
 443         }
 444
 445         return 0;
 446 }
 447
 448 static int tc_add_ports_flow(struct stmmac_priv *priv,
 449                              struct flow_cls_offload *cls,
 450                              struct stmmac_flow_entry *entry)
 451 {
 452         struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 453         struct flow_dissector *dissector = rule->match.dissector;
 454         bool inv = entry->action & STMMAC_FLOW_ACTION_DROP;
 455         struct flow_match_ports match;
 456         u32 hw_match;
 457         bool is_udp;
 458         int ret;
 459
 460         /* Nothing to do here */
 461         if (!dissector_uses_key(dissector, FLOW_DISSECTOR_KEY_PORTS))
 462                 return -EINVAL;
 463
 464         switch (entry->ip_proto) {
 465         case IPPROTO_TCP:
 466                 is_udp = false;
 467                 break;
 468         case IPPROTO_UDP:
 469                 is_udp = true;
 470                 break;
 471         default:
 472                 return -EINVAL;
 473         }
 474
 475         flow_rule_match_ports(rule, &match);
 476
 477         hw_match = ntohs(match.key->src) & ntohs(match.mask->src);
 478         if (hw_match) {
 479                 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
 480                                               is_udp, true, inv, hw_match);
 481                 if (ret)
 482                         return ret;
 483         }
 484
 485         hw_match = ntohs(match.key->dst) & ntohs(match.mask->dst);
 486         if (hw_match) {
 487                 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, true,
 488                                               is_udp, false, inv, hw_match);
 489                 if (ret)
 490                         return ret;
 491         }
 492
 493         entry->is_l4 = true;
 494         return 0;
 495 }
 496
 497 static struct stmmac_flow_entry *tc_find_flow(struct stmmac_priv *priv,
 498                                               struct flow_cls_offload *cls,
 499                                               bool get_free)
 500 {
 501         int i;
 502
 503         for (i = 0; i < priv->flow_entries_max; i++) {
 504                 struct stmmac_flow_entry *entry = &priv->flow_entries[i];
 505
 506                 if (entry->cookie == cls->cookie)
 507                         return entry;
 508                 if (get_free && (entry->in_use == false))
 509                         return entry;
 510         }
 511
 512         return NULL;
 513 }
 514
 515 static struct {
 516         int (*fn)(struct stmmac_priv *priv, struct flow_cls_offload *cls,
 517                   struct stmmac_flow_entry *entry);
 518 } tc_flow_parsers[] = {
 519         { .fn = tc_add_basic_flow },
 520         { .fn = tc_add_ip4_flow },
 521         { .fn = tc_add_ports_flow },
 522 };
 523
 524 static int tc_add_flow(struct stmmac_priv *priv,
 525                        struct flow_cls_offload *cls)
 526 {
 527         struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
 528         struct flow_rule *rule = flow_cls_offload_flow_rule(cls);
 529         int i, ret;
 530
 531         if (!entry) {
 532                 entry = tc_find_flow(priv, cls, true);
 533                 if (!entry)
 534                         return -ENOENT;
 535         }
 536
 537         ret = tc_parse_flow_actions(priv, &rule->action, entry,
 538                                     cls->common.extack);
 539         if (ret)
 540                 return ret;
 541
 542         for (i = 0; i < ARRAY_SIZE(tc_flow_parsers); i++) {
 543                 ret = tc_flow_parsers[i].fn(priv, cls, entry);
 544                 if (!ret) {
 545                         entry->in_use = true;
 546                         continue;
 547                 }
 548         }
 549
 550         if (!entry->in_use)
 551                 return -EINVAL;
 552
 553         entry->cookie = cls->cookie;
 554         return 0;
 555 }
 556
 557 static int tc_del_flow(struct stmmac_priv *priv,
 558                        struct flow_cls_offload *cls)
 559 {
 560         struct stmmac_flow_entry *entry = tc_find_flow(priv, cls, false);
 561         int ret;
 562
 563         if (!entry || !entry->in_use)
 564                 return -ENOENT;
 565
 566         if (entry->is_l4) {
 567                 ret = stmmac_config_l4_filter(priv, priv->hw, entry->idx, false,
 568                                               false, false, false, 0);
 569         } else {
 570                 ret = stmmac_config_l3_filter(priv, priv->hw, entry->idx, false,
 571                                               false, false, false, 0);
 572         }
 573
 574         entry->in_use = false;
 575         entry->cookie = 0;
 576         entry->is_l4 = false;
 577         return ret;
 578 }
 579
 580 static int tc_setup_cls(struct stmmac_priv *priv,
 581                         struct flow_cls_offload *cls)
 582 {
 583         int ret = 0;
 584
 585         /* When RSS is enabled, the filtering will be bypassed */
 586         if (priv->rss.enable)
 587                 return -EBUSY;
 588
 589         switch (cls->command) {
 590         case FLOW_CLS_REPLACE:
 591                 ret = tc_add_flow(priv, cls);
 592                 break;
 593         case FLOW_CLS_DESTROY:
 594                 ret = tc_del_flow(priv, cls);
 595                 break;
 596         default:
 597                 return -EOPNOTSUPP;
 598         }
 599
 600         return ret;
 601 }
 602
 603 static int tc_setup_taprio(struct stmmac_priv *priv,
 604                            struct tc_taprio_qopt_offload *qopt)
 605 {
 606         u32 size, wid = priv->dma_cap.estwid, dep = priv->dma_cap.estdep;
 607         struct plat_stmmacenet_data *plat = priv->plat;
 608         struct timespec64 time;
 609         bool fpe = false;
 610         int i, ret = 0;
 611         u64 ctr;
 612
 613         if (!priv->dma_cap.estsel)
 614                 return -EOPNOTSUPP;
 615
 616         switch (wid) {
 617         case 0x1:
 618                 wid = 16;
 619                 break;
 620         case 0x2:
 621                 wid = 20;
 622                 break;
 623         case 0x3:
 624                 wid = 24;
 625                 break;
 626         default:
 627                 return -EOPNOTSUPP;
 628         }
 629
 630         switch (dep) {
 631         case 0x1:
 632                 dep = 64;
 633                 break;
 634         case 0x2:
 635                 dep = 128;
 636                 break;
 637         case 0x3:
 638                 dep = 256;
 639                 break;
 640         case 0x4:
 641                 dep = 512;
 642                 break;
 643         case 0x5:
 644                 dep = 1024;
 645                 break;
 646         default:
 647                 return -EOPNOTSUPP;
 648         }
 649
 650         if (!qopt->enable)
 651                 goto disable;
 652         if (qopt->num_entries >= dep)
 653                 return -EINVAL;
 654         if (!qopt->base_time)
 655                 return -ERANGE;
 656         if (!qopt->cycle_time)
 657                 return -ERANGE;
 658
 659         if (!plat->est) {
 660                 plat->est = devm_kzalloc(priv->device, sizeof(*plat->est),
 661                                          GFP_KERNEL);
 662                 if (!plat->est)
 663                         return -ENOMEM;
 664         } else {
 665                 memset(plat->est, 0, sizeof(*plat->est));
 666         }
 667
 668         size = qopt->num_entries;
 669
 670         priv->plat->est->gcl_size = size;
 671         priv->plat->est->enable = qopt->enable;
 672
 673         for (i = 0; i < size; i++) {
 674                 s64 delta_ns = qopt->entries[i].interval;
 675                 u32 gates = qopt->entries[i].gate_mask;
 676
 677                 if (delta_ns > GENMASK(wid, 0))
 678                         return -ERANGE;
 679                 if (gates > GENMASK(31 - wid, 0))
 680                         return -ERANGE;
 681
 682                 switch (qopt->entries[i].command) {
 683                 case TC_TAPRIO_CMD_SET_GATES:
 684                         if (fpe)
 685                                 return -EINVAL;
 686                         break;
 687                 case TC_TAPRIO_CMD_SET_AND_HOLD:
 688                         gates |= BIT(0);
 689                         fpe = true;
 690                         break;
 691                 case TC_TAPRIO_CMD_SET_AND_RELEASE:
 692                         gates &= ~BIT(0);
 693                         fpe = true;
 694                         break;
 695                 default:
 696                         return -EOPNOTSUPP;
 697                 }
 698
 699                 priv->plat->est->gcl[i] = delta_ns | (gates << wid);
 700         }
 701
 702         /* Adjust for real system time */
 703         time = ktime_to_timespec64(qopt->base_time);
 704         priv->plat->est->btr[0] = (u32)time.tv_nsec;
 705         priv->plat->est->btr[1] = (u32)time.tv_sec;
 706
 707         ctr = qopt->cycle_time;
 708         priv->plat->est->ctr[0] = do_div(ctr, NSEC_PER_SEC);
 709         priv->plat->est->ctr[1] = (u32)ctr;
 710
 711         if (fpe && !priv->dma_cap.fpesel)
 712                 return -EOPNOTSUPP;
 713
 714         ret = stmmac_fpe_configure(priv, priv->ioaddr,
 715                                    priv->plat->tx_queues_to_use,
 716                                    priv->plat->rx_queues_to_use, fpe);
 717         if (ret && fpe) {
 718                 netdev_err(priv->dev, "failed to enable Frame Preemption\n");
 719                 return ret;
 720         }
 721
 722         ret = stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
 723                                    priv->plat->clk_ptp_rate);
 724         if (ret) {
 725                 netdev_err(priv->dev, "failed to configure EST\n");
 726                 goto disable;
 727         }
 728
 729         netdev_info(priv->dev, "configured EST\n");
 730         return 0;
 731
 732 disable:
 733         priv->plat->est->enable = false;
 734         stmmac_est_configure(priv, priv->ioaddr, priv->plat->est,
 735                              priv->plat->clk_ptp_rate);
 736         return ret;
 737 }
 738
 739 static int tc_setup_etf(struct stmmac_priv *priv,
 740                         struct tc_etf_qopt_offload *qopt)
 741 {
 742         if (!priv->dma_cap.tbssel)
 743                 return -EOPNOTSUPP;
 744         if (qopt->queue >= priv->plat->tx_queues_to_use)
 745                 return -EINVAL;
 746         if (!(priv->tx_queue[qopt->queue].tbs & STMMAC_TBS_AVAIL))
 747                 return -EINVAL;
 748
 749         if (qopt->enable)
 750                 priv->tx_queue[qopt->queue].tbs |= STMMAC_TBS_EN;
 751         else
 752                 priv->tx_queue[qopt->queue].tbs &= ~STMMAC_TBS_EN;
 753
 754         netdev_info(priv->dev, "%s ETF for Queue %d\n",
 755                     qopt->enable ? "enabled" : "disabled", qopt->queue);
 756         return 0;
 757 }
 758
 759 const struct stmmac_tc_ops dwmac510_tc_ops = {
 760         .init = tc_init,
 761         .setup_cls_u32 = tc_setup_cls_u32,
 762         .setup_cbs = tc_setup_cbs,
 763         .setup_cls = tc_setup_cls,
 764         .setup_taprio = tc_setup_taprio,
 765         .setup_etf = tc_setup_etf,
 766 };