ASR_BASE
Change-Id: Icf3719cc0afe3eeb3edc7fa80a2eb5199ca9dda1
diff --git a/target/linux/layerscape/patches-5.4/701-net-0272-net-mscc-ocelot-tsn-configuration-support.patch b/target/linux/layerscape/patches-5.4/701-net-0272-net-mscc-ocelot-tsn-configuration-support.patch
new file mode 100644
index 0000000..953b479
--- /dev/null
+++ b/target/linux/layerscape/patches-5.4/701-net-0272-net-mscc-ocelot-tsn-configuration-support.patch
@@ -0,0 +1,1986 @@
+From eb5556db4c4fb8dff9a7b716c66a1ea3d3e696ce Mon Sep 17 00:00:00 2001
+From: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
+Date: Fri, 29 Nov 2019 11:02:43 +0800
+Subject: [PATCH] net: mscc: ocelot: tsn configuration support
+
+Support TSN configuration for ocelot switch. The TSN configuration
+fucntions are based on tsn netlink interface, it can support Qbv,
+Qbu, Qci, 802.1CB, and Qav configuration now.
+
+Signed-off-by: Xiaoliang Yang <xiaoliang.yang_1@nxp.com>
+---
+ drivers/net/ethernet/mscc/Makefile | 1 +
+ drivers/net/ethernet/mscc/ocelot.c | 11 +-
+ drivers/net/ethernet/mscc/ocelot.h | 2 +
+ drivers/net/ethernet/mscc/ocelot_ana.h | 25 +-
+ drivers/net/ethernet/mscc/ocelot_dev_gmii.h | 153 +++
+ drivers/net/ethernet/mscc/ocelot_tsn.c | 1572 +++++++++++++++++++++++++++
+ drivers/net/ethernet/mscc/ocelot_tsn.h | 51 +
+ include/soc/mscc/ocelot.h | 52 +-
+ 8 files changed, 1857 insertions(+), 10 deletions(-)
+ create mode 100644 drivers/net/ethernet/mscc/ocelot_dev_gmii.h
+ create mode 100644 drivers/net/ethernet/mscc/ocelot_tsn.c
+ create mode 100644 drivers/net/ethernet/mscc/ocelot_tsn.h
+
+--- a/drivers/net/ethernet/mscc/Makefile
++++ b/drivers/net/ethernet/mscc/Makefile
+@@ -2,4 +2,5 @@
+ obj-$(CONFIG_MSCC_OCELOT_SWITCH) += mscc_ocelot_common.o
+ mscc_ocelot_common-y := ocelot.o ocelot_io.o
+ mscc_ocelot_common-y += ocelot_regs.o ocelot_tc.o ocelot_police.o ocelot_ace.o ocelot_flower.o
++mscc_ocelot_common-y += ocelot_tsn.o
+ obj-$(CONFIG_MSCC_OCELOT_SWITCH_OCELOT) += ocelot_board.o
+--- a/drivers/net/ethernet/mscc/ocelot.c
++++ b/drivers/net/ethernet/mscc/ocelot.c
+@@ -780,7 +780,7 @@ static void ocelot_set_rx_mode(struct ne
+ * forwarded to the CPU port.
+ */
+ val = GENMASK(ocelot->num_phys_ports - 1, 0);
+- for (i = ocelot->num_phys_ports + 1; i < PGID_CPU; i++)
++ for (i = ocelot->num_phys_ports + 1; i < PGID_MCRED; i++)
+ ocelot_write_rix(ocelot, val, ANA_PGID_PGID, i);
+
+ __dev_mc_sync(dev, ocelot_mc_sync, ocelot_mc_unsync);
+@@ -2407,10 +2407,11 @@ int ocelot_init(struct ocelot *ocelot)
+ SYS_FRM_AGING_MAX_AGE(307692), SYS_FRM_AGING);
+
+ /* Setup flooding PGIDs */
+- ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
+- ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
+- ANA_FLOODING_FLD_UNICAST(PGID_UC),
+- ANA_FLOODING, 0);
++ for (i = 0; i < 8; i++)
++ ocelot_write_rix(ocelot, ANA_FLOODING_FLD_MULTICAST(PGID_MC) |
++ ANA_FLOODING_FLD_BROADCAST(PGID_MC) |
++ ANA_FLOODING_FLD_UNICAST(PGID_UC),
++ ANA_FLOODING, i);
+ ocelot_write(ocelot, ANA_FLOODING_IPMC_FLD_MC6_DATA(PGID_MCIPV6) |
+ ANA_FLOODING_IPMC_FLD_MC6_CTRL(PGID_MC) |
+ ANA_FLOODING_IPMC_FLD_MC4_DATA(PGID_MCIPV4) |
+--- a/drivers/net/ethernet/mscc/ocelot.h
++++ b/drivers/net/ethernet/mscc/ocelot.h
+@@ -27,11 +27,13 @@
+ #include "ocelot_qs.h"
+ #include "ocelot_tc.h"
+ #include "ocelot_ptp.h"
++#include "ocelot_dev_gmii.h"
+
+ #define PGID_AGGR 64
+ #define PGID_SRC 80
+
+ /* Reserved PGIDs */
++#define PGID_MCRED (PGID_AGGR - 25)
+ #define PGID_CPU (PGID_AGGR - 5)
+ #define PGID_UC (PGID_AGGR - 4)
+ #define PGID_MC (PGID_AGGR - 3)
+--- a/drivers/net/ethernet/mscc/ocelot_ana.h
++++ b/drivers/net/ethernet/mscc/ocelot_ana.h
+@@ -227,6 +227,11 @@
+ #define ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(x) ((x) & GENMASK(1, 0))
+ #define ANA_TABLES_SFIDACCESS_SFID_TBL_CMD_M GENMASK(1, 0)
+
++#define SFIDACCESS_CMD_IDLE 0
++#define SFIDACCESS_CMD_READ 1
++#define SFIDACCESS_CMD_WRITE 2
++#define SFIDACCESS_CMD_INIT 3
++
+ #define ANA_TABLES_SFIDTIDX_SGID_VALID BIT(26)
+ #define ANA_TABLES_SFIDTIDX_SGID(x) (((x) << 18) & GENMASK(25, 18))
+ #define ANA_TABLES_SFIDTIDX_SGID_M GENMASK(25, 18)
+@@ -252,15 +257,23 @@
+ #define ANA_SG_CONFIG_REG_3_LIST_LENGTH_M GENMASK(18, 16)
+ #define ANA_SG_CONFIG_REG_3_LIST_LENGTH_X(x) (((x) & GENMASK(18, 16)) >> 16)
+ #define ANA_SG_CONFIG_REG_3_GATE_ENABLE BIT(20)
+-#define ANA_SG_CONFIG_REG_3_INIT_IPS(x) (((x) << 24) & GENMASK(27, 24))
+-#define ANA_SG_CONFIG_REG_3_INIT_IPS_M GENMASK(27, 24)
+-#define ANA_SG_CONFIG_REG_3_INIT_IPS_X(x) (((x) & GENMASK(27, 24)) >> 24)
+-#define ANA_SG_CONFIG_REG_3_INIT_GATE_STATE BIT(28)
++#define ANA_SG_CONFIG_REG_3_INIT_IPS(x) (((x) << 21) & GENMASK(24, 21))
++#define ANA_SG_CONFIG_REG_3_INIT_IPS_M GENMASK(24, 21)
++#define ANA_SG_CONFIG_REG_3_INIT_IPS_X(x) (((x) & GENMASK(24, 21)) >> 21)
++#define ANA_SG_CONFIG_REG_3_IPV_VALID BIT(24)
++#define ANA_SG_CONFIG_REG_3_IPV_INVALID(x) (((x) << 24) & GENMASK(24, 24))
++#define ANA_SG_CONFIG_REG_3_INIT_IPV(x) (((x) << 21) & GENMASK(23, 21))
++#define ANA_SG_CONFIG_REG_3_INIT_IPV_M GENMASK(23, 21)
++#define ANA_SG_CONFIG_REG_3_INIT_IPV_X(x) (((x) & GENMASK(23, 21)) >> 21)
++#define ANA_SG_CONFIG_REG_3_INIT_GATE_STATE BIT(25)
+
+ #define ANA_SG_GCL_GS_CONFIG_RSZ 0x4
+
+ #define ANA_SG_GCL_GS_CONFIG_IPS(x) ((x) & GENMASK(3, 0))
+ #define ANA_SG_GCL_GS_CONFIG_IPS_M GENMASK(3, 0)
++#define ANA_SG_GCL_GS_CONFIG_IPV_VALID BIT(3)
++#define ANA_SG_GCL_GS_CONFIG_IPV(x) ((x) & GENMASK(2, 0))
++#define ANA_SG_GCL_GS_CONFIG_IPV_M GENMASK(2, 0)
+ #define ANA_SG_GCL_GS_CONFIG_GATE_STATE BIT(4)
+
+ #define ANA_SG_GCL_TI_CONFIG_RSZ 0x4
+@@ -271,6 +284,10 @@
+ #define ANA_SG_STATUS_REG_3_IPS(x) (((x) << 20) & GENMASK(23, 20))
+ #define ANA_SG_STATUS_REG_3_IPS_M GENMASK(23, 20)
+ #define ANA_SG_STATUS_REG_3_IPS_X(x) (((x) & GENMASK(23, 20)) >> 20)
++#define ANA_SG_STATUS_REG_3_IPV_VALID BIT(23)
++#define ANA_SG_STATUS_REG_3_IPV(x) (((x) << 20) & GENMASK(22, 20))
++#define ANA_SG_STATUS_REG_3_IPV_M GENMASK(22, 20)
++#define ANA_SG_STATUS_REG_3_IPV_X(x) (((x) & GENMASK(22, 20)) >> 20)
+ #define ANA_SG_STATUS_REG_3_CONFIG_PENDING BIT(24)
+
+ #define ANA_PORT_VLAN_CFG_GSZ 0x100
+--- /dev/null
++++ b/drivers/net/ethernet/mscc/ocelot_dev_gmii.h
+@@ -0,0 +1,153 @@
++/* SPDX-License-Identifier: (GPL-2.0 OR MIT) */
++/* Microsemi Ocelot Switch driver
++ *
++ * Copyright (c) 2017 Microsemi Corporation
++ */
++
++#ifndef _MSCC_OCELOT_DEV_GMII_H_
++#define _MSCC_OCELOT_DEV_GMII_H_
++
++#define DEV_GMII_PORT_MODE_CLOCK_CFG 0x0
++
++#define DEV_GMII_PORT_MODE_CLOCK_CFG_MAC_TX_RST BIT(5)
++#define DEV_GMII_PORT_MODE_CLOCK_CFG_MAC_RX_RST BIT(4)
++#define DEV_GMII_PORT_MODE_CLOCK_CFG_PORT_RST BIT(3)
++#define DEV_GMII_PORT_MODE_CLOCK_CFG_PHY_RST BIT(2)
++#define DEV_GMII_PORT_MODE_CLOCK_CFG_LINK_SPEED(x) ((x) & GENMASK(1, 0))
++#define DEV_GMII_PORT_MODE_CLOCK_CFG_LINK_SPEED_M GENMASK(1, 0)
++
++#define DEV_GMII_PORT_MODE_PORT_MISC 0x4
++
++#define DEV_GMII_PORT_MODE_PORT_MISC_MPLS_RX_ENA BIT(5)
++#define DEV_GMII_PORT_MODE_PORT_MISC_FWD_ERROR_ENA BIT(4)
++#define DEV_GMII_PORT_MODE_PORT_MISC_FWD_PAUSE_ENA BIT(3)
++#define DEV_GMII_PORT_MODE_PORT_MISC_FWD_CTRL_ENA BIT(2)
++#define DEV_GMII_PORT_MODE_PORT_MISC_GMII_LOOP_ENA BIT(1)
++#define DEV_GMII_PORT_MODE_PORT_MISC_DEV_LOOP_ENA BIT(0)
++
++#define DEV_GMII_PORT_MODE_EVENTS 0x8
++
++#define DEV_GMII_PORT_MODE_EEE_CFG 0xc
++
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_ENA BIT(22)
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_AGE(x) (((x) << 15) & GENMASK(21, 15))
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_AGE_M GENMASK(21, 15)
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_AGE_X(x) (((x) & GENMASK(21, 15)) >> 15)
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_WAKEUP(x) (((x) << 8) & GENMASK(14, 8))
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_WAKEUP_M GENMASK(14, 8)
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_WAKEUP_X(x) (((x) & GENMASK(14, 8)) >> 8)
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_HOLDOFF(x) (((x) << 1) & GENMASK(7, 1))
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_HOLDOFF_M GENMASK(7, 1)
++#define DEV_GMII_PORT_MODE_EEE_CFG_EEE_TIMER_HOLDOFF_X(x) (((x) & GENMASK(7, 1)) >> 1)
++#define DEV_GMII_PORT_MODE_EEE_CFG_PORT_LPI BIT(0)
++
++#define DEV_GMII_PORT_MODE_RX_PATH_DELAY 0x10
++
++#define DEV_GMII_PORT_MODE_TX_PATH_DELAY 0x14
++
++#define DEV_GMII_PORT_MODE_PTP_PREDICT_CFG 0x18
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_ENA_CFG 0x1c
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_ENA_CFG_RX_ENA BIT(4)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_ENA_CFG_TX_ENA BIT(0)
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_MODE_CFG 0x20
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_MODE_CFG_FC_WORD_SYNC_ENA BIT(8)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_MODE_CFG_GIGA_MODE_ENA BIT(4)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_MODE_CFG_FDX_ENA BIT(0)
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_MAXLEN_CFG 0x24
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG 0x28
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_TAG_ID(x) (((x) << 16) & GENMASK(31, 16))
++#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_TAG_ID_M GENMASK(31, 16)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_TAG_ID_X(x) (((x) & GENMASK(31, 16)) >> 16)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_PB_ENA BIT(1)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_VLAN_AWR_ENA BIT(0)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_TAGS_CFG_VLAN_LEN_AWR_ENA BIT(2)
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_ADV_CHK_CFG 0x2c
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_ADV_CHK_CFG_LEN_DROP_ENA BIT(0)
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG 0x30
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RESTORE_OLD_IPG_CHECK BIT(17)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_REDUCED_TX_IFG BIT(16)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_TX_IFG(x) (((x) << 8) & GENMASK(12, 8))
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_TX_IFG_M GENMASK(12, 8)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_TX_IFG_X(x) (((x) & GENMASK(12, 8)) >> 8)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG2(x) (((x) << 4) & GENMASK(7, 4))
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG2_M GENMASK(7, 4)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG2_X(x) (((x) & GENMASK(7, 4)) >> 4)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG1(x) ((x) & GENMASK(3, 0))
++#define DEV_GMII_MAC_CFG_STATUS_MAC_IFG_CFG_RX_IFG1_M GENMASK(3, 0)
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG 0x34
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_BYPASS_COL_SYNC BIT(26)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_OB_ENA BIT(25)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_WEXC_DIS BIT(24)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_SEED(x) (((x) << 16) & GENMASK(23, 16))
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_SEED_M GENMASK(23, 16)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_SEED_X(x) (((x) & GENMASK(23, 16)) >> 16)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_SEED_LOAD BIT(12)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_RETRY_AFTER_EXC_COL_ENA BIT(8)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_LATE_COL_POS(x) ((x) & GENMASK(6, 0))
++#define DEV_GMII_MAC_CFG_STATUS_MAC_HDX_CFG_LATE_COL_POS_M GENMASK(6, 0)
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_DBG_CFG 0x38
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_DBG_CFG_TBI_MODE BIT(4)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_DBG_CFG_IFG_CRS_EXT_CHK_ENA BIT(0)
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_FC_MAC_LOW_CFG 0x3c
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_FC_MAC_HIGH_CFG 0x40
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY 0x44
++
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_IPG_SHRINK_STICKY BIT(9)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_PREAM_SHRINK_STICKY BIT(8)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_CARRIER_EXT_STICKY BIT(7)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_CARRIER_EXT_ERR_STICKY BIT(6)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_RX_JUNK_STICKY BIT(5)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_RETRANSMIT_STICKY BIT(4)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_JAM_STICKY BIT(3)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_FIFO_OFLW_STICKY BIT(2)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_FRM_LEN_OVR_STICKY BIT(1)
++#define DEV_GMII_MAC_CFG_STATUS_MAC_STICKY_TX_ABORT_STICKY BIT(0)
++
++#define DEV_GMII_MM_CONFIG_ENABLE_CONFIG 0x48
++
++#define DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA BIT(0)
++#define DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA BIT(4)
++#define DEV_GMII_MM_CONFIG_ENABLE_CONFIG_KEEP_S_AFTER_D BIT(8)
++
++#define DEV_GMII_MM_CONFIG_VERIF_CONFIG 0x4c
++
++#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_DIS BIT(0)
++#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME(x) (((x) << 4) & GENMASK(11, 4))
++#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME_M GENMASK(11, 4)
++#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_PRM_VERIFY_TIME_X(x) (((x) & GENMASK(11, 4)) >> 4)
++#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS(x) (((x) << 12) & GENMASK(13, 12))
++#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS_M GENMASK(13, 12)
++#define DEV_GMII_MM_CONFIG_VERIF_CONFIG_VERIF_TIMER_UNITS_X(x) (((x) & GENMASK(13, 12)) >> 12)
++
++#define DEV_GMII_MM_STATISTICS_MM_STATUS 0x50
++
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STATUS BIT(0)
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STICKY BIT(4)
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE(x) (((x) << 8) & GENMASK(10, 8))
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE_M GENMASK(10, 8)
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_VERIFY_STATE_X(x) (((x) & GENMASK(10, 8)) >> 8)
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_UNEXP_RX_PFRM_STICKY BIT(12)
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_UNEXP_TX_PFRM_STICKY BIT(16)
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_MM_RX_FRAME_STATUS BIT(20)
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_MM_TX_FRAME_STATUS BIT(24)
++#define DEV_GMII_MM_STATISTICS_MM_STATUS_MM_TX_PRMPT_STATUS BIT(28)
++
++#endif
+--- /dev/null
++++ b/drivers/net/ethernet/mscc/ocelot_tsn.c
+@@ -0,0 +1,1572 @@
++// SPDX-License-Identifier: (GPL-2.0 OR MIT)
++/* Felix Switch TSN driver
++ *
++ * Copyright (c) 2018 Microsemi Corporation
++ * Copyright 2018-2019 NXP
++ */
++
++#include <linux/io.h>
++#include <linux/kernel.h>
++#include <linux/pci.h>
++#include <linux/iopoll.h>
++#include "ocelot.h"
++#include <soc/mscc/ocelot_sys.h>
++#include "ocelot_ana.h"
++#include "ocelot_qsys.h"
++#include "ocelot_rew.h"
++#include "ocelot_dev_gmii.h"
++#include "ocelot_tsn.h"
++
++#define MSCC_NUM_OUT_PORT 4 /* Number of physical output ports */
++#define SE_IX_PORT 64
++
++/* MSCC TSN parameters limited */
++#define NUM_MSCC_QOS_PRIO 8
++#define MSCC_PSFP_SFID_NUM 176
++#define MSCC_FRER_SSID_NUM 128
++
++/* Using the max number of MSCC_PSFP_SFID_NUM and MSCC_FRER_SSID_NUM */
++#define MSCC_STREAM_HANDLE_NUM MSCC_PSFP_SFID_NUM
++
++int streamhandle_map[MSCC_STREAM_HANDLE_NUM] = {0};
++static struct mscc_switch_capa capa __ro_after_init = {
++ .num_tas_gcl = 64,
++ .tas_ct_min = 100,
++ .tas_ct_max = 1000000000,
++ .tas_cte_max = 999999999,
++ .tas_it_max = 999999999,
++ .tas_it_min = 1000,
++ .num_hsch = 72,
++ .num_psfp_sfid = MSCC_PSFP_SFID_NUM,
++ .num_psfp_sgid = 184,
++ .psfp_fmi_max = 246,
++ .psfp_fmi_min = 63,
++ .num_sgi_gcl = 4,
++ .sgi_ct_min = 5000,
++ .sgi_ct_max = 1000000000,
++ .sgi_cte_max = 999999999,
++ .qos_pol_max = 383,
++ /* Maximum allowed value of committed burst size(CBS) is 240 KB */
++ .pol_cbs_max = 60,
++ /* Maximum allowed value of excess burst size(EBS) is 240 KB */
++ .pol_pbs_max = 60,
++ .num_frer_ssid = MSCC_FRER_SSID_NUM,
++ .frer_seq_len_min = 1,
++ .frer_seq_len_max = 28,
++ .frer_his_len_min = 1,
++ .frer_his_len_max = 32,
++ .qos_dscp_max = 63,
++ .qos_cos_max = NUM_MSCC_QOS_PRIO - 1,
++ .qos_dp_max = 1,
++};
++
++static int qos_port_tas_gcl_set(struct ocelot *ocelot, const u8 gcl_ix,
++ struct tsn_qbv_entry *control_list)
++{
++ if (gcl_ix >= capa.num_tas_gcl) {
++ dev_err(ocelot->dev, "Invalid gcl ix %u\n", gcl_ix);
++ return -EINVAL;
++ }
++ if (control_list->time_interval < capa.tas_it_min ||
++ control_list->time_interval > capa.tas_it_max) {
++ dev_err(ocelot->dev, "Invalid time_interval %u\n",
++ control_list->time_interval);
++
++ return -EINVAL;
++ }
++
++ ocelot_write(ocelot,
++ QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM(gcl_ix) |
++ QSYS_GCL_CFG_REG_1_GATE_STATE(control_list->gate_state),
++ QSYS_GCL_CFG_REG_1);
++
++ ocelot_write(ocelot,
++ control_list->time_interval,
++ QSYS_GCL_CFG_REG_2);
++
++ return 0;
++}
++
++static u32 tas_read_status(struct ocelot *ocelot)
++{
++ u32 val;
++
++ val = ocelot_read(ocelot, QSYS_TAS_PARAM_CFG_CTRL);
++
++ return val;
++}
++
++int ocelot_qbv_set(struct ocelot *ocelot, int port_id,
++ struct tsn_qbv_conf *shaper_config)
++{
++ struct tsn_qbv_basic *admin_basic = &shaper_config->admin;
++ struct tsn_qbv_entry *control_list = admin_basic->control_list;
++ u32 base_time_nsec = admin_basic->base_time % 1000000000;
++ u64 base_time_sec = admin_basic->base_time / 1000000000;
++ u64 cur_time;
++ u32 val;
++ u8 speed;
++ int i;
++ int ret;
++
++ if (admin_basic->control_list_length > capa.num_tas_gcl) {
++ dev_err(ocelot->dev,
++ "Invalid admin_control_list_length %u\n",
++ admin_basic->control_list_length);
++ return -EINVAL;
++ }
++
++ if ((admin_basic->cycle_time < capa.tas_ct_min ||
++ admin_basic->cycle_time > capa.tas_ct_max) &&
++ shaper_config->gate_enabled) {
++ dev_err(ocelot->dev, "Invalid admin_cycle_time %u ns\n",
++ admin_basic->cycle_time);
++ return -EINVAL;
++ }
++ if (admin_basic->cycle_time_extension > capa.tas_cte_max) {
++ dev_err(ocelot->dev,
++ "Invalid admin_cycle_time_extension %u\n",
++ admin_basic->cycle_time_extension);
++ return -EINVAL;
++ }
++
++ cur_time = ocelot_read(ocelot, PTP_CUR_SEC_MSB);
++ cur_time = cur_time << 32;
++ cur_time += ocelot_read(ocelot, PTP_CUR_SEC_LSB);
++
++ if (base_time_sec < cur_time) {
++ base_time_sec = cur_time;
++ base_time_nsec = ocelot_read(ocelot, PTP_CUR_NSEC);
++ }
++
++ /* Select port */
++ ocelot_rmw(ocelot,
++ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port_id),
++ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
++ QSYS_TAS_PARAM_CFG_CTRL);
++
++ val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
++ if (val & QSYS_PARAM_STATUS_REG_8_CONFIG_PENDING) {
++ ocelot_rmw_rix(ocelot, 0, QSYS_TAG_CONFIG_ENABLE,
++ QSYS_TAG_CONFIG, port_id);
++ }
++
++ if (!shaper_config->gate_enabled)
++ admin_basic->gate_states = 0xff;
++
++ val = ocelot_read_gix(ocelot, ANA_PFC_PFC_CFG, port_id);
++ speed = ANA_PFC_PFC_CFG_FC_LINK_SPEED(val);
++
++ ocelot_rmw_rix(ocelot,
++ (shaper_config->gate_enabled ?
++ QSYS_TAG_CONFIG_ENABLE : 0) |
++ QSYS_TAG_CONFIG_INIT_GATE_STATE(admin_basic->gate_states) |
++ QSYS_TAG_CONFIG_SCH_TRAFFIC_QUEUES(0xff) |
++ QSYS_TAG_CONFIG_LINK_SPEED(speed),
++ QSYS_TAG_CONFIG_ENABLE |
++ QSYS_TAG_CONFIG_INIT_GATE_STATE_M |
++ QSYS_TAG_CONFIG_SCH_TRAFFIC_QUEUES_M |
++ QSYS_TAG_CONFIG_LINK_SPEED_M,
++ QSYS_TAG_CONFIG,
++ port_id);
++
++ ocelot_write_rix(ocelot, shaper_config->maxsdu,
++ QSYS_PORT_MAX_SDU, port_id);
++ /* TODO: add queue max SDU set */
++
++ if (shaper_config->gate_enabled) {
++ ocelot_write(ocelot, base_time_nsec,
++ QSYS_PARAM_CFG_REG_1);
++
++ ocelot_write(ocelot, base_time_sec & GENMASK(31, 0),
++ QSYS_PARAM_CFG_REG_2);
++
++ ocelot_write(ocelot,
++ QSYS_PARAM_CFG_REG_3_BASE_TIME_SEC_MSB(base_time_sec >> 32) |
++ QSYS_PARAM_CFG_REG_3_LIST_LENGTH(admin_basic->control_list_length),
++ QSYS_PARAM_CFG_REG_3);
++
++ ocelot_write(ocelot, admin_basic->cycle_time,
++ QSYS_PARAM_CFG_REG_4);
++
++ ocelot_write(ocelot, admin_basic->cycle_time_extension,
++ QSYS_PARAM_CFG_REG_5);
++
++ for (i = 0; i < admin_basic->control_list_length; i++) {
++ qos_port_tas_gcl_set(ocelot, i, control_list);
++ control_list++;
++ }
++
++ /* Start configuration change */
++ ocelot_rmw(ocelot,
++ QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE,
++ QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE,
++ QSYS_TAS_PARAM_CFG_CTRL);
++
++ ret = readx_poll_timeout(tas_read_status, ocelot, val,
++ !(QSYS_TAS_PARAM_CFG_CTRL_CONFIG_CHANGE
++ & val), 10, 100000);
++ return ret;
++ }
++
++ return 0;
++}
++
++int ocelot_qbv_get(struct ocelot *ocelot, int port_id,
++ struct tsn_qbv_conf *shaper_config)
++{
++ u32 val, reg;
++ int i;
++ u32 base_timel;
++ u32 base_timeh;
++ struct tsn_qbv_basic *admin = &shaper_config->admin;
++ struct tsn_qbv_entry *list;
++
++ ocelot_rmw(ocelot,
++ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port_id),
++ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
++ QSYS_TAS_PARAM_CFG_CTRL);
++
++ val = ocelot_read_rix(ocelot, QSYS_TAG_CONFIG, port_id);
++ shaper_config->gate_enabled = (val & QSYS_TAG_CONFIG_ENABLE);
++ admin->gate_states = QSYS_TAG_CONFIG_INIT_GATE_STATE_X(val);
++
++ base_timel = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_1);
++ base_timeh = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_2);
++ reg = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_3);
++ admin->base_time = base_timeh |
++ (((u64)QSYS_PARAM_CFG_REG_3_BASE_TIME_SEC_MSB(reg)) << 32);
++
++ admin->base_time = (admin->base_time * 1000000000) + base_timel;
++
++ admin->control_list_length =
++ QSYS_PARAM_CFG_REG_3_LIST_LENGTH_X(reg);
++
++ admin->cycle_time = ocelot_read(ocelot, QSYS_PARAM_CFG_REG_4);
++ admin->cycle_time_extension =
++ ocelot_read(ocelot, QSYS_PARAM_CFG_REG_5);
++
++ list = kmalloc_array(admin->control_list_length,
++ sizeof(struct tsn_qbv_entry), GFP_KERNEL);
++ admin->control_list = list;
++
++ for (i = 0; i < admin->control_list_length; i++) {
++ ocelot_rmw(ocelot,
++ QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM(i),
++ QSYS_GCL_CFG_REG_1_GCL_ENTRY_NUM_M,
++ QSYS_GCL_CFG_REG_1);
++
++ list->time_interval =
++ ocelot_read(ocelot, QSYS_GCL_CFG_REG_2);
++
++ reg = ocelot_read(ocelot, QSYS_GCL_CFG_REG_1);
++ list->gate_state = QSYS_GCL_CFG_REG_1_GATE_STATE_X(reg);
++
++ list++;
++ }
++
++ return 0;
++}
++
++static int qbv_get_gatelist(struct ocelot *ocelot,
++ struct tsn_qbv_basic *oper)
++{
++ u32 base_timel;
++ u32 base_timeh;
++ u32 val;
++ struct tsn_qbv_entry *glist;
++ int i;
++
++ base_timel = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_1);
++ base_timeh = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_2);
++ val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_3);
++ oper->base_time = base_timeh;
++ oper->base_time +=
++ ((u64)QSYS_PARAM_STATUS_REG_3_BASE_TIME_SEC_MSB(val)) <<
++ 32;
++ oper->base_time = (oper->base_time * 1000000000) + base_timel;
++
++ oper->control_list_length =
++ QSYS_PARAM_STATUS_REG_3_LIST_LENGTH_X(val);
++
++ oper->cycle_time = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_4);
++ oper->cycle_time_extension = ocelot_read(ocelot,
++ QSYS_PARAM_STATUS_REG_5);
++
++ val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
++ oper->gate_states = QSYS_PARAM_STATUS_REG_8_OPER_GATE_STATE_X(val);
++
++ glist = kmalloc_array(oper->control_list_length,
++ sizeof(struct tsn_qbv_entry), GFP_KERNEL);
++
++ oper->control_list = glist;
++
++ for (i = 0; i < oper->control_list_length; i++) {
++ ocelot_rmw(ocelot,
++ QSYS_GCL_STATUS_REG_1_GCL_ENTRY_NUM(i),
++ QSYS_GCL_STATUS_REG_1_GCL_ENTRY_NUM_M,
++ QSYS_GCL_STATUS_REG_1);
++
++ val = ocelot_read(ocelot, QSYS_GCL_STATUS_REG_2);
++ glist->time_interval = val;
++ val = ocelot_read(ocelot, QSYS_GCL_STATUS_REG_1);
++ glist->gate_state =
++ QSYS_GCL_STATUS_REG_1_GATE_STATE_X(val);
++
++ glist++;
++ }
++
++ return 0;
++}
++
++int ocelot_qbv_get_status(struct ocelot *ocelot, int port_id,
++ struct tsn_qbv_status *qbvstatus)
++{
++ struct tsn_qbv_basic *oper = &qbvstatus->oper;
++ u32 val;
++ ptptime_t cur_time;
++
++ ocelot_rmw(ocelot,
++ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM(port_id),
++ QSYS_TAS_PARAM_CFG_CTRL_PORT_NUM_M,
++ QSYS_TAS_PARAM_CFG_CTRL);
++
++ qbvstatus->supported_list_max = capa.num_tas_gcl;
++
++ val = ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_8);
++ qbvstatus->config_pending =
++ (val & QSYS_PARAM_STATUS_REG_8_CONFIG_PENDING) ? 1 : 0;
++
++ qbvstatus->config_change_time =
++ ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_7);
++
++ qbvstatus->config_change_time +=
++ ((u64)QSYS_PARAM_STATUS_REG_8_CFG_CHG_TIME_SEC_MSB(val)) <<
++ 32;
++
++ qbvstatus->config_change_time =
++ (qbvstatus->config_change_time * 1000000000) +
++ ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_6);
++
++ qbvstatus->config_change_error =
++ ocelot_read(ocelot, QSYS_PARAM_STATUS_REG_9);
++
++ cur_time = ocelot_read(ocelot, PTP_CUR_SEC_MSB);
++ cur_time = cur_time << 32;
++ cur_time += ocelot_read(ocelot, PTP_CUR_SEC_LSB);
++ cur_time = (cur_time * 1000000000) +
++ ocelot_read(ocelot, PTP_CUR_NSEC);
++
++ qbvstatus->current_time = cur_time;
++ qbv_get_gatelist(ocelot, oper);
++
++ return 0;
++}
++
++int ocelot_cut_thru_set(struct ocelot *ocelot, int port_id, u8 cut_thru)
++{
++ ocelot_write_rix(ocelot, cut_thru, ANA_CUT_THRU_CFG, port_id);
++
++ return 0;
++}
++
++static int qos_shaper_conf_set(struct ocelot *ocelot, int port,
++ u32 port_ix, u8 percent)
++{
++ u32 val;
++ int speed;
++ u32 cbs = 0;
++ u32 cir = 0;
++
++ if (percent > 100) {
++ dev_err(ocelot->dev, "percentage %d larger than 100\n",
++ percent);
++ return -EINVAL;
++ }
++ if (port_ix >= capa.num_hsch) {
++ dev_err(ocelot->dev,
++ "CIR_CFG: id %d is exceed num of HSCH instance\n",
++ port_ix);
++ return -EINVAL;
++ }
++
++ val = ocelot_read_gix(ocelot, ANA_PFC_PFC_CFG, port);
++ speed = ANA_PFC_PFC_CFG_FC_LINK_SPEED(val);
++ switch (speed) {
++ case OCELOT_SPEED_10:
++ cir = 10000;
++ break;
++ case OCELOT_SPEED_100:
++ cir = 100000;
++ break;
++ case OCELOT_SPEED_1000:
++ cir = 1000000;
++ break;
++ case OCELOT_SPEED_2500:
++ cir = 2500000;
++ break;
++ }
++
++ cir = cir * percent / 100;
++ cir = DIV_ROUND_UP(cir, 100); /* Rate unit is 100 kbps */
++ cir = (cir ? cir : 1); /* Avoid using zero rate */
++ cbs = DIV_ROUND_UP(cbs, 4096); /* Burst unit is 4kB */
++ cbs = (cbs ? cbs : 1); /* Avoid using zero burst size */
++ cir = min_t(u32, GENMASK(15, 0), cir);
++ cbs = min_t(u32, GENMASK(6, 0), cbs);
++ ocelot_write_gix(ocelot,
++ QSYS_CIR_CFG_CIR_RATE(cir) |
++ QSYS_CIR_CFG_CIR_BURST(cbs),
++ QSYS_CIR_CFG,
++ port_ix);
++
++ return 0;
++}
++
++static int qos_shaper_conf_get(struct ocelot *ocelot, int port,
++ u32 port_ix)
++{
++ u32 val;
++ u32 bandwidth = 0;
++ u32 cir = 0;
++ int percentage;
++ int speed;
++
++ if (port_ix >= capa.num_hsch) {
++ dev_err(ocelot->dev,
++ "CIR_CFG: id %d is exceed num of HSCH instance\n",
++ port_ix);
++ return -EINVAL;
++ }
++
++ val = ocelot_read_gix(ocelot, ANA_PFC_PFC_CFG, port);
++ speed = ANA_PFC_PFC_CFG_FC_LINK_SPEED(val);
++ switch (speed) {
++ case OCELOT_SPEED_10:
++ bandwidth = 10000;
++ break;
++ case OCELOT_SPEED_100:
++ bandwidth = 100000;
++ break;
++ case OCELOT_SPEED_1000:
++ bandwidth = 1000000;
++ break;
++ case OCELOT_SPEED_2500:
++ bandwidth = 2500000;
++ break;
++ }
++
++ val = ocelot_read_gix(ocelot, QSYS_CIR_CFG, port_ix);
++
++ cir = QSYS_CIR_CFG_CIR_RATE_X(val);
++ cir *= 100;
++ percentage = cir * 100 / bandwidth;
++
++ return percentage;
++}
++
++int ocelot_cbs_set(struct ocelot *ocelot, int port, u8 tc, u8 bw)
++{
++ if (tc > capa.qos_cos_max) {
++ dev_err(ocelot->dev, "Invalid tc: %u\n", tc);
++ return -EINVAL;
++ }
++
++ qos_shaper_conf_set(ocelot, port, port * 8 + tc, bw);
++
++ ocelot_rmw_gix(ocelot,
++ QSYS_SE_CFG_SE_AVB_ENA,
++ QSYS_SE_CFG_SE_AVB_ENA,
++ QSYS_SE_CFG,
++ port * 8 + tc);
++
++ return 0;
++}
++
++int ocelot_cbs_get(struct ocelot *ocelot, int port, u8 tc)
++{
++ int ret;
++
++ if (tc > capa.qos_cos_max) {
++ dev_err(ocelot->dev, "Invalid tc: %u\n", tc);
++ return -EINVAL;
++ }
++
++ ret = qos_shaper_conf_get(ocelot, port, port * 8 + tc);
++
++ return ret;
++}
++
++int ocelot_qbu_set(struct ocelot *ocelot, int port, u8 preemptible)
++{
++ struct ocelot_port *ocelot_port = ocelot->ports[port];
++
++ ocelot_port_rmwl(ocelot_port,
++ DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA |
++ DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA,
++ DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_RX_ENA |
++ DEV_GMII_MM_CONFIG_ENABLE_CONFIG_MM_TX_ENA,
++ DEV_GMII_MM_CONFIG_ENABLE_CONFIG);
++
++ ocelot_rmw_rix(ocelot,
++ QSYS_PREEMPTION_CFG_P_QUEUES(preemptible),
++ QSYS_PREEMPTION_CFG_P_QUEUES_M,
++ QSYS_PREEMPTION_CFG,
++ port);
++
++ return 0;
++}
++
++int ocelot_qbu_get(struct ocelot *ocelot, int port,
++ struct tsn_preempt_status *c)
++{
++ struct ocelot_port *ocelot_port = ocelot->ports[port];
++ u32 val;
++
++ val = ocelot_read_rix(ocelot,
++ QSYS_PREEMPTION_CFG,
++ port);
++
++ c->admin_state = QSYS_PREEMPTION_CFG_P_QUEUES(val);
++ c->hold_advance = QSYS_PREEMPTION_CFG_HOLD_ADVANCE_X(val);
++
++ val = ocelot_port_readl(ocelot_port,
++ DEV_GMII_MM_STATISTICS_MM_STATUS);
++ c->preemption_active =
++ DEV_GMII_MM_STATISTICS_MM_STATUS_PRMPT_ACTIVE_STATUS & val;
++
++ return 0;
++}
++
++int ocelot_cb_streamid_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_cb_streamid *streamid)
++{
++ u32 m_index;
++ u32 bucket;
++ u32 val, dst, reg;
++ u64 dmac;
++ u32 ldmac, hdmac;
++
++ if (index >= MSCC_STREAM_HANDLE_NUM) {
++ dev_err(ocelot->dev,
++ "Invalid stream handle %u, maximum:%u\n",
++ index, MSCC_STREAM_HANDLE_NUM - 1);
++ return -EINVAL;
++ }
++
++ index = streamhandle_map[index];
++ m_index = index / 4;
++ bucket = index % 4;
++ streamid->type = 1;
++ regmap_field_write(ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET],
++ bucket);
++ regmap_field_write(ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX],
++ m_index);
++
++ /*READ command MACACCESS.VALID(11 bit) must be 0 */
++ ocelot_write(ocelot,
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
++ ANA_TABLES_MACACCESS);
++
++ val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
++ dst = ANA_TABLES_MACACCESS_DEST_IDX_X(val);
++ reg = ocelot_read_rix(ocelot, ANA_PGID_PGID, dst);
++ streamid->ofac_oport = ANA_PGID_PGID_PGID(reg);
++
++ /*Get the entry's MAC address and VLAN id*/
++ ldmac = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
++ val = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
++ val &= 0x1fffffff;
++ hdmac = val & 0xffff;
++ dmac = hdmac;
++ dmac = (dmac << 32) | ldmac;
++ streamid->para.nid.dmac = dmac;
++
++ streamid->para.nid.vid = ANA_TABLES_MACHDATA_VID_X(val);
++
++ val = ocelot_read(ocelot, ANA_TABLES_STREAMDATA);
++ if (!(val & ANA_TABLES_STREAMDATA_SFID_VALID))
++ return -EINVAL;
++
++ streamid->handle = ANA_TABLES_STREAMDATA_SFID(val);
++
++ return 0;
++}
++
++static int lookup_mactable(struct ocelot *ocelot, u16 vid, u64 mac)
++{
++ u32 mach, macl;
++ u32 reg1, reg2;
++ u32 index, bucket;
++
++ macl = mac & 0xffffffff;
++ mach = (mac >> 32) & 0xffff;
++ ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
++ ocelot_write(ocelot, ANA_TABLES_MACHDATA_VID(vid) |
++ ANA_TABLES_MACHDATA_MACHDATA(mach),
++ ANA_TABLES_MACHDATA);
++
++ ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
++ ANA_TABLES_MACACCESS);
++
++ reg1 = ocelot_read(ocelot, ANA_TABLES_MACLDATA);
++ reg2 = ocelot_read(ocelot, ANA_TABLES_MACHDATA);
++ if (reg1 == 0 && reg2 == 0)
++ return -1;
++
++ regmap_field_read(ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET],
++ &bucket);
++ regmap_field_read(ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX],
++ &index);
++
++ index = index * 4 + bucket;
++
++ return index;
++}
++
++int ocelot_cb_streamid_set(struct ocelot *ocelot, int port,
++ u32 index, bool enable,
++ struct tsn_cb_streamid *streamid)
++{
++ struct regmap_field *rf;
++ u16 vid;
++ u64 mac;
++ u32 macl, mach;
++ u32 dst_idx;
++ int idx;
++ u32 reg;
++ int sfid, ssid;
++ u32 m_index, bucket;
++
++ if (!enable) {
++ if (index >= MSCC_STREAM_HANDLE_NUM) {
++ dev_err(ocelot->dev,
++ "Invalid index %u, maximum:%u\n",
++ index, MSCC_STREAM_HANDLE_NUM - 1);
++ return -EINVAL;
++ }
++ m_index = streamhandle_map[index] / 4;
++ bucket = streamhandle_map[index] % 4;
++ rf = ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET];
++ regmap_field_write(rf, bucket);
++ rf = ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX];
++ regmap_field_write(rf, m_index);
++
++ /*READ command MACACCESS.VALID(11 bit) must be 0 */
++ ocelot_write(ocelot,
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
++ ANA_TABLES_MACACCESS);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_FORGET),
++ ANA_TABLES_MACACCESS);
++
++ streamhandle_map[index] = 0;
++
++ return 0;
++ }
++
++ if (streamid->type != 1) {
++ dev_err(ocelot->dev, "Invalid stream type\n");
++ return -EINVAL;
++ }
++
++ if (streamid->handle >= MSCC_STREAM_HANDLE_NUM) {
++ dev_err(ocelot->dev,
++ "Invalid stream handle %u, maximum:%u\n",
++ streamid->handle, MSCC_STREAM_HANDLE_NUM - 1);
++ return -EINVAL;
++ }
++
++ sfid = streamid->handle;
++ ssid = (streamid->handle < MSCC_FRER_SSID_NUM ?
++ streamid->handle : (MSCC_FRER_SSID_NUM - 1));
++
++ mac = streamid->para.nid.dmac;
++ macl = mac & 0xffffffff;
++ mach = (mac >> 32) & 0xffff;
++ vid = streamid->para.nid.vid;
++
++ idx = lookup_mactable(ocelot, vid, mac);
++
++ if (idx < 0) {
++ ocelot_write(ocelot, macl, ANA_TABLES_MACLDATA);
++ ocelot_write(ocelot, ANA_TABLES_MACHDATA_VID(vid) |
++ ANA_TABLES_MACHDATA_MACHDATA(mach),
++ ANA_TABLES_MACHDATA);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_STREAMDATA_SFID_VALID |
++ ANA_TABLES_STREAMDATA_SFID(sfid) |
++ ANA_TABLES_STREAMDATA_SSID_VALID |
++ ANA_TABLES_STREAMDATA_SSID(ssid),
++ ANA_TABLES_STREAMDATA);
++
++ dst_idx = port;
++ ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
++ ANA_TABLES_MACACCESS_ENTRYTYPE(1) |
++ ANA_TABLES_MACACCESS_DEST_IDX(dst_idx) |
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_LEARN),
++ ANA_TABLES_MACACCESS);
++
++ ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
++ ANA_TABLES_MACACCESS);
++
++ regmap_field_read(ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET],
++ &bucket);
++ regmap_field_read(ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX],
++ &m_index);
++
++ m_index = m_index * 4 + bucket;
++ streamhandle_map[streamid->handle] = m_index;
++
++ return 0;
++ }
++
++ ocelot_write(ocelot,
++ ANA_TABLES_STREAMDATA_SFID_VALID |
++ ANA_TABLES_STREAMDATA_SFID(sfid) |
++ ANA_TABLES_STREAMDATA_SSID_VALID |
++ ANA_TABLES_STREAMDATA_SSID(ssid),
++ ANA_TABLES_STREAMDATA);
++
++ reg = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
++ dst_idx = ANA_TABLES_MACACCESS_DEST_IDX_X(reg);
++ ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
++ ANA_TABLES_MACACCESS_ENTRYTYPE(1) |
++ ANA_TABLES_MACACCESS_DEST_IDX(dst_idx) |
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_WRITE),
++ ANA_TABLES_MACACCESS);
++
++ streamhandle_map[streamid->handle] = idx;
++
++ return 0;
++}
++
++static int streamid_multi_forward_set(struct ocelot *ocelot, u32 index,
++ u8 fwdmask)
++{
++ u32 m_index;
++ u32 bucket;
++ u32 val;
++ int m, n, i;
++ u8 pgid_val, fwdport;
++ u32 dst_idx;
++
++ m_index = index / 4;
++ bucket = index % 4;
++
++ regmap_field_write(ocelot->regfields[ANA_TABLES_MACTINDX_BUCKET],
++ bucket);
++ regmap_field_write(ocelot->regfields[ANA_TABLES_MACTINDX_M_INDEX],
++ m_index);
++
++ /*READ command MACACCESS.VALID(11 bit) must be 0 */
++ ocelot_write(ocelot,
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_READ),
++ ANA_TABLES_MACACCESS);
++
++ val = ocelot_read(ocelot, ANA_TABLES_MACACCESS);
++ fwdport = ANA_TABLES_MACACCESS_DEST_IDX_X(val);
++
++ if (fwdport >= MSCC_NUM_OUT_PORT) {
++ dst_idx = fwdport;
++ return 0;
++ }
++
++ fwdmask |= (1 << fwdport);
++
++ m = ocelot->num_phys_ports - 1;
++ for (i = m; i >= MSCC_NUM_OUT_PORT; i--) {
++ if (fwdmask & (1 << i)) {
++ dst_idx = PGID_MCRED +
++ (m - i) * MSCC_NUM_OUT_PORT +
++ fwdport;
++
++ pgid_val = (1 << i) | (1 << fwdport);
++ break;
++ }
++ }
++
++ if (i < MSCC_NUM_OUT_PORT) {
++ m = PGID_MCRED +
++ (ocelot->num_phys_ports - MSCC_NUM_OUT_PORT) *
++ MSCC_NUM_OUT_PORT;
++
++ for (; i > 0; i--) {
++ if (fwdmask & (1 << i))
++ break;
++
++ m = m + (1 << i) - 1;
++ }
++ n = fwdmask & ((1 << i) - 1);
++ if (n) {
++ dst_idx = m + n;
++ pgid_val = fwdmask & ((1 << MSCC_NUM_OUT_PORT) - 1);
++ } else {
++ dst_idx = fwdport;
++ }
++ }
++
++ if (dst_idx < PGID_MCRED)
++ return 0;
++
++ ocelot_write(ocelot, ANA_TABLES_MACACCESS_VALID |
++ ANA_TABLES_MACACCESS_ENTRYTYPE(1) |
++ ANA_TABLES_MACACCESS_DEST_IDX(dst_idx) |
++ ANA_TABLES_MACACCESS_MAC_TABLE_CMD(MACACCESS_CMD_WRITE),
++ ANA_TABLES_MACACCESS);
++
++ ocelot_write_rix(ocelot, pgid_val, ANA_PGID_PGID, dst_idx);
++
++ return 0;
++}
++
++int ocelot_qci_sfi_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_sfi_conf *sfi)
++{
++ u32 val, reg, fmeter_id, max_sdu;
++ u32 sfid = index;
++
++ if (sfid >= capa.num_psfp_sfid) {
++ dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
++ sfid, capa.num_psfp_sfid);
++ return -EINVAL;
++ }
++
++ ocelot_rmw(ocelot,
++ ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
++ ANA_TABLES_SFIDTIDX_SFID_INDEX_M,
++ ANA_TABLES_SFIDTIDX);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_READ),
++ ANA_TABLES_SFIDACCESS);
++
++ val = ocelot_read(ocelot, ANA_TABLES_SFIDTIDX);
++ if (!(val & ANA_TABLES_SFIDTIDX_SGID_VALID))
++ return -EINVAL;
++
++ sfi->stream_gate_instance_id = ANA_TABLES_SFIDTIDX_SGID_X(val);
++ fmeter_id = ANA_TABLES_SFIDTIDX_POL_IDX_X(val);
++ sfi->stream_filter.flow_meter_instance_id = fmeter_id;
++
++ reg = ocelot_read(ocelot, ANA_TABLES_SFIDACCESS);
++ max_sdu = ANA_TABLES_SFIDACCESS_MAX_SDU_LEN_X(reg);
++ sfi->stream_filter.maximum_sdu_size = max_sdu;
++
++ if (reg & ANA_TABLES_SFIDACCESS_IGR_PRIO_MATCH_ENA)
++ sfi->priority_spec = ANA_TABLES_SFIDACCESS_IGR_PRIO_X(reg);
++ else
++ dev_err(ocelot->dev, "priority not enable\n");
++
++ return 0;
++}
++
++int ocelot_qci_sfi_set(struct ocelot *ocelot, int port,
++ u32 index, bool enable,
++ struct tsn_qci_psfp_sfi_conf *sfi)
++{
++ int igr_prio = sfi->priority_spec;
++ u16 sgid = sfi->stream_gate_instance_id;
++ u16 pol_idx;
++ int fmid = sfi->stream_filter.flow_meter_instance_id;
++ u16 max_sdu_len = sfi->stream_filter.maximum_sdu_size;
++ int sfid = index;
++ u32 val;
++
++ if (fmid == -1)
++ pol_idx = capa.psfp_fmi_max;
++ else
++ pol_idx = (u16)fmid;
++
++ if (sfid >= capa.num_psfp_sfid) {
++ dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
++ sfid, capa.num_psfp_sfid);
++ return -EINVAL;
++ }
++
++ if (!enable) {
++ val = ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE);
++ ocelot_write(ocelot,
++ ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
++ ANA_TABLES_SFIDTIDX);
++ ocelot_write(ocelot, val, ANA_TABLES_SFIDACCESS);
++ return 0;
++ }
++
++ if (sgid >= capa.num_psfp_sgid) {
++ dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
++ sgid, capa.num_psfp_sgid);
++ return -EINVAL;
++ }
++ if (pol_idx > capa.psfp_fmi_max || pol_idx < capa.psfp_fmi_min) {
++ dev_err(ocelot->dev, "Invalid pol_idx %u, range:%d~%d\n",
++ pol_idx, capa.psfp_fmi_min, capa.psfp_fmi_max);
++ return -EINVAL;
++ }
++
++ ocelot_write(ocelot, ANA_TABLES_SFIDTIDX_SGID_VALID |
++ ANA_TABLES_SFIDTIDX_SGID(sgid) |
++ ((fmid != -1) ? ANA_TABLES_SFIDTIDX_POL_ENA : 0) |
++ ANA_TABLES_SFIDTIDX_POL_IDX(pol_idx) |
++ ANA_TABLES_SFIDTIDX_SFID_INDEX(sfid),
++ ANA_TABLES_SFIDTIDX);
++
++ ocelot_write(ocelot,
++ ((igr_prio >= 0) ?
++ ANA_TABLES_SFIDACCESS_IGR_PRIO_MATCH_ENA : 0) |
++ ANA_TABLES_SFIDACCESS_IGR_PRIO(igr_prio) |
++ ANA_TABLES_SFIDACCESS_MAX_SDU_LEN(max_sdu_len) |
++ ANA_TABLES_SFIDACCESS_SFID_TBL_CMD(SFIDACCESS_CMD_WRITE),
++ ANA_TABLES_SFIDACCESS);
++
++ return 0;
++}
++
++int ocelot_qci_sfi_counters_get(struct ocelot *ocelot, int port,
++ u32 index,
++ struct tsn_qci_psfp_sfi_counters *sfi_cnt)
++{
++ u32 sfid = index;
++ u32 match, not_pass, not_pass_sdu, red;
++
++ if (sfid >= capa.num_psfp_sfid) {
++ dev_err(ocelot->dev, "Invalid index %u, maximum:%u\n",
++ sfid, capa.num_psfp_sfid);
++ return -EINVAL;
++ }
++
++ ocelot_rmw(ocelot,
++ SYS_STAT_CFG_STAT_VIEW(sfid),
++ SYS_STAT_CFG_STAT_VIEW_M,
++ SYS_STAT_CFG);
++
++ match = ocelot_read_gix(ocelot, SYS_CNT, 0x200);
++ not_pass = ocelot_read_gix(ocelot, SYS_CNT, 0x201);
++ not_pass_sdu = ocelot_read_gix(ocelot, SYS_CNT, 0x202);
++ red = ocelot_read_gix(ocelot, SYS_CNT, 0x203);
++
++ sfi_cnt->matching_frames_count = match;
++ sfi_cnt->not_passing_frames_count = not_pass;
++ sfi_cnt->not_passing_sdu_count = not_pass_sdu;
++ sfi_cnt->red_frames_count = red;
++
++ sfi_cnt->passing_frames_count = match - not_pass;
++ sfi_cnt->passing_sdu_count = match - not_pass - not_pass_sdu;
++
++ return 0;
++}
++
++int ocelot_qci_max_cap_get(struct ocelot *ocelot,
++ struct tsn_qci_psfp_stream_param *stream_para)
++{
++ /* MaxStreamFilterInstances */
++ stream_para->max_sf_instance = capa.num_psfp_sfid;
++ /* MaxStreamGateInstances */
++ stream_para->max_sg_instance = capa.num_psfp_sgid;
++ /* MaxFlowMeterInstances */
++ stream_para->max_fm_instance = capa.psfp_fmi_max -
++ capa.psfp_fmi_min + 1;
++ /* SupportedListMax */
++ stream_para->supported_list_max = capa.num_sgi_gcl;
++
++ return 0;
++}
++
++static int sgi_set_glist(struct ocelot *ocelot,
++ struct tsn_qci_psfp_gcl *gcl, uint32_t num)
++{
++ u32 time_sum = 0;
++ int i;
++
++ if (num > capa.num_sgi_gcl)
++ return -EINVAL;
++
++ for (i = 0; i < num; i++) {
++ u32 val = ANA_SG_GCL_GS_CONFIG_IPS((gcl->ipv < 0) ?
++ 0 : gcl->ipv + 8);
++ val |= (gcl->gate_state ? ANA_SG_GCL_GS_CONFIG_GATE_STATE : 0);
++ ocelot_write_rix(ocelot, val, ANA_SG_GCL_GS_CONFIG, i);
++
++ time_sum += gcl->time_interval;
++ ocelot_write_rix(ocelot, time_sum, ANA_SG_GCL_TI_CONFIG, i);
++
++ gcl++;
++ }
++
++ return 0;
++}
++
++static u32 sgi_read_status(struct ocelot *ocelot)
++{
++ u32 val;
++
++ val = ocelot_read(ocelot, ANA_SG_ACCESS_CTRL);
++
++ return val;
++}
++
++int ocelot_qci_sgi_set(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_sgi_conf *sgi_conf)
++{
++ struct tsn_qci_sg_control *admin_list = &sgi_conf->admin;
++ u32 sgid = index;
++ u32 list_length = sgi_conf->admin.control_list_length;
++ u32 cycle_time = sgi_conf->admin.cycle_time;
++ u32 cycle_time_ex = sgi_conf->admin.cycle_time_extension;
++ u32 l_basetime = sgi_conf->admin.base_time % 1000000000;
++ u64 h_basetime = sgi_conf->admin.base_time / 1000000000;
++ u64 cur_time;
++ u32 val;
++ int ret;
++
++ if (sgid >= capa.num_psfp_sgid) {
++ dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
++ sgid, capa.num_psfp_sgid);
++ return -EINVAL;
++ }
++ if ((cycle_time < capa.sgi_ct_min ||
++ cycle_time > capa.sgi_ct_max) &&
++ sgi_conf->gate_enabled) {
++ dev_err(ocelot->dev, "Invalid cycle_time %u ns\n",
++ cycle_time);
++ return -EINVAL;
++ }
++ if (cycle_time_ex > capa.sgi_cte_max) {
++ dev_err(ocelot->dev,
++ "Invalid cycle_time_extension %u\n",
++ cycle_time_ex);
++ return -EINVAL;
++ }
++ if (list_length > capa.num_sgi_gcl) {
++ dev_err(ocelot->dev,
++ "Invalid sgi_gcl len %u, maximum:%u\n",
++ list_length, capa.num_sgi_gcl);
++ return -EINVAL;
++ }
++
++ /*configure SGID*/
++ ocelot_rmw(ocelot,
++ ANA_SG_ACCESS_CTRL_SGID(sgid),
++ ANA_SG_ACCESS_CTRL_SGID_M,
++ ANA_SG_ACCESS_CTRL);
++
++ /*Disable SG*/
++ if (!sgi_conf->gate_enabled) {
++ ocelot_rmw(ocelot,
++ ANA_SG_CONFIG_REG_3_INIT_GATE_STATE,
++ ANA_SG_CONFIG_REG_3_INIT_GATE_STATE |
++ ANA_SG_CONFIG_REG_3_GATE_ENABLE,
++ ANA_SG_CONFIG_REG_3);
++ return 0;
++ }
++
++ /*admin parameters*/
++ cur_time = ocelot_read(ocelot, PTP_CUR_SEC_MSB);
++ cur_time = cur_time << 32;
++ cur_time += ocelot_read(ocelot, PTP_CUR_SEC_LSB);
++ if (h_basetime < cur_time) {
++ h_basetime = cur_time;
++ l_basetime = ocelot_read(ocelot, PTP_CUR_NSEC);
++ }
++
++ ocelot_write(ocelot, l_basetime, ANA_SG_CONFIG_REG_1);
++ ocelot_write(ocelot, h_basetime, ANA_SG_CONFIG_REG_2);
++
++ ocelot_write(ocelot,
++ (sgi_conf->admin.init_ipv < 0 ?
++ 0 : ANA_SG_CONFIG_REG_3_IPV_VALID) |
++ ANA_SG_CONFIG_REG_3_INIT_IPV(sgi_conf->admin.init_ipv) |
++ ANA_SG_CONFIG_REG_3_GATE_ENABLE |
++ ANA_SG_CONFIG_REG_3_LIST_LENGTH(list_length) |
++ (sgi_conf->admin.gate_states > 0 ?
++ ANA_SG_CONFIG_REG_3_INIT_GATE_STATE : 0) |
++ ANA_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB(h_basetime >> 32),
++ ANA_SG_CONFIG_REG_3);
++
++ ocelot_write(ocelot, cycle_time, ANA_SG_CONFIG_REG_4);
++ ocelot_write(ocelot, cycle_time_ex, ANA_SG_CONFIG_REG_5);
++
++ ret = sgi_set_glist(ocelot, admin_list->gcl, list_length);
++ if (ret < 0)
++ return ret;
++
++ /* Start configuration change */
++ ocelot_rmw(ocelot,
++ ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
++ ANA_SG_ACCESS_CTRL_CONFIG_CHANGE,
++ ANA_SG_ACCESS_CTRL);
++
++ ret = readx_poll_timeout(sgi_read_status, ocelot, val,
++ (!(ANA_SG_ACCESS_CTRL_CONFIG_CHANGE & val)),
++ 10, 100000);
++
++ return ret;
++}
++
++static int sgi_get_glist(struct ocelot *ocelot,
++ struct tsn_qci_psfp_gcl *gcl,
++ uint32_t num)
++{
++ int i;
++ u16 val;
++ u32 time = 0;
++ u32 reg;
++
++ if (num > capa.num_sgi_gcl)
++ return -EINVAL;
++
++ for (i = 0; i < num; i++) {
++ val = ocelot_read_rix(ocelot, ANA_SG_GCL_GS_CONFIG, i);
++ gcl->gate_state = (val & ANA_SG_GCL_GS_CONFIG_GATE_STATE);
++
++ if (val & ANA_SG_GCL_GS_CONFIG_IPV_VALID)
++ gcl->ipv = ANA_SG_GCL_GS_CONFIG_IPV(val);
++ else
++ gcl->ipv = -1;
++
++ reg = ocelot_read_rix(ocelot, ANA_SG_GCL_TI_CONFIG, i);
++ gcl->time_interval = (reg - time);
++ time = reg;
++
++ gcl++;
++ }
++
++ return 0;
++}
++
++int ocelot_qci_sgi_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_sgi_conf *sgi_conf)
++{
++ struct tsn_qci_sg_control *admin = &sgi_conf->admin;
++ struct tsn_qci_psfp_gcl *glist;
++ u32 val, reg;
++ u32 list_num;
++ int ret;
++
++ if (index >= capa.num_psfp_sgid) {
++ dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
++ index, capa.num_psfp_sgid);
++ return -EINVAL;
++ }
++
++ ocelot_rmw(ocelot,
++ ANA_SG_ACCESS_CTRL_SGID(index),
++ ANA_SG_ACCESS_CTRL_SGID_M,
++ ANA_SG_ACCESS_CTRL);
++
++ admin->cycle_time = ocelot_read(ocelot, ANA_SG_CONFIG_REG_4);
++ admin->cycle_time_extension =
++ ocelot_read(ocelot, ANA_SG_CONFIG_REG_5);
++
++ val = ocelot_read(ocelot, ANA_SG_CONFIG_REG_2);
++ admin->base_time = val;
++
++ reg = ocelot_read(ocelot, ANA_SG_CONFIG_REG_1);
++ val = ocelot_read(ocelot, ANA_SG_CONFIG_REG_3);
++
++ admin->base_time +=
++ ANA_SG_CONFIG_REG_3_BASE_TIME_SEC_MSB(val) << 32;
++
++ admin->base_time = admin->base_time * 1000000000 + reg;
++
++ if (val & ANA_SG_CONFIG_REG_3_IPV_VALID)
++ admin->init_ipv = ANA_SG_CONFIG_REG_3_INIT_IPV_X(val);
++ else
++ admin->init_ipv = -1;
++
++ if (val & ANA_SG_CONFIG_REG_3_GATE_ENABLE)
++ sgi_conf->gate_enabled = TRUE;
++
++ admin->control_list_length = ANA_SG_CONFIG_REG_3_LIST_LENGTH_X(val);
++
++ list_num = admin->control_list_length;
++
++ glist = kmalloc_array(list_num, sizeof(struct tsn_qci_psfp_gcl),
++ GFP_KERNEL);
++ admin->gcl = glist;
++
++ ret = sgi_get_glist(ocelot, glist, list_num);
++
++ return ret;
++}
++
++int ocelot_qci_sgi_status_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_psfp_sgi_status *sgi_status)
++{
++ u32 val, reg;
++
++ if (index >= capa.num_psfp_sgid) {
++ dev_err(ocelot->dev, "Invalid sgid %u, maximum:%u\n",
++ index, capa.num_psfp_sgid);
++ return -EINVAL;
++ }
++
++ ocelot_rmw(ocelot,
++ ANA_SG_ACCESS_CTRL_SGID(index),
++ ANA_SG_ACCESS_CTRL_SGID_M,
++ ANA_SG_ACCESS_CTRL);
++
++ val = ocelot_read(ocelot, ANA_SG_STATUS_REG_2);
++ sgi_status->config_change_time = val;
++
++ reg = ocelot_read(ocelot, ANA_SG_STATUS_REG_1);
++ val = ocelot_read(ocelot, ANA_SG_STATUS_REG_3);
++ sgi_status->config_change_time +=
++ ANA_SG_STATUS_REG_3_CFG_CHG_TIME_SEC_MSB(val) << 32;
++ sgi_status->config_change_time =
++ sgi_status->config_change_time * 1000000000 + reg;
++
++ if (val & ANA_SG_STATUS_REG_3_CONFIG_PENDING)
++ sgi_status->config_pending = TRUE;
++ else
++ sgi_status->config_pending = FALSE;
++
++ if (val & ANA_SG_STATUS_REG_3_GATE_STATE)
++ sgi_status->oper.gate_states = TRUE;
++ else
++ sgi_status->oper.gate_states = FALSE;
++ /*bit 3 encoding 0:IPV [0:2]is invalid . 1:IPV[0:2] is valid*/
++ if (val & ANA_SG_STATUS_REG_3_IPV_VALID)
++ sgi_status->oper.init_ipv = ANA_SG_STATUS_REG_3_IPV_X(val);
++ else
++ sgi_status->oper.init_ipv = -1;
++
++ return 0;
++}
++
++int ocelot_qci_fmi_set(struct ocelot *ocelot, int port, u32 index,
++ bool enable, struct tsn_qci_psfp_fmi *fmi)
++{
++ u32 cir = 0, cbs = 0, pir = 0, pbs = 0;
++ u32 cir_ena = 0;
++ u32 pbs_max = 0, cbs_max = 0;
++ bool cir_discard = 0, pir_discard = 0;
++
++ if (index > capa.qos_pol_max) {
++ dev_err(ocelot->dev, "Invalid pol_idx %u, maximum: %u\n",
++ index, capa.qos_pol_max);
++ return -EINVAL;
++ }
++
++ if (fmi->mark_red_enable && fmi->mark_red) {
++ fmi->eir = 0;
++ fmi->ebs = 0;
++ fmi->cir = 0;
++ fmi->cbs = 0;
++ }
++
++ pir = fmi->eir;
++ pbs = fmi->ebs;
++
++ if (!fmi->drop_on_yellow)
++ cir_ena = 1;
++
++ if (cir_ena) {
++ cir = fmi->cir;
++ cbs = fmi->cbs;
++ if (cir == 0 && cbs == 0) {
++ cir_discard = 1;
++ } else {
++ cir = DIV_ROUND_UP(cir, 100);
++ cir *= 3; /* Rate unit is 33 1/3 kbps */
++ cbs = DIV_ROUND_UP(cbs, 4096);
++ cbs = (cbs ? cbs : 1);
++ cbs_max = capa.pol_cbs_max;
++ if (fmi->cf)
++ pir += fmi->cir;
++ }
++ }
++
++ if (pir == 0 && pbs == 0) {
++ pir_discard = 1;
++ } else {
++ pir = DIV_ROUND_UP(pir, 100);
++ pir *= 3; /* Rate unit is 33 1/3 kbps */
++ pbs = DIV_ROUND_UP(pbs, 4096);
++ pbs = (pbs ? pbs : 1);
++ pbs_max = capa.pol_pbs_max;
++ }
++ pir = min_t(u32, GENMASK(15, 0), pir);
++ cir = min_t(u32, GENMASK(15, 0), cir);
++ pbs = min(pbs_max, pbs);
++ cbs = min(cbs_max, cbs);
++
++ ocelot_write_gix(ocelot, (ANA_POL_MODE_CFG_IPG_SIZE(20) |
++ ANA_POL_MODE_CFG_FRM_MODE(1) |
++ (fmi->cf ? ANA_POL_MODE_CFG_DLB_COUPLED : 0) |
++ (cir_ena ? ANA_POL_MODE_CFG_CIR_ENA : 0) |
++ ANA_POL_MODE_CFG_OVERSHOOT_ENA),
++ ANA_POL_MODE_CFG, index);
++
++ ocelot_write_gix(ocelot, ANA_POL_PIR_CFG_PIR_RATE(pir) |
++ ANA_POL_PIR_CFG_PIR_BURST(pbs),
++ ANA_POL_PIR_CFG, index);
++
++ ocelot_write_gix(ocelot,
++ (pir_discard ? GENMASK(22, 0) : 0),
++ ANA_POL_PIR_STATE, index);
++
++ ocelot_write_gix(ocelot, ANA_POL_CIR_CFG_CIR_RATE(cir) |
++ ANA_POL_CIR_CFG_CIR_BURST(cbs),
++ ANA_POL_CIR_CFG, index);
++
++ ocelot_write_gix(ocelot,
++ (cir_discard ? GENMASK(22, 0) : 0),
++ ANA_POL_CIR_STATE, index);
++
++ return 0;
++}
++
++int ocelot_qci_fmi_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_fmi *fmi,
++ struct tsn_qci_psfp_fmi_counters *counters)
++{
++ u32 val, reg;
++
++ if (index > capa.qos_pol_max) {
++ dev_err(ocelot->dev, "Invalid pol_idx %u, maximum: %u\n",
++ index, capa.qos_pol_max);
++ return -EINVAL;
++ }
++
++ val = ocelot_read_gix(ocelot, ANA_POL_PIR_CFG, index);
++ reg = ocelot_read_gix(ocelot, ANA_POL_CIR_CFG, index);
++
++ fmi->eir = ANA_POL_PIR_CFG_PIR_RATE_X(val);
++ fmi->eir = fmi->eir * 100 / 3;
++ fmi->ebs = ANA_POL_PIR_CFG_PIR_BURST(val);
++ fmi->ebs *= 4096;
++ fmi->cir = ANA_POL_CIR_CFG_CIR_RATE_X(reg);
++ fmi->cir = fmi->cir * 100 / 3;
++ fmi->cbs = ANA_POL_CIR_CFG_CIR_BURST(reg);
++ fmi->cbs *= 4096;
++ if (!(fmi->eir | fmi->ebs | fmi->cir | fmi->cbs))
++ fmi->mark_red = TRUE;
++ else
++ fmi->mark_red = FALSE;
++
++ val = ocelot_read_gix(ocelot, ANA_POL_MODE_CFG, index);
++ if (val & ANA_POL_MODE_CFG_DLB_COUPLED)
++ fmi->cf = TRUE;
++ else
++ fmi->cf = FALSE;
++ if (val & ANA_POL_MODE_CFG_CIR_ENA)
++ fmi->drop_on_yellow = FALSE;
++ else
++ fmi->drop_on_yellow = TRUE;
++
++ return 0;
++}
++
++int ocelot_seq_gen_set(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_seq_gen_conf *sg_conf)
++{
++ u8 iport_mask = sg_conf->iport_mask;
++ u8 split_mask = sg_conf->split_mask;
++ u8 seq_len = sg_conf->seq_len;
++ u32 seq_num = sg_conf->seq_num;
++
++ if (index >= capa.num_frer_ssid) {
++ dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
++ index, capa.num_frer_ssid - 1);
++ return -EINVAL;
++ }
++ if (seq_len < capa.frer_seq_len_min ||
++ seq_len > capa.frer_seq_len_max) {
++ dev_err(ocelot->dev,
++ "Invalid seq_space_bits num %u,range:%d~%d\n",
++ seq_len,
++ capa.frer_seq_len_min,
++ capa.frer_seq_len_max);
++ return -EINVAL;
++ }
++
++ streamid_multi_forward_set(ocelot,
++ streamhandle_map[index],
++ split_mask);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_SEQ_MASK_SPLIT_MASK(split_mask) |
++ ANA_TABLES_SEQ_MASK_INPUT_PORT_MASK(iport_mask),
++ ANA_TABLES_SEQ_MASK);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_STREAMTIDX_S_INDEX(index) |
++ ANA_TABLES_STREAMTIDX_STREAM_SPLIT |
++ ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(seq_len),
++ ANA_TABLES_STREAMTIDX);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_STREAMACCESS_GEN_REC_SEQ_NUM(seq_num) |
++ ANA_TABLES_STREAMACCESS_SEQ_GEN_REC_ENA |
++ ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_WRITE),
++ ANA_TABLES_STREAMACCESS);
++
++ return 0;
++}
++
++int ocelot_seq_rec_set(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_seq_rec_conf *sr_conf)
++{
++ u8 seq_len = sr_conf->seq_len;
++ u8 hislen = sr_conf->his_len;
++
++ if (index >= capa.num_frer_ssid) {
++ dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
++ index, capa.num_frer_ssid - 1);
++ return -EINVAL;
++ }
++ if (seq_len < capa.frer_seq_len_min ||
++ seq_len > capa.frer_seq_len_max) {
++ dev_err(ocelot->dev,
++ "Invalid seq_space_bits num %u,range:%d~%d\n",
++ seq_len,
++ capa.frer_seq_len_min,
++ capa.frer_seq_len_max);
++ return -EINVAL;
++ }
++ if (hislen < capa.frer_his_len_min ||
++ hislen > capa.frer_his_len_max) {
++ dev_err(ocelot->dev,
++ "Invalid history_bits num %u,range:%d~%d\n",
++ hislen,
++ capa.frer_his_len_min,
++ capa.frer_his_len_max);
++ return -EINVAL;
++ }
++
++ ocelot_write(ocelot,
++ ANA_TABLES_STREAMTIDX_S_INDEX(index) |
++ ANA_TABLES_STREAMTIDX_FORCE_SF_BEHAVIOUR |
++ ANA_TABLES_STREAMTIDX_SEQ_HISTORY_LEN(hislen) |
++ ANA_TABLES_STREAMTIDX_RESET_ON_ROGUE |
++ (sr_conf->rtag_pop_en ?
++ ANA_TABLES_STREAMTIDX_REDTAG_POP : 0) |
++ ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(seq_len),
++ ANA_TABLES_STREAMTIDX);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_STREAMACCESS_SEQ_GEN_REC_ENA |
++ ANA_TABLES_STREAMACCESS_GEN_REC_TYPE |
++ ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_WRITE),
++ ANA_TABLES_STREAMACCESS);
++
++ return 0;
++}
++
++int ocelot_cb_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_cb_status *c)
++{
++ u32 val;
++
++ if (index >= capa.num_frer_ssid) {
++ dev_err(ocelot->dev, "Invalid SSID %u, maximum:%u\n",
++ index, capa.num_frer_ssid - 1);
++ return -EINVAL;
++ }
++
++ ocelot_write(ocelot,
++ ANA_TABLES_STREAMTIDX_S_INDEX(index),
++ ANA_TABLES_STREAMTIDX);
++
++ ocelot_write(ocelot,
++ ANA_TABLES_STREAMACCESS_STREAM_TBL_CMD(SFIDACCESS_CMD_READ),
++ ANA_TABLES_STREAMACCESS);
++
++ val = ocelot_read(ocelot, ANA_TABLES_STREAMACCESS);
++ c->gen_rec = (ANA_TABLES_STREAMACCESS_GEN_REC_TYPE & val) >> 2;
++ c->seq_num = ANA_TABLES_STREAMACCESS_GEN_REC_SEQ_NUM_X(val);
++
++ val = ocelot_read(ocelot, ANA_TABLES_STREAMTIDX);
++ c->err = ANA_TABLES_STREAMTIDX_SEQ_GEN_ERR_STATUS_X(val);
++ c->his_len = ANA_TABLES_STREAMTIDX_SEQ_HISTORY_LEN_X(val);
++ c->seq_len = ANA_TABLES_STREAMTIDX_SEQ_SPACE_LOG2(val);
++
++ val = ocelot_read(ocelot, ANA_TABLES_SEQ_MASK);
++ c->split_mask = ANA_TABLES_SEQ_MASK_SPLIT_MASK_X(val);
++ c->iport_mask = ANA_TABLES_SEQ_MASK_INPUT_PORT_MASK(val);
++
++ c->seq_his = ocelot_read(ocelot, ANA_TABLES_SEQ_HISTORY);
++
++ return 0;
++}
++
++int ocelot_pcp_map_enable(struct ocelot *ocelot, u8 port)
++{
++ int i;
++
++ ocelot_rmw_gix(ocelot,
++ ANA_PORT_QOS_CFG_QOS_PCP_ENA,
++ ANA_PORT_QOS_CFG_QOS_PCP_ENA,
++ ANA_PORT_QOS_CFG,
++ port);
++
++ for (i = 0; i < NUM_MSCC_QOS_PRIO * 2; i++) {
++ ocelot_rmw_ix(ocelot,
++ (ANA_PORT_PCP_DEI_MAP_DP_PCP_DEI_VAL & i) |
++ ANA_PORT_PCP_DEI_MAP_QOS_PCP_DEI_VAL(i),
++ ANA_PORT_PCP_DEI_MAP_DP_PCP_DEI_VAL |
++ ANA_PORT_PCP_DEI_MAP_QOS_PCP_DEI_VAL_M,
++ ANA_PORT_PCP_DEI_MAP,
++ port, i);
++ }
++
++ return 0;
++}
++
++int ocelot_rtag_parse_enable(struct ocelot *ocelot, u8 port)
++{
++ ocelot_rmw_rix(ocelot,
++ ANA_PORT_MODE_REDTAG_PARSE_CFG,
++ ANA_PORT_MODE_REDTAG_PARSE_CFG,
++ ANA_PORT_MODE,
++ port);
++
++ return 0;
++}
++
++int ocelot_dscp_set(struct ocelot *ocelot, int port,
++ bool enable, const u8 dscp_ix,
++ struct tsn_qos_switch_dscp_conf *c)
++{
++ u32 val, ri = dscp_ix;
++
++ c->dscp = 0;
++ c->trust = 1;
++ c->remark = 0;
++
++ if (dscp_ix > capa.qos_dscp_max) {
++ dev_err(ocelot->dev, "Invalid dscp_ix %u\n", dscp_ix);
++ return -EINVAL;
++ }
++ if (c->cos > capa.qos_cos_max) {
++ dev_err(ocelot->dev, "Invalid cos %d\n", c->cos);
++ return -EINVAL;
++ }
++ if (c->dpl > capa.qos_dp_max) {
++ dev_err(ocelot->dev, "Invalid dpl %d\n", c->dpl);
++ return -EINVAL;
++ }
++
++ ocelot_rmw_gix(ocelot,
++ (enable ? ANA_PORT_QOS_CFG_QOS_DSCP_ENA : 0) |
++ (c->dscp ? ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA : 0),
++ ANA_PORT_QOS_CFG_QOS_DSCP_ENA |
++ ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA,
++ ANA_PORT_QOS_CFG,
++ port);
++
++ val = (c->dpl ? ANA_DSCP_CFG_DP_DSCP_VAL : 0) |
++ ANA_DSCP_CFG_QOS_DSCP_VAL(c->cos) |
++ ANA_DSCP_CFG_DSCP_TRANSLATE_VAL(c->dscp) |
++ (c->trust ? ANA_DSCP_CFG_DSCP_TRUST_ENA : 0) |
++ (c->remark ? ANA_DSCP_CFG_DSCP_REWR_ENA : 0);
++
++ ocelot_write_rix(ocelot, val, ANA_DSCP_CFG, ri);
++
++ return 0;
++}
+--- /dev/null
++++ b/drivers/net/ethernet/mscc/ocelot_tsn.h
+@@ -0,0 +1,51 @@
++/* SPDX-License-Identifier: (GPL-2.0 OR MIT)
++ *
++ * TSN_SWITCH driver
++ *
++ * Copyright 2018-2019 NXP
++ */
++
++#ifndef _MSCC_OCELOT_SWITCH_TSN_H_
++#define _MSCC_OCELOT_SWITCH_TSN_H_
++
++#define TRUE 1
++#define FALSE 0
++
++struct mscc_switch_capa {
++ u8 num_tas_gcl; /* Number of TAS Gate Control Lists */
++ u32 tas_ct_min; /* Minimum supported TAS CycleTime in nS */
++ u32 tas_ct_max; /* Maximum supported TAS CycleTime in nS */
++ u32 tas_cte_max; /* Maximum supported TAS CycleTimeExtension in nS
++ */
++ u32 tas_it_max;
++ u32 tas_it_min;
++ u8 num_hsch;
++ u8 num_psfp_sfid;
++ u8 num_frer_ssid;
++ u8 num_psfp_sgid;
++ u16 psfp_fmi_max;
++ u16 psfp_fmi_min;
++ u8 num_sgi_gcl;
++ u32 sgi_ct_min;
++ u32 sgi_ct_max;
++ u32 sgi_cte_max;
++ u16 qos_pol_max;
++ u8 pol_cbs_max;
++ u8 pol_pbs_max;
++ u8 frer_seq_len_min;
++ u8 frer_seq_len_max;
++ u8 frer_his_len_min;
++ u8 frer_his_len_max;
++ u8 qos_dscp_max;
++ u8 qos_cos_max;
++ u8 qos_dp_max;
++};
++
++static inline void ocelot_port_rmwl(struct ocelot_port *port, u32 val,
++ u32 mask, u32 reg)
++{
++ u32 cur = ocelot_port_readl(port, reg);
++
++ ocelot_port_writel(port, (cur & (~mask)) | val, reg);
++}
++#endif
+--- a/include/soc/mscc/ocelot.h
++++ b/include/soc/mscc/ocelot.h
+@@ -10,6 +10,7 @@
+ #include <linux/if_vlan.h>
+ #include <linux/regmap.h>
+ #include <net/dsa.h>
++#include <net/tsn.h>
+
+ #define IFH_INJ_BYPASS BIT(31)
+ #define IFH_INJ_POP_CNT_DISABLE (3 << 28)
+@@ -328,6 +329,10 @@ enum ocelot_reg {
+ PTP_CFG_MISC,
+ PTP_CLK_CFG_ADJ_CFG,
+ PTP_CLK_CFG_ADJ_FREQ,
++ PTP_CUR_NSF,
++ PTP_CUR_NSEC,
++ PTP_CUR_SEC_LSB,
++ PTP_CUR_SEC_MSB,
+ GCB_SOFT_RST = GCB << TARGET_OFFSET,
+ };
+
+@@ -539,5 +544,50 @@ int ocelot_ptp_gettime64(struct ptp_cloc
+ int ocelot_port_add_txtstamp_skb(struct ocelot_port *ocelot_port,
+ struct sk_buff *skb);
+ void ocelot_get_txtstamp(struct ocelot *ocelot);
+-
++int ocelot_qbv_set(struct ocelot *ocelot, int port_id,
++ struct tsn_qbv_conf *shaper_config);
++int ocelot_qbv_get(struct ocelot *ocelot, int port_id,
++ struct tsn_qbv_conf *shaper_config);
++int ocelot_qbv_get_status(struct ocelot *ocelot, int port_id,
++ struct tsn_qbv_status *qbvstatus);
++int ocelot_cut_thru_set(struct ocelot *ocelot, int port_id, u8 cut_thru);
++int ocelot_cbs_set(struct ocelot *ocelot, int port, u8 tc, u8 bw);
++int ocelot_cbs_get(struct ocelot *ocelot, int port, u8 tc);
++int ocelot_qbu_set(struct ocelot *ocelot, int port, u8 preemptible);
++int ocelot_qbu_get(struct ocelot *ocelot, int port,
++ struct tsn_preempt_status *c);
++int ocelot_cb_streamid_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_cb_streamid *streamid);
++int ocelot_cb_streamid_set(struct ocelot *ocelot, int port, u32 index,
++ bool enable, struct tsn_cb_streamid *streamid);
++int ocelot_qci_sfi_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_sfi_conf *sfi);
++int ocelot_qci_sfi_set(struct ocelot *ocelot, int port, u32 index,
++ bool enable, struct tsn_qci_psfp_sfi_conf *sfi);
++int ocelot_qci_sfi_counters_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_sfi_counters *sfi_counters);
++int ocelot_qci_max_cap_get(struct ocelot *ocelot,
++ struct tsn_qci_psfp_stream_param *stream_para);
++int ocelot_qci_sgi_set(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_sgi_conf *sgi_conf);
++int ocelot_qci_sgi_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_sgi_conf *sgi_conf);
++int ocelot_qci_sgi_status_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_psfp_sgi_status *sgi_status);
++int ocelot_qci_fmi_set(struct ocelot *ocelot, int port, u32 index,
++ bool enable, struct tsn_qci_psfp_fmi *fmi);
++int ocelot_qci_fmi_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_qci_psfp_fmi *fmi,
++ struct tsn_qci_psfp_fmi_counters *counters);
++int ocelot_seq_gen_set(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_seq_gen_conf *sg_conf);
++int ocelot_seq_rec_set(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_seq_rec_conf *sr_conf);
++int ocelot_cb_get(struct ocelot *ocelot, int port, u32 index,
++ struct tsn_cb_status *c);
++int ocelot_pcp_map_enable(struct ocelot *ocelot, u8 port);
++int ocelot_rtag_parse_enable(struct ocelot *ocelot, u8 port);
++int ocelot_dscp_set(struct ocelot *ocelot, int port,
++ bool enable, const u8 dscp_ix,
++ struct tsn_qos_switch_dscp_conf *c);
+ #endif