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