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