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