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