drivers/net/ethernet/freescale/enetc/enetc_qos.c

   1 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
   2 /* Copyright 2019 NXP */
   3
   4 #include "enetc.h"
   5
   6 #include <net/pkt_sched.h>
   7 #include <linux/math64.h>
   8 #include <linux/refcount.h>
   9 #include <net/pkt_cls.h>
  10 #include <net/tc_act/tc_gate.h>
  11
  12 static u16 enetc_get_max_gcl_len(struct enetc_hw *hw)
  13 {
  14         return enetc_rd(hw, ENETC_QBV_PTGCAPR_OFFSET)
  15                 & ENETC_QBV_MAX_GCL_LEN_MASK;
  16 }
  17
  18 void enetc_sched_speed_set(struct enetc_ndev_priv *priv, int speed)
  19 {
  20         u32 old_speed = priv->speed;
  21         u32 pspeed;
  22
  23         if (speed == old_speed)
  24                 return;
  25
  26         switch (speed) {
  27         case SPEED_1000:
  28                 pspeed = ENETC_PMR_PSPEED_1000M;
  29                 break;
  30         case SPEED_2500:
  31                 pspeed = ENETC_PMR_PSPEED_2500M;
  32                 break;
  33         case SPEED_100:
  34                 pspeed = ENETC_PMR_PSPEED_100M;
  35                 break;
  36         case SPEED_10:
  37         default:
  38                 pspeed = ENETC_PMR_PSPEED_10M;
  39         }
  40
  41         priv->speed = speed;
  42         enetc_port_wr(&priv->si->hw, ENETC_PMR,
  43                       (enetc_port_rd(&priv->si->hw, ENETC_PMR)
  44                       & (~ENETC_PMR_PSPEED_MASK))
  45                       | pspeed);
  46 }
  47
  48 static int enetc_setup_taprio(struct net_device *ndev,
  49                               struct tc_taprio_qopt_offload *admin_conf)
  50 {
  51         struct enetc_ndev_priv *priv = netdev_priv(ndev);
  52         struct enetc_cbd cbd = {.cmd = 0};
  53         struct tgs_gcl_conf *gcl_config;
  54         struct tgs_gcl_data *gcl_data;
  55         dma_addr_t dma;
  56         struct gce *gce;
  57         u16 data_size;
  58         u16 gcl_len;
  59         void *tmp;
  60         u32 tge;
  61         int err;
  62         int i;
  63
  64         if (admin_conf->num_entries > enetc_get_max_gcl_len(&priv->si->hw))
  65                 return -EINVAL;
  66         gcl_len = admin_conf->num_entries;
  67
  68         tge = enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET);
  69         if (!admin_conf->enable) {
  70                 enetc_wr(&priv->si->hw,
  71                          ENETC_QBV_PTGCR_OFFSET,
  72                          tge & (~ENETC_QBV_TGE));
  73                 return 0;
  74         }
  75
  76         if (admin_conf->cycle_time > U32_MAX ||
  77             admin_conf->cycle_time_extension > U32_MAX)
  78                 return -EINVAL;
  79
  80         /* Configure the (administrative) gate control list using the
  81          * control BD descriptor.
  82          */
  83         gcl_config = &cbd.gcl_conf;
  84
  85         data_size = struct_size(gcl_data, entry, gcl_len);
  86         tmp = enetc_cbd_alloc_data_mem(priv->si, &cbd, data_size,
  87                                        &dma, (void *)&gcl_data);
  88         if (!tmp)
  89                 return -ENOMEM;
  90
  91         gce = (struct gce *)(gcl_data + 1);
  92
  93         /* Set all gates open as default */
  94         gcl_config->atc = 0xff;
  95         gcl_config->acl_len = cpu_to_le16(gcl_len);
  96
  97         gcl_data->btl = cpu_to_le32(lower_32_bits(admin_conf->base_time));
  98         gcl_data->bth = cpu_to_le32(upper_32_bits(admin_conf->base_time));
  99         gcl_data->ct = cpu_to_le32(admin_conf->cycle_time);
 100         gcl_data->cte = cpu_to_le32(admin_conf->cycle_time_extension);
 101
 102         for (i = 0; i < gcl_len; i++) {
 103                 struct tc_taprio_sched_entry *temp_entry;
 104                 struct gce *temp_gce = gce + i;
 105
 106                 temp_entry = &admin_conf->entries[i];
 107
 108                 temp_gce->gate = (u8)temp_entry->gate_mask;
 109                 temp_gce->period = cpu_to_le32(temp_entry->interval);
 110         }
 111
 112         cbd.status_flags = 0;
 113
 114         cbd.cls = BDCR_CMD_PORT_GCL;
 115         cbd.status_flags = 0;
 116
 117         enetc_wr(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET,
 118                  tge | ENETC_QBV_TGE);
 119
 120         err = enetc_send_cmd(priv->si, &cbd);
 121         if (err)
 122                 enetc_wr(&priv->si->hw,
 123                          ENETC_QBV_PTGCR_OFFSET,
 124                          tge & (~ENETC_QBV_TGE));
 125
 126         enetc_cbd_free_data_mem(priv->si, data_size, tmp, &dma);
 127
 128         return err;
 129 }
 130
 131 int enetc_setup_tc_taprio(struct net_device *ndev, void *type_data)
 132 {
 133         struct tc_taprio_qopt_offload *taprio = type_data;
 134         struct enetc_ndev_priv *priv = netdev_priv(ndev);
 135         int err;
 136         int i;
 137
 138         /* TSD and Qbv are mutually exclusive in hardware */
 139         for (i = 0; i < priv->num_tx_rings; i++)
 140                 if (priv->tx_ring[i]->tsd_enable)
 141                         return -EBUSY;
 142
 143         for (i = 0; i < priv->num_tx_rings; i++)
 144                 enetc_set_bdr_prio(&priv->si->hw,
 145                                    priv->tx_ring[i]->index,
 146                                    taprio->enable ? i : 0);
 147
 148         err = enetc_setup_taprio(ndev, taprio);
 149
 150         if (err)
 151                 for (i = 0; i < priv->num_tx_rings; i++)
 152                         enetc_set_bdr_prio(&priv->si->hw,
 153                                            priv->tx_ring[i]->index,
 154                                            taprio->enable ? 0 : i);
 155
 156         return err;
 157 }
 158
 159 static u32 enetc_get_cbs_enable(struct enetc_hw *hw, u8 tc)
 160 {
 161         return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBSE;
 162 }
 163
 164 static u8 enetc_get_cbs_bw(struct enetc_hw *hw, u8 tc)
 165 {
 166         return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBS_BW_MASK;
 167 }
 168
 169 int enetc_setup_tc_cbs(struct net_device *ndev, void *type_data)
 170 {
 171         struct enetc_ndev_priv *priv = netdev_priv(ndev);
 172         struct tc_cbs_qopt_offload *cbs = type_data;
 173         u32 port_transmit_rate = priv->speed;
 174         u8 tc_nums = netdev_get_num_tc(ndev);
 175         struct enetc_si *si = priv->si;
 176         u32 hi_credit_bit, hi_credit_reg;
 177         u32 max_interference_size;
 178         u32 port_frame_max_size;
 179         u8 tc = cbs->queue;
 180         u8 prio_top, prio_next;
 181         int bw_sum = 0;
 182         u8 bw;
 183
 184         prio_top = netdev_get_prio_tc_map(ndev, tc_nums - 1);
 185         prio_next = netdev_get_prio_tc_map(ndev, tc_nums - 2);
 186
 187         /* Support highest prio and second prio tc in cbs mode */
 188         if (tc != prio_top && tc != prio_next)
 189                 return -EOPNOTSUPP;
 190
 191         if (!cbs->enable) {
 192                 /* Make sure the other TC that are numerically
 193                  * lower than this TC have been disabled.
 194                  */
 195                 if (tc == prio_top &&
 196                     enetc_get_cbs_enable(&si->hw, prio_next)) {
 197                         dev_err(&ndev->dev,
 198                                 "Disable TC%d before disable TC%d\n",
 199                                 prio_next, tc);
 200                         return -EINVAL;
 201                 }
 202
 203                 enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), 0);
 204                 enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), 0);
 205
 206                 return 0;
 207         }
 208
 209         if (cbs->idleslope - cbs->sendslope != port_transmit_rate * 1000L ||
 210             cbs->idleslope < 0 || cbs->sendslope > 0)
 211                 return -EOPNOTSUPP;
 212
 213         port_frame_max_size = ndev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN;
 214
 215         bw = cbs->idleslope / (port_transmit_rate * 10UL);
 216
 217         /* Make sure the other TC that are numerically
 218          * higher than this TC have been enabled.
 219          */
 220         if (tc == prio_next) {
 221                 if (!enetc_get_cbs_enable(&si->hw, prio_top)) {
 222                         dev_err(&ndev->dev,
 223                                 "Enable TC%d first before enable TC%d\n",
 224                                 prio_top, prio_next);
 225                         return -EINVAL;
 226                 }
 227                 bw_sum += enetc_get_cbs_bw(&si->hw, prio_top);
 228         }
 229
 230         if (bw_sum + bw >= 100) {
 231                 dev_err(&ndev->dev,
 232                         "The sum of all CBS Bandwidth can't exceed 100\n");
 233                 return -EINVAL;
 234         }
 235
 236         enetc_port_rd(&si->hw, ENETC_PTCMSDUR(tc));
 237
 238         /* For top prio TC, the max_interfrence_size is maxSizedFrame.
 239          *
 240          * For next prio TC, the max_interfrence_size is calculated as below:
 241          *
 242          *      max_interference_size = M0 + Ma + Ra * M0 / (R0 - Ra)
 243          *
 244          *      - RA: idleSlope for AVB Class A
 245          *      - R0: port transmit rate
 246          *      - M0: maximum sized frame for the port
 247          *      - MA: maximum sized frame for AVB Class A
 248          */
 249
 250         if (tc == prio_top) {
 251                 max_interference_size = port_frame_max_size * 8;
 252         } else {
 253                 u32 m0, ma, r0, ra;
 254
 255                 m0 = port_frame_max_size * 8;
 256                 ma = enetc_port_rd(&si->hw, ENETC_PTCMSDUR(prio_top)) * 8;
 257                 ra = enetc_get_cbs_bw(&si->hw, prio_top) *
 258                         port_transmit_rate * 10000ULL;
 259                 r0 = port_transmit_rate * 1000000ULL;
 260                 max_interference_size = m0 + ma +
 261                         (u32)div_u64((u64)ra * m0, r0 - ra);
 262         }
 263
 264         /* hiCredit bits calculate by:
 265          *
 266          * maxSizedFrame * (idleSlope/portTxRate)
 267          */
 268         hi_credit_bit = max_interference_size * bw / 100;
 269
 270         /* hiCredit bits to hiCredit register need to calculated as:
 271          *
 272          * (enetClockFrequency / portTransmitRate) * 100
 273          */
 274         hi_credit_reg = (u32)div_u64((ENETC_CLK * 100ULL) * hi_credit_bit,
 275                                      port_transmit_rate * 1000000ULL);
 276
 277         enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), hi_credit_reg);
 278
 279         /* Set bw register and enable this traffic class */
 280         enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), bw | ENETC_CBSE);
 281
 282         return 0;
 283 }
 284
 285 int enetc_setup_tc_txtime(struct net_device *ndev, void *type_data)
 286 {
 287         struct enetc_ndev_priv *priv = netdev_priv(ndev);
 288         struct tc_etf_qopt_offload *qopt = type_data;
 289         u8 tc_nums = netdev_get_num_tc(ndev);
 290         int tc;
 291
 292         if (!tc_nums)
 293                 return -EOPNOTSUPP;
 294
 295         tc = qopt->queue;
 296
 297         if (tc < 0 || tc >= priv->num_tx_rings)
 298                 return -EINVAL;
 299
 300         /* TSD and Qbv are mutually exclusive in hardware */
 301         if (enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET) & ENETC_QBV_TGE)
 302                 return -EBUSY;
 303
 304         priv->tx_ring[tc]->tsd_enable = qopt->enable;
 305         enetc_port_wr(&priv->si->hw, ENETC_PTCTSDR(tc),
 306                       qopt->enable ? ENETC_TSDE : 0);
 307
 308         return 0;
 309 }
 310
 311 enum streamid_type {
 312         STREAMID_TYPE_RESERVED = 0,
 313         STREAMID_TYPE_NULL,
 314         STREAMID_TYPE_SMAC,
 315 };
 316
 317 enum streamid_vlan_tagged {
 318         STREAMID_VLAN_RESERVED = 0,
 319         STREAMID_VLAN_TAGGED,
 320         STREAMID_VLAN_UNTAGGED,
 321         STREAMID_VLAN_ALL,
 322 };
 323
 324 #define ENETC_PSFP_WILDCARD -1
 325 #define HANDLE_OFFSET 100
 326
 327 enum forward_type {
 328         FILTER_ACTION_TYPE_PSFP = BIT(0),
 329         FILTER_ACTION_TYPE_ACL = BIT(1),
 330         FILTER_ACTION_TYPE_BOTH = GENMASK(1, 0),
 331 };
 332
 333 /* This is for limit output type for input actions */
 334 struct actions_fwd {
 335         u64 actions;
 336         u64 keys;       /* include the must needed keys */
 337         enum forward_type output;
 338 };
 339
 340 struct psfp_streamfilter_counters {
 341         u64 matching_frames_count;
 342         u64 passing_frames_count;
 343         u64 not_passing_frames_count;
 344         u64 passing_sdu_count;
 345         u64 not_passing_sdu_count;
 346         u64 red_frames_count;
 347 };
 348
 349 struct enetc_streamid {
 350         u32 index;
 351         union {
 352                 u8 src_mac[6];
 353                 u8 dst_mac[6];
 354         };
 355         u8 filtertype;
 356         u16 vid;
 357         u8 tagged;
 358         s32 handle;
 359 };
 360
 361 struct enetc_psfp_filter {
 362         u32 index;
 363         s32 handle;
 364         s8 prio;
 365         u32 maxsdu;
 366         u32 gate_id;
 367         s32 meter_id;
 368         refcount_t refcount;
 369         struct hlist_node node;
 370 };
 371
 372 struct enetc_psfp_gate {
 373         u32 index;
 374         s8 init_ipv;
 375         u64 basetime;
 376         u64 cycletime;
 377         u64 cycletimext;
 378         u32 num_entries;
 379         refcount_t refcount;
 380         struct hlist_node node;
 381         struct action_gate_entry entries[];
 382 };
 383
 384 /* Only enable the green color frame now
 385  * Will add eir and ebs color blind, couple flag etc when
 386  * policing action add more offloading parameters
 387  */
 388 struct enetc_psfp_meter {
 389         u32 index;
 390         u32 cir;
 391         u32 cbs;
 392         refcount_t refcount;
 393         struct hlist_node node;
 394 };
 395
 396 #define ENETC_PSFP_FLAGS_FMI BIT(0)
 397
 398 struct enetc_stream_filter {
 399         struct enetc_streamid sid;
 400         u32 sfi_index;
 401         u32 sgi_index;
 402         u32 flags;
 403         u32 fmi_index;
 404         struct flow_stats stats;
 405         struct hlist_node node;
 406 };
 407
 408 struct enetc_psfp {
 409         unsigned long dev_bitmap;
 410         unsigned long *psfp_sfi_bitmap;
 411         struct hlist_head stream_list;
 412         struct hlist_head psfp_filter_list;
 413         struct hlist_head psfp_gate_list;
 414         struct hlist_head psfp_meter_list;
 415         spinlock_t psfp_lock; /* spinlock for the struct enetc_psfp r/w */
 416 };
 417
 418 static struct actions_fwd enetc_act_fwd[] = {
 419         {
 420                 BIT(FLOW_ACTION_GATE),
 421                 BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS),
 422                 FILTER_ACTION_TYPE_PSFP
 423         },
 424         {
 425                 BIT(FLOW_ACTION_POLICE) |
 426                 BIT(FLOW_ACTION_GATE),
 427                 BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS),
 428                 FILTER_ACTION_TYPE_PSFP
 429         },
 430         /* example for ACL actions */
 431         {
 432                 BIT(FLOW_ACTION_DROP),
 433                 0,
 434                 FILTER_ACTION_TYPE_ACL
 435         }
 436 };
 437
 438 static struct enetc_psfp epsfp = {
 439         .dev_bitmap = 0,
 440         .psfp_sfi_bitmap = NULL,
 441 };
 442
 443 static LIST_HEAD(enetc_block_cb_list);
 444
 445 /* Stream Identity Entry Set Descriptor */
 446 static int enetc_streamid_hw_set(struct enetc_ndev_priv *priv,
 447                                  struct enetc_streamid *sid,
 448                                  u8 enable)
 449 {
 450         struct enetc_cbd cbd = {.cmd = 0};
 451         struct streamid_data *si_data;
 452         struct streamid_conf *si_conf;
 453         dma_addr_t dma;
 454         u16 data_size;
 455         void *tmp;
 456         int port;
 457         int err;
 458
 459         port = enetc_pf_to_port(priv->si->pdev);
 460         if (port < 0)
 461                 return -EINVAL;
 462
 463         if (sid->index >= priv->psfp_cap.max_streamid)
 464                 return -EINVAL;
 465
 466         if (sid->filtertype != STREAMID_TYPE_NULL &&
 467             sid->filtertype != STREAMID_TYPE_SMAC)
 468                 return -EOPNOTSUPP;
 469
 470         /* Disable operation before enable */
 471         cbd.index = cpu_to_le16((u16)sid->index);
 472         cbd.cls = BDCR_CMD_STREAM_IDENTIFY;
 473         cbd.status_flags = 0;
 474
 475         data_size = sizeof(struct streamid_data);
 476         tmp = enetc_cbd_alloc_data_mem(priv->si, &cbd, data_size,
 477                                        &dma, (void *)&si_data);
 478         if (!tmp)
 479                 return -ENOMEM;
 480
 481         eth_broadcast_addr(si_data->dmac);
 482         si_data->vid_vidm_tg = (ENETC_CBDR_SID_VID_MASK
 483                                + ((0x3 << 14) | ENETC_CBDR_SID_VIDM));
 484
 485         si_conf = &cbd.sid_set;
 486         /* Only one port supported for one entry, set itself */
 487         si_conf->iports = cpu_to_le32(1 << port);
 488         si_conf->id_type = 1;
 489         si_conf->oui[2] = 0x0;
 490         si_conf->oui[1] = 0x80;
 491         si_conf->oui[0] = 0xC2;
 492
 493         err = enetc_send_cmd(priv->si, &cbd);
 494         if (err)
 495                 goto out;
 496
 497         if (!enable)
 498                 goto out;
 499
 500         /* Enable the entry overwrite again incase space flushed by hardware */
 501         cbd.status_flags = 0;
 502
 503         si_conf->en = 0x80;
 504         si_conf->stream_handle = cpu_to_le32(sid->handle);
 505         si_conf->iports = cpu_to_le32(1 << port);
 506         si_conf->id_type = sid->filtertype;
 507         si_conf->oui[2] = 0x0;
 508         si_conf->oui[1] = 0x80;
 509         si_conf->oui[0] = 0xC2;
 510
 511         memset(si_data, 0, data_size);
 512
 513         /* VIDM default to be 1.
 514          * VID Match. If set (b1) then the VID must match, otherwise
 515          * any VID is considered a match. VIDM setting is only used
 516          * when TG is set to b01.
 517          */
 518         if (si_conf->id_type == STREAMID_TYPE_NULL) {
 519                 ether_addr_copy(si_data->dmac, sid->dst_mac);
 520                 si_data->vid_vidm_tg = (sid->vid & ENETC_CBDR_SID_VID_MASK) +
 521                                        ((((u16)(sid->tagged) & 0x3) << 14)
 522                                        | ENETC_CBDR_SID_VIDM);
 523         } else if (si_conf->id_type == STREAMID_TYPE_SMAC) {
 524                 ether_addr_copy(si_data->smac, sid->src_mac);
 525                 si_data->vid_vidm_tg = (sid->vid & ENETC_CBDR_SID_VID_MASK) +
 526                                        ((((u16)(sid->tagged) & 0x3) << 14)
 527                                        | ENETC_CBDR_SID_VIDM);
 528         }
 529
 530         err = enetc_send_cmd(priv->si, &cbd);
 531 out:
 532         enetc_cbd_free_data_mem(priv->si, data_size, tmp, &dma);
 533
 534         return err;
 535 }
 536
 537 /* Stream Filter Instance Set Descriptor */
 538 static int enetc_streamfilter_hw_set(struct enetc_ndev_priv *priv,
 539                                      struct enetc_psfp_filter *sfi,
 540                                      u8 enable)
 541 {
 542         struct enetc_cbd cbd = {.cmd = 0};
 543         struct sfi_conf *sfi_config;
 544         int port;
 545
 546         port = enetc_pf_to_port(priv->si->pdev);
 547         if (port < 0)
 548                 return -EINVAL;
 549
 550         cbd.index = cpu_to_le16(sfi->index);
 551         cbd.cls = BDCR_CMD_STREAM_FILTER;
 552         cbd.status_flags = 0x80;
 553         cbd.length = cpu_to_le16(1);
 554
 555         sfi_config = &cbd.sfi_conf;
 556         if (!enable)
 557                 goto exit;
 558
 559         sfi_config->en = 0x80;
 560
 561         if (sfi->handle >= 0) {
 562                 sfi_config->stream_handle =
 563                         cpu_to_le32(sfi->handle);
 564                 sfi_config->sthm |= 0x80;
 565         }
 566
 567         sfi_config->sg_inst_table_index = cpu_to_le16(sfi->gate_id);
 568         sfi_config->input_ports = cpu_to_le32(1 << port);
 569
 570         /* The priority value which may be matched against the
 571          * frame’s priority value to determine a match for this entry.
 572          */
 573         if (sfi->prio >= 0)
 574                 sfi_config->multi |= (sfi->prio & 0x7) | 0x8;
 575
 576         /* Filter Type. Identifies the contents of the MSDU/FM_INST_INDEX
 577          * field as being either an MSDU value or an index into the Flow
 578          * Meter Instance table.
 579          */
 580         if (sfi->maxsdu) {
 581                 sfi_config->msdu =
 582                 cpu_to_le16(sfi->maxsdu);
 583                 sfi_config->multi |= 0x40;
 584         }
 585
 586         if (sfi->meter_id >= 0) {
 587                 sfi_config->fm_inst_table_index = cpu_to_le16(sfi->meter_id);
 588                 sfi_config->multi |= 0x80;
 589         }
 590
 591 exit:
 592         return enetc_send_cmd(priv->si, &cbd);
 593 }
 594
 595 static int enetc_streamcounter_hw_get(struct enetc_ndev_priv *priv,
 596                                       u32 index,
 597                                       struct psfp_streamfilter_counters *cnt)
 598 {
 599         struct enetc_cbd cbd = { .cmd = 2 };
 600         struct sfi_counter_data *data_buf;
 601         dma_addr_t dma;
 602         u16 data_size;
 603         void *tmp;
 604         int err;
 605
 606         cbd.index = cpu_to_le16((u16)index);
 607         cbd.cmd = 2;
 608         cbd.cls = BDCR_CMD_STREAM_FILTER;
 609         cbd.status_flags = 0;
 610
 611         data_size = sizeof(struct sfi_counter_data);
 612
 613         tmp = enetc_cbd_alloc_data_mem(priv->si, &cbd, data_size,
 614                                        &dma, (void *)&data_buf);
 615         if (!tmp)
 616                 return -ENOMEM;
 617
 618         err = enetc_send_cmd(priv->si, &cbd);
 619         if (err)
 620                 goto exit;
 621
 622         cnt->matching_frames_count = ((u64)data_buf->matchh << 32) +
 623                                      data_buf->matchl;
 624
 625         cnt->not_passing_sdu_count = ((u64)data_buf->msdu_droph << 32) +
 626                                      data_buf->msdu_dropl;
 627
 628         cnt->passing_sdu_count = cnt->matching_frames_count
 629                                 - cnt->not_passing_sdu_count;
 630
 631         cnt->not_passing_frames_count =
 632                                 ((u64)data_buf->stream_gate_droph << 32) +
 633                                 data_buf->stream_gate_dropl;
 634
 635         cnt->passing_frames_count = cnt->matching_frames_count -
 636                                     cnt->not_passing_sdu_count -
 637                                     cnt->not_passing_frames_count;
 638
 639         cnt->red_frames_count = ((u64)data_buf->flow_meter_droph << 32) +
 640                                 data_buf->flow_meter_dropl;
 641
 642 exit:
 643         enetc_cbd_free_data_mem(priv->si, data_size, tmp, &dma);
 644
 645         return err;
 646 }
 647
 648 static u64 get_ptp_now(struct enetc_hw *hw)
 649 {
 650         u64 now_lo, now_hi, now;
 651
 652         now_lo = enetc_rd(hw, ENETC_SICTR0);
 653         now_hi = enetc_rd(hw, ENETC_SICTR1);
 654         now = now_lo | now_hi << 32;
 655
 656         return now;
 657 }
 658
 659 static int get_start_ns(u64 now, u64 cycle, u64 *start)
 660 {
 661         u64 n;
 662
 663         if (!cycle)
 664                 return -EFAULT;
 665
 666         n = div64_u64(now, cycle);
 667
 668         *start = (n + 1) * cycle;
 669
 670         return 0;
 671 }
 672
 673 /* Stream Gate Instance Set Descriptor */
 674 static int enetc_streamgate_hw_set(struct enetc_ndev_priv *priv,
 675                                    struct enetc_psfp_gate *sgi,
 676                                    u8 enable)
 677 {
 678         struct enetc_cbd cbd = { .cmd = 0 };
 679         struct sgi_table *sgi_config;
 680         struct sgcl_conf *sgcl_config;
 681         struct sgcl_data *sgcl_data;
 682         struct sgce *sgce;
 683         dma_addr_t dma;
 684         u16 data_size;
 685         int err, i;
 686         void *tmp;
 687         u64 now;
 688
 689         cbd.index = cpu_to_le16(sgi->index);
 690         cbd.cmd = 0;
 691         cbd.cls = BDCR_CMD_STREAM_GCL;
 692         cbd.status_flags = 0x80;
 693
 694         /* disable */
 695         if (!enable)
 696                 return enetc_send_cmd(priv->si, &cbd);
 697
 698         if (!sgi->num_entries)
 699                 return 0;
 700
 701         if (sgi->num_entries > priv->psfp_cap.max_psfp_gatelist ||
 702             !sgi->cycletime)
 703                 return -EINVAL;
 704
 705         /* enable */
 706         sgi_config = &cbd.sgi_table;
 707
 708         /* Keep open before gate list start */
 709         sgi_config->ocgtst = 0x80;
 710
 711         sgi_config->oipv = (sgi->init_ipv < 0) ?
 712                                 0x0 : ((sgi->init_ipv & 0x7) | 0x8);
 713
 714         sgi_config->en = 0x80;
 715
 716         /* Basic config */
 717         err = enetc_send_cmd(priv->si, &cbd);
 718         if (err)
 719                 return -EINVAL;
 720
 721         memset(&cbd, 0, sizeof(cbd));
 722
 723         cbd.index = cpu_to_le16(sgi->index);
 724         cbd.cmd = 1;
 725         cbd.cls = BDCR_CMD_STREAM_GCL;
 726         cbd.status_flags = 0;
 727
 728         sgcl_config = &cbd.sgcl_conf;
 729
 730         sgcl_config->acl_len = (sgi->num_entries - 1) & 0x3;
 731
 732         data_size = struct_size(sgcl_data, sgcl, sgi->num_entries);
 733         tmp = enetc_cbd_alloc_data_mem(priv->si, &cbd, data_size,
 734                                        &dma, (void *)&sgcl_data);
 735         if (!tmp)
 736                 return -ENOMEM;
 737
 738         sgce = &sgcl_data->sgcl[0];
 739
 740         sgcl_config->agtst = 0x80;
 741
 742         sgcl_data->ct = sgi->cycletime;
 743         sgcl_data->cte = sgi->cycletimext;
 744
 745         if (sgi->init_ipv >= 0)
 746                 sgcl_config->aipv = (sgi->init_ipv & 0x7) | 0x8;
 747
 748         for (i = 0; i < sgi->num_entries; i++) {
 749                 struct action_gate_entry *from = &sgi->entries[i];
 750                 struct sgce *to = &sgce[i];
 751
 752                 if (from->gate_state)
 753                         to->multi |= 0x10;
 754
 755                 if (from->ipv >= 0)
 756                         to->multi |= ((from->ipv & 0x7) << 5) | 0x08;
 757
 758                 if (from->maxoctets >= 0) {
 759                         to->multi |= 0x01;
 760                         to->msdu[0] = from->maxoctets & 0xFF;
 761                         to->msdu[1] = (from->maxoctets >> 8) & 0xFF;
 762                         to->msdu[2] = (from->maxoctets >> 16) & 0xFF;
 763                 }
 764
 765                 to->interval = from->interval;
 766         }
 767
 768         /* If basetime is less than now, calculate start time */
 769         now = get_ptp_now(&priv->si->hw);
 770
 771         if (sgi->basetime < now) {
 772                 u64 start;
 773
 774                 err = get_start_ns(now, sgi->cycletime, &start);
 775                 if (err)
 776                         goto exit;
 777                 sgcl_data->btl = lower_32_bits(start);
 778                 sgcl_data->bth = upper_32_bits(start);
 779         } else {
 780                 u32 hi, lo;
 781
 782                 hi = upper_32_bits(sgi->basetime);
 783                 lo = lower_32_bits(sgi->basetime);
 784                 sgcl_data->bth = hi;
 785                 sgcl_data->btl = lo;
 786         }
 787
 788         err = enetc_send_cmd(priv->si, &cbd);
 789
 790 exit:
 791         enetc_cbd_free_data_mem(priv->si, data_size, tmp, &dma);
 792         return err;
 793 }
 794
 795 static int enetc_flowmeter_hw_set(struct enetc_ndev_priv *priv,
 796                                   struct enetc_psfp_meter *fmi,
 797                                   u8 enable)
 798 {
 799         struct enetc_cbd cbd = { .cmd = 0 };
 800         struct fmi_conf *fmi_config;
 801         u64 temp = 0;
 802
 803         cbd.index = cpu_to_le16((u16)fmi->index);
 804         cbd.cls = BDCR_CMD_FLOW_METER;
 805         cbd.status_flags = 0x80;
 806
 807         if (!enable)
 808                 return enetc_send_cmd(priv->si, &cbd);
 809
 810         fmi_config = &cbd.fmi_conf;
 811         fmi_config->en = 0x80;
 812
 813         if (fmi->cir) {
 814                 temp = (u64)8000 * fmi->cir;
 815                 temp = div_u64(temp, 3725);
 816         }
 817
 818         fmi_config->cir = cpu_to_le32((u32)temp);
 819         fmi_config->cbs = cpu_to_le32(fmi->cbs);
 820
 821         /* Default for eir ebs disable */
 822         fmi_config->eir = 0;
 823         fmi_config->ebs = 0;
 824
 825         /* Default:
 826          * mark red disable
 827          * drop on yellow disable
 828          * color mode disable
 829          * couple flag disable
 830          */
 831         fmi_config->conf = 0;
 832
 833         return enetc_send_cmd(priv->si, &cbd);
 834 }
 835
 836 static struct enetc_stream_filter *enetc_get_stream_by_index(u32 index)
 837 {
 838         struct enetc_stream_filter *f;
 839
 840         hlist_for_each_entry(f, &epsfp.stream_list, node)
 841                 if (f->sid.index == index)
 842                         return f;
 843
 844         return NULL;
 845 }
 846
 847 static struct enetc_psfp_gate *enetc_get_gate_by_index(u32 index)
 848 {
 849         struct enetc_psfp_gate *g;
 850
 851         hlist_for_each_entry(g, &epsfp.psfp_gate_list, node)
 852                 if (g->index == index)
 853                         return g;
 854
 855         return NULL;
 856 }
 857
 858 static struct enetc_psfp_filter *enetc_get_filter_by_index(u32 index)
 859 {
 860         struct enetc_psfp_filter *s;
 861
 862         hlist_for_each_entry(s, &epsfp.psfp_filter_list, node)
 863                 if (s->index == index)
 864                         return s;
 865
 866         return NULL;
 867 }
 868
 869 static struct enetc_psfp_meter *enetc_get_meter_by_index(u32 index)
 870 {
 871         struct enetc_psfp_meter *m;
 872
 873         hlist_for_each_entry(m, &epsfp.psfp_meter_list, node)
 874                 if (m->index == index)
 875                         return m;
 876
 877         return NULL;
 878 }
 879
 880 static struct enetc_psfp_filter
 881         *enetc_psfp_check_sfi(struct enetc_psfp_filter *sfi)
 882 {
 883         struct enetc_psfp_filter *s;
 884
 885         hlist_for_each_entry(s, &epsfp.psfp_filter_list, node)
 886                 if (s->gate_id == sfi->gate_id &&
 887                     s->prio == sfi->prio &&
 888                     s->maxsdu == sfi->maxsdu &&
 889                     s->meter_id == sfi->meter_id)
 890                         return s;
 891
 892         return NULL;
 893 }
 894
 895 static int enetc_get_free_index(struct enetc_ndev_priv *priv)
 896 {
 897         u32 max_size = priv->psfp_cap.max_psfp_filter;
 898         unsigned long index;
 899
 900         index = find_first_zero_bit(epsfp.psfp_sfi_bitmap, max_size);
 901         if (index == max_size)
 902                 return -1;
 903
 904         return index;
 905 }
 906
 907 static void stream_filter_unref(struct enetc_ndev_priv *priv, u32 index)
 908 {
 909         struct enetc_psfp_filter *sfi;
 910         u8 z;
 911
 912         sfi = enetc_get_filter_by_index(index);
 913         WARN_ON(!sfi);
 914         z = refcount_dec_and_test(&sfi->refcount);
 915
 916         if (z) {
 917                 enetc_streamfilter_hw_set(priv, sfi, false);
 918                 hlist_del(&sfi->node);
 919                 kfree(sfi);
 920                 clear_bit(index, epsfp.psfp_sfi_bitmap);
 921         }
 922 }
 923
 924 static void stream_gate_unref(struct enetc_ndev_priv *priv, u32 index)
 925 {
 926         struct enetc_psfp_gate *sgi;
 927         u8 z;
 928
 929         sgi = enetc_get_gate_by_index(index);
 930         WARN_ON(!sgi);
 931         z = refcount_dec_and_test(&sgi->refcount);
 932         if (z) {
 933                 enetc_streamgate_hw_set(priv, sgi, false);
 934                 hlist_del(&sgi->node);
 935                 kfree(sgi);
 936         }
 937 }
 938
 939 static void flow_meter_unref(struct enetc_ndev_priv *priv, u32 index)
 940 {
 941         struct enetc_psfp_meter *fmi;
 942         u8 z;
 943
 944         fmi = enetc_get_meter_by_index(index);
 945         WARN_ON(!fmi);
 946         z = refcount_dec_and_test(&fmi->refcount);
 947         if (z) {
 948                 enetc_flowmeter_hw_set(priv, fmi, false);
 949                 hlist_del(&fmi->node);
 950                 kfree(fmi);
 951         }
 952 }
 953
 954 static void remove_one_chain(struct enetc_ndev_priv *priv,
 955                              struct enetc_stream_filter *filter)
 956 {
 957         if (filter->flags & ENETC_PSFP_FLAGS_FMI)
 958                 flow_meter_unref(priv, filter->fmi_index);
 959
 960         stream_gate_unref(priv, filter->sgi_index);
 961         stream_filter_unref(priv, filter->sfi_index);
 962
 963         hlist_del(&filter->node);
 964         kfree(filter);
 965 }
 966
 967 static int enetc_psfp_hw_set(struct enetc_ndev_priv *priv,
 968                              struct enetc_streamid *sid,
 969                              struct enetc_psfp_filter *sfi,
 970                              struct enetc_psfp_gate *sgi,
 971                              struct enetc_psfp_meter *fmi)
 972 {
 973         int err;
 974
 975         err = enetc_streamid_hw_set(priv, sid, true);
 976         if (err)
 977                 return err;
 978
 979         if (sfi) {
 980                 err = enetc_streamfilter_hw_set(priv, sfi, true);
 981                 if (err)
 982                         goto revert_sid;
 983         }
 984
 985         err = enetc_streamgate_hw_set(priv, sgi, true);
 986         if (err)
 987                 goto revert_sfi;
 988
 989         if (fmi) {
 990                 err = enetc_flowmeter_hw_set(priv, fmi, true);
 991                 if (err)
 992                         goto revert_sgi;
 993         }
 994
 995         return 0;
 996
 997 revert_sgi:
 998         enetc_streamgate_hw_set(priv, sgi, false);
 999 revert_sfi:
1000         if (sfi)
1001                 enetc_streamfilter_hw_set(priv, sfi, false);
1002 revert_sid:
1003         enetc_streamid_hw_set(priv, sid, false);
1004         return err;
1005 }
1006
1007 static struct actions_fwd *enetc_check_flow_actions(u64 acts,
1008                                                     unsigned int inputkeys)
1009 {
1010         int i;
1011
1012         for (i = 0; i < ARRAY_SIZE(enetc_act_fwd); i++)
1013                 if (acts == enetc_act_fwd[i].actions &&
1014                     inputkeys & enetc_act_fwd[i].keys)
1015                         return &enetc_act_fwd[i];
1016
1017         return NULL;
1018 }
1019
1020 static int enetc_psfp_policer_validate(const struct flow_action *action,
1021                                        const struct flow_action_entry *act,
1022                                        struct netlink_ext_ack *extack)
1023 {
1024         if (act->police.exceed.act_id != FLOW_ACTION_DROP) {
1025                 NL_SET_ERR_MSG_MOD(extack,
1026                                    "Offload not supported when exceed action is not drop");
1027                 return -EOPNOTSUPP;
1028         }
1029
1030         if (act->police.notexceed.act_id != FLOW_ACTION_PIPE &&
1031             act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) {
1032                 NL_SET_ERR_MSG_MOD(extack,
1033                                    "Offload not supported when conform action is not pipe or ok");
1034                 return -EOPNOTSUPP;
1035         }
1036
1037         if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT &&
1038             !flow_action_is_last_entry(action, act)) {
1039                 NL_SET_ERR_MSG_MOD(extack,
1040                                    "Offload not supported when conform action is ok, but action is not last");
1041                 return -EOPNOTSUPP;
1042         }
1043
1044         if (act->police.peakrate_bytes_ps ||
1045             act->police.avrate || act->police.overhead) {
1046                 NL_SET_ERR_MSG_MOD(extack,
1047                                    "Offload not supported when peakrate/avrate/overhead is configured");
1048                 return -EOPNOTSUPP;
1049         }
1050
1051         if (act->police.rate_pkt_ps) {
1052                 NL_SET_ERR_MSG_MOD(extack,
1053                                    "QoS offload not support packets per second");
1054                 return -EOPNOTSUPP;
1055         }
1056
1057         return 0;
1058 }
1059
1060 static int enetc_psfp_parse_clsflower(struct enetc_ndev_priv *priv,
1061                                       struct flow_cls_offload *f)
1062 {
1063         struct flow_action_entry *entryg = NULL, *entryp = NULL;
1064         struct flow_rule *rule = flow_cls_offload_flow_rule(f);
1065         struct netlink_ext_ack *extack = f->common.extack;
1066         struct enetc_stream_filter *filter, *old_filter;
1067         struct enetc_psfp_meter *fmi = NULL, *old_fmi;
1068         struct enetc_psfp_filter *sfi, *old_sfi;
1069         struct enetc_psfp_gate *sgi, *old_sgi;
1070         struct flow_action_entry *entry;
1071         struct action_gate_entry *e;
1072         u8 sfi_overwrite = 0;
1073         int entries_size;
1074         int i, err;
1075
1076         if (f->common.chain_index >= priv->psfp_cap.max_streamid) {
1077                 NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!");
1078                 return -ENOSPC;
1079         }
1080
1081         flow_action_for_each(i, entry, &rule->action)
1082                 if (entry->id == FLOW_ACTION_GATE)
1083                         entryg = entry;
1084                 else if (entry->id == FLOW_ACTION_POLICE)
1085                         entryp = entry;
1086
1087         /* Not support without gate action */
1088         if (!entryg)
1089                 return -EINVAL;
1090
1091         filter = kzalloc(sizeof(*filter), GFP_KERNEL);
1092         if (!filter)
1093                 return -ENOMEM;
1094
1095         filter->sid.index = f->common.chain_index;
1096
1097         if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
1098                 struct flow_match_eth_addrs match;
1099
1100                 flow_rule_match_eth_addrs(rule, &match);
1101
1102                 if (!is_zero_ether_addr(match.mask->dst) &&
1103                     !is_zero_ether_addr(match.mask->src)) {
1104                         NL_SET_ERR_MSG_MOD(extack,
1105                                            "Cannot match on both source and destination MAC");
1106                         err = -EINVAL;
1107                         goto free_filter;
1108                 }
1109
1110                 if (!is_zero_ether_addr(match.mask->dst)) {
1111                         if (!is_broadcast_ether_addr(match.mask->dst)) {
1112                                 NL_SET_ERR_MSG_MOD(extack,
1113                                                    "Masked matching on destination MAC not supported");
1114                                 err = -EINVAL;
1115                                 goto free_filter;
1116                         }
1117                         ether_addr_copy(filter->sid.dst_mac, match.key->dst);
1118                         filter->sid.filtertype = STREAMID_TYPE_NULL;
1119                 }
1120
1121                 if (!is_zero_ether_addr(match.mask->src)) {
1122                         if (!is_broadcast_ether_addr(match.mask->src)) {
1123                                 NL_SET_ERR_MSG_MOD(extack,
1124                                                    "Masked matching on source MAC not supported");
1125                                 err = -EINVAL;
1126                                 goto free_filter;
1127                         }
1128                         ether_addr_copy(filter->sid.src_mac, match.key->src);
1129                         filter->sid.filtertype = STREAMID_TYPE_SMAC;
1130                 }
1131         } else {
1132                 NL_SET_ERR_MSG_MOD(extack, "Unsupported, must include ETH_ADDRS");
1133                 err = -EINVAL;
1134                 goto free_filter;
1135         }
1136
1137         if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
1138                 struct flow_match_vlan match;
1139
1140                 flow_rule_match_vlan(rule, &match);
1141                 if (match.mask->vlan_priority) {
1142                         if (match.mask->vlan_priority !=
1143                             (VLAN_PRIO_MASK >> VLAN_PRIO_SHIFT)) {
1144                                 NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN priority");
1145                                 err = -EINVAL;
1146                                 goto free_filter;
1147                         }
1148                 }
1149
1150                 if (match.mask->vlan_id) {
1151                         if (match.mask->vlan_id != VLAN_VID_MASK) {
1152                                 NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN id");
1153                                 err = -EINVAL;
1154                                 goto free_filter;
1155                         }
1156
1157                         filter->sid.vid = match.key->vlan_id;
1158                         if (!filter->sid.vid)
1159                                 filter->sid.tagged = STREAMID_VLAN_UNTAGGED;
1160                         else
1161                                 filter->sid.tagged = STREAMID_VLAN_TAGGED;
1162                 }
1163         } else {
1164                 filter->sid.tagged = STREAMID_VLAN_ALL;
1165         }
1166
1167         /* parsing gate action */
1168         if (entryg->hw_index >= priv->psfp_cap.max_psfp_gate) {
1169                 NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!");
1170                 err = -ENOSPC;
1171                 goto free_filter;
1172         }
1173
1174         if (entryg->gate.num_entries >= priv->psfp_cap.max_psfp_gatelist) {
1175                 NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!");
1176                 err = -ENOSPC;
1177                 goto free_filter;
1178         }
1179
1180         entries_size = struct_size(sgi, entries, entryg->gate.num_entries);
1181         sgi = kzalloc(entries_size, GFP_KERNEL);
1182         if (!sgi) {
1183                 err = -ENOMEM;
1184                 goto free_filter;
1185         }
1186
1187         refcount_set(&sgi->refcount, 1);
1188         sgi->index = entryg->hw_index;
1189         sgi->init_ipv = entryg->gate.prio;
1190         sgi->basetime = entryg->gate.basetime;
1191         sgi->cycletime = entryg->gate.cycletime;
1192         sgi->num_entries = entryg->gate.num_entries;
1193
1194         e = sgi->entries;
1195         for (i = 0; i < entryg->gate.num_entries; i++) {
1196                 e[i].gate_state = entryg->gate.entries[i].gate_state;
1197                 e[i].interval = entryg->gate.entries[i].interval;
1198                 e[i].ipv = entryg->gate.entries[i].ipv;
1199                 e[i].maxoctets = entryg->gate.entries[i].maxoctets;
1200         }
1201
1202         filter->sgi_index = sgi->index;
1203
1204         sfi = kzalloc(sizeof(*sfi), GFP_KERNEL);
1205         if (!sfi) {
1206                 err = -ENOMEM;
1207                 goto free_gate;
1208         }
1209
1210         refcount_set(&sfi->refcount, 1);
1211         sfi->gate_id = sgi->index;
1212         sfi->meter_id = ENETC_PSFP_WILDCARD;
1213
1214         /* Flow meter and max frame size */
1215         if (entryp) {
1216                 err = enetc_psfp_policer_validate(&rule->action, entryp, extack);
1217                 if (err)
1218                         goto free_sfi;
1219
1220                 if (entryp->police.burst) {
1221                         fmi = kzalloc(sizeof(*fmi), GFP_KERNEL);
1222                         if (!fmi) {
1223                                 err = -ENOMEM;
1224                                 goto free_sfi;
1225                         }
1226                         refcount_set(&fmi->refcount, 1);
1227                         fmi->cir = entryp->police.rate_bytes_ps;
1228                         fmi->cbs = entryp->police.burst;
1229                         fmi->index = entryp->hw_index;
1230                         filter->flags |= ENETC_PSFP_FLAGS_FMI;
1231                         filter->fmi_index = fmi->index;
1232                         sfi->meter_id = fmi->index;
1233                 }
1234
1235                 if (entryp->police.mtu)
1236                         sfi->maxsdu = entryp->police.mtu;
1237         }
1238
1239         /* prio ref the filter prio */
1240         if (f->common.prio && f->common.prio <= BIT(3))
1241                 sfi->prio = f->common.prio - 1;
1242         else
1243                 sfi->prio = ENETC_PSFP_WILDCARD;
1244
1245         old_sfi = enetc_psfp_check_sfi(sfi);
1246         if (!old_sfi) {
1247                 int index;
1248
1249                 index = enetc_get_free_index(priv);
1250                 if (sfi->handle < 0) {
1251                         NL_SET_ERR_MSG_MOD(extack, "No Stream Filter resource!");
1252                         err = -ENOSPC;
1253                         goto free_fmi;
1254                 }
1255
1256                 sfi->index = index;
1257                 sfi->handle = index + HANDLE_OFFSET;
1258                 /* Update the stream filter handle also */
1259                 filter->sid.handle = sfi->handle;
1260                 filter->sfi_index = sfi->index;
1261                 sfi_overwrite = 0;
1262         } else {
1263                 filter->sfi_index = old_sfi->index;
1264                 filter->sid.handle = old_sfi->handle;
1265                 sfi_overwrite = 1;
1266         }
1267
1268         err = enetc_psfp_hw_set(priv, &filter->sid,
1269                                 sfi_overwrite ? NULL : sfi, sgi, fmi);
1270         if (err)
1271                 goto free_fmi;
1272
1273         spin_lock(&epsfp.psfp_lock);
1274         if (filter->flags & ENETC_PSFP_FLAGS_FMI) {
1275                 old_fmi = enetc_get_meter_by_index(filter->fmi_index);
1276                 if (old_fmi) {
1277                         fmi->refcount = old_fmi->refcount;
1278                         refcount_set(&fmi->refcount,
1279                                      refcount_read(&old_fmi->refcount) + 1);
1280                         hlist_del(&old_fmi->node);
1281                         kfree(old_fmi);
1282                 }
1283                 hlist_add_head(&fmi->node, &epsfp.psfp_meter_list);
1284         }
1285
1286         /* Remove the old node if exist and update with a new node */
1287         old_sgi = enetc_get_gate_by_index(filter->sgi_index);
1288         if (old_sgi) {
1289                 refcount_set(&sgi->refcount,
1290                              refcount_read(&old_sgi->refcount) + 1);
1291                 hlist_del(&old_sgi->node);
1292                 kfree(old_sgi);
1293         }
1294
1295         hlist_add_head(&sgi->node, &epsfp.psfp_gate_list);
1296
1297         if (!old_sfi) {
1298                 hlist_add_head(&sfi->node, &epsfp.psfp_filter_list);
1299                 set_bit(sfi->index, epsfp.psfp_sfi_bitmap);
1300         } else {
1301                 kfree(sfi);
1302                 refcount_inc(&old_sfi->refcount);
1303         }
1304
1305         old_filter = enetc_get_stream_by_index(filter->sid.index);
1306         if (old_filter)
1307                 remove_one_chain(priv, old_filter);
1308
1309         filter->stats.lastused = jiffies;
1310         hlist_add_head(&filter->node, &epsfp.stream_list);
1311
1312         spin_unlock(&epsfp.psfp_lock);
1313
1314         return 0;
1315
1316 free_fmi:
1317         kfree(fmi);
1318 free_sfi:
1319         kfree(sfi);
1320 free_gate:
1321         kfree(sgi);
1322 free_filter:
1323         kfree(filter);
1324
1325         return err;
1326 }
1327
1328 static int enetc_config_clsflower(struct enetc_ndev_priv *priv,
1329                                   struct flow_cls_offload *cls_flower)
1330 {
1331         struct flow_rule *rule = flow_cls_offload_flow_rule(cls_flower);
1332         struct netlink_ext_ack *extack = cls_flower->common.extack;
1333         struct flow_dissector *dissector = rule->match.dissector;
1334         struct flow_action *action = &rule->action;
1335         struct flow_action_entry *entry;
1336         struct actions_fwd *fwd;
1337         u64 actions = 0;
1338         int i, err;
1339
1340         if (!flow_action_has_entries(action)) {
1341                 NL_SET_ERR_MSG_MOD(extack, "At least one action is needed");
1342                 return -EINVAL;
1343         }
1344
1345         flow_action_for_each(i, entry, action)
1346                 actions |= BIT(entry->id);
1347
1348         fwd = enetc_check_flow_actions(actions, dissector->used_keys);
1349         if (!fwd) {
1350                 NL_SET_ERR_MSG_MOD(extack, "Unsupported filter type!");
1351                 return -EOPNOTSUPP;
1352         }
1353
1354         if (fwd->output & FILTER_ACTION_TYPE_PSFP) {
1355                 err = enetc_psfp_parse_clsflower(priv, cls_flower);
1356                 if (err) {
1357                         NL_SET_ERR_MSG_MOD(extack, "Invalid PSFP inputs");
1358                         return err;
1359                 }
1360         } else {
1361                 NL_SET_ERR_MSG_MOD(extack, "Unsupported actions");
1362                 return -EOPNOTSUPP;
1363         }
1364
1365         return 0;
1366 }
1367
1368 static int enetc_psfp_destroy_clsflower(struct enetc_ndev_priv *priv,
1369                                         struct flow_cls_offload *f)
1370 {
1371         struct enetc_stream_filter *filter;
1372         struct netlink_ext_ack *extack = f->common.extack;
1373         int err;
1374
1375         if (f->common.chain_index >= priv->psfp_cap.max_streamid) {
1376                 NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!");
1377                 return -ENOSPC;
1378         }
1379
1380         filter = enetc_get_stream_by_index(f->common.chain_index);
1381         if (!filter)
1382                 return -EINVAL;
1383
1384         err = enetc_streamid_hw_set(priv, &filter->sid, false);
1385         if (err)
1386                 return err;
1387
1388         remove_one_chain(priv, filter);
1389
1390         return 0;
1391 }
1392
1393 static int enetc_destroy_clsflower(struct enetc_ndev_priv *priv,
1394                                    struct flow_cls_offload *f)
1395 {
1396         return enetc_psfp_destroy_clsflower(priv, f);
1397 }
1398
1399 static int enetc_psfp_get_stats(struct enetc_ndev_priv *priv,
1400                                 struct flow_cls_offload *f)
1401 {
1402         struct psfp_streamfilter_counters counters = {};
1403         struct enetc_stream_filter *filter;
1404         struct flow_stats stats = {};
1405         int err;
1406
1407         filter = enetc_get_stream_by_index(f->common.chain_index);
1408         if (!filter)
1409                 return -EINVAL;
1410
1411         err = enetc_streamcounter_hw_get(priv, filter->sfi_index, &counters);
1412         if (err)
1413                 return -EINVAL;
1414
1415         spin_lock(&epsfp.psfp_lock);
1416         stats.pkts = counters.matching_frames_count +
1417                      counters.not_passing_sdu_count -
1418                      filter->stats.pkts;
1419         stats.drops = counters.not_passing_frames_count +
1420                       counters.not_passing_sdu_count +
1421                       counters.red_frames_count -
1422                       filter->stats.drops;
1423         stats.lastused = filter->stats.lastused;
1424         filter->stats.pkts += stats.pkts;
1425         filter->stats.drops += stats.drops;
1426         spin_unlock(&epsfp.psfp_lock);
1427
1428         flow_stats_update(&f->stats, 0x0, stats.pkts, stats.drops,
1429                           stats.lastused, FLOW_ACTION_HW_STATS_DELAYED);
1430
1431         return 0;
1432 }
1433
1434 static int enetc_setup_tc_cls_flower(struct enetc_ndev_priv *priv,
1435                                      struct flow_cls_offload *cls_flower)
1436 {
1437         switch (cls_flower->command) {
1438         case FLOW_CLS_REPLACE:
1439                 return enetc_config_clsflower(priv, cls_flower);
1440         case FLOW_CLS_DESTROY:
1441                 return enetc_destroy_clsflower(priv, cls_flower);
1442         case FLOW_CLS_STATS:
1443                 return enetc_psfp_get_stats(priv, cls_flower);
1444         default:
1445                 return -EOPNOTSUPP;
1446         }
1447 }
1448
1449 static inline void clean_psfp_sfi_bitmap(void)
1450 {
1451         bitmap_free(epsfp.psfp_sfi_bitmap);
1452         epsfp.psfp_sfi_bitmap = NULL;
1453 }
1454
1455 static void clean_stream_list(void)
1456 {
1457         struct enetc_stream_filter *s;
1458         struct hlist_node *tmp;
1459
1460         hlist_for_each_entry_safe(s, tmp, &epsfp.stream_list, node) {
1461                 hlist_del(&s->node);
1462                 kfree(s);
1463         }
1464 }
1465
1466 static void clean_sfi_list(void)
1467 {
1468         struct enetc_psfp_filter *sfi;
1469         struct hlist_node *tmp;
1470
1471         hlist_for_each_entry_safe(sfi, tmp, &epsfp.psfp_filter_list, node) {
1472                 hlist_del(&sfi->node);
1473                 kfree(sfi);
1474         }
1475 }
1476
1477 static void clean_sgi_list(void)
1478 {
1479         struct enetc_psfp_gate *sgi;
1480         struct hlist_node *tmp;
1481
1482         hlist_for_each_entry_safe(sgi, tmp, &epsfp.psfp_gate_list, node) {
1483                 hlist_del(&sgi->node);
1484                 kfree(sgi);
1485         }
1486 }
1487
1488 static void clean_psfp_all(void)
1489 {
1490         /* Disable all list nodes and free all memory */
1491         clean_sfi_list();
1492         clean_sgi_list();
1493         clean_stream_list();
1494         epsfp.dev_bitmap = 0;
1495         clean_psfp_sfi_bitmap();
1496 }
1497
1498 int enetc_setup_tc_block_cb(enum tc_setup_type type, void *type_data,
1499                             void *cb_priv)
1500 {
1501         struct net_device *ndev = cb_priv;
1502
1503         if (!tc_can_offload(ndev))
1504                 return -EOPNOTSUPP;
1505
1506         switch (type) {
1507         case TC_SETUP_CLSFLOWER:
1508                 return enetc_setup_tc_cls_flower(netdev_priv(ndev), type_data);
1509         default:
1510                 return -EOPNOTSUPP;
1511         }
1512 }
1513
1514 int enetc_psfp_init(struct enetc_ndev_priv *priv)
1515 {
1516         if (epsfp.psfp_sfi_bitmap)
1517                 return 0;
1518
1519         epsfp.psfp_sfi_bitmap = bitmap_zalloc(priv->psfp_cap.max_psfp_filter,
1520                                               GFP_KERNEL);
1521         if (!epsfp.psfp_sfi_bitmap)
1522                 return -ENOMEM;
1523
1524         spin_lock_init(&epsfp.psfp_lock);
1525
1526         if (list_empty(&enetc_block_cb_list))
1527                 epsfp.dev_bitmap = 0;
1528
1529         return 0;
1530 }
1531
1532 int enetc_psfp_clean(struct enetc_ndev_priv *priv)
1533 {
1534         if (!list_empty(&enetc_block_cb_list))
1535                 return -EBUSY;
1536
1537         clean_psfp_all();
1538
1539         return 0;
1540 }
1541
1542 int enetc_setup_tc_psfp(struct net_device *ndev, void *type_data)
1543 {
1544         struct enetc_ndev_priv *priv = netdev_priv(ndev);
1545         struct flow_block_offload *f = type_data;
1546         int port, err;
1547
1548         err = flow_block_cb_setup_simple(f, &enetc_block_cb_list,
1549                                          enetc_setup_tc_block_cb,
1550                                          ndev, ndev, true);
1551         if (err)
1552                 return err;
1553
1554         switch (f->command) {
1555         case FLOW_BLOCK_BIND:
1556                 port = enetc_pf_to_port(priv->si->pdev);
1557                 if (port < 0)
1558                         return -EINVAL;
1559
1560                 set_bit(port, &epsfp.dev_bitmap);
1561                 break;
1562         case FLOW_BLOCK_UNBIND:
1563                 port = enetc_pf_to_port(priv->si->pdev);
1564                 if (port < 0)
1565                         return -EINVAL;
1566
1567                 clear_bit(port, &epsfp.dev_bitmap);
1568                 if (!epsfp.dev_bitmap)
1569                         clean_psfp_all();
1570                 break;
1571         }
1572
1573         return 0;
1574 }