target/linux/layerscape/patches-5.4/701-net-0272-net-mscc-ocelot-tsn-configuration-support.patch

   1 From eb5556db4c4fb8dff9a7b716c66a1ea3d3e696ce Mon Sep 17 00:00:00 2001
   2 From: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
   3 Date: Fri, 29 Nov 2019 11:02:43 +0800
   4 Subject: [PATCH] net: mscc: ocelot: tsn configuration support
   5
   6 Support TSN configuration for ocelot switch. The TSN configuration
   7 fucntions are based on tsn netlink interface, it can support Qbv,
   8 Qbu, Qci, 802.1CB, and Qav configuration now.
   9
  10 Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
  11 ---
  12  drivers/net/ethernet/mscc/Makefile          |    1 +
  13  drivers/net/ethernet/mscc/ocelot.c          |   11 +-
  14  drivers/net/ethernet/mscc/ocelot.h          |    2 +
  15  drivers/net/ethernet/mscc/ocelot_ana.h      |   25 +-
  16  drivers/net/ethernet/mscc/ocelot_dev_gmii.h |  153 +++
  17  drivers/net/ethernet/mscc/ocelot_tsn.c      | 1572 +++++++++++++++++++++++++++
  18  drivers/net/ethernet/mscc/ocelot_tsn.h      |   51 +
  19  include/soc/mscc/ocelot.h                   |   52 +-
  20  8 files changed, 1857 insertions(+), 10 deletions(-)
  21  create mode 100644 drivers/net/ethernet/mscc/ocelot_dev_gmii.h
  22  create mode 100644 drivers/net/ethernet/mscc/ocelot_tsn.c
  23  create mode 100644 drivers/net/ethernet/mscc/ocelot_tsn.h
  24
  25 --- a/drivers/net/ethernet/mscc/Makefile
  26 +++ b/drivers/net/ethernet/mscc/Makefile
  27 @@ -2,4 +2,5 @@
  28  obj-$(CONFIG_MSCC_OCELOT_SWITCH) += mscc_ocelot_common.o
  29  mscc_ocelot_common-y := ocelot.o ocelot_io.o
  30  mscc_ocelot_common-y += ocelot_regs.o ocelot_tc.o ocelot_police.o ocelot_ace.o ocelot_flower.o
  31 +mscc_ocelot_common-y += ocelot_tsn.o
  32  obj-$(CONFIG_MSCC_OCELOT_SWITCH_OCELOT) += ocelot_board.o
  33 --- a/drivers/net/ethernet/mscc/ocelot.c
  34 +++ b/drivers/net/ethernet/mscc/ocelot.c
  35 @@ -780,7 +780,7 @@ static void ocelot_set_rx_mode(struct ne
  36          * forwarded to the CPU port.
  37          */
  38         val = GENMASK(ocelot->num_phys_ports - 1, 0);
  39 -       for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
  40 +       for (i = ocelot->num_phys_ports + 1; i < PGID_MCRED; i++)
  41                 ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
  42
  43         __dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
  44 @@ -2412,10 +2412,11 @@ int ocelot_init(struct ocelot *ocelot)
  45                      SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
  46
  47         /* Setup flooding PGIDs */
  48 -       ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
  49 -                        ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
  50 -                        ANA_FLOODING_FLD_UNICAST(PGID_UC),
  51 -                        ANA_FLOODING, 0);
  52 +       for (i = 0; i < 8; i++)
  53 +               ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
  54 +                                ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
  55 +                                ANA_FLOODING_FLD_UNICAST(PGID_UC),
  56 +                                ANA_FLOODING, i);
  57         ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
  58                      ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
  59                      ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
  60 --- a/drivers/net/ethernet/mscc/ocelot.h
  61 +++ b/drivers/net/ethernet/mscc/ocelot.h
  62 @@ -27,11 +27,13 @@
  63  #include "ocelot_qs.h"
  64  #include "ocelot_tc.h"
  65  #include "ocelot_ptp.h"
  66 +#include "ocelot_dev_gmii.h"
  67
  68  #define PGID_AGGR    64
  69  #define PGID_SRC     80
  70
  71  /* Reserved PGIDs */
  72 +#define PGID_MCRED   (PGID_AGGR - 25)
  73  #define PGID_CPU     (PGID_AGGR - 5)
  74  #define PGID_UC      (PGID_AGGR - 4)
  75  #define PGID_MC      (PGID_AGGR - 3)
  76 --- a/drivers/net/ethernet/mscc/ocelot_ana.h
  77 +++ b/drivers/net/ethernet/mscc/ocelot_ana.h
  78 @@ -227,6 +227,11 @@
  79  #define ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(x)             ((x) & GENMASK(1, 0))
  80  #define ANA_TABLES_SFIDACCESS_SFID_TBL_CMD_M              GENMASK(1, 0)
  81
  82 +#define SFIDACCESS_CMD_IDLE                             0
  83 +#define SFIDACCESS_CMD_READ                             1
  84 +#define SFIDACCESS_CMD_WRITE                            2
  85 +#define SFIDACCESS_CMD_INIT                            3
  86 +
  87  #define ANA_TABLES_SFIDTIDX_SGID_VALID                    BIT(26)
  88  #define ANA_TABLES_SFIDTIDX_SGID(x)                       (((x) << 18) & GENMASK(25, 18))
  89  #define ANA_TABLES_SFIDTIDX_SGID_M                        GENMASK(25, 18)
  90 @@ -252,15 +257,23 @@
  91  #define ANA_SG_CONFIG_REG_3_LIST_LENGTH_M                 GENMASK(18, 16)
  92  #define ANA_SG_CONFIG_REG_3_LIST_LENGTH_X(x)              (((x) & GENMASK(18, 16)) >> 16)
  93  #define ANA_SG_CONFIG_REG_3_GATE_ENABLE                   BIT(20)
  94 -#define ANA_SG_CONFIG_REG_3_INIT_IPS(x)                   (((x) << 24) & GENMASK(27, 24))
  95 -#define ANA_SG_CONFIG_REG_3_INIT_IPS_M                    GENMASK(27, 24)
  96 -#define ANA_SG_CONFIG_REG_3_INIT_IPS_X(x)                 (((x) & GENMASK(27, 24)) >> 24)
  97 -#define ANA_SG_CONFIG_REG_3_INIT_GATE_STATE               BIT(28)
  98 +#define ANA_SG_CONFIG_REG_3_INIT_IPS(x)                   (((x) << 21) & GENMASK(24, 21))
  99 +#define ANA_SG_CONFIG_REG_3_INIT_IPS_M                    GENMASK(24, 21)
 100 +#define ANA_SG_CONFIG_REG_3_INIT_IPS_X(x)                 (((x) & GENMASK(24, 21)) >> 21)
 101 +#define ANA_SG_CONFIG_REG_3_IPV_VALID                     BIT(24)
 102 +#define ANA_SG_CONFIG_REG_3_IPV_INVALID(x)               (((x) << 24) & GENMASK(24, 24))
 103 +#define ANA_SG_CONFIG_REG_3_INIT_IPV(x)                   (((x) << 21) & GENMASK(23, 21))
 104 +#define ANA_SG_CONFIG_REG_3_INIT_IPV_M                    GENMASK(23, 21)
 105 +#define ANA_SG_CONFIG_REG_3_INIT_IPV_X(x)                 (((x) & GENMASK(23, 21)) >> 21)
 106 +#define ANA_SG_CONFIG_REG_3_INIT_GATE_STATE               BIT(25)
 107
 108  #define ANA_SG_GCL_GS_CONFIG_RSZ                          0x4
 109
 110  #define ANA_SG_GCL_GS_CONFIG_IPS(x)                       ((x) & GENMASK(3, 0))
 111  #define ANA_SG_GCL_GS_CONFIG_IPS_M                        GENMASK(3, 0)
 112 +#define ANA_SG_GCL_GS_CONFIG_IPV_VALID                    BIT(3)
 113 +#define ANA_SG_GCL_GS_CONFIG_IPV(x)                       ((x) & GENMASK(2, 0))
 114 +#define ANA_SG_GCL_GS_CONFIG_IPV_M                        GENMASK(2, 0)
 115  #define ANA_SG_GCL_GS_CONFIG_GATE_STATE                   BIT(4)
 116
 117  #define ANA_SG_GCL_TI_CONFIG_RSZ                          0x4
 118 @@ -271,6 +284,10 @@
 119  #define ANA_SG_STATUS_REG_3_IPS(x)                        (((x) << 20) & GENMASK(23, 20))
 120  #define ANA_SG_STATUS_REG_3_IPS_M                         GENMASK(23, 20)
 121  #define ANA_SG_STATUS_REG_3_IPS_X(x)                      (((x) & GENMASK(23, 20)) >> 20)
 122 +#define ANA_SG_STATUS_REG_3_IPV_VALID                     BIT(23)
 123 +#define ANA_SG_STATUS_REG_3_IPV(x)                        (((x) << 20) & GENMASK(22, 20))
 124 +#define ANA_SG_STATUS_REG_3_IPV_M                         GENMASK(22, 20)
 125 +#define ANA_SG_STATUS_REG_3_IPV_X(x)                      (((x) & GENMASK(22, 20)) >> 20)
 126  #define ANA_SG_STATUS_REG_3_CONFIG_PENDING                BIT(24)
 127
 128  #define ANA_PORT_VLAN_CFG_GSZ                             0x100
 129 --- /dev/null
 130 +++ b/drivers/net/ethernet/mscc/ocelot_dev_gmii.h
 131 @@ -0,0 +1,153 @@
 132 +/* SPDX-License-Identifier: (GPL-2.0 OR MIT) */
 133 +/* Microsemi Ocelot Switch driver
 134 + *
 135 + * Copyright (c) 2017 Microsemi Corporation
 136 + */
 137 +
 138 +#ifndef _MSCC_OCELOT_DEV_GMII_H_
 139 +#define _MSCC_OCELOT_DEV_GMII_H_
 140 +
 141 +#define DEV_GMII_PORT_MODE_CLOCK_CFG                      0x0
 142 +
 143 +#define DEV_GMII_PORT_MODE_CLOCK_CFG_MAC_TX_RST           BIT(5)
 144 +#define DEV_GMII_PORT_MODE_CLOCK_CFG_MAC_RX_RST           BIT(4)
 145 +#define DEV_GMII_PORT_MODE_CLOCK_CFG_PORT_RST             BIT(3)
 146 +#define DEV_GMII_PORT_MODE_CLOCK_CFG_PHY_RST              BIT(2)
 147 +#define DEV_GMII_PORT_MODE_CLOCK_CFG_LINK_SPEED(x)        ((x) & GENMASK(1, 0))
 148 +#define DEV_GMII_PORT_MODE_CLOCK_CFG_LINK_SPEED_M         GENMASK(1, 0)
 149 +
 150 +#define DEV_GMII_PORT_MODE_PORT_MISC                      0x4
 151 +
 152 +#define DEV_GMII_PORT_MODE_PORT_MISC_MPLS_RX_ENA          BIT(5)
 153 +#define DEV_GMII_PORT_MODE_PORT_MISC_FWD_ERROR_ENA        BIT(4)
 154 +#define DEV_GMII_PORT_MODE_PORT_MISC_FWD_PAUSE_ENA        BIT(3)
 155 +#define DEV_GMII_PORT_MODE_PORT_MISC_FWD_CTRL_ENA         BIT(2)
 156 +#define DEV_GMII_PORT_MODE_PORT_MISC_GMII_LOOP_ENA        BIT(1)
 157 +#define DEV_GMII_PORT_MODE_PORT_MISC_DEV_LOOP_ENA         BIT(0)
 158 +
 159 +#define DEV_GMII_PORT_MODE_EVENTS                         0x8
 160 +
 161 +#define DEV_GMII_PORT_MODE_EEE_CFG                        0xc
 162 +
 163 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_ENA                BIT(22)
 164 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_AGE(x)       (((x) << 15) & GENMASK(21, 15))
 165 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_AGE_M        GENMASK(21, 15)
 166 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_AGE_X(x)     (((x) & GENMASK(21, 15)) >> 15)
 167 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_WAKEUP(x)    (((x) << 8) & GENMASK(14, 8))
 168 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_WAKEUP_M     GENMASK(14, 8)
 169 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_WAKEUP_X(x)  (((x) & GENMASK(14, 8)) >> 8)
 170 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_HOLDOFF(x)   (((x) << 1) & GENMASK(7, 1))
 171 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_HOLDOFF_M    GENMASK(7, 1)
 172 +#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_HOLDOFF_X(x) (((x) & GENMASK(7, 1)) >> 1)
 173 +#define DEV_GMII_PORT_MODE_EEE_CFG_PORT_LPI               BIT(0)
 174 +
 175 +#define DEV_GMII_PORT_MODE_RX_PATH_DELAY                  0x10
 176 +
 177 +#define DEV_GMII_PORT_MODE_TX_PATH_DELAY                  0x14
 178 +
 179 +#define DEV_GMII_PORT_MODE_PTP_PREDICT_CFG                0x18
 180 +
 181 +#define DEV_GMII_MAC_CFG_STATUS_MAC_ENA_CFG               0x1c
 182 +
 183 +#define DEV_GMII_MAC_CFG_STATUS_MAC_ENA_CFG_RX_ENA        BIT(4)
 184 +#define DEV_GMII_MAC_CFG_STATUS_MAC_ENA_CFG_TX_ENA        BIT(0)
 185 +
 186 +#define DEV_GMII_MAC_CFG_STATUS_MAC_MODE_CFG              0x20
 187 +
 188 +#define DEV_GMII_MAC_CFG_STATUS_MAC_MODE_CFG_FC_WORD_SYNC_ENA BIT(8)
 189 +#define DEV_GMII_MAC_CFG_STATUS_MAC_MODE_CFG_GIGA_MODE_ENA BIT(4)
 190 +#define DEV_GMII_MAC_CFG_STATUS_MAC_MODE_CFG_FDX_ENA      BIT(0)
 191 +
 192 +#define DEV_GMII_MAC_CFG_STATUS_MAC_MAXLEN_CFG            0x24
 193 +
 194 +#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG              0x28
 195 +
 196 +#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_TAG_ID(x)    (((x) << 16) & GENMASK(31, 16))
 197 +#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_TAG_ID_M     GENMASK(31, 16)
 198 +#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_TAG_ID_X(x)  (((x) & GENMASK(31, 16)) >> 16)
 199 +#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_PB_ENA       BIT(1)
 200 +#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_VLAN_AWR_ENA BIT(0)
 201 +#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA BIT(2)
 202 +
 203 +#define DEV_GMII_MAC_CFG_STATUS_MAC_ADV_CHK_CFG           0x2c
 204 +
 205 +#define DEV_GMII_MAC_CFG_STATUS_MAC_ADV_CHK_CFG_LEN_DROP_ENA BIT(0)
 206 +
 207 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG               0x30
 208 +
 209 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RESTORE_OLD_IPG_CHECK BIT(17)
 210 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_REDUCED_TX_IFG BIT(16)
 211 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_TX_IFG(x)     (((x) << 8) & GENMASK(12, 8))
 212 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_TX_IFG_M      GENMASK(12, 8)
 213 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_TX_IFG_X(x)   (((x) & GENMASK(12, 8)) >> 8)
 214 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG2(x)    (((x) << 4) & GENMASK(7, 4))
 215 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG2_M     GENMASK(7, 4)
 216 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG2_X(x)  (((x) & GENMASK(7, 4)) >> 4)
 217 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG1(x)    ((x) & GENMASK(3, 0))
 218 +#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG1_M     GENMASK(3, 0)
 219 +
 220 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG               0x34
 221 +
 222 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_BYPASS_COL_SYNC BIT(26)
 223 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_OB_ENA        BIT(25)
 224 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_WEXC_DIS      BIT(24)
 225 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_SEED(x)       (((x) << 16) & GENMASK(23, 16))
 226 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_SEED_M        GENMASK(23, 16)
 227 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_SEED_X(x)     (((x) & GENMASK(23, 16)) >> 16)
 228 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_SEED_LOAD     BIT(12)
 229 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_RETRY_AFTER_EXC_COL_ENA BIT(8)
 230 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_LATE_COL_POS(x) ((x) & GENMASK(6, 0))
 231 +#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_LATE_COL_POS_M GENMASK(6, 0)
 232 +
 233 +#define DEV_GMII_MAC_CFG_STATUS_MAC_DBG_CFG               0x38
 234 +
 235 +#define DEV_GMII_MAC_CFG_STATUS_MAC_DBG_CFG_TBI_MODE      BIT(4)
 236 +#define DEV_GMII_MAC_CFG_STATUS_MAC_DBG_CFG_IFG_CRS_EXT_CHK_ENA BIT(0)
 237 +
 238 +#define DEV_GMII_MAC_CFG_STATUS_MAC_FC_MAC_LOW_CFG        0x3c
 239 +
 240 +#define DEV_GMII_MAC_CFG_STATUS_MAC_FC_MAC_HIGH_CFG       0x40
 241 +
 242 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY                0x44
 243 +
 244 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_IPG_SHRINK_STICKY BIT(9)
 245 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_PREAM_SHRINK_STICKY BIT(8)
 246 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_CARRIER_EXT_STICKY BIT(7)
 247 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_CARRIER_EXT_ERR_STICKY BIT(6)
 248 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_JUNK_STICKY BIT(5)
 249 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_RETRANSMIT_STICKY BIT(4)
 250 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_JAM_STICKY  BIT(3)
 251 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_FIFO_OFLW_STICKY BIT(2)
 252 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_FRM_LEN_OVR_STICKY BIT(1)
 253 +#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_ABORT_STICKY BIT(0)
 254 +
 255 +#define DEV_GMII_MM_CONFIG_ENABLE_CONFIG                  0x48
 256 +
 257 +#define DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA        BIT(0)
 258 +#define DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA        BIT(4)
 259 +#define DEV_GMII_MM_CONFIG_ENABLE_CONFIG_KEEP_S_AFTER_D   BIT(8)
 260 +
 261 +#define DEV_GMII_MM_CONFIG_VERIF_CONFIG                   0x4c
 262 +
 263 +#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_DIS    BIT(0)
 264 +#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME(x) (((x) << 4) & GENMASK(11, 4))
 265 +#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME_M GENMASK(11, 4)
 266 +#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME_X(x) (((x) & GENMASK(11, 4)) >> 4)
 267 +#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS(x) (((x) << 12) & GENMASK(13, 12))
 268 +#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS_M GENMASK(13, 12)
 269 +#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS_X(x) (((x) & GENMASK(13, 12)) >> 12)
 270 +
 271 +#define DEV_GMII_MM_STATISTICS_MM_STATUS                  0x50
 272 +
 273 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STATUS BIT(0)
 274 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STICKY BIT(4)
 275 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE(x) (((x) << 8) & GENMASK(10, 8))
 276 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE_M GENMASK(10, 8)
 277 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE_X(x) (((x) & GENMASK(10, 8)) >> 8)
 278 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_UNEXP_RX_PFRM_STICKY BIT(12)
 279 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_UNEXP_TX_PFRM_STICKY BIT(16)
 280 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_MM_RX_FRAME_STATUS BIT(20)
 281 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_MM_TX_FRAME_STATUS BIT(24)
 282 +#define DEV_GMII_MM_STATISTICS_MM_STATUS_MM_TX_PRMPT_STATUS BIT(28)
 283 +
 284 +#endif
 285 --- /dev/null
 286 +++ b/drivers/net/ethernet/mscc/ocelot_tsn.c
 287 @@ -0,0 +1,1572 @@
 288 +// SPDX-License-Identifier: (GPL-2.0 OR MIT)
 289 +/* Felix Switch TSN driver
 290 + *
 291 + * Copyright (c) 2018 Microsemi Corporation
 292 + * Copyright 2018-2019 NXP
 293 + */
 294 +
 295 +#include <linux/io.h>
 296 +#include <linux/kernel.h>
 297 +#include <linux/pci.h>
 298 +#include <linux/iopoll.h>
 299 +#include "ocelot.h"
 300 +#include <soc/mscc/ocelot_sys.h>
 301 +#include "ocelot_ana.h"
 302 +#include "ocelot_qsys.h"
 303 +#include "ocelot_rew.h"
 304 +#include "ocelot_dev_gmii.h"
 305 +#include "ocelot_tsn.h"
 306 +
 307 +#define MSCC_NUM_OUT_PORT 4 /* Number of physical output ports */
 308 +#define SE_IX_PORT 64
 309 +
 310 +/* MSCC TSN parameters limited */
 311 +#define NUM_MSCC_QOS_PRIO 8
 312 +#define MSCC_PSFP_SFID_NUM 176
 313 +#define MSCC_FRER_SSID_NUM 128
 314 +
 315 +/* Using the max number of MSCC_PSFP_SFID_NUM and MSCC_FRER_SSID_NUM */
 316 +#define MSCC_STREAM_HANDLE_NUM MSCC_PSFP_SFID_NUM
 317 +
 318 +int streamhandle_map[MSCC_STREAM_HANDLE_NUM] = {0};
 319 +static struct mscc_switch_capa capa __ro_after_init = {
 320 +       .num_tas_gcl    = 64,
 321 +       .tas_ct_min     = 100,
 322 +       .tas_ct_max     = 1000000000,
 323 +       .tas_cte_max    = 999999999,
 324 +       .tas_it_max     = 999999999,
 325 +       .tas_it_min     = 1000,
 326 +       .num_hsch       = 72,
 327 +       .num_psfp_sfid  = MSCC_PSFP_SFID_NUM,
 328 +       .num_psfp_sgid  = 184,
 329 +       .psfp_fmi_max   = 246,
 330 +       .psfp_fmi_min   = 63,
 331 +       .num_sgi_gcl    = 4,
 332 +       .sgi_ct_min     = 5000,
 333 +       .sgi_ct_max     = 1000000000,
 334 +       .sgi_cte_max    = 999999999,
 335 +       .qos_pol_max    = 383,
 336 +       /* Maximum allowed value of committed burst size(CBS) is 240 KB */
 337 +       .pol_cbs_max    = 60,
 338 +       /* Maximum allowed value of excess burst size(EBS) is 240 KB */
 339 +       .pol_pbs_max    = 60,
 340 +       .num_frer_ssid  = MSCC_FRER_SSID_NUM,
 341 +       .frer_seq_len_min = 1,
 342 +       .frer_seq_len_max = 28,
 343 +       .frer_his_len_min = 1,
 344 +       .frer_his_len_max = 32,
 345 +       .qos_dscp_max   = 63,
 346 +       .qos_cos_max    = NUM_MSCC_QOS_PRIO - 1,
 347 +       .qos_dp_max     = 1,
 348 +};
 349 +
 350 +static int qos_port_tas_gcl_set(struct ocelot *ocelot, const u8 gcl_ix,
 351 +                               struct tsn_qbv_entry *control_list)
 352 +{
 353 +       if (gcl_ix >= capa.num_tas_gcl) {
 354 +               dev_err(ocelot->dev, "Invalid gcl ix %u\n", gcl_ix);
 355 +               return -EINVAL;
 356 +       }
 357 +       if (control_list->time_interval < capa.tas_it_min ||
 358 +           control_list->time_interval > capa.tas_it_max) {
 359 +               dev_err(ocelot->dev, "Invalid time_interval %u\n",
 360 +                       control_list->time_interval);
 361 +
 362 +               return -EINVAL;
 363 +       }
 364 +
 365 +       ocelot_write(ocelot,
 366 +                    QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM(gcl_ix) |
 367 +                    QSYS_GCL_CFG_REG_1_GATE_STATE(control_list->gate_state),
 368 +                    QSYS_GCL_CFG_REG_1);
 369 +
 370 +       ocelot_write(ocelot,
 371 +                    control_list->time_interval,
 372 +                    QSYS_GCL_CFG_REG_2);
 373 +
 374 +       return 0;
 375 +}
 376 +
 377 +static u32 tas_read_status(struct ocelot *ocelot)
 378 +{
 379 +       u32 val;
 380 +
 381 +       val = ocelot_read(ocelot, QSYS_TAS_PARAM_CFG_CTRL);
 382 +
 383 +       return val;
 384 +}
 385 +
 386 +int ocelot_qbv_set(struct ocelot *ocelot, int port_id,
 387 +                  struct tsn_qbv_conf *shaper_config)
 388 +{
 389 +       struct tsn_qbv_basic *admin_basic = &shaper_config->admin;
 390 +       struct tsn_qbv_entry *control_list = admin_basic->control_list;
 391 +       u32 base_time_nsec = admin_basic->base_time % 1000000000;
 392 +       u64 base_time_sec = admin_basic->base_time / 1000000000;
 393 +       u64 cur_time;
 394 +       u32 val;
 395 +       u8 speed;
 396 +       int i;
 397 +       int ret;
 398 +
 399 +       if (admin_basic->control_list_length > capa.num_tas_gcl) {
 400 +               dev_err(ocelot->dev,
 401 +                       "Invalid admin_control_list_length %u\n",
 402 +                       admin_basic->control_list_length);
 403 +               return -EINVAL;
 404 +       }
 405 +
 406 +       if ((admin_basic->cycle_time < capa.tas_ct_min ||
 407 +            admin_basic->cycle_time > capa.tas_ct_max) &&
 408 +           shaper_config->gate_enabled) {
 409 +               dev_err(ocelot->dev, "Invalid admin_cycle_time %u ns\n",
 410 +                       admin_basic->cycle_time);
 411 +               return -EINVAL;
 412 +       }
 413 +       if (admin_basic->cycle_time_extension > capa.tas_cte_max) {
 414 +               dev_err(ocelot->dev,
 415 +                       "Invalid admin_cycle_time_extension %u\n",
 416 +                       admin_basic->cycle_time_extension);
 417 +               return -EINVAL;
 418 +       }
 419 +
 420 +       cur_time = ocelot_read(ocelot, PTP_CUR_SEC_MSB);
 421 +       cur_time = cur_time << 32;
 422 +       cur_time += ocelot_read(ocelot, PTP_CUR_SEC_LSB);
 423 +
 424 +       if (base_time_sec < cur_time) {
 425 +               base_time_sec = cur_time;
 426 +               base_time_nsec = ocelot_read(ocelot, PTP_CUR_NSEC);
 427 +       }
 428 +
 429 +       /* Select port */
 430 +       ocelot_rmw(ocelot,
 431 +                  QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port_id),
 432 +                  QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
 433 +                  QSYS_TAS_PARAM_CFG_CTRL);
 434 +
 435 +       val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
 436 +       if (val & QSYS_PARAM_STATUS_REG_8_CONFIG_PENDING) {
 437 +               ocelot_rmw_rix(ocelot, 0, QSYS_TAG_CONFIG_ENABLE,
 438 +                              QSYS_TAG_CONFIG, port_id);
 439 +       }
 440 +
 441 +       if (!shaper_config->gate_enabled)
 442 +               admin_basic->gate_states = 0xff;
 443 +
 444 +       val = ocelot_read_gix(ocelot, ANA_PFC_PFC_CFG, port_id);
 445 +       speed = ANA_PFC_PFC_CFG_FC_LINK_SPEED(val);
 446 +
 447 +       ocelot_rmw_rix(ocelot,
 448 +                      (shaper_config->gate_enabled ?
 449 +                       QSYS_TAG_CONFIG_ENABLE : 0) |
 450 +                      QSYS_TAG_CONFIG_INIT_GATE_STATE(admin_basic->gate_states) |
 451 +                      QSYS_TAG_CONFIG_SCH_TRAFFIC_QUEUES(0xff) |
 452 +                      QSYS_TAG_CONFIG_LINK_SPEED(speed),
 453 +                      QSYS_TAG_CONFIG_ENABLE |
 454 +                      QSYS_TAG_CONFIG_INIT_GATE_STATE_M |
 455 +                      QSYS_TAG_CONFIG_SCH_TRAFFIC_QUEUES_M |
 456 +                      QSYS_TAG_CONFIG_LINK_SPEED_M,
 457 +                      QSYS_TAG_CONFIG,
 458 +                      port_id);
 459 +
 460 +       ocelot_write_rix(ocelot, shaper_config->maxsdu,
 461 +                        QSYS_PORT_MAX_SDU, port_id);
 462 +       /* TODO: add queue max SDU set */
 463 +
 464 +       if (shaper_config->gate_enabled) {
 465 +               ocelot_write(ocelot, base_time_nsec,
 466 +                            QSYS_PARAM_CFG_REG_1);
 467 +
 468 +               ocelot_write(ocelot, base_time_sec & GENMASK(31, 0),
 469 +                            QSYS_PARAM_CFG_REG_2);
 470 +
 471 +               ocelot_write(ocelot,
 472 +                            QSYS_PARAM_CFG_REG_3_BASE_TIME_SEC_MSB(base_time_sec >> 32) |
 473 +                            QSYS_PARAM_CFG_REG_3_LIST_LENGTH(admin_basic->control_list_length),
 474 +                            QSYS_PARAM_CFG_REG_3);
 475 +
 476 +               ocelot_write(ocelot, admin_basic->cycle_time,
 477 +                            QSYS_PARAM_CFG_REG_4);
 478 +
 479 +               ocelot_write(ocelot, admin_basic->cycle_time_extension,
 480 +                            QSYS_PARAM_CFG_REG_5);
 481 +
 482 +               for (i = 0; i < admin_basic->control_list_length; i++) {
 483 +                       qos_port_tas_gcl_set(ocelot, i, control_list);
 484 +                       control_list++;
 485 +               }
 486 +
 487 +               /* Start configuration change */
 488 +               ocelot_rmw(ocelot,
 489 +                          QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE,
 490 +                          QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE,
 491 +                          QSYS_TAS_PARAM_CFG_CTRL);
 492 +
 493 +               ret = readx_poll_timeout(tas_read_status, ocelot, val,
 494 +                                        !(QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE
 495 +                                        & val), 10, 100000);
 496 +               return ret;
 497 +       }
 498 +
 499 +       return 0;
 500 +}
 501 +
 502 +int ocelot_qbv_get(struct ocelot *ocelot, int port_id,
 503 +                  struct tsn_qbv_conf *shaper_config)
 504 +{
 505 +       u32 val, reg;
 506 +       int i;
 507 +       u32 base_timel;
 508 +       u32 base_timeh;
 509 +       struct tsn_qbv_basic *admin = &shaper_config->admin;
 510 +       struct tsn_qbv_entry *list;
 511 +
 512 +       ocelot_rmw(ocelot,
 513 +                  QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port_id),
 514 +                  QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
 515 +                  QSYS_TAS_PARAM_CFG_CTRL);
 516 +
 517 +       val = ocelot_read_rix(ocelot, QSYS_TAG_CONFIG, port_id);
 518 +       shaper_config->gate_enabled = (val & QSYS_TAG_CONFIG_ENABLE);
 519 +       admin->gate_states = QSYS_TAG_CONFIG_INIT_GATE_STATE_X(val);
 520 +
 521 +       base_timel = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_1);
 522 +       base_timeh = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_2);
 523 +       reg = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_3);
 524 +       admin->base_time = base_timeh |
 525 +               (((u64)QSYS_PARAM_CFG_REG_3_BASE_TIME_SEC_MSB(reg)) << 32);
 526 +
 527 +       admin->base_time = (admin->base_time * 1000000000) + base_timel;
 528 +
 529 +       admin->control_list_length =
 530 +               QSYS_PARAM_CFG_REG_3_LIST_LENGTH_X(reg);
 531 +
 532 +       admin->cycle_time = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_4);
 533 +       admin->cycle_time_extension =
 534 +               ocelot_read(ocelot, QSYS_PARAM_CFG_REG_5);
 535 +
 536 +       list = kmalloc_array(admin->control_list_length,
 537 +                            sizeof(struct tsn_qbv_entry), GFP_KERNEL);
 538 +       admin->control_list = list;
 539 +
 540 +       for (i = 0; i < admin->control_list_length; i++) {
 541 +               ocelot_rmw(ocelot,
 542 +                          QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM(i),
 543 +                          QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM_M,
 544 +                          QSYS_GCL_CFG_REG_1);
 545 +
 546 +               list->time_interval =
 547 +                       ocelot_read(ocelot, QSYS_GCL_CFG_REG_2);
 548 +
 549 +               reg = ocelot_read(ocelot, QSYS_GCL_CFG_REG_1);
 550 +               list->gate_state = QSYS_GCL_CFG_REG_1_GATE_STATE_X(reg);
 551 +
 552 +               list++;
 553 +       }
 554 +
 555 +       return 0;
 556 +}
 557 +
 558 +static int qbv_get_gatelist(struct ocelot *ocelot,
 559 +                           struct tsn_qbv_basic *oper)
 560 +{
 561 +       u32 base_timel;
 562 +       u32 base_timeh;
 563 +       u32 val;
 564 +       struct tsn_qbv_entry *glist;
 565 +       int i;
 566 +
 567 +       base_timel = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_1);
 568 +       base_timeh = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_2);
 569 +       val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_3);
 570 +       oper->base_time = base_timeh;
 571 +       oper->base_time +=
 572 +               ((u64)QSYS_PARAM_STATUS_REG_3_BASE_TIME_SEC_MSB(val)) <<
 573 +               32;
 574 +       oper->base_time = (oper->base_time * 1000000000) + base_timel;
 575 +
 576 +       oper->control_list_length =
 577 +               QSYS_PARAM_STATUS_REG_3_LIST_LENGTH_X(val);
 578 +
 579 +       oper->cycle_time = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_4);
 580 +       oper->cycle_time_extension = ocelot_read(ocelot,
 581 +                                                QSYS_PARAM_STATUS_REG_5);
 582 +
 583 +       val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
 584 +       oper->gate_states = QSYS_PARAM_STATUS_REG_8_OPER_GATE_STATE_X(val);
 585 +
 586 +       glist = kmalloc_array(oper->control_list_length,
 587 +                             sizeof(struct tsn_qbv_entry), GFP_KERNEL);
 588 +
 589 +       oper->control_list = glist;
 590 +
 591 +       for (i = 0; i < oper->control_list_length; i++) {
 592 +               ocelot_rmw(ocelot,
 593 +                          QSYS_GCL_STATUS_REG_1_GCL_ENTRY_NUM(i),
 594 +                          QSYS_GCL_STATUS_REG_1_GCL_ENTRY_NUM_M,
 595 +                          QSYS_GCL_STATUS_REG_1);
 596 +
 597 +               val = ocelot_read(ocelot, QSYS_GCL_STATUS_REG_2);
 598 +               glist->time_interval = val;
 599 +               val = ocelot_read(ocelot, QSYS_GCL_STATUS_REG_1);
 600 +               glist->gate_state =
 601 +                       QSYS_GCL_STATUS_REG_1_GATE_STATE_X(val);
 602 +
 603 +               glist++;
 604 +       }
 605 +
 606 +       return 0;
 607 +}
 608 +
 609 +int ocelot_qbv_get_status(struct ocelot *ocelot, int port_id,
 610 +                         struct tsn_qbv_status *qbvstatus)
 611 +{
 612 +       struct tsn_qbv_basic *oper = &qbvstatus->oper;
 613 +       u32 val;
 614 +       ptptime_t cur_time;
 615 +
 616 +       ocelot_rmw(ocelot,
 617 +                  QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port_id),
 618 +                  QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
 619 +                  QSYS_TAS_PARAM_CFG_CTRL);
 620 +
 621 +       qbvstatus->supported_list_max = capa.num_tas_gcl;
 622 +
 623 +       val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
 624 +       qbvstatus->config_pending =
 625 +               (val & QSYS_PARAM_STATUS_REG_8_CONFIG_PENDING) ? 1 : 0;
 626 +
 627 +       qbvstatus->config_change_time =
 628 +               ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_7);
 629 +
 630 +       qbvstatus->config_change_time +=
 631 +               ((u64)QSYS_PARAM_STATUS_REG_8_CFG_CHG_TIME_SEC_MSB(val)) <<
 632 +               32;
 633 +
 634 +       qbvstatus->config_change_time =
 635 +               (qbvstatus->config_change_time * 1000000000) +
 636 +               ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_6);
 637 +
 638 +       qbvstatus->config_change_error =
 639 +               ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_9);
 640 +
 641 +       cur_time = ocelot_read(ocelot, PTP_CUR_SEC_MSB);
 642 +       cur_time = cur_time << 32;
 643 +       cur_time += ocelot_read(ocelot, PTP_CUR_SEC_LSB);
 644 +       cur_time = (cur_time * 1000000000) +
 645 +                  ocelot_read(ocelot, PTP_CUR_NSEC);
 646 +
 647 +       qbvstatus->current_time = cur_time;
 648 +       qbv_get_gatelist(ocelot, oper);
 649 +
 650 +       return 0;
 651 +}
 652 +
 653 +int ocelot_cut_thru_set(struct ocelot *ocelot, int port_id, u8 cut_thru)
 654 +{
 655 +       ocelot_write_rix(ocelot, cut_thru, ANA_CUT_THRU_CFG, port_id);
 656 +
 657 +       return 0;
 658 +}
 659 +
 660 +static int qos_shaper_conf_set(struct ocelot *ocelot, int port,
 661 +                              u32 port_ix, u8 percent)
 662 +{
 663 +       u32 val;
 664 +       int speed;
 665 +       u32 cbs = 0;
 666 +       u32 cir = 0;
 667 +
 668 +       if (percent > 100) {
 669 +               dev_err(ocelot->dev, "percentage %d larger than 100\n",
 670 +                       percent);
 671 +               return -EINVAL;
 672 +       }
 673 +       if (port_ix >= capa.num_hsch) {
 674 +               dev_err(ocelot->dev,
 675 +                       "CIR_CFG: id %d is exceed num of HSCH instance\n",
 676 +                       port_ix);
 677 +               return -EINVAL;
 678 +       }
 679 +
 680 +       val = ocelot_read_gix(ocelot, ANA_PFC_PFC_CFG, port);
 681 +       speed = ANA_PFC_PFC_CFG_FC_LINK_SPEED(val);
 682 +       switch (speed) {
 683 +       case OCELOT_SPEED_10:
 684 +               cir = 10000;
 685 +               break;
 686 +       case OCELOT_SPEED_100:
 687 +               cir = 100000;
 688 +               break;
 689 +       case OCELOT_SPEED_1000:
 690 +               cir = 1000000;
 691 +               break;
 692 +       case OCELOT_SPEED_2500:
 693 +               cir = 2500000;
 694 +               break;
 695 +       }
 696 +
 697 +       cir = cir * percent / 100;
 698 +       cir = DIV_ROUND_UP(cir, 100);  /* Rate unit is 100 kbps */
 699 +       cir = (cir ? cir : 1);              /* Avoid using zero rate */
 700 +       cbs = DIV_ROUND_UP(cbs, 4096); /* Burst unit is 4kB */
 701 +       cbs = (cbs ? cbs : 1);          /* Avoid using zero burst size */
 702 +       cir = min_t(u32, GENMASK(15, 0), cir);
 703 +       cbs = min_t(u32, GENMASK(6, 0), cbs);
 704 +       ocelot_write_gix(ocelot,
 705 +                        QSYS_CIR_CFG_CIR_RATE(cir) |
 706 +                        QSYS_CIR_CFG_CIR_BURST(cbs),
 707 +                        QSYS_CIR_CFG,
 708 +                        port_ix);
 709 +
 710 +       return 0;
 711 +}
 712 +
 713 +static int qos_shaper_conf_get(struct ocelot *ocelot, int port,
 714 +                              u32 port_ix)
 715 +{
 716 +       u32 val;
 717 +       u32 bandwidth = 0;
 718 +       u32 cir = 0;
 719 +       int percentage;
 720 +       int speed;
 721 +
 722 +       if (port_ix >= capa.num_hsch) {
 723 +               dev_err(ocelot->dev,
 724 +                       "CIR_CFG: id %d is exceed num of HSCH instance\n",
 725 +                       port_ix);
 726 +               return -EINVAL;
 727 +       }
 728 +
 729 +       val = ocelot_read_gix(ocelot, ANA_PFC_PFC_CFG, port);
 730 +       speed = ANA_PFC_PFC_CFG_FC_LINK_SPEED(val);
 731 +       switch (speed) {
 732 +       case OCELOT_SPEED_10:
 733 +               bandwidth = 10000;
 734 +               break;
 735 +       case OCELOT_SPEED_100:
 736 +               bandwidth = 100000;
 737 +               break;
 738 +       case OCELOT_SPEED_1000:
 739 +               bandwidth = 1000000;
 740 +               break;
 741 +       case OCELOT_SPEED_2500:
 742 +               bandwidth = 2500000;
 743 +               break;
 744 +       }
 745 +
 746 +       val = ocelot_read_gix(ocelot, QSYS_CIR_CFG, port_ix);
 747 +
 748 +       cir = QSYS_CIR_CFG_CIR_RATE_X(val);
 749 +       cir *= 100;
 750 +       percentage = cir * 100 / bandwidth;
 751 +
 752 +       return percentage;
 753 +}
 754 +
 755 +int ocelot_cbs_set(struct ocelot *ocelot, int port, u8 tc, u8 bw)
 756 +{
 757 +       if (tc > capa.qos_cos_max) {
 758 +               dev_err(ocelot->dev, "Invalid tc: %u\n", tc);
 759 +               return -EINVAL;
 760 +       }
 761 +
 762 +       qos_shaper_conf_set(ocelot, port, port * 8 + tc, bw);
 763 +
 764 +       ocelot_rmw_gix(ocelot,
 765 +                      QSYS_SE_CFG_SE_AVB_ENA,
 766 +                      QSYS_SE_CFG_SE_AVB_ENA,
 767 +                      QSYS_SE_CFG,
 768 +                      port * 8 + tc);
 769 +
 770 +       return 0;
 771 +}
 772 +
 773 +int ocelot_cbs_get(struct ocelot *ocelot, int port, u8 tc)
 774 +{
 775 +       int ret;
 776 +
 777 +       if (tc > capa.qos_cos_max) {
 778 +               dev_err(ocelot->dev, "Invalid tc: %u\n", tc);
 779 +               return -EINVAL;
 780 +       }
 781 +
 782 +       ret = qos_shaper_conf_get(ocelot, port, port * 8 + tc);
 783 +
 784 +       return ret;
 785 +}
 786 +
 787 +int ocelot_qbu_set(struct ocelot *ocelot, int port, u8 preemptible)
 788 +{
 789 +       struct ocelot_port *ocelot_port = ocelot->ports[port];
 790 +
 791 +       ocelot_port_rmwl(ocelot_port,
 792 +                        DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA |
 793 +                        DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA,
 794 +                        DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA |
 795 +                        DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA,
 796 +                        DEV_GMII_MM_CONFIG_ENABLE_CONFIG);
 797 +
 798 +       ocelot_rmw_rix(ocelot,
 799 +                      QSYS_PREEMPTION_CFG_P_QUEUES(preemptible),
 800 +                      QSYS_PREEMPTION_CFG_P_QUEUES_M,
 801 +                      QSYS_PREEMPTION_CFG,
 802 +                      port);
 803 +
 804 +       return 0;
 805 +}
 806 +
 807 +int ocelot_qbu_get(struct ocelot *ocelot, int port,
 808 +                  struct tsn_preempt_status *c)
 809 +{
 810 +       struct ocelot_port *ocelot_port = ocelot->ports[port];
 811 +       u32 val;
 812 +
 813 +       val = ocelot_read_rix(ocelot,
 814 +                             QSYS_PREEMPTION_CFG,
 815 +                             port);
 816 +
 817 +       c->admin_state = QSYS_PREEMPTION_CFG_P_QUEUES(val);
 818 +       c->hold_advance = QSYS_PREEMPTION_CFG_HOLD_ADVANCE_X(val);
 819 +
 820 +       val = ocelot_port_readl(ocelot_port,
 821 +                               DEV_GMII_MM_STATISTICS_MM_STATUS);
 822 +       c->preemption_active =
 823 +               DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STATUS & val;
 824 +
 825 +       return 0;
 826 +}
 827 +
 828 +int ocelot_cb_streamid_get(struct ocelot *ocelot, int port, u32 index,
 829 +                          struct tsn_cb_streamid *streamid)
 830 +{
 831 +       u32 m_index;
 832 +       u32 bucket;
 833 +       u32 val, dst, reg;
 834 +       u64 dmac;
 835 +       u32 ldmac, hdmac;
 836 +
 837 +       if (index >= MSCC_STREAM_HANDLE_NUM) {
 838 +               dev_err(ocelot->dev,
 839 +                       "Invalid stream handle %u, maximum:%u\n",
 840 +                       index, MSCC_STREAM_HANDLE_NUM - 1);
 841 +               return -EINVAL;
 842 +       }
 843 +
 844 +       index = streamhandle_map[index];
 845 +       m_index = index / 4;
 846 +       bucket =  index % 4;
 847 +       streamid->type = 1;
 848 +       regmap_field_write(ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET],
 849 +                          bucket);
 850 +       regmap_field_write(ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX],
 851 +                          m_index);
 852 +
 853 +       /*READ command MACACCESS.VALID(11 bit) must be 0 */
 854 +       ocelot_write(ocelot,
 855 +                    ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 856 +                    ANA_TABLES_MACACCESS);
 857 +
 858 +       val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
 859 +       dst = ANA_TABLES_MACACCESS_DEST_IDX_X(val);
 860 +       reg = ocelot_read_rix(ocelot, ANA_PGID_PGID, dst);
 861 +       streamid->ofac_oport = ANA_PGID_PGID_PGID(reg);
 862 +
 863 +       /*Get the entry's MAC address and VLAN id*/
 864 +       ldmac = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
 865 +       val = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
 866 +       val &= 0x1fffffff;
 867 +       hdmac = val & 0xffff;
 868 +       dmac = hdmac;
 869 +       dmac = (dmac << 32) | ldmac;
 870 +       streamid->para.nid.dmac = dmac;
 871 +
 872 +       streamid->para.nid.vid = ANA_TABLES_MACHDATA_VID_X(val);
 873 +
 874 +       val = ocelot_read(ocelot, ANA_TABLES_STREAMDATA);
 875 +       if (!(val & ANA_TABLES_STREAMDATA_SFID_VALID))
 876 +               return -EINVAL;
 877 +
 878 +       streamid->handle = ANA_TABLES_STREAMDATA_SFID(val);
 879 +
 880 +       return 0;
 881 +}
 882 +
 883 +static int lookup_mactable(struct ocelot *ocelot, u16 vid, u64 mac)
 884 +{
 885 +       u32 mach, macl;
 886 +       u32 reg1, reg2;
 887 +       u32 index, bucket;
 888 +
 889 +       macl = mac & 0xffffffff;
 890 +       mach = (mac >> 32) & 0xffff;
 891 +       ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
 892 +       ocelot_write(ocelot, ANA_TABLES_MACHDATA_VID(vid) |
 893 +                    ANA_TABLES_MACHDATA_MACHDATA(mach),
 894 +                    ANA_TABLES_MACHDATA);
 895 +
 896 +       ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
 897 +                    ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 898 +                    ANA_TABLES_MACACCESS);
 899 +
 900 +       reg1 = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
 901 +       reg2 = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
 902 +       if (reg1 == 0 && reg2 == 0)
 903 +               return -1;
 904 +
 905 +       regmap_field_read(ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET],
 906 +                         &bucket);
 907 +       regmap_field_read(ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX],
 908 +                         &index);
 909 +
 910 +       index = index * 4 + bucket;
 911 +
 912 +       return index;
 913 +}
 914 +
 915 +int ocelot_cb_streamid_set(struct ocelot *ocelot, int port,
 916 +                          u32 index, bool enable,
 917 +                          struct tsn_cb_streamid *streamid)
 918 +{
 919 +       struct regmap_field *rf;
 920 +       u16 vid;
 921 +       u64 mac;
 922 +       u32 macl, mach;
 923 +       u32 dst_idx;
 924 +       int idx;
 925 +       u32 reg;
 926 +       int sfid, ssid;
 927 +       u32 m_index, bucket;
 928 +
 929 +       if (!enable) {
 930 +               if (index >= MSCC_STREAM_HANDLE_NUM) {
 931 +                       dev_err(ocelot->dev,
 932 +                               "Invalid index %u, maximum:%u\n",
 933 +                               index, MSCC_STREAM_HANDLE_NUM - 1);
 934 +                       return -EINVAL;
 935 +               }
 936 +               m_index = streamhandle_map[index] / 4;
 937 +               bucket =  streamhandle_map[index] % 4;
 938 +               rf = ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET];
 939 +               regmap_field_write(rf, bucket);
 940 +               rf = ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX];
 941 +               regmap_field_write(rf, m_index);
 942 +
 943 +               /*READ command MACACCESS.VALID(11 bit) must be 0 */
 944 +               ocelot_write(ocelot,
 945 +                            ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
 946 +                            ANA_TABLES_MACACCESS);
 947 +
 948 +               ocelot_write(ocelot,
 949 +                            ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
 950 +                            ANA_TABLES_MACACCESS);
 951 +
 952 +               streamhandle_map[index] = 0;
 953 +
 954 +               return 0;
 955 +       }
 956 +
 957 +       if (streamid->type != 1) {
 958 +               dev_err(ocelot->dev, "Invalid stream type\n");
 959 +               return -EINVAL;
 960 +       }
 961 +
 962 +       if (streamid->handle >= MSCC_STREAM_HANDLE_NUM) {
 963 +               dev_err(ocelot->dev,
 964 +                       "Invalid stream handle %u, maximum:%u\n",
 965 +                       streamid->handle, MSCC_STREAM_HANDLE_NUM - 1);
 966 +               return -EINVAL;
 967 +       }
 968 +
 969 +       sfid = streamid->handle;
 970 +       ssid = (streamid->handle < MSCC_FRER_SSID_NUM ?
 971 +               streamid->handle : (MSCC_FRER_SSID_NUM - 1));
 972 +
 973 +       mac = streamid->para.nid.dmac;
 974 +       macl = mac & 0xffffffff;
 975 +       mach = (mac >> 32) & 0xffff;
 976 +       vid = streamid->para.nid.vid;
 977 +
 978 +       idx = lookup_mactable(ocelot, vid, mac);
 979 +
 980 +       if (idx < 0) {
 981 +               ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
 982 +               ocelot_write(ocelot, ANA_TABLES_MACHDATA_VID(vid) |
 983 +                            ANA_TABLES_MACHDATA_MACHDATA(mach),
 984 +                            ANA_TABLES_MACHDATA);
 985 +
 986 +               ocelot_write(ocelot,
 987 +                            ANA_TABLES_STREAMDATA_SFID_VALID |
 988 +                            ANA_TABLES_STREAMDATA_SFID(sfid) |
 989 +                            ANA_TABLES_STREAMDATA_SSID_VALID |
 990 +                            ANA_TABLES_STREAMDATA_SSID(ssid),
 991 +                            ANA_TABLES_STREAMDATA);
 992 +
 993 +               dst_idx = port;
 994 +               ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
 995 +                            ANA_TABLES_MACACCESS_ENTRYTYPE(1) |
 996 +                            ANA_TABLES_MACACCESS_DEST_IDX(dst_idx) |
 997 +                            ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
 998 +                            ANA_TABLES_MACACCESS);
 999 +
1000 +               ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
1001 +                            ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
1002 +                            ANA_TABLES_MACACCESS);
1003 +
1004 +               regmap_field_read(ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET],
1005 +                                 &bucket);
1006 +               regmap_field_read(ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX],
1007 +                                 &m_index);
1008 +
1009 +               m_index = m_index * 4 + bucket;
1010 +               streamhandle_map[streamid->handle] = m_index;
1011 +
1012 +               return 0;
1013 +       }
1014 +
1015 +       ocelot_write(ocelot,
1016 +                    ANA_TABLES_STREAMDATA_SFID_VALID |
1017 +                    ANA_TABLES_STREAMDATA_SFID(sfid) |
1018 +                    ANA_TABLES_STREAMDATA_SSID_VALID |
1019 +                    ANA_TABLES_STREAMDATA_SSID(ssid),
1020 +                    ANA_TABLES_STREAMDATA);
1021 +
1022 +       reg = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
1023 +       dst_idx = ANA_TABLES_MACACCESS_DEST_IDX_X(reg);
1024 +       ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
1025 +                    ANA_TABLES_MACACCESS_ENTRYTYPE(1) |
1026 +                    ANA_TABLES_MACACCESS_DEST_IDX(dst_idx) |
1027 +                    ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_WRITE),
1028 +                    ANA_TABLES_MACACCESS);
1029 +
1030 +       streamhandle_map[streamid->handle] = idx;
1031 +
1032 +       return 0;
1033 +}
1034 +
1035 +static int streamid_multi_forward_set(struct ocelot *ocelot, u32 index,
1036 +                                     u8 fwdmask)
1037 +{
1038 +       u32 m_index;
1039 +       u32 bucket;
1040 +       u32 val;
1041 +       int m, n, i;
1042 +       u8 pgid_val, fwdport;
1043 +       u32 dst_idx;
1044 +
1045 +       m_index = index / 4;
1046 +       bucket =  index % 4;
1047 +
1048 +       regmap_field_write(ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET],
1049 +                          bucket);
1050 +       regmap_field_write(ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX],
1051 +                          m_index);
1052 +
1053 +       /*READ command MACACCESS.VALID(11 bit) must be 0 */
1054 +       ocelot_write(ocelot,
1055 +                    ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
1056 +                    ANA_TABLES_MACACCESS);
1057 +
1058 +       val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
1059 +       fwdport = ANA_TABLES_MACACCESS_DEST_IDX_X(val);
1060 +
1061 +       if (fwdport >= MSCC_NUM_OUT_PORT) {
1062 +               dst_idx = fwdport;
1063 +               return 0;
1064 +       }
1065 +
1066 +       fwdmask |= (1 << fwdport);
1067 +
1068 +       m = ocelot->num_phys_ports - 1;
1069 +       for (i = m; i >= MSCC_NUM_OUT_PORT; i--) {
1070 +               if (fwdmask & (1 << i)) {
1071 +                       dst_idx = PGID_MCRED +
1072 +                                 (m - i) * MSCC_NUM_OUT_PORT +
1073 +                                 fwdport;
1074 +
1075 +                       pgid_val = (1 << i) | (1 << fwdport);
1076 +                       break;
1077 +               }
1078 +       }
1079 +
1080 +       if (i < MSCC_NUM_OUT_PORT) {
1081 +               m = PGID_MCRED +
1082 +                   (ocelot->num_phys_ports - MSCC_NUM_OUT_PORT) *
1083 +                   MSCC_NUM_OUT_PORT;
1084 +
1085 +               for (; i > 0; i--) {
1086 +                       if (fwdmask & (1 << i))
1087 +                               break;
1088 +
1089 +                       m = m + (1 << i) - 1;
1090 +               }
1091 +               n = fwdmask & ((1 << i) - 1);
1092 +               if (n) {
1093 +                       dst_idx = m + n;
1094 +                       pgid_val = fwdmask & ((1 << MSCC_NUM_OUT_PORT) - 1);
1095 +               } else {
1096 +                       dst_idx = fwdport;
1097 +               }
1098 +       }
1099 +
1100 +       if (dst_idx < PGID_MCRED)
1101 +               return 0;
1102 +
1103 +       ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
1104 +                    ANA_TABLES_MACACCESS_ENTRYTYPE(1) |
1105 +                    ANA_TABLES_MACACCESS_DEST_IDX(dst_idx) |
1106 +                    ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_WRITE),
1107 +                    ANA_TABLES_MACACCESS);
1108 +
1109 +       ocelot_write_rix(ocelot, pgid_val, ANA_PGID_PGID, dst_idx);
1110 +
1111 +       return 0;
1112 +}
1113 +
1114 +int ocelot_qci_sfi_get(struct ocelot *ocelot, int port, u32 index,
1115 +                      struct tsn_qci_psfp_sfi_conf *sfi)
1116 +{
1117 +       u32 val, reg, fmeter_id, max_sdu;
1118 +       u32 sfid = index;
1119 +
1120 +       if (sfid >= capa.num_psfp_sfid) {
1121 +               dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
1122 +                       sfid, capa.num_psfp_sfid);
1123 +               return -EINVAL;
1124 +       }
1125 +
1126 +       ocelot_rmw(ocelot,
1127 +                  ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
1128 +                  ANA_TABLES_SFIDTIDX_SFID_INDEX_M,
1129 +                  ANA_TABLES_SFIDTIDX);
1130 +
1131 +       ocelot_write(ocelot,
1132 +                    ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_READ),
1133 +                    ANA_TABLES_SFIDACCESS);
1134 +
1135 +       val = ocelot_read(ocelot, ANA_TABLES_SFIDTIDX);
1136 +       if (!(val & ANA_TABLES_SFIDTIDX_SGID_VALID))
1137 +               return -EINVAL;
1138 +
1139 +       sfi->stream_gate_instance_id = ANA_TABLES_SFIDTIDX_SGID_X(val);
1140 +       fmeter_id = ANA_TABLES_SFIDTIDX_POL_IDX_X(val);
1141 +       sfi->stream_filter.flow_meter_instance_id = fmeter_id;
1142 +
1143 +       reg = ocelot_read(ocelot, ANA_TABLES_SFIDACCESS);
1144 +       max_sdu = ANA_TABLES_SFIDACCESS_MAX_SDU_LEN_X(reg);
1145 +       sfi->stream_filter.maximum_sdu_size  = max_sdu;
1146 +
1147 +       if (reg & ANA_TABLES_SFIDACCESS_IGR_PRIO_MATCH_ENA)
1148 +               sfi->priority_spec = ANA_TABLES_SFIDACCESS_IGR_PRIO_X(reg);
1149 +       else
1150 +               dev_err(ocelot->dev, "priority not enable\n");
1151 +
1152 +       return 0;
1153 +}
1154 +
1155 +int ocelot_qci_sfi_set(struct ocelot *ocelot, int port,
1156 +                      u32 index, bool enable,
1157 +                      struct tsn_qci_psfp_sfi_conf *sfi)
1158 +{
1159 +       int igr_prio = sfi->priority_spec;
1160 +       u16 sgid  = sfi->stream_gate_instance_id;
1161 +       u16 pol_idx;
1162 +       int fmid = sfi->stream_filter.flow_meter_instance_id;
1163 +       u16 max_sdu_len = sfi->stream_filter.maximum_sdu_size;
1164 +       int sfid = index;
1165 +       u32 val;
1166 +
1167 +       if (fmid == -1)
1168 +               pol_idx = capa.psfp_fmi_max;
1169 +       else
1170 +               pol_idx = (u16)fmid;
1171 +
1172 +       if (sfid >= capa.num_psfp_sfid) {
1173 +               dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
1174 +                       sfid, capa.num_psfp_sfid);
1175 +               return -EINVAL;
1176 +       }
1177 +
1178 +       if (!enable) {
1179 +               val = ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE);
1180 +               ocelot_write(ocelot,
1181 +                            ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
1182 +                            ANA_TABLES_SFIDTIDX);
1183 +               ocelot_write(ocelot, val, ANA_TABLES_SFIDACCESS);
1184 +               return 0;
1185 +       }
1186 +
1187 +       if (sgid >= capa.num_psfp_sgid) {
1188 +               dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
1189 +                       sgid, capa.num_psfp_sgid);
1190 +               return -EINVAL;
1191 +       }
1192 +       if (pol_idx > capa.psfp_fmi_max || pol_idx < capa.psfp_fmi_min) {
1193 +               dev_err(ocelot->dev, "Invalid pol_idx %u, range:%d~%d\n",
1194 +                       pol_idx, capa.psfp_fmi_min, capa.psfp_fmi_max);
1195 +               return -EINVAL;
1196 +       }
1197 +
1198 +       ocelot_write(ocelot, ANA_TABLES_SFIDTIDX_SGID_VALID |
1199 +                    ANA_TABLES_SFIDTIDX_SGID(sgid) |
1200 +                    ((fmid != -1) ? ANA_TABLES_SFIDTIDX_POL_ENA : 0) |
1201 +                    ANA_TABLES_SFIDTIDX_POL_IDX(pol_idx) |
1202 +                    ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
1203 +                    ANA_TABLES_SFIDTIDX);
1204 +
1205 +       ocelot_write(ocelot,
1206 +                    ((igr_prio >= 0) ?
1207 +                     ANA_TABLES_SFIDACCESS_IGR_PRIO_MATCH_ENA : 0) |
1208 +                    ANA_TABLES_SFIDACCESS_IGR_PRIO(igr_prio) |
1209 +                    ANA_TABLES_SFIDACCESS_MAX_SDU_LEN(max_sdu_len) |
1210 +                    ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE),
1211 +                    ANA_TABLES_SFIDACCESS);
1212 +
1213 +       return 0;
1214 +}
1215 +
1216 +int ocelot_qci_sfi_counters_get(struct ocelot *ocelot, int port,
1217 +                               u32 index,
1218 +                               struct tsn_qci_psfp_sfi_counters *sfi_cnt)
1219 +{
1220 +       u32 sfid = index;
1221 +       u32 match, not_pass, not_pass_sdu, red;
1222 +
1223 +       if (sfid >= capa.num_psfp_sfid) {
1224 +               dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
1225 +                       sfid, capa.num_psfp_sfid);
1226 +               return -EINVAL;
1227 +       }
1228 +
1229 +       ocelot_rmw(ocelot,
1230 +                  SYS_STAT_CFG_STAT_VIEW(sfid),
1231 +                  SYS_STAT_CFG_STAT_VIEW_M,
1232 +                  SYS_STAT_CFG);
1233 +
1234 +       match = ocelot_read_gix(ocelot, SYS_CNT, 0x200);
1235 +       not_pass = ocelot_read_gix(ocelot, SYS_CNT, 0x201);
1236 +       not_pass_sdu = ocelot_read_gix(ocelot, SYS_CNT, 0x202);
1237 +       red = ocelot_read_gix(ocelot, SYS_CNT, 0x203);
1238 +
1239 +       sfi_cnt->matching_frames_count = match;
1240 +       sfi_cnt->not_passing_frames_count = not_pass;
1241 +       sfi_cnt->not_passing_sdu_count = not_pass_sdu;
1242 +       sfi_cnt->red_frames_count  =  red;
1243 +
1244 +       sfi_cnt->passing_frames_count = match - not_pass;
1245 +       sfi_cnt->passing_sdu_count = match - not_pass - not_pass_sdu;
1246 +
1247 +       return 0;
1248 +}
1249 +
1250 +int ocelot_qci_max_cap_get(struct ocelot *ocelot,
1251 +                          struct tsn_qci_psfp_stream_param *stream_para)
1252 +{
1253 +       /* MaxStreamFilterInstances */
1254 +       stream_para->max_sf_instance = capa.num_psfp_sfid;
1255 +       /* MaxStreamGateInstances */
1256 +       stream_para->max_sg_instance = capa.num_psfp_sgid;
1257 +       /* MaxFlowMeterInstances */
1258 +       stream_para->max_fm_instance = capa.psfp_fmi_max -
1259 +                                      capa.psfp_fmi_min + 1;
1260 +       /* SupportedListMax */
1261 +       stream_para->supported_list_max = capa.num_sgi_gcl;
1262 +
1263 +       return 0;
1264 +}
1265 +
1266 +static int sgi_set_glist(struct ocelot *ocelot,
1267 +                        struct tsn_qci_psfp_gcl *gcl, uint32_t num)
1268 +{
1269 +       u32 time_sum = 0;
1270 +       int i;
1271 +
1272 +       if (num > capa.num_sgi_gcl)
1273 +               return -EINVAL;
1274 +
1275 +       for (i = 0; i < num; i++) {
1276 +               u32 val = ANA_SG_GCL_GS_CONFIG_IPS((gcl->ipv < 0) ?
1277 +                                                  0 : gcl->ipv + 8);
1278 +               val |= (gcl->gate_state ? ANA_SG_GCL_GS_CONFIG_GATE_STATE : 0);
1279 +               ocelot_write_rix(ocelot, val, ANA_SG_GCL_GS_CONFIG, i);
1280 +
1281 +               time_sum += gcl->time_interval;
1282 +               ocelot_write_rix(ocelot, time_sum, ANA_SG_GCL_TI_CONFIG, i);
1283 +
1284 +               gcl++;
1285 +       }
1286 +
1287 +       return 0;
1288 +}
1289 +
1290 +static u32 sgi_read_status(struct ocelot *ocelot)
1291 +{
1292 +       u32 val;
1293 +
1294 +       val = ocelot_read(ocelot, ANA_SG_ACCESS_CTRL);
1295 +
1296 +       return val;
1297 +}
1298 +
1299 +int ocelot_qci_sgi_set(struct ocelot *ocelot, int port, u32 index,
1300 +                      struct tsn_qci_psfp_sgi_conf *sgi_conf)
1301 +{
1302 +       struct tsn_qci_sg_control *admin_list = &sgi_conf->admin;
1303 +       u32 sgid = index;
1304 +       u32 list_length = sgi_conf->admin.control_list_length;
1305 +       u32 cycle_time = sgi_conf->admin.cycle_time;
1306 +       u32 cycle_time_ex = sgi_conf->admin.cycle_time_extension;
1307 +       u32 l_basetime = sgi_conf->admin.base_time % 1000000000;
1308 +       u64 h_basetime = sgi_conf->admin.base_time / 1000000000;
1309 +       u64 cur_time;
1310 +       u32 val;
1311 +       int ret;
1312 +
1313 +       if (sgid >= capa.num_psfp_sgid) {
1314 +               dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
1315 +                       sgid, capa.num_psfp_sgid);
1316 +               return -EINVAL;
1317 +       }
1318 +       if ((cycle_time < capa.sgi_ct_min ||
1319 +            cycle_time > capa.sgi_ct_max) &&
1320 +            sgi_conf->gate_enabled) {
1321 +               dev_err(ocelot->dev, "Invalid cycle_time %u ns\n",
1322 +                       cycle_time);
1323 +               return -EINVAL;
1324 +       }
1325 +       if (cycle_time_ex > capa.sgi_cte_max) {
1326 +               dev_err(ocelot->dev,
1327 +                       "Invalid cycle_time_extension %u\n",
1328 +                       cycle_time_ex);
1329 +               return -EINVAL;
1330 +       }
1331 +       if (list_length > capa.num_sgi_gcl) {
1332 +               dev_err(ocelot->dev,
1333 +                       "Invalid sgi_gcl len %u, maximum:%u\n",
1334 +                       list_length, capa.num_sgi_gcl);
1335 +               return -EINVAL;
1336 +       }
1337 +
1338 +       /*configure SGID*/
1339 +       ocelot_rmw(ocelot,
1340 +                  ANA_SG_ACCESS_CTRL_SGID(sgid),
1341 +                  ANA_SG_ACCESS_CTRL_SGID_M,
1342 +                  ANA_SG_ACCESS_CTRL);
1343 +
1344 +       /*Disable SG*/
1345 +       if (!sgi_conf->gate_enabled) {
1346 +               ocelot_rmw(ocelot,
1347 +                          ANA_SG_CONFIG_REG_3_INIT_GATE_STATE,
1348 +                          ANA_SG_CONFIG_REG_3_INIT_GATE_STATE |
1349 +                          ANA_SG_CONFIG_REG_3_GATE_ENABLE,
1350 +                          ANA_SG_CONFIG_REG_3);
1351 +               return 0;
1352 +       }
1353 +
1354 +       /*admin parameters*/
1355 +       cur_time = ocelot_read(ocelot, PTP_CUR_SEC_MSB);
1356 +       cur_time = cur_time << 32;
1357 +       cur_time += ocelot_read(ocelot, PTP_CUR_SEC_LSB);
1358 +       if (h_basetime < cur_time) {
1359 +               h_basetime = cur_time;
1360 +               l_basetime = ocelot_read(ocelot, PTP_CUR_NSEC);
1361 +       }
1362 +
1363 +       ocelot_write(ocelot, l_basetime, ANA_SG_CONFIG_REG_1);
1364 +       ocelot_write(ocelot, h_basetime, ANA_SG_CONFIG_REG_2);
1365 +
1366 +       ocelot_write(ocelot,
1367 +                    (sgi_conf->admin.init_ipv < 0 ?
1368 +                     0 : ANA_SG_CONFIG_REG_3_IPV_VALID) |
1369 +                    ANA_SG_CONFIG_REG_3_INIT_IPV(sgi_conf->admin.init_ipv) |
1370 +                    ANA_SG_CONFIG_REG_3_GATE_ENABLE |
1371 +                    ANA_SG_CONFIG_REG_3_LIST_LENGTH(list_length) |
1372 +                    (sgi_conf->admin.gate_states > 0 ?
1373 +                     ANA_SG_CONFIG_REG_3_INIT_GATE_STATE : 0) |
1374 +                    ANA_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB(h_basetime >> 32),
1375 +                    ANA_SG_CONFIG_REG_3);
1376 +
1377 +       ocelot_write(ocelot, cycle_time, ANA_SG_CONFIG_REG_4);
1378 +       ocelot_write(ocelot, cycle_time_ex, ANA_SG_CONFIG_REG_5);
1379 +
1380 +       ret = sgi_set_glist(ocelot, admin_list->gcl, list_length);
1381 +       if (ret < 0)
1382 +               return ret;
1383 +
1384 +       /* Start configuration change */
1385 +       ocelot_rmw(ocelot,
1386 +                  ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
1387 +                  ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
1388 +                  ANA_SG_ACCESS_CTRL);
1389 +
1390 +       ret = readx_poll_timeout(sgi_read_status, ocelot, val,
1391 +                                (!(ANA_SG_ACCESS_CTRL_CONFIG_CHANGE & val)),
1392 +                                10, 100000);
1393 +
1394 +       return ret;
1395 +}
1396 +
1397 +static int sgi_get_glist(struct ocelot *ocelot,
1398 +                        struct tsn_qci_psfp_gcl *gcl,
1399 +                        uint32_t num)
1400 +{
1401 +       int i;
1402 +       u16 val;
1403 +       u32 time = 0;
1404 +       u32 reg;
1405 +
1406 +       if (num > capa.num_sgi_gcl)
1407 +               return -EINVAL;
1408 +
1409 +       for (i = 0; i < num; i++) {
1410 +               val = ocelot_read_rix(ocelot, ANA_SG_GCL_GS_CONFIG, i);
1411 +               gcl->gate_state = (val & ANA_SG_GCL_GS_CONFIG_GATE_STATE);
1412 +
1413 +               if (val & ANA_SG_GCL_GS_CONFIG_IPV_VALID)
1414 +                       gcl->ipv = ANA_SG_GCL_GS_CONFIG_IPV(val);
1415 +               else
1416 +                       gcl->ipv = -1;
1417 +
1418 +               reg = ocelot_read_rix(ocelot, ANA_SG_GCL_TI_CONFIG, i);
1419 +               gcl->time_interval = (reg - time);
1420 +               time = reg;
1421 +
1422 +               gcl++;
1423 +       }
1424 +
1425 +       return 0;
1426 +}
1427 +
1428 +int ocelot_qci_sgi_get(struct ocelot *ocelot, int port, u32 index,
1429 +                      struct tsn_qci_psfp_sgi_conf *sgi_conf)
1430 +{
1431 +       struct tsn_qci_sg_control *admin  = &sgi_conf->admin;
1432 +       struct tsn_qci_psfp_gcl *glist;
1433 +       u32 val, reg;
1434 +       u32 list_num;
1435 +       int ret;
1436 +
1437 +       if (index >= capa.num_psfp_sgid) {
1438 +               dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
1439 +                       index, capa.num_psfp_sgid);
1440 +               return -EINVAL;
1441 +       }
1442 +
1443 +       ocelot_rmw(ocelot,
1444 +                  ANA_SG_ACCESS_CTRL_SGID(index),
1445 +                  ANA_SG_ACCESS_CTRL_SGID_M,
1446 +                  ANA_SG_ACCESS_CTRL);
1447 +
1448 +       admin->cycle_time = ocelot_read(ocelot, ANA_SG_CONFIG_REG_4);
1449 +       admin->cycle_time_extension =
1450 +               ocelot_read(ocelot, ANA_SG_CONFIG_REG_5);
1451 +
1452 +       val = ocelot_read(ocelot, ANA_SG_CONFIG_REG_2);
1453 +       admin->base_time = val;
1454 +
1455 +       reg = ocelot_read(ocelot, ANA_SG_CONFIG_REG_1);
1456 +       val = ocelot_read(ocelot, ANA_SG_CONFIG_REG_3);
1457 +
1458 +       admin->base_time +=
1459 +               ANA_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB(val) << 32;
1460 +
1461 +       admin->base_time = admin->base_time * 1000000000 + reg;
1462 +
1463 +       if (val & ANA_SG_CONFIG_REG_3_IPV_VALID)
1464 +               admin->init_ipv = ANA_SG_CONFIG_REG_3_INIT_IPV_X(val);
1465 +       else
1466 +               admin->init_ipv = -1;
1467 +
1468 +       if (val & ANA_SG_CONFIG_REG_3_GATE_ENABLE)
1469 +               sgi_conf->gate_enabled = TRUE;
1470 +
1471 +       admin->control_list_length = ANA_SG_CONFIG_REG_3_LIST_LENGTH_X(val);
1472 +
1473 +       list_num = admin->control_list_length;
1474 +
1475 +       glist = kmalloc_array(list_num, sizeof(struct tsn_qci_psfp_gcl),
1476 +                             GFP_KERNEL);
1477 +       admin->gcl = glist;
1478 +
1479 +       ret = sgi_get_glist(ocelot, glist, list_num);
1480 +
1481 +       return ret;
1482 +}
1483 +
1484 +int ocelot_qci_sgi_status_get(struct ocelot *ocelot, int port, u32 index,
1485 +                             struct tsn_psfp_sgi_status *sgi_status)
1486 +{
1487 +       u32 val, reg;
1488 +
1489 +       if (index >= capa.num_psfp_sgid) {
1490 +               dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
1491 +                       index, capa.num_psfp_sgid);
1492 +               return -EINVAL;
1493 +       }
1494 +
1495 +       ocelot_rmw(ocelot,
1496 +                  ANA_SG_ACCESS_CTRL_SGID(index),
1497 +                  ANA_SG_ACCESS_CTRL_SGID_M,
1498 +                  ANA_SG_ACCESS_CTRL);
1499 +
1500 +       val = ocelot_read(ocelot, ANA_SG_STATUS_REG_2);
1501 +       sgi_status->config_change_time = val;
1502 +
1503 +       reg = ocelot_read(ocelot, ANA_SG_STATUS_REG_1);
1504 +       val = ocelot_read(ocelot, ANA_SG_STATUS_REG_3);
1505 +       sgi_status->config_change_time +=
1506 +               ANA_SG_STATUS_REG_3_CFG_CHG_TIME_SEC_MSB(val) << 32;
1507 +       sgi_status->config_change_time =
1508 +               sgi_status->config_change_time * 1000000000 + reg;
1509 +
1510 +       if (val & ANA_SG_STATUS_REG_3_CONFIG_PENDING)
1511 +               sgi_status->config_pending  = TRUE;
1512 +       else
1513 +               sgi_status->config_pending = FALSE;
1514 +
1515 +       if (val & ANA_SG_STATUS_REG_3_GATE_STATE)
1516 +               sgi_status->oper.gate_states  =  TRUE;
1517 +       else
1518 +               sgi_status->oper.gate_states  =  FALSE;
1519 +       /*bit 3 encoding 0:IPV [0:2]is invalid . 1:IPV[0:2] is valid*/
1520 +       if (val & ANA_SG_STATUS_REG_3_IPV_VALID)
1521 +               sgi_status->oper.init_ipv = ANA_SG_STATUS_REG_3_IPV_X(val);
1522 +       else
1523 +               sgi_status->oper.init_ipv = -1;
1524 +
1525 +       return 0;
1526 +}
1527 +
1528 +int ocelot_qci_fmi_set(struct ocelot *ocelot, int port, u32 index,
1529 +                      bool enable, struct tsn_qci_psfp_fmi *fmi)
1530 +{
1531 +       u32 cir = 0, cbs = 0, pir = 0, pbs = 0;
1532 +       u32 cir_ena = 0;
1533 +       u32 pbs_max = 0, cbs_max = 0;
1534 +       bool cir_discard = 0, pir_discard = 0;
1535 +
1536 +       if (index > capa.qos_pol_max) {
1537 +               dev_err(ocelot->dev, "Invalid pol_idx %u, maximum: %u\n",
1538 +                       index, capa.qos_pol_max);
1539 +               return -EINVAL;
1540 +       }
1541 +
1542 +       if (fmi->mark_red_enable && fmi->mark_red) {
1543 +               fmi->eir = 0;
1544 +               fmi->ebs = 0;
1545 +               fmi->cir = 0;
1546 +               fmi->cbs = 0;
1547 +       }
1548 +
1549 +       pir = fmi->eir;
1550 +       pbs = fmi->ebs;
1551 +
1552 +       if (!fmi->drop_on_yellow)
1553 +               cir_ena = 1;
1554 +
1555 +       if (cir_ena) {
1556 +               cir = fmi->cir;
1557 +               cbs = fmi->cbs;
1558 +               if (cir == 0 && cbs == 0) {
1559 +                       cir_discard = 1;
1560 +               } else {
1561 +                       cir = DIV_ROUND_UP(cir, 100);
1562 +                       cir *= 3;  /* Rate unit is 33 1/3 kbps */
1563 +                       cbs = DIV_ROUND_UP(cbs, 4096);
1564 +                       cbs = (cbs ? cbs : 1);
1565 +                       cbs_max = capa.pol_cbs_max;
1566 +                       if (fmi->cf)
1567 +                               pir += fmi->cir;
1568 +               }
1569 +       }
1570 +
1571 +       if (pir == 0 && pbs == 0) {
1572 +               pir_discard = 1;
1573 +       } else {
1574 +               pir = DIV_ROUND_UP(pir, 100);
1575 +               pir *= 3;  /* Rate unit is 33 1/3 kbps */
1576 +               pbs = DIV_ROUND_UP(pbs, 4096);
1577 +               pbs = (pbs ? pbs : 1);
1578 +               pbs_max = capa.pol_pbs_max;
1579 +       }
1580 +       pir = min_t(u32, GENMASK(15, 0), pir);
1581 +       cir = min_t(u32, GENMASK(15, 0), cir);
1582 +       pbs = min(pbs_max, pbs);
1583 +       cbs = min(cbs_max, cbs);
1584 +
1585 +       ocelot_write_gix(ocelot, (ANA_POL_MODE_CFG_IPG_SIZE(20) |
1586 +                        ANA_POL_MODE_CFG_FRM_MODE(1) |
1587 +                        (fmi->cf ? ANA_POL_MODE_CFG_DLB_COUPLED : 0) |
1588 +                        (cir_ena ? ANA_POL_MODE_CFG_CIR_ENA : 0) |
1589 +                        ANA_POL_MODE_CFG_OVERSHOOT_ENA),
1590 +                        ANA_POL_MODE_CFG, index);
1591 +
1592 +       ocelot_write_gix(ocelot, ANA_POL_PIR_CFG_PIR_RATE(pir) |
1593 +                        ANA_POL_PIR_CFG_PIR_BURST(pbs),
1594 +                        ANA_POL_PIR_CFG, index);
1595 +
1596 +       ocelot_write_gix(ocelot,
1597 +                        (pir_discard ? GENMASK(22, 0) : 0),
1598 +                        ANA_POL_PIR_STATE, index);
1599 +
1600 +       ocelot_write_gix(ocelot, ANA_POL_CIR_CFG_CIR_RATE(cir) |
1601 +                        ANA_POL_CIR_CFG_CIR_BURST(cbs),
1602 +                        ANA_POL_CIR_CFG, index);
1603 +
1604 +       ocelot_write_gix(ocelot,
1605 +                        (cir_discard ? GENMASK(22, 0) : 0),
1606 +                        ANA_POL_CIR_STATE, index);
1607 +
1608 +       return 0;
1609 +}
1610 +
1611 +int ocelot_qci_fmi_get(struct ocelot *ocelot, int port, u32 index,
1612 +                      struct tsn_qci_psfp_fmi *fmi,
1613 +                      struct tsn_qci_psfp_fmi_counters *counters)
1614 +{
1615 +       u32 val, reg;
1616 +
1617 +       if (index > capa.qos_pol_max) {
1618 +               dev_err(ocelot->dev, "Invalid pol_idx %u, maximum: %u\n",
1619 +                       index, capa.qos_pol_max);
1620 +               return -EINVAL;
1621 +       }
1622 +
1623 +       val = ocelot_read_gix(ocelot, ANA_POL_PIR_CFG, index);
1624 +       reg = ocelot_read_gix(ocelot, ANA_POL_CIR_CFG, index);
1625 +
1626 +       fmi->eir = ANA_POL_PIR_CFG_PIR_RATE_X(val);
1627 +       fmi->eir = fmi->eir * 100 / 3;
1628 +       fmi->ebs = ANA_POL_PIR_CFG_PIR_BURST(val);
1629 +       fmi->ebs *= 4096;
1630 +       fmi->cir = ANA_POL_CIR_CFG_CIR_RATE_X(reg);
1631 +       fmi->cir = fmi->cir * 100 / 3;
1632 +       fmi->cbs = ANA_POL_CIR_CFG_CIR_BURST(reg);
1633 +       fmi->cbs *= 4096;
1634 +       if (!(fmi->eir | fmi->ebs | fmi->cir | fmi->cbs))
1635 +               fmi->mark_red = TRUE;
1636 +       else
1637 +               fmi->mark_red = FALSE;
1638 +
1639 +       val = ocelot_read_gix(ocelot, ANA_POL_MODE_CFG, index);
1640 +       if (val & ANA_POL_MODE_CFG_DLB_COUPLED)
1641 +               fmi->cf = TRUE;
1642 +       else
1643 +               fmi->cf = FALSE;
1644 +       if (val & ANA_POL_MODE_CFG_CIR_ENA)
1645 +               fmi->drop_on_yellow = FALSE;
1646 +       else
1647 +               fmi->drop_on_yellow = TRUE;
1648 +
1649 +       return 0;
1650 +}
1651 +
1652 +int ocelot_seq_gen_set(struct ocelot *ocelot, int port, u32 index,
1653 +                      struct tsn_seq_gen_conf *sg_conf)
1654 +{
1655 +       u8 iport_mask = sg_conf->iport_mask;
1656 +       u8 split_mask = sg_conf->split_mask;
1657 +       u8 seq_len = sg_conf->seq_len;
1658 +       u32 seq_num = sg_conf->seq_num;
1659 +
1660 +       if (index >= capa.num_frer_ssid) {
1661 +               dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
1662 +                       index, capa.num_frer_ssid - 1);
1663 +               return -EINVAL;
1664 +       }
1665 +       if (seq_len < capa.frer_seq_len_min ||
1666 +           seq_len > capa.frer_seq_len_max) {
1667 +               dev_err(ocelot->dev,
1668 +                       "Invalid seq_space_bits num %u,range:%d~%d\n",
1669 +                       seq_len,
1670 +                       capa.frer_seq_len_min,
1671 +                       capa.frer_seq_len_max);
1672 +               return -EINVAL;
1673 +       }
1674 +
1675 +       streamid_multi_forward_set(ocelot,
1676 +                                  streamhandle_map[index],
1677 +                                  split_mask);
1678 +
1679 +       ocelot_write(ocelot,
1680 +                    ANA_TABLES_SEQ_MASK_SPLIT_MASK(split_mask) |
1681 +                    ANA_TABLES_SEQ_MASK_INPUT_PORT_MASK(iport_mask),
1682 +                    ANA_TABLES_SEQ_MASK);
1683 +
1684 +       ocelot_write(ocelot,
1685 +                    ANA_TABLES_STREAMTIDX_S_INDEX(index) |
1686 +                    ANA_TABLES_STREAMTIDX_STREAM_SPLIT |
1687 +                    ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(seq_len),
1688 +                    ANA_TABLES_STREAMTIDX);
1689 +
1690 +       ocelot_write(ocelot,
1691 +                    ANA_TABLES_STREAMACCESS_GEN_REC_SEQ_NUM(seq_num) |
1692 +                    ANA_TABLES_STREAMACCESS_SEQ_GEN_REC_ENA |
1693 +                    ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_WRITE),
1694 +                    ANA_TABLES_STREAMACCESS);
1695 +
1696 +       return 0;
1697 +}
1698 +
1699 +int ocelot_seq_rec_set(struct ocelot *ocelot, int port, u32 index,
1700 +                      struct tsn_seq_rec_conf *sr_conf)
1701 +{
1702 +       u8 seq_len = sr_conf->seq_len;
1703 +       u8 hislen = sr_conf->his_len;
1704 +
1705 +       if (index >= capa.num_frer_ssid) {
1706 +               dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
1707 +                       index, capa.num_frer_ssid - 1);
1708 +               return -EINVAL;
1709 +       }
1710 +       if (seq_len < capa.frer_seq_len_min ||
1711 +           seq_len > capa.frer_seq_len_max) {
1712 +               dev_err(ocelot->dev,
1713 +                       "Invalid seq_space_bits num %u,range:%d~%d\n",
1714 +                       seq_len,
1715 +                       capa.frer_seq_len_min,
1716 +                       capa.frer_seq_len_max);
1717 +               return -EINVAL;
1718 +       }
1719 +       if (hislen < capa.frer_his_len_min ||
1720 +           hislen > capa.frer_his_len_max) {
1721 +               dev_err(ocelot->dev,
1722 +                       "Invalid history_bits num %u,range:%d~%d\n",
1723 +                       hislen,
1724 +                       capa.frer_his_len_min,
1725 +                       capa.frer_his_len_max);
1726 +               return -EINVAL;
1727 +       }
1728 +
1729 +       ocelot_write(ocelot,
1730 +                    ANA_TABLES_STREAMTIDX_S_INDEX(index) |
1731 +                    ANA_TABLES_STREAMTIDX_FORCE_SF_BEHAVIOUR |
1732 +                    ANA_TABLES_STREAMTIDX_SEQ_HISTORY_LEN(hislen) |
1733 +                    ANA_TABLES_STREAMTIDX_RESET_ON_ROGUE |
1734 +                    (sr_conf->rtag_pop_en ?
1735 +                     ANA_TABLES_STREAMTIDX_REDTAG_POP : 0) |
1736 +                    ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(seq_len),
1737 +                    ANA_TABLES_STREAMTIDX);
1738 +
1739 +       ocelot_write(ocelot,
1740 +                    ANA_TABLES_STREAMACCESS_SEQ_GEN_REC_ENA |
1741 +                    ANA_TABLES_STREAMACCESS_GEN_REC_TYPE |
1742 +                    ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_WRITE),
1743 +                    ANA_TABLES_STREAMACCESS);
1744 +
1745 +       return 0;
1746 +}
1747 +
1748 +int ocelot_cb_get(struct ocelot *ocelot, int port, u32 index,
1749 +                 struct tsn_cb_status *c)
1750 +{
1751 +       u32 val;
1752 +
1753 +       if (index >= capa.num_frer_ssid) {
1754 +               dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
1755 +                       index, capa.num_frer_ssid - 1);
1756 +               return -EINVAL;
1757 +       }
1758 +
1759 +       ocelot_write(ocelot,
1760 +                    ANA_TABLES_STREAMTIDX_S_INDEX(index),
1761 +                    ANA_TABLES_STREAMTIDX);
1762 +
1763 +       ocelot_write(ocelot,
1764 +                    ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_READ),
1765 +                    ANA_TABLES_STREAMACCESS);
1766 +
1767 +       val = ocelot_read(ocelot, ANA_TABLES_STREAMACCESS);
1768 +       c->gen_rec = (ANA_TABLES_STREAMACCESS_GEN_REC_TYPE & val) >> 2;
1769 +       c->seq_num = ANA_TABLES_STREAMACCESS_GEN_REC_SEQ_NUM_X(val);
1770 +
1771 +       val = ocelot_read(ocelot, ANA_TABLES_STREAMTIDX);
1772 +       c->err = ANA_TABLES_STREAMTIDX_SEQ_GEN_ERR_STATUS_X(val);
1773 +       c->his_len = ANA_TABLES_STREAMTIDX_SEQ_HISTORY_LEN_X(val);
1774 +       c->seq_len = ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(val);
1775 +
1776 +       val = ocelot_read(ocelot, ANA_TABLES_SEQ_MASK);
1777 +       c->split_mask = ANA_TABLES_SEQ_MASK_SPLIT_MASK_X(val);
1778 +       c->iport_mask = ANA_TABLES_SEQ_MASK_INPUT_PORT_MASK(val);
1779 +
1780 +       c->seq_his = ocelot_read(ocelot, ANA_TABLES_SEQ_HISTORY);
1781 +
1782 +       return 0;
1783 +}
1784 +
1785 +int ocelot_pcp_map_enable(struct ocelot *ocelot, u8 port)
1786 +{
1787 +       int i;
1788 +
1789 +       ocelot_rmw_gix(ocelot,
1790 +                      ANA_PORT_QOS_CFG_QOS_PCP_ENA,
1791 +                      ANA_PORT_QOS_CFG_QOS_PCP_ENA,
1792 +                      ANA_PORT_QOS_CFG,
1793 +                      port);
1794 +
1795 +       for (i = 0; i < NUM_MSCC_QOS_PRIO * 2; i++) {
1796 +               ocelot_rmw_ix(ocelot,
1797 +                             (ANA_PORT_PCP_DEI_MAP_DP_PCP_DEI_VAL & i) |
1798 +                             ANA_PORT_PCP_DEI_MAP_QOS_PCP_DEI_VAL(i),
1799 +                             ANA_PORT_PCP_DEI_MAP_DP_PCP_DEI_VAL |
1800 +                             ANA_PORT_PCP_DEI_MAP_QOS_PCP_DEI_VAL_M,
1801 +                             ANA_PORT_PCP_DEI_MAP,
1802 +                             port, i);
1803 +       }
1804 +
1805 +       return 0;
1806 +}
1807 +
1808 +int ocelot_rtag_parse_enable(struct ocelot *ocelot, u8 port)
1809 +{
1810 +       ocelot_rmw_rix(ocelot,
1811 +                      ANA_PORT_MODE_REDTAG_PARSE_CFG,
1812 +                      ANA_PORT_MODE_REDTAG_PARSE_CFG,
1813 +                      ANA_PORT_MODE,
1814 +                      port);
1815 +
1816 +       return 0;
1817 +}
1818 +
1819 +int ocelot_dscp_set(struct ocelot *ocelot, int port,
1820 +                   bool enable, const u8 dscp_ix,
1821 +                   struct tsn_qos_switch_dscp_conf *c)
1822 +{
1823 +       u32 val, ri = dscp_ix;
1824 +
1825 +       c->dscp = 0;
1826 +       c->trust = 1;
1827 +       c->remark = 0;
1828 +
1829 +       if (dscp_ix > capa.qos_dscp_max) {
1830 +               dev_err(ocelot->dev, "Invalid dscp_ix %u\n", dscp_ix);
1831 +               return -EINVAL;
1832 +       }
1833 +       if (c->cos > capa.qos_cos_max) {
1834 +               dev_err(ocelot->dev, "Invalid cos %d\n", c->cos);
1835 +               return -EINVAL;
1836 +       }
1837 +       if (c->dpl > capa.qos_dp_max) {
1838 +               dev_err(ocelot->dev, "Invalid dpl %d\n", c->dpl);
1839 +               return -EINVAL;
1840 +       }
1841 +
1842 +       ocelot_rmw_gix(ocelot,
1843 +                      (enable ? ANA_PORT_QOS_CFG_QOS_DSCP_ENA : 0) |
1844 +                      (c->dscp ? ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA : 0),
1845 +                      ANA_PORT_QOS_CFG_QOS_DSCP_ENA |
1846 +                      ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA,
1847 +                      ANA_PORT_QOS_CFG,
1848 +                      port);
1849 +
1850 +       val = (c->dpl ? ANA_DSCP_CFG_DP_DSCP_VAL : 0) |
1851 +              ANA_DSCP_CFG_QOS_DSCP_VAL(c->cos) |
1852 +              ANA_DSCP_CFG_DSCP_TRANSLATE_VAL(c->dscp) |
1853 +              (c->trust ? ANA_DSCP_CFG_DSCP_TRUST_ENA : 0) |
1854 +              (c->remark ? ANA_DSCP_CFG_DSCP_REWR_ENA : 0);
1855 +
1856 +       ocelot_write_rix(ocelot, val, ANA_DSCP_CFG, ri);
1857 +
1858 +       return 0;
1859 +}
1860 --- /dev/null
1861 +++ b/drivers/net/ethernet/mscc/ocelot_tsn.h
1862 @@ -0,0 +1,51 @@
1863 +/* SPDX-License-Identifier: (GPL-2.0 OR MIT)
1864 + *
1865 + * TSN_SWITCH driver
1866 + *
1867 + * Copyright 2018-2019 NXP
1868 + */
1869 +
1870 +#ifndef _MSCC_OCELOT_SWITCH_TSN_H_
1871 +#define _MSCC_OCELOT_SWITCH_TSN_H_
1872 +
1873 +#define TRUE 1
1874 +#define FALSE 0
1875 +
1876 +struct mscc_switch_capa {
1877 +       u8 num_tas_gcl; /* Number of TAS Gate Control Lists */
1878 +       u32 tas_ct_min; /* Minimum supported TAS CycleTime in nS */
1879 +       u32 tas_ct_max; /* Maximum supported TAS CycleTime in nS */
1880 +       u32 tas_cte_max; /* Maximum supported TAS CycleTimeExtension in nS
1881 +                         */
1882 +       u32 tas_it_max;
1883 +       u32 tas_it_min;
1884 +       u8 num_hsch;
1885 +       u8 num_psfp_sfid;
1886 +       u8 num_frer_ssid;
1887 +       u8 num_psfp_sgid;
1888 +       u16 psfp_fmi_max;
1889 +       u16 psfp_fmi_min;
1890 +       u8 num_sgi_gcl;
1891 +       u32 sgi_ct_min;
1892 +       u32 sgi_ct_max;
1893 +       u32 sgi_cte_max;
1894 +       u16 qos_pol_max;
1895 +       u8 pol_cbs_max;
1896 +       u8 pol_pbs_max;
1897 +       u8 frer_seq_len_min;
1898 +       u8 frer_seq_len_max;
1899 +       u8 frer_his_len_min;
1900 +       u8 frer_his_len_max;
1901 +       u8 qos_dscp_max;
1902 +       u8 qos_cos_max;
1903 +       u8 qos_dp_max;
1904 +};
1905 +
1906 +static inline void ocelot_port_rmwl(struct ocelot_port *port, u32 val,
1907 +                                   u32 mask, u32 reg)
1908 +{
1909 +       u32 cur = ocelot_port_readl(port, reg);
1910 +
1911 +       ocelot_port_writel(port, (cur & (~mask)) | val, reg);
1912 +}
1913 +#endif
1914 --- a/include/soc/mscc/ocelot.h
1915 +++ b/include/soc/mscc/ocelot.h
1916 @@ -10,6 +10,7 @@
1917  #include <linux/if_vlan.h>
1918  #include <linux/regmap.h>
1919  #include <net/dsa.h>
1920 +#include <net/tsn.h>
1921
1922  #define IFH_INJ_BYPASS                 BIT(31)
1923  #define IFH_INJ_POP_CNT_DISABLE                (3 << 28)
1924 @@ -328,6 +329,10 @@ enum ocelot_reg {
1925         PTP_CFG_MISC,
1926         PTP_CLK_CFG_ADJ_CFG,
1927         PTP_CLK_CFG_ADJ_FREQ,
1928 +       PTP_CUR_NSF,
1929 +       PTP_CUR_NSEC,
1930 +       PTP_CUR_SEC_LSB,
1931 +       PTP_CUR_SEC_MSB,
1932         GCB_SOFT_RST = GCB << TARGET_OFFSET,
1933  };
1934
1935 @@ -539,5 +544,50 @@ int ocelot_ptp_gettime64(struct ptp_cloc
1936  int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
1937                                  struct sk_buff *skb);
1938  void ocelot_get_txtstamp(struct ocelot *ocelot);
1939 -
1940 +int ocelot_qbv_set(struct ocelot *ocelot, int port_id,
1941 +                  struct tsn_qbv_conf *shaper_config);
1942 +int ocelot_qbv_get(struct ocelot *ocelot, int port_id,
1943 +                  struct tsn_qbv_conf *shaper_config);
1944 +int ocelot_qbv_get_status(struct ocelot *ocelot, int port_id,
1945 +                         struct tsn_qbv_status *qbvstatus);
1946 +int ocelot_cut_thru_set(struct ocelot *ocelot, int port_id, u8 cut_thru);
1947 +int ocelot_cbs_set(struct ocelot *ocelot, int port, u8 tc, u8 bw);
1948 +int ocelot_cbs_get(struct ocelot *ocelot, int port, u8 tc);
1949 +int ocelot_qbu_set(struct ocelot *ocelot, int port, u8 preemptible);
1950 +int ocelot_qbu_get(struct ocelot *ocelot, int port,
1951 +                  struct tsn_preempt_status *c);
1952 +int ocelot_cb_streamid_get(struct ocelot *ocelot, int port, u32 index,
1953 +                          struct tsn_cb_streamid *streamid);
1954 +int ocelot_cb_streamid_set(struct ocelot *ocelot, int port, u32 index,
1955 +                          bool enable, struct tsn_cb_streamid *streamid);
1956 +int ocelot_qci_sfi_get(struct ocelot *ocelot, int port, u32 index,
1957 +                      struct tsn_qci_psfp_sfi_conf *sfi);
1958 +int ocelot_qci_sfi_set(struct ocelot *ocelot, int port, u32 index,
1959 +                      bool enable, struct tsn_qci_psfp_sfi_conf *sfi);
1960 +int ocelot_qci_sfi_counters_get(struct ocelot *ocelot, int port, u32 index,
1961 +                       struct tsn_qci_psfp_sfi_counters *sfi_counters);
1962 +int ocelot_qci_max_cap_get(struct ocelot *ocelot,
1963 +                          struct tsn_qci_psfp_stream_param *stream_para);
1964 +int ocelot_qci_sgi_set(struct ocelot *ocelot, int port, u32 index,
1965 +                      struct tsn_qci_psfp_sgi_conf *sgi_conf);
1966 +int ocelot_qci_sgi_get(struct ocelot *ocelot, int port, u32 index,
1967 +                      struct tsn_qci_psfp_sgi_conf *sgi_conf);
1968 +int ocelot_qci_sgi_status_get(struct ocelot *ocelot, int port, u32 index,
1969 +                             struct tsn_psfp_sgi_status *sgi_status);
1970 +int ocelot_qci_fmi_set(struct ocelot *ocelot, int port, u32 index,
1971 +                      bool enable, struct tsn_qci_psfp_fmi *fmi);
1972 +int ocelot_qci_fmi_get(struct ocelot *ocelot, int port, u32 index,
1973 +                      struct tsn_qci_psfp_fmi *fmi,
1974 +                      struct tsn_qci_psfp_fmi_counters *counters);
1975 +int ocelot_seq_gen_set(struct ocelot *ocelot, int port, u32 index,
1976 +                      struct tsn_seq_gen_conf *sg_conf);
1977 +int ocelot_seq_rec_set(struct ocelot *ocelot, int port, u32 index,
1978 +                      struct tsn_seq_rec_conf *sr_conf);
1979 +int ocelot_cb_get(struct ocelot *ocelot, int port, u32 index,
1980 +                 struct tsn_cb_status *c);
1981 +int ocelot_pcp_map_enable(struct ocelot *ocelot, u8 port);
1982 +int ocelot_rtag_parse_enable(struct ocelot *ocelot, u8 port);
1983 +int ocelot_dscp_set(struct ocelot *ocelot, int port,
1984 +                   bool enable, const u8 dscp_ix,
1985 +                   struct tsn_qos_switch_dscp_conf *c);
1986  #endif