ASR_BASE
Change-Id: Icf3719cc0afe3eeb3edc7fa80a2eb5199ca9dda1
diff --git a/target/linux/layerscape/patches-5.4/701-net-0337-enetc-add-support-tsn-capabilities-qbv-qci-qbu-cbs.patch b/target/linux/layerscape/patches-5.4/701-net-0337-enetc-add-support-tsn-capabilities-qbv-qci-qbu-cbs.patch
new file mode 100644
index 0000000..dddb32a
--- /dev/null
+++ b/target/linux/layerscape/patches-5.4/701-net-0337-enetc-add-support-tsn-capabilities-qbv-qci-qbu-cbs.patch
@@ -0,0 +1,2911 @@
+From bf3f81f3773cc9f6b273d769aca96512780c6189 Mon Sep 17 00:00:00 2001
+From: Po Liu <Po.Liu@nxp.com>
+Date: Tue, 3 Dec 2019 16:52:57 +0800
+Subject: [PATCH] enetc: add support tsn capabilities qbv/qci/qbu/cbs
+
+Support Qbv/Qci/Qbu/Credit Base Shaper etc.
+This patch using the generic netlink adapt layer driver net/tsn/*
+and include/net/tsn.h interface load by user space. The user space
+refer the include/uapi/linux/tsn.h.
+
+Signed-off-by: Po Liu <Po.Liu@nxp.com>
+---
+ drivers/net/ethernet/freescale/enetc/Kconfig | 10 +
+ drivers/net/ethernet/freescale/enetc/Makefile | 1 +
+ drivers/net/ethernet/freescale/enetc/enetc.c | 13 +-
+ drivers/net/ethernet/freescale/enetc/enetc.h | 38 +
+ .../net/ethernet/freescale/enetc/enetc_ethtool.c | 59 +
+ drivers/net/ethernet/freescale/enetc/enetc_hw.h | 438 ++++-
+ drivers/net/ethernet/freescale/enetc/enetc_pf.c | 15 +-
+ drivers/net/ethernet/freescale/enetc/enetc_tsn.c | 2049 ++++++++++++++++++++
+ 8 files changed, 2614 insertions(+), 9 deletions(-)
+ create mode 100644 drivers/net/ethernet/freescale/enetc/enetc_tsn.c
+
+--- a/drivers/net/ethernet/freescale/enetc/Kconfig
++++ b/drivers/net/ethernet/freescale/enetc/Kconfig
+@@ -60,3 +60,13 @@ config FSL_ENETC_QOS
+ enable/disable from user space via Qos commands(tc). In the kernel
+ side, it can be loaded by Qos driver. Currently, it is only support
+ taprio(802.1Qbv) and Credit Based Shaper(802.1Qbu).
++
++config ENETC_TSN
++ bool "TSN Support for NXP ENETC driver"
++ default n
++ depends on TSN && FSL_ENETC
++ help
++ This driver supports TSN on Freescale ENETC driver. Provide
++ interface to config the tsn capabilities of ENETC. The interface link
++ to the /net/tsn/* and include/net/tsn.h. User space refer the
++ include/uapi/linux/tsn.h.
+--- a/drivers/net/ethernet/freescale/enetc/Makefile
++++ b/drivers/net/ethernet/freescale/enetc/Makefile
+@@ -6,6 +6,7 @@ obj-$(CONFIG_FSL_ENETC) += fsl-enetc.o
+ fsl-enetc-y := enetc_pf.o enetc_mdio.o $(common-objs)
+ fsl-enetc-$(CONFIG_PCI_IOV) += enetc_msg.o
+ fsl-enetc-$(CONFIG_FSL_ENETC_QOS) += enetc_qos.o
++fsl-enetc-$(CONFIG_ENETC_TSN) += enetc_tsn.o
+
+ obj-$(CONFIG_FSL_ENETC_VF) += fsl-enetc-vf.o
+ fsl-enetc-vf-y := enetc_vf.o $(common-objs)
+--- a/drivers/net/ethernet/freescale/enetc/enetc.c
++++ b/drivers/net/ethernet/freescale/enetc/enetc.c
+@@ -145,7 +145,8 @@ static int enetc_map_tx_buffs(struct ene
+ do_tstamp = (active_offloads & ENETC_F_TX_TSTAMP) &&
+ (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP);
+ tx_swbd->do_tstamp = do_tstamp;
+- tx_swbd->check_wb = tx_swbd->do_tstamp;
++ tx_swbd->qbv_en = !!(active_offloads & ENETC_F_QBV);
++ tx_swbd->check_wb = tx_swbd->do_tstamp || tx_swbd->qbv_en;
+
+ if (do_vlan || do_tstamp)
+ flags |= ENETC_TXBD_FLAGS_EX;
+@@ -342,7 +343,7 @@ static void enetc_tstamp_tx(struct sk_bu
+ static bool enetc_clean_tx_ring(struct enetc_bdr *tx_ring, int napi_budget)
+ {
+ struct net_device *ndev = tx_ring->ndev;
+- int tx_frm_cnt = 0, tx_byte_cnt = 0;
++ int tx_frm_cnt = 0, tx_byte_cnt = 0, tx_win_drop = 0;
+ struct enetc_tx_swbd *tx_swbd;
+ int i, bds_to_clean;
+ bool do_tstamp;
+@@ -372,6 +373,10 @@ static bool enetc_clean_tx_ring(struct e
+ &tstamp);
+ do_tstamp = true;
+ }
++
++ if (tx_swbd->qbv_en &&
++ txbd->wb.status & ENETC_TXBD_STATS_WIN)
++ tx_win_drop++;
+ }
+
+ if (likely(tx_swbd->dma))
+@@ -415,6 +420,7 @@ static bool enetc_clean_tx_ring(struct e
+ tx_ring->next_to_clean = i;
+ tx_ring->stats.packets += tx_frm_cnt;
+ tx_ring->stats.bytes += tx_byte_cnt;
++ tx_ring->stats.win_drop += tx_win_drop;
+
+ if (unlikely(tx_frm_cnt && netif_carrier_ok(ndev) &&
+ __netif_subqueue_stopped(ndev, tx_ring->index) &&
+@@ -778,6 +784,9 @@ void enetc_get_si_caps(struct enetc_si *
+
+ if (val & ENETC_SIPCAPR0_QBV)
+ si->hw_features |= ENETC_SI_F_QBV;
++
++ if (val & ENETC_SIPCAPR0_QBU)
++ si->hw_features |= ENETC_SI_F_QBU;
+ }
+
+ static int enetc_dma_alloc_bdr(struct enetc_bdr *r, size_t bd_size)
+--- a/drivers/net/ethernet/freescale/enetc/enetc.h
++++ b/drivers/net/ethernet/freescale/enetc/enetc.h
+@@ -10,6 +10,7 @@
+ #include <linux/ethtool.h>
+ #include <linux/if_vlan.h>
+ #include <linux/phy.h>
++#include <net/tsn.h>
+
+ #include "enetc_hw.h"
+
+@@ -24,6 +25,7 @@ struct enetc_tx_swbd {
+ u8 is_dma_page:1;
+ u8 check_wb:1;
+ u8 do_tstamp:1;
++ u8 qbv_en:1;
+ };
+
+ #define ENETC_RX_MAXFRM_SIZE ENETC_MAC_MAXFRM_SIZE
+@@ -42,6 +44,7 @@ struct enetc_ring_stats {
+ unsigned int packets;
+ unsigned int bytes;
+ unsigned int rx_alloc_errs;
++ unsigned int win_drop;
+ };
+
+ #define ENETC_BDR_DEFAULT_SIZE 1024
+@@ -111,6 +114,28 @@ struct enetc_msg_swbd {
+ int size;
+ };
+
++#ifdef CONFIG_ENETC_TSN
++/* Credit-Based Shaper parameters */
++struct cbs {
++ u8 tc;
++ bool enable;
++ u8 bw;
++ u32 hi_credit;
++ u32 lo_credit;
++ u32 idle_slope;
++ u32 send_slope;
++ u32 tc_max_sized_frame;
++ u32 max_interfrence_size;
++};
++
++struct enetc_cbs {
++ u32 port_transmit_rate;
++ u32 port_max_size_frame;
++ u8 tc_nums;
++ struct cbs cbs[0];
++};
++#endif
++
+ #define ENETC_REV1 0x1
+ enum enetc_errata {
+ ENETC_ERR_TXCSUM = BIT(0),
+@@ -119,6 +144,7 @@ enum enetc_errata {
+ };
+
+ #define ENETC_SI_F_QBV BIT(0)
++#define ENETC_SI_F_QBU BIT(1)
+
+ /* PCI IEP device data */
+ struct enetc_si {
+@@ -136,6 +162,10 @@ struct enetc_si {
+ int num_rss; /* number of RSS buckets */
+ unsigned short pad;
+ int hw_features;
++#ifdef CONFIG_ENETC_TSN
++ struct enetc_cbs *ecbs;
++#endif
++
+ };
+
+ #define ENETC_SI_ALIGN 32
+@@ -177,6 +207,7 @@ enum enetc_active_offloads {
+ ENETC_F_RX_TSTAMP = BIT(0),
+ ENETC_F_TX_TSTAMP = BIT(1),
+ ENETC_F_QBV = BIT(2),
++ ENETC_F_QBU = BIT(3),
+ };
+
+ struct enetc_ndev_priv {
+@@ -266,3 +297,10 @@ int enetc_setup_tc_cbs(struct net_device
+ #define enetc_sched_speed_set(ndev) (void)0
+ #define enetc_setup_tc_cbs(ndev, type_data) -EOPNOTSUPP
+ #endif
++#ifdef CONFIG_ENETC_TSN
++void enetc_tsn_pf_init(struct net_device *netdev, struct pci_dev *pdev);
++void enetc_tsn_pf_deinit(struct net_device *netdev);
++#else
++#define enetc_tsn_pf_init(netdev, pdev) (void)0
++#define enetc_tsn_pf_deinit(netdev) (void)0
++#endif
+--- a/drivers/net/ethernet/freescale/enetc/enetc_ethtool.c
++++ b/drivers/net/ethernet/freescale/enetc/enetc_ethtool.c
+@@ -187,6 +187,21 @@ static const struct {
+ { ENETC_PICDR(3), "ICM DR3 discarded frames" },
+ };
+
++static const struct {
++ int reg;
++ char name[ETH_GSTRING_LEN];
++} enetc_pmac_counters[] = {
++ { ENETC_PM1_RFRM, "PMAC rx frames" },
++ { ENETC_PM1_RPKT, "PMAC rx packets" },
++ { ENETC_PM1_RDRP, "PMAC rx dropped packets" },
++ { ENETC_PM1_RFRG, "PMAC rx fragment packets" },
++ { ENETC_PM1_TFRM, "PMAC tx frames" },
++ { ENETC_PM1_TERR, "PMAC tx error frames" },
++ { ENETC_PM1_TPKT, "PMAC tx packets" },
++ { ENETC_MAC_MERGE_MMFCRXR, "MAC merge fragment rx counter" },
++ { ENETC_MAC_MERGE_MMFCTXR, "MAC merge fragment tx counter"},
++};
++
+ static const char rx_ring_stats[][ETH_GSTRING_LEN] = {
+ "Rx ring %2d frames",
+ "Rx ring %2d alloc errors",
+@@ -196,6 +211,10 @@ static const char tx_ring_stats[][ETH_GS
+ "Tx ring %2d frames",
+ };
+
++static const char tx_windrop_stats[][ETH_GSTRING_LEN] = {
++ "Tx window drop %2d frames",
++};
++
+ static int enetc_get_sset_count(struct net_device *ndev, int sset)
+ {
+ struct enetc_ndev_priv *priv = netdev_priv(ndev);
+@@ -213,6 +232,12 @@ static int enetc_get_sset_count(struct n
+
+ len += ARRAY_SIZE(enetc_port_counters);
+
++ if (priv->active_offloads & ENETC_F_QBU)
++ len += ARRAY_SIZE(enetc_pmac_counters);
++
++ if (priv->active_offloads & ENETC_F_QBV)
++ len += ARRAY_SIZE(tx_windrop_stats) * priv->num_tx_rings;
++
+ return len;
+ }
+
+@@ -251,6 +276,28 @@ static void enetc_get_strings(struct net
+ ETH_GSTRING_LEN);
+ p += ETH_GSTRING_LEN;
+ }
++
++ if (!(priv->active_offloads & ENETC_F_QBU))
++ break;
++
++ for (i = 0; i < ARRAY_SIZE(enetc_pmac_counters); i++) {
++ strlcpy(p, enetc_pmac_counters[i].name,
++ ETH_GSTRING_LEN);
++ p += ETH_GSTRING_LEN;
++ }
++
++ if (!((priv->active_offloads & ENETC_F_QBV)))
++ break;
++
++ for (i = 0; i < priv->num_tx_rings; i++) {
++ for (j = 0; j < ARRAY_SIZE(tx_windrop_stats); j++) {
++ snprintf(p, ETH_GSTRING_LEN,
++ tx_windrop_stats[j],
++ i);
++ p += ETH_GSTRING_LEN;
++ }
++ }
++
+ break;
+ }
+ }
+@@ -278,6 +325,18 @@ static void enetc_get_ethtool_stats(stru
+
+ for (i = 0; i < ARRAY_SIZE(enetc_port_counters); i++)
+ data[o++] = enetc_port_rd(hw, enetc_port_counters[i].reg);
++
++ if (!(priv->active_offloads & ENETC_F_QBU))
++ return;
++
++ for (i = 0; i < ARRAY_SIZE(enetc_pmac_counters); i++)
++ data[o++] = enetc_port_rd(hw, enetc_pmac_counters[i].reg);
++
++ if (!((priv->active_offloads & ENETC_F_QBV)))
++ return;
++
++ for (i = 0; i < priv->num_tx_rings; i++)
++ data[o++] = priv->tx_ring[i]->stats.win_drop;
+ }
+
+ #define ENETC_RSSHASH_L3 (RXH_L2DA | RXH_VLAN | RXH_L3_PROTO | RXH_IP_SRC | \
+--- a/drivers/net/ethernet/freescale/enetc/enetc_hw.h
++++ b/drivers/net/ethernet/freescale/enetc/enetc_hw.h
+@@ -19,6 +19,7 @@
+ #define ENETC_SICTR1 0x1c
+ #define ENETC_SIPCAPR0 0x20
+ #define ENETC_SIPCAPR0_QBV BIT(4)
++#define ENETC_SIPCAPR0_QBU BIT(3)
+ #define ENETC_SIPCAPR0_RSS BIT(8)
+ #define ENETC_SIPCAPR1 0x24
+ #define ENETC_SITGTGR 0x30
+@@ -243,10 +244,20 @@ enum enetc_bdr_type {TX, RX};
+ #define ENETC_PCS_IF_MODE_SGMII_AN 0x0003
+
+ #define ENETC_PM0_IF_MODE 0x8300
++#define ENETC_PM1_IF_MODE 0x9300
+ #define ENETC_PMO_IFM_RG BIT(2)
+ #define ENETC_PM0_IFM_RLP (BIT(5) | BIT(11))
+ #define ENETC_PM0_IFM_RGAUTO (BIT(15) | ENETC_PMO_IFM_RG | BIT(1))
+ #define ENETC_PM0_IFM_XGMII BIT(12)
++#define ENETC_PSIDCAPR 0x1b08
++#define ENETC_PSIDCAPR_MSK GENMASK(15, 0)
++#define ENETC_PSFCAPR 0x1b18
++#define ENETC_PSFCAPR_MSK GENMASK(15, 0)
++#define ENETC_PSGCAPR 0x1b28
++#define ENETC_PSGCAPR_GCL_MSK GENMASK(18, 16)
++#define ENETC_PSGCAPR_SGIT_MSK GENMASK(15, 0)
++#define ENETC_PFMCAPR 0x1b38
++#define ENETC_PFMCAPR_MSK GENMASK(15, 0)
+
+ /* MAC counters */
+ #define ENETC_PM0_REOCT 0x8100
+@@ -300,6 +311,15 @@ enum enetc_bdr_type {TX, RX};
+ #define ENETC_PM0_TSCOL 0x82E0
+ #define ENETC_PM0_TLCOL 0x82E8
+ #define ENETC_PM0_TECOL 0x82F0
++#define ENETC_PM1_RFRM 0x9120
++#define ENETC_PM1_RDRP 0x9158
++#define ENETC_PM1_RPKT 0x9160
++#define ENETC_PM1_RFRG 0x91B8
++#define ENETC_PM1_TFRM 0x9220
++#define ENETC_PM1_TERR 0x9238
++#define ENETC_PM1_TPKT 0x9260
++#define ENETC_MAC_MERGE_MMFCRXR 0x1f14
++#define ENETC_MAC_MERGE_MMFCTXR 0x1f18
+
+ /* Port counters */
+ #define ENETC_PICDR(n) (0x0700 + (n) * 8) /* n = [0..3] */
+@@ -458,6 +478,7 @@ union enetc_tx_bd {
+ #define ENETC_TXBD_FLAGS_CSUM BIT(3)
+ #define ENETC_TXBD_FLAGS_EX BIT(6)
+ #define ENETC_TXBD_FLAGS_F BIT(7)
++#define ENETC_TXBD_STATS_WIN BIT(7)
+
+ static inline void enetc_clear_tx_bd(union enetc_tx_bd *txbd)
+ {
+@@ -485,6 +506,8 @@ static inline __le16 enetc_txbd_l3_csoff
+ #define ENETC_TXBD_L4_UDP BIT(5)
+ #define ENETC_TXBD_L4_TCP BIT(6)
+
++#define enetc_tsn_is_enabled() IS_ENABLED(CONFIG_ENETC_TSN)
++
+ union enetc_rx_bd {
+ struct {
+ __le64 addr;
+@@ -631,21 +654,307 @@ enum bdcr_cmd_class {
+ BDCR_CMD_RFS,
+ BDCR_CMD_PORT_GCL,
+ BDCR_CMD_RECV_CLASSIFIER,
++ BDCR_CMD_STREAM_IDENTIFY,
++ BDCR_CMD_STREAM_FILTER,
++ BDCR_CMD_STREAM_GCL,
++ BDCR_CMD_FLOW_METER,
+ __BDCR_CMD_MAX_LEN,
+ BDCR_CMD_MAX_LEN = __BDCR_CMD_MAX_LEN - 1,
+ };
+
++/* class 7, command 0, Stream Identity Entry Configuration */
++struct streamid_conf {
++ __le32 stream_handle; /* init gate value */
++ __le32 iports;
++ u8 id_type;
++ u8 oui[3];
++ u8 res[3];
++ u8 en;
++};
++
++#define ENETC_CBDR_SID_VID_MASK 0xfff
++#define ENETC_CBDR_SID_VIDM BIT(12)
++#define ENETC_CBDR_SID_TG_MASK 0xc000
++/* streamid_conf address point to this data space */
++struct null_streamid_data {
++ u8 dmac[6];
++ u16 vid_vidm_tg;
++};
++
++struct smac_streamid_data {
++ u8 smac[6];
++ u16 vid_vidm_tg;
++};
++
++/* class 7, command 1, query config , long format */
++/* No need structure define */
++
++#define ENETC_CDBR_SID_ENABLE BIT(7)
++/* Stream ID Query Response Data Buffer */
++struct streamid_query_resp {
++ u32 stream_handle;
++ u32 input_ports;
++ u8 id_type;
++ u8 oui[3];
++ u8 mac[6];
++ u16 vid_vidm_tg;
++ u8 res[3];
++ u8 en;
++};
++
++/* class 7, command 2, qeury status count, Stream ID query long format */
++struct streamid_stat_query {
++ u8 res[12];
++ __le32 input_ports;
++};
++
++/* Stream Identity Statistics Query */
++struct streamid_stat_query_resp {
++ u32 psinl;
++ u32 psinh;
++ u64 pspi[32];
++};
++
++#define ENETC_CBDR_SFI_PRI_MASK 0x7
++#define ENETC_CBDR_SFI_PRIM BIT(3)
++#define ENETC_CBDR_SFI_BLOV BIT(4)
++#define ENETC_CBDR_SFI_BLEN BIT(5)
++#define ENETC_CBDR_SFI_MSDUEN BIT(6)
++#define ENETC_CBDR_SFI_FMITEN BIT(7)
++#define ENETC_CBDR_SFI_ENABLE BIT(7)
++/* class 8, command 0, Stream Filter Instance, Short Format */
++struct sfi_conf {
++ __le32 stream_handle;
++ u8 multi;
++ u8 res[2];
++ u8 sthm;
++ /* Max Service Data Unit or Flow Meter Instance Table index.
++ * Depending on the value of FLT this represents either Max
++ * Service Data Unit (max frame size) allowed by the filter
++ * entry or is an index into the Flow Meter Instance table
++ * index identifying the policer which will be used to police
++ * it.
++ */
++ __le16 fm_inst_table_index;
++ __le16 msdu;
++ __le16 sg_inst_table_index;
++ u8 res1[2];
++ __le32 input_ports;
++ u8 res2[3];
++ u8 en;
++};
++
++/* class 8, command 1, Stream Filter Instance, write back, short Format */
++struct sfi_query {
++ u32 stream_handle;
++ u8 multi;
++ u8 res[2];
++ u8 sthm;
++ u16 fm_inst_table_index;
++ u16 msdu;
++ u16 sg_inst_table_index;
++ u8 res1[2];
++ u32 input_ports;
++ u8 res2[3];
++ u8 en;
++};
++
++/* class 8, command 2 stream Filter Instance status query short format
++ * command no need structure define
++ * Stream Filter Instance Query Statistics Response data
++ */
++struct sfi_counter_data {
++ u32 matchl;
++ u32 matchh;
++ u32 msdu_dropl;
++ u32 msdu_droph;
++ u32 stream_gate_dropl;
++ u32 stream_gate_droph;
++ u32 flow_meter_dropl;
++ u32 flow_meter_droph;
++};
++
++#define ENETC_CBDR_SGI_OIPV_MASK 0x7
++#define ENETC_CBDR_SGI_OIPV_EN BIT(3)
++#define ENETC_CBDR_SGI_CGTST BIT(6)
++#define ENETC_CBDR_SGI_OGTST BIT(7)
++#define ENETC_CBDR_SGI_CFG_CHG BIT(1)
++#define ENETC_CBDR_SGI_CFG_PND BIT(2)
++#define ENETC_CBDR_SGI_OEX BIT(4)
++#define ENETC_CBDR_SGI_OEXEN BIT(5)
++#define ENETC_CBDR_SGI_IRX BIT(6)
++#define ENETC_CBDR_SGI_IRXEN BIT(7)
++#define ENETC_CBDR_SGI_ACLLEN_MASK 0x3
++#define ENETC_CBDR_SGI_OCLLEN_MASK 0xc
++#define ENETC_CBDR_SGI_EN BIT(7)
++/* class 9, command 0, Stream Gate Instance Table, Short Format
++ * class 9, command 2, Stream Gate Instance Table entry query write back
++ * Short Format
++ */
++struct sgi_table {
++ u8 res[8];
++ u8 oipv;
++ u8 res0[2];
++ u8 ocgtst;
++ u8 res1[7];
++ u8 gset;
++ u8 oacl_len;
++ u8 res2[2];
++ u8 en;
++};
++
++#define ENETC_CBDR_SGI_AIPV_MASK 0x7
++#define ENETC_CBDR_SGI_AIPV_EN BIT(3)
++#define ENETC_CBDR_SGI_AGTST BIT(7)
++
++/* class 9, command 1, Stream Gate Control List, Long Format */
++struct sgcl_conf {
++ u8 aipv;
++ u8 res[2];
++ u8 agtst;
++ u8 res1[4];
++ union {
++ struct {
++ u8 res2[4];
++ u8 acl_len;
++ u8 res3[3];
++ };
++ u8 cct[8]; /* Config change time */
++ };
++};
++
++/* stream control list class 9 , cmd 1 data buffer */
++struct sgcl_data {
++ u32 btl;
++ u32 bth;
++ u32 ct;
++ u32 cte;
++ /*struct sgce *sgcl;*/
++};
++
++/* class 9, command 2, stream gate instant table enery query, short format
++ * write back see struct sgi_table. Do not need define.
++ * class 9, command 3 Stream Gate Control List Query Descriptor - Long Format
++ * ocl_len or acl_len to be 0, oper or admin would not show in the data space
++ * true len will be write back in the space.
++ */
++struct sgcl_query {
++ u8 res[12];
++ u8 oacl_len;
++ u8 res1[3];
++};
++
++/* define for 'stat' */
++#define ENETC_CBDR_SGIQ_AIPV_MASK 0x7
++#define ENETC_CBDR_SGIQ_AIPV_EN BIT(3)
++#define ENETC_CBDR_SGIQ_AGTST BIT(4)
++#define ENETC_CBDR_SGIQ_ACL_LEN_MASK 0x60
++#define ENETC_CBDR_SGIQ_OIPV_MASK 0x380
++#define ENETC_CBDR_SGIQ_OIPV_EN BIT(10)
++#define ENETC_CBDR_SGIQ_OGTST BIT(11)
++#define ENETC_CBDR_SGIQ_OCL_LEN_MASK 0x3000
++/* class 9, command 3 data space */
++struct sgcl_query_resp {
++ u16 stat;
++ u16 res;
++ u32 abtl;
++ u32 abth;
++ u32 act;
++ u32 acte;
++ u32 cctl;
++ u32 ccth;
++ u32 obtl;
++ u32 obth;
++ u32 oct;
++ u32 octe;
++};
++
++/* class 9, command 4 Stream Gate Instance Table Query Statistics Response
++ * short command, write back, no command define
++ */
++struct sgi_query_stat_resp {
++ u32 pgcl;
++ u32 pgch;
++ u32 dgcl;
++ u32 dgch;
++ u16 msdu_avail;
++ u8 res[6];
++};
++
++#define ENETC_CBDR_FMI_MR BIT(0)
++#define ENETC_CBDR_FMI_MREN BIT(1)
++#define ENETC_CBDR_FMI_DOY BIT(2)
++#define ENETC_CBDR_FMI_CM BIT(3)
++#define ENETC_CBDR_FMI_CF BIT(4)
++#define ENETC_CBDR_FMI_NDOR BIT(5)
++#define ENETC_CBDR_FMI_OALEN BIT(6)
++#define ENETC_CBDR_FMI_IRFPP_MASK 0x1f
++/* class 10: command 0/1, Flow Meter Instance Set, short Format */
++struct fmi_conf {
++ __le32 cir;
++ __le32 cbs;
++ __le32 eir;
++ __le32 ebs;
++ u8 conf;
++ u8 res1;
++ u8 ir_fpp;
++ u8 res2[4];
++ u8 en;
++};
++
++/* class:10, command:2, Flow Meter Instance Statistics Query Response */
++struct fmi_query_stat_resp {
++ u32 bcl;
++ u32 bch;
++ u32 dfl;
++ u32 dfh;
++ u32 d0gfl;
++ u32 d0gfh;
++ u32 d1gfl;
++ u32 d1gfh;
++ u32 dyfl;
++ u32 dyfh;
++ u32 ryfl;
++ u32 ryfh;
++ u32 drfl;
++ u32 drfh;
++ u32 rrfl;
++ u32 rrfh;
++ u32 lts;
++ u32 bci;
++ u32 bcf;
++ u32 bei;
++ u32 bef;
++};
++
+ /* class 5, command 0 */
+ struct tgs_gcl_conf {
+ u8 atc; /* init gate value */
+ u8 res[7];
+- struct {
+- u8 res1[4];
+- __le16 acl_len;
+- u8 res2[2];
++ union {
++ struct {
++ u8 res1[4];
++ __le16 acl_len;
++ u8 res2[2];
++ };
++ struct {
++ u32 cctl;
++ u32 ccth;
++ };
+ };
+ };
+
++#define ENETC_CBDR_SGL_IOMEN BIT(0)
++#define ENETC_CBDR_SGL_IPVEN BIT(3)
++#define ENETC_CBDR_SGL_GTST BIT(4)
++#define ENETC_CBDR_SGL_IPV_MASK 0xe
++/* Stream Gate Control List Entry */
++struct sgce {
++ u32 interval;
++ u8 msdu[3];
++ u8 multi;
++};
++
+ /* gate control list entry */
+ struct gce {
+ __le32 period;
+@@ -662,13 +971,55 @@ struct tgs_gcl_data {
+ struct gce entry[0];
+ };
+
++/* class 5, command 1 */
++struct tgs_gcl_query {
++ u8 res[12];
++ union {
++ struct {
++ __le16 acl_len; /* admin list length */
++ __le16 ocl_len; /* operation list length */
++ };
++ struct {
++ u16 admin_list_len;
++ u16 oper_list_len;
++ };
++ };
++
++};
++
++/* tgs_gcl_query command response data format */
++struct tgs_gcl_resp {
++ u32 abtl; /* base time */
++ u32 abth;
++ u32 act; /* cycle time */
++ u32 acte; /* cycle time extend */
++ u32 cctl; /* config change time */
++ u32 ccth;
++ u32 obtl; /* operation base time */
++ u32 obth;
++ u32 oct; /* operation cycle time */
++ u32 octe; /* operation cycle time extend */
++ u32 ccel; /* config change error */
++ u32 cceh;
++ /*struct gce *gcl;*/
++};
++
+ struct enetc_cbd {
+ union{
++ struct sfi_conf sfi_conf;
++ struct sgi_table sgi_table;
++ struct sgi_query_stat_resp sgi_query_stat_resp;
++ struct fmi_conf fmi_conf;
+ struct {
+ __le32 addr[2];
+ union {
+ __le32 opt[4];
+- struct tgs_gcl_conf gcl_conf;
++ struct tgs_gcl_conf gcl_conf;
++ struct tgs_gcl_query gcl_query;
++ struct streamid_conf sid_set;
++ struct streamid_stat_query sid_stat;
++ struct sgcl_conf sgcl_conf;
++ struct sgcl_query sgcl_query;
+ };
+ }; /* Long format */
+ __le32 data[6];
+@@ -683,11 +1034,88 @@ struct enetc_cbd {
+
+ #define ENETC_CLK 400000000ULL
+
++#define ENETC_PTCFPR(n) (0x1910 + (n) * 4) /* n = [0 ..7] */
++#define ENETC_FPE BIT(31)
++
++/* Port capability register 0 */
++#define ENETC_PCAPR0_PSFPM BIT(10)
++#define ENETC_PCAPR0_PSFP BIT(9)
++#define ENETC_PCAPR0_TSN BIT(4)
++#define ENETC_PCAPR0_QBU BIT(3)
++
+ /* port time gating control register */
+ #define ENETC_QBV_PTGCR_OFFSET 0x11a00
+ #define ENETC_QBV_TGE BIT(31)
+ #define ENETC_QBV_TGPE BIT(30)
++#define ENETC_QBV_TGDROP_DISABLE BIT(29)
+
+ /* Port time gating capability register */
+ #define ENETC_QBV_PTGCAPR_OFFSET 0x11a08
+ #define ENETC_QBV_MAX_GCL_LEN_MASK GENMASK(15, 0)
++
++/* Port time gating tick granularity register */
++#define ENETC_QBV_PTGTGR_OFFSET 0x11a0c
++#define ENETC_QBV_TICK_GRAN_MASK 0xffffffff
++
++/* Port time gating admin gate list status register */
++#define ENETC_QBV_PTGAGLSR_OFFSET 0x11a10
++
++#define ENETC_QBV_CFG_PEND_MASK 0x00000002
++
++/* Port time gating admin gate list length register */
++#define ENETC_QBV_PTGAGLLR_OFFSET 0x11a14
++#define ENETC_QBV_ADMIN_GATE_LIST_LENGTH_MASK 0xffff
++
++/* Port time gating operational gate list status register */
++#define ENETC_QBV_PTGOGLSR_OFFSET 0x11a18
++#define ENETC_QBV_HTA_POS_MASK 0xffff0000
++
++#define ENETC_QBV_CURR_POS_MASK 0x0000ffff
++
++/* Port time gating operational gate list length register */
++#define ENETC_QBV_PTGOGLLR_OFFSET 0x11a1c
++#define ENETC_QBV_OPER_GATE_LIST_LENGTH_MASK 0xffff
++
++/* Port time gating current time register */
++#define ENETC_QBV_PTGCTR_OFFSET 0x11a20
++#define ENETC_QBV_CURR_TIME_MASK 0xffffffffffffffff
++
++/* Port traffic class a time gating control register */
++#define ENETC_QBV_PTC0TGCR_OFFSET 0x11a40
++#define ENETC_QBV_PTC1TGCR_OFFSET 0x11a50
++#define ENETC_QBV_PTC2TGCR_OFFSET 0x11a60
++#define ENETC_QBV_PTC3TGCR_OFFSET 0x11a70
++#define ENETC_QBV_PTC4TGCR_OFFSET 0x11a80
++#define ENETC_QBV_PTC5TGCR_OFFSET 0x11a90
++#define ENETC_QBV_PTC6TGCR_OFFSET 0x11aa0
++#define ENETC_QBV_PTC7TGCR_OFFSET 0x11ab0
++
++/* Maximum Service Data Unit. */
++#define ENETC_PTC0MSDUR 0x12020
++#define ENETC_PTC1MSDUR 0x12024
++#define ENETC_PTC2MSDUR 0x12028
++#define ENETC_PTC3MSDUR 0x1202c
++#define ENETC_PTC4MSDUR 0x12030
++#define ENETC_PTC5MSDUR 0x12034
++#define ENETC_PTC6MSDUR 0x12038
++#define ENETC_PTC7MSDUR 0x1203c
++
++#define ENETC_QBV_MAXSDU_MASK 0xffff
++
++/* Port traffic class a time gating status register */
++#define ENETC_QBV_PTC0TGSR_OFFSET 0x11a44
++#define ENETC_QBV_HTA_STATE_MASK 0x10000
++#define ENETC_QBV_CURR_STATE_MASK 0x1
++
++/* Port traffic class a time gating transmission overrun counter register*/
++#define ENETC_QBV_PTC0TGTOCR_OFFSET 0x11a48
++#define ENETC_QBV_TX_OVERRUN_MASK 0xffffffffffffffff
++#define ENETC_TGLSTR 0xa200
++#define ENETC_TGS_MIN_DIS_MASK 0x80000000
++#define ENETC_MIN_LOOKAHEAD_MASK 0xffff
++
++#define ENETC_PPSFPMR 0x11b00
++#define ENETC_PPSFPMR_PSFPEN BIT(0)
++#define ENETC_PPSFPMR_VS BIT(1)
++#define ENETC_PPSFPMR_PVC BIT(2)
++#define ENETC_PPSFPMR_PVZC BIT(3)
+--- a/drivers/net/ethernet/freescale/enetc/enetc_pf.c
++++ b/drivers/net/ethernet/freescale/enetc/enetc_pf.c
+@@ -525,12 +525,16 @@ static void enetc_configure_port_mac(str
+ ENETC_PM0_TX_EN | ENETC_PM0_RX_EN);
+ /* set auto-speed for RGMII */
+ if (enetc_port_rd(hw, ENETC_PM0_IF_MODE) & ENETC_PMO_IFM_RG ||
+- phy_mode == PHY_INTERFACE_MODE_RGMII)
++ phy_mode == PHY_INTERFACE_MODE_RGMII) {
+ enetc_port_wr(hw, ENETC_PM0_IF_MODE, ENETC_PM0_IFM_RGAUTO);
++ enetc_port_wr(hw, ENETC_PM1_IF_MODE, ENETC_PM0_IFM_RGAUTO);
++ }
+
+ if (phy_mode == PHY_INTERFACE_MODE_XGMII ||
+- phy_mode == PHY_INTERFACE_MODE_USXGMII)
++ phy_mode == PHY_INTERFACE_MODE_USXGMII) {
+ enetc_port_wr(hw, ENETC_PM0_IF_MODE, ENETC_PM0_IFM_XGMII);
++ enetc_port_wr(hw, ENETC_PM1_IF_MODE, ENETC_PM0_IFM_XGMII);
++ }
+ }
+
+ static void enetc_configure_port_pmac(struct enetc_hw *hw)
+@@ -749,6 +753,9 @@ static void enetc_pf_netdev_setup(struct
+ if (si->hw_features & ENETC_SI_F_QBV)
+ priv->active_offloads |= ENETC_F_QBV;
+
++ if (enetc_tsn_is_enabled() && (si->hw_features & ENETC_SI_F_QBU))
++ priv->active_offloads |= ENETC_F_QBU;
++
+ /* pick up primary MAC address from SI */
+ enetc_get_primary_mac_addr(&si->hw, ndev->dev_addr);
+ }
+@@ -1027,6 +1034,8 @@ static int enetc_pf_probe(struct pci_dev
+ netif_info(priv, probe, ndev, "%s v%s\n",
+ enetc_drv_name, enetc_drv_ver);
+
++ enetc_tsn_pf_init(ndev, pdev);
++
+ return 0;
+
+ err_reg_netdev:
+@@ -1063,6 +1072,8 @@ static void enetc_pf_remove(struct pci_d
+ netif_info(priv, drv, si->ndev, "%s v%s remove\n",
+ enetc_drv_name, enetc_drv_ver);
+
++ enetc_tsn_pf_deinit(si->ndev);
++
+ unregister_netdev(si->ndev);
+
+ enetc_mdio_remove(pf);
+--- /dev/null
++++ b/drivers/net/ethernet/freescale/enetc/enetc_tsn.c
+@@ -0,0 +1,2049 @@
++// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
++/* Copyright 2017-2019 NXP */
++
++#ifdef CONFIG_ENETC_TSN
++#include "enetc.h"
++
++#include <net/tsn.h>
++#include <linux/module.h>
++#include <linux/irqflags.h>
++#include <linux/preempt.h>
++
++static u32 get_ndev_speed(struct net_device *netdev);
++
++static int alloc_cbdr(struct enetc_si *si, struct enetc_cbd **curr_cbd)
++{
++ struct enetc_cbdr *ring = &si->cbd_ring;
++ int i;
++
++ i = ring->next_to_use;
++ *curr_cbd = ENETC_CBD(*ring, i);
++
++ memset(*curr_cbd, 0, sizeof(struct enetc_cbd));
++ return i;
++}
++
++/* Transmit the BD control ring by writing the pir register.
++ * Update the counters maintained by software.
++ */
++static int xmit_cbdr(struct enetc_si *si, int i)
++{
++ struct enetc_cbdr *ring = &si->cbd_ring;
++ struct enetc_cbd *dest_cbd;
++ int nc, timeout;
++
++ i = (i + 1) % ring->bd_count;
++
++ ring->next_to_use = i;
++ /* let H/W know BD ring has been updated */
++ enetc_wr_reg(ring->pir, i);
++
++ timeout = ENETC_CBDR_TIMEOUT;
++
++ do {
++ if (enetc_rd_reg(ring->cir) == i)
++ break;
++ usleep_range(10, 20);
++ timeout -= 10;
++ } while (timeout);
++
++ if (!timeout)
++ return -EBUSY;
++
++ nc = ring->next_to_clean;
++
++ while (enetc_rd_reg(ring->cir) != nc) {
++ dest_cbd = ENETC_CBD(*ring, nc);
++ if (dest_cbd->status_flags & ENETC_CBD_STATUS_MASK)
++ WARN_ON(1);
++
++ nc = (nc + 1) % ring->bd_count;
++ }
++
++ ring->next_to_clean = nc;
++
++ return 0;
++}
++
++static inline u64 get_current_time(struct enetc_si *si)
++{
++ u64 tmp = 0;
++
++ tmp = (u64)enetc_rd(&si->hw, ENETC_SICTR0);
++ return ((u64)enetc_rd(&si->hw, ENETC_SICTR1) << 32) + tmp;
++}
++
++/* Class 10: Flow Meter Instance Statistics Query Descriptor - Long Format */
++int enetc_qci_fmi_counters_get(struct net_device *ndev, u32 index,
++ struct fmi_query_stat_resp *counters)
++{
++ struct enetc_cbd *cbdr;
++ struct fmi_query_stat_resp *fmi_data;
++ dma_addr_t dma;
++ u16 data_size, dma_size;
++ int curr_cbd;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16((u16)index);
++ cbdr->cmd = 2;
++ cbdr->cls = BDCR_CMD_FLOW_METER;
++ cbdr->status_flags = 0;
++
++ data_size = sizeof(struct fmi_query_stat_resp);
++
++ fmi_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!fmi_data)
++ return -ENOMEM;
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr->length = dma_size;
++
++ dma = dma_map_single(&priv->si->pdev->dev, fmi_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ kfree(fmi_data);
++ return -ENOMEM;
++ }
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ memcpy(counters, fmi_data, sizeof(struct fmi_query_stat_resp));
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(fmi_data);
++ return 0;
++}
++
++u16 enetc_get_max_gcl_len(struct enetc_hw *hw)
++{
++ return (enetc_rd(hw, ENETC_QBV_PTGCAPR_OFFSET)
++ & ENETC_QBV_MAX_GCL_LEN_MASK);
++}
++
++void enetc_pspeed_set(struct net_device *ndev)
++{
++ u32 speed, pspeed;
++ u32 difflag = 0;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++
++ speed = get_ndev_speed(ndev);
++ pspeed = enetc_port_rd(&priv->si->hw, ENETC_PMR)
++ & ENETC_PMR_PSPEED_MASK;
++ switch (speed) {
++ case SPEED_1000:
++ if (pspeed != ENETC_PMR_PSPEED_1000M) {
++ difflag = 1;
++ pspeed = ENETC_PMR_PSPEED_1000M;
++ }
++ break;
++ case SPEED_2500:
++ if (pspeed != ENETC_PMR_PSPEED_2500M) {
++ difflag = 1;
++ pspeed = ENETC_PMR_PSPEED_2500M;
++ }
++
++ break;
++ case SPEED_100:
++ if (pspeed != ENETC_PMR_PSPEED_100M) {
++ difflag = 1;
++ pspeed = ENETC_PMR_PSPEED_100M;
++ }
++ break;
++ case SPEED_10:
++ if (pspeed != ENETC_PMR_PSPEED_10M) {
++ difflag = 1;
++ pspeed = ENETC_PMR_PSPEED_10M;
++ }
++ break;
++ default:
++ netdev_err(ndev, "not support speed\n");
++ }
++
++ if (difflag) {
++ enetc_port_wr(&priv->si->hw, ENETC_PMR,
++ (enetc_port_rd(&priv->si->hw, ENETC_PMR)
++ & (~ENETC_PMR_PSPEED_MASK))
++ | pspeed);
++ }
++}
++
++/* CBD Class 5: Time Gated Scheduling Gate Control List configuration
++ * Descriptor - Long Format
++ */
++int enetc_qbv_set(struct net_device *ndev, struct tsn_qbv_conf *admin_conf)
++{
++ struct enetc_cbd *cbdr;
++ struct tgs_gcl_data *gcl_data;
++ struct tgs_gcl_conf *gcl_config;
++ struct gce *gce;
++ u16 gcl_len;
++ u16 data_size;
++ int i;
++ dma_addr_t dma;
++ int curr_cbd;
++ struct tsn_qbv_basic *admin_basic = &admin_conf->admin;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ u32 temp;
++ u64 tempclock;
++ struct tsn_port *port;
++
++ port = tsn_get_port(ndev);
++ if (!port) {
++ netdev_err(priv->si->ndev, "TSN device not registered!\n");
++ return -ENODEV;
++ }
++
++ enetc_pspeed_set(ndev);
++
++ gcl_len = admin_basic->control_list_length;
++ if (gcl_len > enetc_get_max_gcl_len(&priv->si->hw))
++ return -EINVAL;
++
++ temp = enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET);
++ if (admin_conf->gate_enabled && !(temp & ENETC_QBV_TGE)) {
++ enetc_wr(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET,
++ temp & (~ENETC_QBV_TGE));
++ usleep_range(10, 20);
++ enetc_wr(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET,
++ temp | ENETC_QBV_TGE);
++ } else if (!admin_conf->gate_enabled) {
++ enetc_wr(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET,
++ temp & (~ENETC_QBV_TGE));
++ memcpy(&port->nd.ntdata, admin_conf, sizeof(*admin_conf));
++ call_tsn_notifiers(TSN_QBV_CONFIGCHANGETIME_ARRIVE,
++ ndev, &port->nd);
++ return 0;
++ }
++
++ /* Set the maximum frame size for each traffic class index
++ * PTCaMSDUR[MAXSDU]. The maximum frame size cannot exceed
++ * 9,600 bytes (0x2580). Frames that exceed the limit are
++ * discarded.
++ */
++ if (admin_conf->maxsdu) {
++ enetc_wr(&priv->si->hw, ENETC_PTC0MSDUR, admin_conf->maxsdu);
++ enetc_wr(&priv->si->hw, ENETC_PTC1MSDUR, admin_conf->maxsdu);
++ enetc_wr(&priv->si->hw, ENETC_PTC2MSDUR, admin_conf->maxsdu);
++ enetc_wr(&priv->si->hw, ENETC_PTC3MSDUR, admin_conf->maxsdu);
++ enetc_wr(&priv->si->hw, ENETC_PTC4MSDUR, admin_conf->maxsdu);
++ enetc_wr(&priv->si->hw, ENETC_PTC5MSDUR, admin_conf->maxsdu);
++ enetc_wr(&priv->si->hw, ENETC_PTC6MSDUR, admin_conf->maxsdu);
++ enetc_wr(&priv->si->hw, ENETC_PTC7MSDUR, admin_conf->maxsdu);
++ }
++
++ /* Configure the (administrative) gate control list using the
++ * control BD descriptor.
++ */
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ gcl_config = &cbdr->gcl_conf;
++
++ data_size = struct_size(gcl_data, entry, gcl_len);
++
++ gcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!gcl_data)
++ return -ENOMEM;
++
++ gce = &gcl_data->entry[0];
++
++ gcl_config->atc = admin_basic->gate_states;
++ gcl_config->acl_len = cpu_to_le16(gcl_len);
++
++ if (!admin_basic->base_time) {
++ gcl_data->btl =
++ cpu_to_le32(enetc_rd(&priv->si->hw, ENETC_SICTR0));
++ gcl_data->bth =
++ cpu_to_le32(enetc_rd(&priv->si->hw, ENETC_SICTR1));
++ } else {
++ gcl_data->btl =
++ cpu_to_le32(lower_32_bits(admin_basic->base_time));
++ gcl_data->bth =
++ cpu_to_le32(upper_32_bits(admin_basic->base_time));
++ }
++
++ gcl_data->ct = cpu_to_le32(admin_basic->cycle_time);
++ gcl_data->cte = cpu_to_le32(admin_basic->cycle_time_extension);
++
++ for (i = 0; i < gcl_len; i++) {
++ struct gce *temp_gce = gce + i;
++ struct tsn_qbv_entry *temp_entry;
++
++ temp_entry = admin_basic->control_list + i;
++
++ temp_gce->gate = temp_entry->gate_state;
++ temp_gce->period = cpu_to_le32(temp_entry->time_interval);
++ }
++
++ cbdr->length = cpu_to_le16(data_size);
++ cbdr->status_flags = 0;
++
++ dma = dma_map_single(&priv->si->pdev->dev, gcl_data,
++ data_size, DMA_TO_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ kfree(gcl_data);
++ return -ENOMEM;
++ }
++
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++ cbdr->cmd = 0;
++ cbdr->cls = BDCR_CMD_PORT_GCL;
++
++ /* Updated by ENETC on completion of the configuration
++ * command. A zero value indicates success.
++ */
++ cbdr->status_flags = 0;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ memcpy(&port->nd.ntdata, admin_conf, sizeof(*admin_conf));
++
++ tempclock = ((u64)le32_to_cpu(gcl_config->ccth)) << 32;
++ port->nd.ntdata.qbv_notify.admin.base_time =
++ le32_to_cpu(gcl_config->cctl) + tempclock;
++
++ memset(cbdr, 0, sizeof(struct enetc_cbd));
++ dma_unmap_single(&priv->si->pdev->dev, dma, data_size, DMA_TO_DEVICE);
++ kfree(gcl_data);
++
++ call_tsn_notifiers(TSN_QBV_CONFIGCHANGETIME_ARRIVE,
++ ndev, &port->nd);
++
++ return 0;
++}
++
++/* CBD Class 5: Time Gated Scheduling Gate Control List query
++ * Descriptor - Long Format
++ */
++int enetc_qbv_get(struct net_device *ndev, struct tsn_qbv_conf *admin_conf)
++{
++ struct enetc_cbd *cbdr;
++ struct tgs_gcl_resp *gcl_data;
++ struct tgs_gcl_query *gcl_query;
++ struct gce *gce;
++ struct tsn_qbv_basic *admin_basic = &admin_conf->admin;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ dma_addr_t dma;
++ int curr_cbd;
++ u16 maxlen;
++ u16 data_size, dma_size;
++ u16 admin_len;
++ u16 oper_len;
++ u64 temp;
++ int i;
++
++ if (enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET) & ENETC_QBV_TGE) {
++ admin_conf->gate_enabled = true;
++ } else {
++ admin_conf->gate_enabled = false;
++ return 0;
++ }
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ gcl_query = &cbdr->gcl_query;
++
++ maxlen = enetc_get_max_gcl_len(&priv->si->hw);
++
++ data_size = sizeof(struct tgs_gcl_resp)
++ + sizeof(struct gce) * 2 * maxlen;
++
++ gcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!gcl_data)
++ return -ENOMEM;
++
++ gce = (struct gce *)(gcl_data + 1);
++
++ gcl_query->acl_len = cpu_to_le16(maxlen);
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr->length = dma_size;
++ cbdr->status_flags = 0;
++
++ dma = dma_map_single(&priv->si->pdev->dev, gcl_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ kfree(gcl_data);
++ return -ENOMEM;
++ }
++
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++ cbdr->cmd = 1;
++ cbdr->cls = BDCR_CMD_PORT_GCL;
++ xmit_cbdr(priv->si, curr_cbd);
++ dma_unmap_single(&priv->si->pdev->dev, dma, data_size, DMA_FROM_DEVICE);
++
++ /* since cbdr already passed to free, below could be get wrong */
++ admin_len = le16_to_cpu(gcl_query->admin_list_len);
++ oper_len = le16_to_cpu(gcl_query->oper_list_len);
++
++ admin_basic->control_list_length = admin_len;
++
++ temp = ((u64)le32_to_cpu(gcl_data->abth)) << 32;
++ admin_basic->base_time = le32_to_cpu(gcl_data->abtl) + temp;
++
++ admin_basic->cycle_time = le32_to_cpu(gcl_data->act);
++ admin_basic->cycle_time_extension = le32_to_cpu(gcl_data->acte);
++
++ admin_basic->control_list = kcalloc(admin_len,
++ sizeof(admin_basic->control_list),
++ GFP_KERNEL);
++ if (!admin_basic->control_list) {
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(gcl_data);
++ return -ENOMEM;
++ }
++
++ for (i = 0; i < admin_len; i++) {
++ struct gce *temp_gce = gce + i;
++ struct tsn_qbv_entry *temp_entry;
++
++ temp_entry = admin_basic->control_list + i;
++
++ temp_entry->gate_state = temp_gce->gate;
++ temp_entry->time_interval = le32_to_cpu(temp_gce->period);
++ }
++
++ /* Updated by ENETC on completion of the configuration
++ * command. A zero value indicates success.
++ */
++ admin_conf->config_change = true;
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(gcl_data);
++
++ return 0;
++}
++
++int enetc_qbv_get_status(struct net_device *ndev,
++ struct tsn_qbv_status *status)
++{
++ struct enetc_cbd *cbdr;
++ struct tgs_gcl_resp *gcl_data;
++ struct tgs_gcl_query *gcl_query;
++ struct gce *gce;
++ struct tsn_qbv_basic *oper_basic;
++ struct enetc_ndev_priv *priv;
++ dma_addr_t dma;
++ int curr_cbd;
++ u16 maxlen;
++ u16 data_size, dma_size;
++ u16 admin_len;
++ u16 oper_len;
++ u64 temp;
++ int i;
++
++ if (!ndev)
++ return -EINVAL;
++
++ if (!status)
++ return -EINVAL;
++
++ oper_basic = &status->oper;
++ priv = netdev_priv(ndev);
++
++ if (!(enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET) & ENETC_QBV_TGE))
++ return -EINVAL;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ gcl_query = &cbdr->gcl_query;
++
++ maxlen = enetc_get_max_gcl_len(&priv->si->hw);
++
++ data_size = sizeof(struct tgs_gcl_resp) +
++ sizeof(struct gce) * 2 * maxlen;
++
++ gcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!gcl_data)
++ return -ENOMEM;
++
++ gce = (struct gce *)(gcl_data + 1);
++
++ gcl_query->acl_len = cpu_to_le16(maxlen);
++ gcl_query->ocl_len = cpu_to_le16(maxlen);
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr->length = dma_size;
++ cbdr->status_flags = 0; /* long format command no ie */
++
++ dma = dma_map_single(&priv->si->pdev->dev, gcl_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ kfree(gcl_data);
++ return -ENOMEM;
++ }
++
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++ cbdr->cmd = 1;
++ cbdr->cls = BDCR_CMD_PORT_GCL;
++ xmit_cbdr(priv->si, curr_cbd);
++ dma_unmap_single(&priv->si->pdev->dev, dma, data_size, DMA_FROM_DEVICE);
++
++ /* since cbdr already passed to free, below could be get wrong */
++ admin_len = le16_to_cpu(gcl_query->admin_list_len);
++ oper_len = le16_to_cpu(gcl_query->oper_list_len);
++
++ if (enetc_rd(&priv->si->hw, ENETC_QBV_PTGAGLSR_OFFSET) &
++ ENETC_QBV_CFG_PEND_MASK) {
++ status->config_pending = true;
++ goto exit;
++ }
++
++ /* The Oper and Admin timing fields exist in the response buffer even
++ * if no valid corresponding lists exists. These fields are considered
++ * invalid if the corresponding list does not exist.
++ */
++ status->config_pending = false;
++ temp = ((u64)le32_to_cpu(gcl_data->ccth)) << 32;
++ status->config_change_time = le32_to_cpu(gcl_data->cctl) + temp;
++
++ temp = ((u64)le32_to_cpu(gcl_data->cceh)) << 32;
++ status->config_change_error = le32_to_cpu(gcl_data->ccel) + temp;
++
++ /* changed to SITGTGR */
++ status->tick_granularity = enetc_rd(&priv->si->hw, ENETC_SITGTGR);
++
++ /* current time */
++ status->current_time = get_current_time(priv->si);
++
++ status->supported_list_max = maxlen;
++
++ /* status->oper.gate_states , no init oper/admin gate state */
++ status->oper.control_list_length = oper_len;
++ temp = ((u64)le32_to_cpu(gcl_data->obth)) << 32;
++ status->oper.base_time = le32_to_cpu(gcl_data->obtl) + temp;
++ status->oper.cycle_time = le32_to_cpu(gcl_data->oct);
++ status->oper.cycle_time_extension = le32_to_cpu(gcl_data->octe);
++
++ oper_basic->control_list =
++ kcalloc(oper_len, sizeof(oper_basic->control_list), GFP_KERNEL);
++ if (!oper_basic->control_list) {
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(gcl_data);
++ return -ENOMEM;
++ }
++
++ for (i = 0; i < oper_len; i++) {
++ struct gce *temp_gce = gce + maxlen + i;
++ struct tsn_qbv_entry *temp_entry = oper_basic->control_list + i;
++
++ temp_entry->gate_state = temp_gce->gate;
++ temp_entry->time_interval = le32_to_cpu(temp_gce->period);
++ }
++
++exit:
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(gcl_data);
++ return 0;
++}
++
++/* CBD Class 7: Stream Identity Entry Set Descriptor - Long Format */
++int enetc_cb_streamid_set(struct net_device *ndev, u32 index,
++ bool en, struct tsn_cb_streamid *streamid)
++{
++ struct enetc_cbd *cbdr;
++ void *si_data;
++ struct null_streamid_data *si_data1;
++ struct smac_streamid_data *si_data2;
++ struct streamid_conf *si_conf;
++ struct enetc_ndev_priv *priv;
++ dma_addr_t dma;
++ u16 data_size, dma_size;
++ int curr_cbd;
++
++ if (!ndev)
++ return -EINVAL;
++
++ priv = netdev_priv(ndev);
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16((u16)index);
++ cbdr->cmd = 0;
++ cbdr->cls = BDCR_CMD_STREAM_IDENTIFY;
++ cbdr->status_flags = 0;
++
++ data_size = sizeof(struct null_streamid_data);
++ si_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ cbdr->length = cpu_to_le16(data_size);
++
++ dma = dma_map_single(&priv->si->pdev->dev, si_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ kfree(si_data);
++ return -ENOMEM;
++ }
++
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++ si_data1 = (struct null_streamid_data *)si_data;
++ si_data1->dmac[0] = 0xFF;
++ si_data1->dmac[1] = 0xFF;
++ si_data1->dmac[2] = 0xFF;
++ si_data1->dmac[3] = 0xFF;
++ si_data1->dmac[4] = 0xFF;
++ si_data1->dmac[5] = 0xFF;
++ si_data1->vid_vidm_tg =
++ cpu_to_le16(ENETC_CBDR_SID_VID_MASK
++ + ((0x3 << 14) | ENETC_CBDR_SID_VIDM));
++
++ si_conf = &cbdr->sid_set;
++ /* Only one port supported for one entry, set itself */
++ si_conf->iports = 1 << (priv->si->pdev->devfn & 0x7);
++ si_conf->id_type = 1;
++ si_conf->oui[2] = 0x0;
++ si_conf->oui[1] = 0x80;
++ si_conf->oui[0] = 0xC2;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(si_data);
++
++ if (!en)
++ return 0;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16((u16)index);
++ cbdr->cmd = 0;
++ cbdr->cls = BDCR_CMD_STREAM_IDENTIFY;
++ cbdr->status_flags = 0;
++
++ si_conf = &cbdr->sid_set;
++ si_conf->en = 0x80;
++ si_conf->stream_handle = cpu_to_le32(streamid->handle);
++ si_conf->iports = 1 << (priv->si->pdev->devfn & 0x7);
++ si_conf->id_type = streamid->type;
++ si_conf->oui[2] = 0x0;
++ si_conf->oui[1] = 0x80;
++ si_conf->oui[0] = 0xC2;
++
++ if (si_conf->id_type == 1) {
++ data_size = sizeof(struct null_streamid_data);
++ si_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ } else if (si_conf->id_type == 2) {
++ data_size = sizeof(struct smac_streamid_data);
++ si_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ } else {
++ return -EINVAL;
++ }
++
++ if (!si_data)
++ return -ENOMEM;
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr->length = dma_size;
++ cbdr->status_flags = 0;
++
++ dma = dma_map_single(&priv->si->pdev->dev, si_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(si_data);
++ return -ENOMEM;
++ }
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++
++ /* VIDM default to be 1.
++ * VID Match. If set (b1) then the VID must match, otherwise
++ * any VID is considered a match. VIDM setting is only used
++ * when TG is set to b01.
++ */
++ if (si_conf->id_type == 1) {
++ si_data1 = (struct null_streamid_data *)si_data;
++ si_data1->dmac[0] = streamid->para.nid.dmac & 0xFF;
++ si_data1->dmac[1] = (streamid->para.nid.dmac >> 8) & 0xFF;
++ si_data1->dmac[2] = (streamid->para.nid.dmac >> 16) & 0xFF;
++ si_data1->dmac[3] = (streamid->para.nid.dmac >> 24) & 0xFF;
++ si_data1->dmac[4] = (streamid->para.nid.dmac >> 32) & 0xFF;
++ si_data1->dmac[5] = (streamid->para.nid.dmac >> 40) & 0xFF;
++ si_data1->vid_vidm_tg =
++ cpu_to_le16((streamid->para.nid.vid & ENETC_CBDR_SID_VID_MASK) +
++ ((((u16)(streamid->para.nid.tagged) & 0x3) << 14)
++ | ENETC_CBDR_SID_VIDM));
++ } else if (si_conf->id_type == 2) {
++ si_data2 = (struct smac_streamid_data *)si_data;
++ si_data2->smac[0] = streamid->para.sid.smac & 0xFF;
++ si_data2->smac[1] = (streamid->para.sid.smac >> 8) & 0xFF;
++ si_data2->smac[2] = (streamid->para.sid.smac >> 16) & 0xFF;
++ si_data2->smac[3] = (streamid->para.sid.smac >> 24) & 0xFF;
++ si_data2->smac[4] = (streamid->para.sid.smac >> 32) & 0xFF;
++ si_data2->smac[5] = (streamid->para.sid.smac >> 40) & 0xFF;
++ si_data2->vid_vidm_tg =
++ cpu_to_le16((streamid->para.sid.vid & ENETC_CBDR_SID_VID_MASK) +
++ ((((u16)(streamid->para.sid.tagged) & 0x3) << 14)
++ | ENETC_CBDR_SID_VIDM));
++ }
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(si_data);
++
++ return 0;
++}
++
++/* CBD Class 7: Stream Identity Entry Query Descriptor - Long Format */
++int enetc_cb_streamid_get(struct net_device *ndev, u32 index,
++ struct tsn_cb_streamid *streamid)
++{
++ struct enetc_cbd *cbdr;
++ struct streamid_query_resp *si_data;
++ struct enetc_ndev_priv *priv;
++ dma_addr_t dma;
++ u16 data_size, dma_size;
++ int curr_cbd;
++ int valid;
++
++ if (!ndev)
++ return -EINVAL;
++
++ priv = netdev_priv(ndev);
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le32(index);
++ cbdr->cmd = 1;
++ cbdr->cls = BDCR_CMD_STREAM_IDENTIFY;
++ cbdr->status_flags = 0;
++
++ data_size = sizeof(struct streamid_query_resp);
++ si_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!si_data)
++ return -ENOMEM;
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr->length = dma_size;
++ cbdr->status_flags = 0; /* long format command no ie */
++
++ dma = dma_map_single(&priv->si->pdev->dev, si_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ kfree(si_data);
++ return -ENOMEM;
++ }
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ streamid->type = si_data->id_type;
++
++ if (streamid->type == 1) {
++ streamid->para.nid.dmac = si_data->mac[0]
++ + ((u64)si_data->mac[1] << 8)
++ + ((u64)si_data->mac[2] << 16)
++ + ((u64)si_data->mac[3] << 24)
++ + ((u64)si_data->mac[4] << 32)
++ + ((u64)si_data->mac[5] << 40);
++ /* VID Match. If set (b1) then the VID must match, otherwise
++ * any VID is considered a match.
++ */
++ streamid->para.nid.vid =
++ le16_to_cpu(si_data->vid_vidm_tg
++ & ENETC_CBDR_SID_VID_MASK);
++ streamid->para.nid.tagged =
++ le16_to_cpu(si_data->vid_vidm_tg >> 14 & 0x3);
++ } else if (streamid->type == 2) {
++ streamid->para.sid.smac = si_data->mac[0]
++ + ((u64)si_data->mac[1] << 8)
++ + ((u64)si_data->mac[2] << 16)
++ + ((u64)si_data->mac[3] << 24)
++ + ((u64)si_data->mac[4] << 32)
++ + ((u64)si_data->mac[5] << 40);
++ /* VID Match. If set (b1) then the VID must match, otherwise
++ * any VID is considered a match.
++ */
++ streamid->para.sid.vid =
++ le16_to_cpu(si_data->vid_vidm_tg
++ & ENETC_CBDR_SID_VID_MASK);
++ streamid->para.sid.tagged =
++ le16_to_cpu(si_data->vid_vidm_tg >> 14 & 0x3);
++ }
++
++ streamid->handle = le32_to_cpu(si_data->stream_handle);
++ streamid->ifac_iport = le32_to_cpu(si_data->input_ports);
++ valid = si_data->en ? 1 : 0;
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ kfree(si_data);
++
++ return valid;
++}
++
++/* CBD Class 7: Stream Identity Statistics Query Descriptor - Long Format */
++int enetc_cb_streamid_counters_get(struct net_device *ndev, u32 index,
++ struct tsn_cb_streamid_counters *counters)
++{
++ return 0;
++}
++
++void enetc_qci_enable(struct enetc_hw *hw)
++{
++ enetc_wr(hw, ENETC_PPSFPMR, enetc_rd(hw, ENETC_PPSFPMR)
++ | ENETC_PPSFPMR_PSFPEN | ENETC_PPSFPMR_VS
++ | ENETC_PPSFPMR_PVC | ENETC_PPSFPMR_PVZC);
++}
++
++void enetc_qci_disable(struct enetc_hw *hw)
++{
++ enetc_wr(hw, ENETC_PPSFPMR, enetc_rd(hw, ENETC_PPSFPMR)
++ & ~ENETC_PPSFPMR_PSFPEN & ~ENETC_PPSFPMR_VS
++ & ~ENETC_PPSFPMR_PVC & ~ENETC_PPSFPMR_PVZC);
++}
++
++/* CBD Class 8: Stream Filter Instance Set Descriptor - Short Format */
++int enetc_qci_sfi_set(struct net_device *ndev, u32 index, bool en,
++ struct tsn_qci_psfp_sfi_conf *tsn_qci_sfi)
++{
++ struct enetc_cbd *cbdr;
++ struct sfi_conf *sfi_config;
++
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ int curr_cbd;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16(index);
++ cbdr->cmd = 0;
++ cbdr->cls = BDCR_CMD_STREAM_FILTER;
++ cbdr->status_flags = 0x80;
++ cbdr->length = cpu_to_le16(1);
++
++ sfi_config = &cbdr->sfi_conf;
++ if (en)
++ sfi_config->en = 0x80;
++
++ if (tsn_qci_sfi->stream_handle_spec >= 0) {
++ sfi_config->stream_handle =
++ cpu_to_le32(tsn_qci_sfi->stream_handle_spec);
++ sfi_config->sthm |= 0x80;
++ }
++
++ sfi_config->sg_inst_table_index =
++ cpu_to_le16(tsn_qci_sfi->stream_gate_instance_id);
++ sfi_config->input_ports = 1 << (priv->si->pdev->devfn & 0x7);
++
++ /* The priority value which may be matched against the
++ * frame’s priority value to determine a match for this entry.
++ */
++ if (tsn_qci_sfi->priority_spec >= 0)
++ sfi_config->multi |= (tsn_qci_sfi->priority_spec & 0x7) | 0x8;
++
++ /* Filter Type. Identifies the contents of the MSDU/FM_INST_INDEX
++ * field as being either an MSDU value or an index into the Flow
++ * Meter Instance table.
++ */
++ if (tsn_qci_sfi->stream_filter.maximum_sdu_size != 0) {
++ sfi_config->msdu =
++ cpu_to_le16(tsn_qci_sfi->stream_filter.maximum_sdu_size);
++ sfi_config->multi |= 0x40;
++ }
++
++ if (tsn_qci_sfi->stream_filter.flow_meter_instance_id >= 0) {
++ sfi_config->fm_inst_table_index =
++ cpu_to_le16(tsn_qci_sfi->stream_filter.flow_meter_instance_id);
++ sfi_config->multi |= 0x80;
++ }
++
++ /* Stream blocked due to oversized frame enable. TRUE or FALSE */
++ if (tsn_qci_sfi->block_oversize_enable)
++ sfi_config->multi |= 0x20;
++
++ /* Stream blocked due to oversized frame. TRUE or FALSE */
++ if (tsn_qci_sfi->block_oversize)
++ sfi_config->multi |= 0x10;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 0;
++}
++
++/* CBD Class 8: Stream Filter Instance Query Descriptor - Short Format */
++int enetc_qci_sfi_get(struct net_device *ndev, u32 index,
++ struct tsn_qci_psfp_sfi_conf *tsn_qci_sfi)
++{
++ struct enetc_cbd *cbdr;
++ struct sfi_conf *sfi_config;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ int curr_cbd;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16(index);
++ cbdr->cmd = 1;
++ cbdr->cls = BDCR_CMD_STREAM_FILTER;
++ cbdr->status_flags = 0x80;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ sfi_config = &cbdr->sfi_conf;
++ if (sfi_config->sthm & 0x80)
++ tsn_qci_sfi->stream_handle_spec =
++ le32_to_cpu(sfi_config->stream_handle);
++ else
++ tsn_qci_sfi->stream_handle_spec = -1;
++
++ tsn_qci_sfi->stream_gate_instance_id =
++ le16_to_cpu(sfi_config->sg_inst_table_index);
++
++ if (sfi_config->multi & 0x8)
++ tsn_qci_sfi->priority_spec =
++ le16_to_cpu(sfi_config->multi & 0x7);
++ else
++ tsn_qci_sfi->priority_spec = -1;
++
++ /* Filter Type. Identifies the contents of the MSDU/FM_INST_INDEX
++ * field as being either an MSDU value or an index into the Flow
++ * Meter Instance table.
++ */
++ if (sfi_config->multi & 0x80)
++ tsn_qci_sfi->stream_filter.flow_meter_instance_id =
++ le16_to_cpu(sfi_config->fm_inst_table_index);
++ else
++ tsn_qci_sfi->stream_filter.flow_meter_instance_id = -1;
++
++ if (sfi_config->multi & 0x40)
++ tsn_qci_sfi->stream_filter.maximum_sdu_size =
++ le16_to_cpu(sfi_config->msdu);
++
++ /* Stream blocked due to oversized frame enable. TRUE or FALSE */
++ if (sfi_config->multi & 0x20)
++ tsn_qci_sfi->block_oversize_enable = true;
++ /* Stream blocked due to oversized frame. TRUE or FALSE */
++ if (sfi_config->multi & 0x10)
++ tsn_qci_sfi->block_oversize = true;
++
++ if (sfi_config->en & 0x80) {
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 1;
++ }
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 0;
++}
++
++/* CBD Class 8: Stream Filter Instance Query Statistics
++ * Descriptor - Long Format
++ */
++int enetc_qci_sfi_counters_get(struct net_device *ndev, u32 index,
++ struct tsn_qci_psfp_sfi_counters *counters)
++{
++ struct enetc_cbd *cbdr;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ int curr_cbd;
++ struct sfi_counter_data *sfi_counter_data;
++ dma_addr_t dma;
++ u16 data_size, dma_size;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16((u16)index);
++ cbdr->cmd = 2;
++ cbdr->cls = BDCR_CMD_STREAM_FILTER;
++ cbdr->status_flags = 0;
++
++ data_size = sizeof(struct sfi_counter_data);
++ sfi_counter_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!sfi_counter_data)
++ return -ENOMEM;
++
++ dma = dma_map_single(&priv->si->pdev->dev, sfi_counter_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ kfree(sfi_counter_data);
++ return -ENOMEM;
++ }
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr->length = dma_size;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ counters->matching_frames_count =
++ ((u64)le32_to_cpu(sfi_counter_data->matchh) << 32)
++ + sfi_counter_data->matchl;
++
++ counters->not_passing_sdu_count =
++ ((u64)le32_to_cpu(sfi_counter_data->msdu_droph) << 32)
++ + sfi_counter_data->msdu_dropl;
++
++ counters->passing_sdu_count = counters->matching_frames_count
++ - counters->not_passing_sdu_count;
++
++ counters->not_passing_frames_count =
++ ((u64)le32_to_cpu(sfi_counter_data->stream_gate_droph) << 32)
++ + le32_to_cpu(sfi_counter_data->stream_gate_dropl);
++
++ counters->passing_frames_count = counters->matching_frames_count
++ - counters->not_passing_sdu_count
++ - counters->not_passing_frames_count;
++
++ counters->red_frames_count =
++ ((u64)le32_to_cpu(sfi_counter_data->flow_meter_droph) << 32)
++ + le32_to_cpu(sfi_counter_data->flow_meter_dropl);
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 0;
++}
++
++/* CBD Class 9: Stream Gate Instance Table Entry Set
++ * Descriptor - Short Format
++ */
++int enetc_qci_sgi_set(struct net_device *ndev, u32 index,
++ struct tsn_qci_psfp_sgi_conf *tsn_qci_sgi)
++{
++ struct enetc_cbd *cbdr, *cbdr_sgcl;
++ struct sgi_table *sgi_config;
++ struct sgcl_conf *sgcl_config;
++ struct sgcl_data *sgcl_data;
++ struct sgce *sgce;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++
++ dma_addr_t dma;
++ u16 data_size, dma_size;
++ int curr_cbd, i;
++
++ /* disable first */
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++ memset(cbdr, 0, sizeof(*cbdr));
++
++ cbdr->index = cpu_to_le16(index);
++ cbdr->cmd = 0;
++ cbdr->cls = BDCR_CMD_STREAM_GCL;
++ cbdr->status_flags = 0x80;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ if (!tsn_qci_sgi->gate_enabled) {
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 0;
++ }
++
++ /* Re-enable */
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++ memset(cbdr, 0, sizeof(*cbdr));
++
++ cbdr->index = cpu_to_le16(index);
++ cbdr->cmd = 0;
++ cbdr->cls = BDCR_CMD_STREAM_GCL;
++ cbdr->status_flags = 0x80;
++
++ sgi_config = &cbdr->sgi_table;
++
++ sgi_config->ocgtst = tsn_qci_sgi->admin.control_list_length ?
++ 0x80 : (tsn_qci_sgi->admin.gate_states ? 0x80 : 0x0);
++
++ sgi_config->oipv =
++ tsn_qci_sgi->admin.control_list_length ?
++ 0x0 : ((tsn_qci_sgi->admin.init_ipv < 0) ?
++ 0x0 : ((tsn_qci_sgi->admin.init_ipv & 0x7) | 0x8));
++
++ sgi_config->en = 0x80;
++
++ if (tsn_qci_sgi->block_invalid_rx_enable)
++ sgi_config->gset |= 0x80;
++ if (tsn_qci_sgi->block_invalid_rx)
++ sgi_config->gset |= 0x40;
++ if (tsn_qci_sgi->block_octets_exceeded)
++ sgi_config->gset |= 0x10;
++ if (tsn_qci_sgi->block_octets_exceeded_enable)
++ sgi_config->gset |= 0x20;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ if (tsn_qci_sgi->admin.control_list_length == 0)
++ goto exit;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr_sgcl);
++ memset(cbdr, 0, sizeof(*cbdr));
++
++ cbdr_sgcl->index = cpu_to_le16(index);
++ cbdr_sgcl->cmd = 1;
++ cbdr_sgcl->cls = BDCR_CMD_STREAM_GCL;
++ cbdr_sgcl->status_flags = 0;
++
++ sgcl_config = &cbdr_sgcl->sgcl_conf;
++
++ /* tsn_qci_sgi->admin.control_list_length is not zero now */
++ if (tsn_qci_sgi->admin.control_list_length > 4)
++ return -EINVAL;
++
++ sgcl_config->acl_len =
++ (tsn_qci_sgi->admin.control_list_length - 1) & 0x3;
++
++ data_size = sizeof(struct sgcl_data) +
++ (sgcl_config->acl_len + 1) * sizeof(struct sgce);
++
++ sgcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!sgcl_data)
++ return -ENOMEM;
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr_sgcl->length = dma_size;
++
++ dma = dma_map_single(&priv->si->pdev->dev,
++ sgcl_data, data_size,
++ DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ memset(cbdr, 0, sizeof(*cbdr));
++ memset(cbdr_sgcl, 0, sizeof(*cbdr_sgcl));
++ kfree(sgcl_data);
++ return -ENOMEM;
++ }
++ cbdr_sgcl->addr[0] = lower_32_bits(dma);
++ cbdr_sgcl->addr[1] = upper_32_bits(dma);
++
++ sgce = (struct sgce *)(sgcl_data + 1);
++
++ if (tsn_qci_sgi->admin.gate_states)
++ sgcl_config->agtst = 0x80;
++
++ sgcl_data->ct = cpu_to_le32(tsn_qci_sgi->admin.cycle_time);
++ sgcl_data->cte = cpu_to_le32(tsn_qci_sgi->admin.cycle_time_extension);
++
++ if (tsn_qci_sgi->admin.init_ipv >= 0)
++ sgcl_config->aipv = (tsn_qci_sgi->admin.init_ipv & 0x7) | 0x8;
++
++ for (i = 0; i < tsn_qci_sgi->admin.control_list_length; i++) {
++ struct tsn_qci_psfp_gcl *temp_sgcl = tsn_qci_sgi->admin.gcl + i;
++ struct sgce *temp_entry = (struct sgce *)(sgce + i);
++
++ if (temp_sgcl->gate_state)
++ temp_entry->multi |= 0x10;
++
++ if (temp_sgcl->ipv >= 0)
++ temp_entry->multi |= ((temp_sgcl->ipv & 0x7) << 5)
++ | 0x08;
++
++ if (temp_sgcl->octet_max)
++ temp_entry->multi |= 0x01;
++
++ temp_entry->interval = cpu_to_le32(temp_sgcl->time_interval);
++ temp_entry->msdu[0] = temp_sgcl->octet_max & 0xFF;
++ temp_entry->msdu[1] = (temp_sgcl->octet_max >> 8) & 0xFF;
++ temp_entry->msdu[2] = (temp_sgcl->octet_max >> 16) & 0xFF;
++ }
++
++ if (!tsn_qci_sgi->admin.base_time) {
++ sgcl_data->btl =
++ cpu_to_le32(enetc_rd(&priv->si->hw, ENETC_SICTR0));
++ sgcl_data->bth =
++ cpu_to_le32(enetc_rd(&priv->si->hw, ENETC_SICTR1));
++ } else {
++ u32 tempu, templ;
++
++ tempu = upper_32_bits(tsn_qci_sgi->admin.base_time);
++ templ = lower_32_bits(tsn_qci_sgi->admin.base_time);
++ sgcl_data->bth = cpu_to_le32(tempu);
++ sgcl_data->btl = cpu_to_le32(templ);
++ }
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ memset(cbdr_sgcl, 0, sizeof(*cbdr_sgcl));
++ kfree(sgcl_data);
++
++exit:
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 0;
++}
++
++/* CBD Class 9: Stream Gate Instance Table Entry Query
++ * Descriptor - Short Format
++ */
++int enetc_qci_sgi_get(struct net_device *ndev, u32 index,
++ struct tsn_qci_psfp_sgi_conf *tsn_qci_sgi)
++{
++ struct enetc_cbd *cbdr, *cbdr_sgcl;
++ struct sgi_table *sgi_config;
++ struct sgcl_query *sgcl_query;
++ struct sgcl_query_resp *sgcl_data;
++ struct sgce *sgce;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ dma_addr_t dma;
++ u16 data_size, dma_size, gcl_data_stat = 0;
++ u8 admin_len = 0;
++ int curr_cbd, i;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16(index);
++ cbdr->cmd = 2;
++ cbdr->cls = BDCR_CMD_STREAM_GCL;
++ cbdr->status_flags = 0x80;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ sgi_config = &cbdr->sgi_table;
++
++ tsn_qci_sgi->admin.gate_states = (sgi_config->ocgtst & 0x80) ?
++ true : false;
++ if (sgi_config->oipv & 0x08)
++ tsn_qci_sgi->admin.init_ipv = sgi_config->oipv & 0x7;
++ else
++ tsn_qci_sgi->admin.init_ipv = -1;
++
++ if (sgi_config->en & 0x80)
++ tsn_qci_sgi->gate_enabled = true;
++ if (sgi_config->gset & 0x80)
++ tsn_qci_sgi->block_invalid_rx_enable = true;
++ if (sgi_config->gset & 0x40)
++ tsn_qci_sgi->block_invalid_rx = true;
++ if (sgi_config->gset & 0x20)
++ tsn_qci_sgi->block_octets_exceeded_enable = true;
++ if (sgi_config->gset & 0x10)
++ tsn_qci_sgi->block_octets_exceeded = true;
++
++ /* Check gate list length is zero? */
++ if (!(sgi_config->oacl_len & 0x30)) {
++ tsn_qci_sgi->admin.control_list_length = 0;
++ goto exit;
++ }
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr_sgcl);
++
++ cbdr_sgcl->index = cpu_to_le16(index);
++ cbdr_sgcl->cmd = 3;
++ cbdr_sgcl->cls = BDCR_CMD_STREAM_GCL;
++ cbdr_sgcl->status_flags = 0;
++
++ data_size = sizeof(struct sgcl_query_resp) + 4 * sizeof(struct sgce);
++
++ sgcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!sgcl_data)
++ return -ENOMEM;
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr_sgcl->length = dma_size;
++ cbdr_sgcl->status_flags = 0;
++
++ sgcl_query = &cbdr_sgcl->sgcl_query;
++
++ sgcl_query->oacl_len = 0x10;
++
++ dma = dma_map_single(&priv->si->pdev->dev, sgcl_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ memset(cbdr, 0, sizeof(*cbdr));
++ memset(cbdr_sgcl, 0, sizeof(*cbdr_sgcl));
++ kfree(sgcl_data);
++ return -ENOMEM;
++ }
++ cbdr_sgcl->addr[0] = lower_32_bits(dma);
++ cbdr_sgcl->addr[1] = upper_32_bits(dma);
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ sgce = (struct sgce *)(sgcl_data + 1);
++
++ gcl_data_stat = le16_to_cpu(sgcl_data->stat);
++ if (gcl_data_stat & 0x10)
++ tsn_qci_sgi->admin.gate_states = true;
++
++ if (gcl_data_stat & 0x80)
++ tsn_qci_sgi->admin.init_ipv = gcl_data_stat & 0x7;
++ else
++ tsn_qci_sgi->admin.init_ipv = -1;
++
++ /* admin_len can also get from gcl_data_stat bit 5,6
++ * OR sgi_config->oacl_len
++ */
++ admin_len = (sgcl_query->oacl_len & 0x3) + 1;
++ tsn_qci_sgi->admin.control_list_length = admin_len;
++ tsn_qci_sgi->admin.cycle_time = le32_to_cpu(sgcl_data->act);
++ tsn_qci_sgi->admin.cycle_time_extension = le32_to_cpu(sgcl_data->acte);
++ tsn_qci_sgi->admin.base_time = ((u64)(le32_to_cpu(sgcl_data->abth))
++ << 32)
++ + le32_to_cpu(sgcl_data->abtl);
++
++ tsn_qci_sgi->admin.gcl = kcalloc(admin_len,
++ sizeof(struct tsn_qci_psfp_gcl),
++ GFP_KERNEL);
++ if (!tsn_qci_sgi->admin.gcl) {
++ kfree(sgcl_data);
++ return -ENOMEM;
++ }
++
++ for (i = 0; i < admin_len; i++) {
++ struct tsn_qci_psfp_gcl *temp_sgcl = tsn_qci_sgi->admin.gcl + i;
++ struct sgce *temp_entry = (struct sgce *)(sgce + i);
++
++ if (temp_entry->multi & 0x10)
++ temp_sgcl->gate_state = true;
++
++ if (temp_entry->multi & 0x08)
++ temp_sgcl->ipv = temp_entry->multi >> 5;
++ else
++ temp_sgcl->ipv = -1;
++
++ temp_sgcl->time_interval = le32_to_cpu(temp_entry->interval);
++
++ if (temp_entry->multi & 0x01)
++ temp_sgcl->octet_max = (temp_entry->msdu[0] & 0xff)
++ | (((u32)temp_entry->msdu[1] << 8) & 0xff00)
++ | (((u32)temp_entry->msdu[1] << 16) & 0xff0000);
++ else
++ temp_sgcl->octet_max = 0;
++ }
++
++ memset(cbdr_sgcl, 0, sizeof(*cbdr_sgcl));
++ kfree(sgcl_data);
++
++exit:
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 0;
++}
++
++/* CBD Class 9: Stream Gate Instance Table Entry Query Descriptor
++ * CBD Class 9: Stream Gate Control List Query Descriptor
++ */
++int enetc_qci_sgi_status_get(struct net_device *ndev, u16 index,
++ struct tsn_psfp_sgi_status *status)
++{
++ struct enetc_cbd *cbdr_sgi, *cbdr_sgcl;
++ struct sgi_table *sgi_config;
++ struct sgcl_query *sgcl_query;
++ struct sgcl_query_resp *sgcl_data;
++ struct sgce *sgce;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ dma_addr_t dma;
++ u16 data_size, dma_size, gcl_data_stat = 0;
++ u8 oper_len = 0;
++ int curr_cbd, i;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr_sgi);
++
++ cbdr_sgi->index = cpu_to_le16(index);
++ cbdr_sgi->cmd = 2;
++ cbdr_sgi->cls = BDCR_CMD_STREAM_GCL;
++ cbdr_sgi->status_flags = 0x80;
++
++ sgi_config = &cbdr_sgi->sgi_table;
++
++ if (sgi_config->gset & 0x4)
++ status->config_pending = true;
++
++ status->oper.gate_states = ((sgi_config->ocgtst & 0x80) ? true : false);
++
++ /* Check gate list length is zero */
++ if (!(sgi_config->oacl_len & 0x30)) {
++ status->oper.control_list_length = 0;
++ goto cmd2quit;
++ }
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr_sgcl);
++
++ cbdr_sgcl->index = cpu_to_le16(index);
++ cbdr_sgcl->cmd = 3;
++ cbdr_sgcl->cls = BDCR_CMD_STREAM_GCL;
++ cbdr_sgcl->status_flags = 0;
++
++ /* Max size */
++ data_size = sizeof(struct sgcl_query_resp) + 4 * sizeof(struct sgce);
++
++ sgcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!sgcl_data)
++ return -ENOMEM;
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr_sgcl->length = dma_size;
++ cbdr_sgcl->status_flags = 0;
++
++ sgcl_query = &cbdr_sgcl->sgcl_query;
++
++ sgcl_query->oacl_len = 0x20;
++
++ dma = dma_map_single(&priv->si->pdev->dev, sgcl_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ memset(cbdr_sgi, 0, sizeof(*cbdr_sgi));
++ memset(cbdr_sgcl, 0, sizeof(*cbdr_sgcl));
++ kfree(sgcl_data);
++ return -ENOMEM;
++ }
++ cbdr_sgcl->addr[0] = lower_32_bits(dma);
++ cbdr_sgcl->addr[1] = upper_32_bits(dma);
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ sgce = (struct sgce *)(sgcl_data + 1);
++
++ /* oper_len can also get from gcl_data_stat bit 5,6
++ * OR sgi_config->oacl_len
++ */
++ oper_len = ((sgcl_query->oacl_len & 0x0c) >> 2) + 1;
++
++ /* Get Stream Gate Control List */
++ status->oper.cycle_time = le32_to_cpu(sgcl_data->oct);
++ status->oper.cycle_time_extension = le32_to_cpu(sgcl_data->octe);
++ status->oper.base_time = le32_to_cpu(sgcl_data->obtl)
++ + ((u64)le32_to_cpu(sgcl_data->obth) << 32);
++ status->oper.control_list_length = oper_len;
++
++ gcl_data_stat = le16_to_cpu(sgcl_data->stat);
++ if (gcl_data_stat & 0x400)
++ status->oper.init_ipv = gcl_data_stat & 0x38 >> 7;
++ else
++ status->oper.init_ipv = -1;
++
++ if (gcl_data_stat & 0x800)
++ status->oper.gate_states = true;
++
++ status->oper.gcl = kcalloc(oper_len,
++ sizeof(struct tsn_qci_psfp_gcl),
++ GFP_KERNEL);
++ if (!status->oper.gcl) {
++ memset(cbdr_sgi, 0, sizeof(*cbdr_sgi));
++ memset(cbdr_sgcl, 0, sizeof(*cbdr_sgcl));
++ kfree(sgcl_data);
++ return -ENOMEM;
++ }
++
++ for (i = 0; i < oper_len; i++) {
++ struct tsn_qci_psfp_gcl *temp_sgcl = status->oper.gcl + i;
++ struct sgce *temp_entry = (struct sgce *)(sgce + i);
++
++ if (temp_entry->multi & 0x10)
++ temp_sgcl->gate_state = true;
++
++ if (temp_entry->multi & 0x08)
++ temp_sgcl->ipv = temp_entry->multi >> 5;
++ else
++ temp_sgcl->ipv = -1;
++
++ temp_sgcl->time_interval = le32_to_cpu(temp_entry->interval);
++
++ if (temp_entry->multi & 0x01)
++ temp_sgcl->octet_max = temp_entry->msdu[0]
++ | ((((u32)temp_entry->msdu[1]) << 8)
++ & 0xff00)
++ | ((((u32)temp_entry->msdu[2]) << 16)
++ & 0xff0000);
++ else
++ temp_sgcl->octet_max = 0;
++ }
++
++ status->config_change_time = le32_to_cpu(sgcl_data->cctl)
++ + ((u64)le32_to_cpu(sgcl_data->ccth) << 32);
++
++ memset(cbdr_sgcl, 0, sizeof(*cbdr_sgcl));
++ kfree(sgcl_data);
++
++cmd2quit:
++ /* changed to SITGTGR */
++ status->tick_granularity = enetc_rd(&priv->si->hw, ENETC_SITGTGR);
++
++ /* current time */
++ status->current_time = get_current_time(priv->si);
++
++ memset(cbdr_sgi, 0, sizeof(*cbdr_sgi));
++
++ return 0;
++}
++
++/* CBD Class 10: Flow Meter Instance Set Descriptor - Short Format */
++int enetc_qci_fmi_set(struct net_device *ndev, u32 index, bool enable,
++ struct tsn_qci_psfp_fmi *tsn_qci_fmi)
++{
++ struct enetc_cbd *cbdr;
++ struct fmi_conf *fmi_config;
++
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ int curr_cbd;
++ u64 temp = 0;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16((u16)index);
++ cbdr->cmd = 0;
++ cbdr->cls = BDCR_CMD_FLOW_METER;
++ cbdr->status_flags = 0x80;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ if (!enable) {
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 0;
++ }
++
++ /* Re-enable */
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++ memset(cbdr, 0, sizeof(*cbdr));
++ cbdr->index = cpu_to_le16((u16)index);
++ cbdr->cmd = 0;
++ cbdr->cls = BDCR_CMD_FLOW_METER;
++ cbdr->status_flags = 0x80;
++
++ fmi_config = &cbdr->fmi_conf;
++ fmi_config->en = 0x80;
++ if (tsn_qci_fmi->cir) {
++ temp = (u64)1000 * tsn_qci_fmi->cir;
++ temp = temp / 3725;
++ }
++ fmi_config->cir = cpu_to_le32((u32)temp);
++ fmi_config->cbs = cpu_to_le32(tsn_qci_fmi->cbs);
++ temp = 0;
++ if (tsn_qci_fmi->eir) {
++ temp = (u64)1000 * tsn_qci_fmi->eir;
++ temp = temp / 3725;
++ }
++ fmi_config->eir = cpu_to_le32((u32)temp);
++ fmi_config->ebs = cpu_to_le32(tsn_qci_fmi->ebs);
++
++ if (tsn_qci_fmi->mark_red)
++ fmi_config->conf |= 0x1;
++
++ if (tsn_qci_fmi->mark_red_enable)
++ fmi_config->conf |= 0x2;
++
++ if (tsn_qci_fmi->drop_on_yellow)
++ fmi_config->conf |= 0x4;
++
++ if (tsn_qci_fmi->cm)
++ fmi_config->conf |= 0x8;
++
++ if (tsn_qci_fmi->cf)
++ fmi_config->conf |= 0x10;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ memset(cbdr, 0, sizeof(*cbdr));
++ return 0;
++}
++
++/* CBD Class 10: Flow Meter Instance Query Descriptor - Short Format */
++int enetc_qci_fmi_get(struct net_device *ndev, u32 index,
++ struct tsn_qci_psfp_fmi *tsn_qci_fmi,
++ struct tsn_qci_psfp_fmi_counters *counters)
++{
++ struct enetc_cbd *cbdr;
++ struct fmi_conf *fmi_config;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ int curr_cbd;
++ u16 data_size, dma_size;
++ dma_addr_t dma;
++ struct fmi_query_stat_resp *fmi_counter_data;
++ u64 temp = 0;
++
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16(index);
++ cbdr->cmd = 1;
++ cbdr->cls = BDCR_CMD_FLOW_METER;
++ cbdr->status_flags = 0x80;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ fmi_config = &cbdr->fmi_conf;
++ if (fmi_config->cir) {
++ temp = (u64)3725 * fmi_config->cir;
++ temp = temp / 1000;
++ }
++ tsn_qci_fmi->cir = le32_to_cpu((u32)temp);
++ tsn_qci_fmi->cbs = le32_to_cpu(fmi_config->cbs);
++ temp = 0;
++ if (fmi_config->eir) {
++ temp = (u64)3725 * fmi_config->eir;
++ temp = temp / 1000;
++ }
++ tsn_qci_fmi->eir = le32_to_cpu((u32)temp);
++ tsn_qci_fmi->ebs = le32_to_cpu(fmi_config->ebs);
++
++ if (fmi_config->conf & 0x1)
++ tsn_qci_fmi->mark_red = true;
++
++ if (fmi_config->conf & 0x2)
++ tsn_qci_fmi->mark_red_enable = true;
++
++ if (fmi_config->conf & 0x4)
++ tsn_qci_fmi->drop_on_yellow = true;
++
++ if (fmi_config->conf & 0x8)
++ tsn_qci_fmi->cm = true;
++
++ if (fmi_config->conf & 0x10)
++ tsn_qci_fmi->cf = true;
++
++ memset(cbdr, 0, sizeof(*cbdr));
++
++ /* Get counters */
++ curr_cbd = alloc_cbdr(priv->si, &cbdr);
++
++ cbdr->index = cpu_to_le16(index);
++ cbdr->cmd = 2;
++ cbdr->cls = BDCR_CMD_FLOW_METER;
++ cbdr->status_flags = 0x0;
++
++ data_size = sizeof(struct fmi_query_stat_resp);
++ fmi_counter_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
++ if (!fmi_counter_data)
++ return -ENOMEM;
++
++ dma = dma_map_single(&priv->si->pdev->dev, fmi_counter_data,
++ data_size, DMA_FROM_DEVICE);
++ if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
++ netdev_err(priv->si->ndev, "DMA mapping failed!\n");
++ kfree(fmi_counter_data);
++ return -ENOMEM;
++ }
++ cbdr->addr[0] = lower_32_bits(dma);
++ cbdr->addr[1] = upper_32_bits(dma);
++
++ dma_size = cpu_to_le16(data_size);
++ cbdr->length = dma_size;
++
++ xmit_cbdr(priv->si, curr_cbd);
++
++ memcpy(counters, fmi_counter_data, sizeof(*counters));
++
++ return 0;
++}
++
++int enetc_qbu_set(struct net_device *ndev, u8 ptvector)
++{
++ u32 temp;
++ int i;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++
++ temp = enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET);
++ if (temp & ENETC_QBV_TGE)
++ enetc_wr(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET,
++ temp & (~ENETC_QBV_TGPE));
++
++ for (i = 0; i < 8; i++) {
++ /* 1 Enabled. Traffic is transmitted on the preemptive MAC. */
++ temp = enetc_port_rd(&priv->si->hw, ENETC_PTCFPR(i));
++
++ if ((ptvector >> i) & 0x1)
++ enetc_port_wr(&priv->si->hw,
++ ENETC_PTCFPR(i),
++ temp | ENETC_FPE);
++ else
++ enetc_port_wr(&priv->si->hw,
++ ENETC_PTCFPR(i),
++ temp & ~ENETC_FPE);
++ }
++
++ return 0;
++}
++
++int enetc_qbu_get(struct net_device *ndev,
++ struct tsn_preempt_status *preemptstat)
++{
++ int i;
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++
++ if (enetc_port_rd(&priv->si->hw, ENETC_PFPMR) & ENETC_PFPMR_PMACE) {
++ preemptstat->preemption_active = true;
++ if (enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET)
++ & ENETC_QBV_TGE)
++ preemptstat->hold_request = 1;
++ else
++ preemptstat->hold_request = 2;
++ } else {
++ preemptstat->preemption_active = false;
++ return 0;
++ }
++
++ for (i = 0; i < 8; i++)
++ if (enetc_port_rd(&priv->si->hw, ENETC_PTCFPR(i)) & 0x80000000)
++ preemptstat->admin_state |= 1 << i;
++
++ preemptstat->hold_advance =
++ enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET) & 0xFFFF;
++ preemptstat->release_advance =
++ enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET) & 0xFFFF;
++
++ return 0;
++}
++
++u32 __enetc_tsn_get_cap(struct enetc_si *si)
++{
++ u32 reg = 0;
++ u32 cap = 0;
++
++ reg = enetc_port_rd(&si->hw, ENETC_PCAPR0);
++
++ if (reg & ENETC_PCAPR0_PSFP)
++ cap |= TSN_CAP_QCI;
++
++ if (reg & ENETC_PCAPR0_TSN)
++ cap |= TSN_CAP_QBV;
++
++ if (reg & ENETC_PCAPR0_QBU)
++ cap |= TSN_CAP_QBU;
++
++ cap |= TSN_CAP_CBS;
++ cap |= TSN_CAP_TBS;
++
++ return cap;
++}
++
++u32 enetc_tsn_get_capability(struct net_device *ndev)
++{
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++
++ return __enetc_tsn_get_cap(priv->si);
++}
++
++static int __enetc_get_max_cap(struct enetc_si *si,
++ struct tsn_qci_psfp_stream_param *stream_para)
++{
++ u32 reg = 0;
++
++ /* Port stream filter capability */
++ reg = enetc_port_rd(&si->hw, ENETC_PSFCAPR);
++ stream_para->max_sf_instance = reg & ENETC_PSFCAPR_MSK;
++ /* Port stream filter capability */
++ reg = enetc_port_rd(&si->hw, ENETC_PSGCAPR);
++ stream_para->max_sg_instance = (reg & ENETC_PSGCAPR_SGIT_MSK);
++ stream_para->supported_list_max = (reg & ENETC_PSGCAPR_GCL_MSK) >> 16;
++ /* Port flow meter capability */
++ reg = enetc_port_rd(&si->hw, ENETC_PFMCAPR);
++ stream_para->max_fm_instance = reg & ENETC_PFMCAPR_MSK;
++
++ return 0;
++}
++
++int enetc_get_max_cap(struct net_device *ndev,
++ struct tsn_qci_psfp_stream_param *stream_para)
++{
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++
++ return __enetc_get_max_cap(priv->si, stream_para);
++}
++
++static int enetc_set_cbs(struct net_device *ndev, u8 tc, u8 bw)
++{
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ struct enetc_si *si = priv->si;
++ struct enetc_cbs *ecbs = si->ecbs;
++ struct cbs *cbs;
++
++ int bw_sum = 0;
++ u32 port_transmit_rate;
++ u32 port_frame_max_size;
++ u8 tc_nums;
++ int i;
++
++ u32 max_interfrence_size;
++ u32 send_slope;
++ u32 hi_credit;
++
++ if (!ecbs)
++ return -ENOMEM;
++
++ port_transmit_rate = get_ndev_speed(si->ndev);
++ if (port_transmit_rate != ecbs->port_transmit_rate)
++ ecbs->port_transmit_rate = port_transmit_rate;
++ port_frame_max_size = ecbs->port_max_size_frame;
++ tc_nums = ecbs->tc_nums;
++ cbs = ecbs->cbs;
++
++ if (tc >= tc_nums) {
++ dev_err(&ndev->dev, "Make sure the TC less than %d\n", tc_nums);
++ return -EINVAL;
++ }
++
++ if (!bw) {
++ if (cbs[tc].enable) {
++ /* Make sure the other TC that are numerically
++ * lower than this TC have been disabled.
++ */
++ for (i = 0; i < tc; i++) {
++ if (cbs[i].enable)
++ break;
++ }
++ if (i < tc) {
++ dev_err(&ndev->dev,
++ "TC%d has been disabled first\n", i);
++ return -EINVAL;
++ }
++ memset(&cbs[tc], 0, sizeof(*cbs));
++ cbs[tc].enable = false;
++ enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), 0);
++ enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), 0);
++ }
++ return 0;
++ }
++
++ /* Make sure the other TC that are numerically
++ * higher than this TC have been enabled.
++ */
++ for (i = tc_nums - 1; i > tc; i--) {
++ if (!cbs[i].enable) {
++ dev_err(&ndev->dev,
++ "TC%d has been enabled first\n", i);
++ return -EINVAL;
++ }
++ bw_sum += cbs[i].bw;
++ }
++
++ if (bw_sum + bw >= 100) {
++ dev_err(&ndev->dev,
++ "The sum of all CBS Bandwidth cann't exceed 100\n");
++ return -EINVAL;
++ }
++
++ cbs[tc].bw = bw;
++ cbs[tc].tc_max_sized_frame = enetc_port_rd(&si->hw, ENETC_PTCMSDUR(tc));
++ cbs[tc].idle_slope = port_transmit_rate / 100 * bw;
++ cbs[tc].send_slope = port_transmit_rate - cbs[tc].idle_slope;
++
++ /* For TC7, the max_interfrence_size is ENETC_MAC_MAXFRM_SIZE.
++ * For TC6, the max_interfrence_size is calculated as below:
++ *
++ * max_interfrence_size = (M0 + Ma + Ra * M0 / (R0 - Ra))
++ *
++ * For other traffic class, for example SR class Q:
++ *
++ * R0 * (M0 + Ma + ... + Mp)
++ * max_interfrence_size = ------------------------------
++ * (R0 - Ra) + ... + (R0 - Rp)
++ *
++ */
++
++ if (tc == tc_nums - 1) {
++ cbs[tc].max_interfrence_size = port_frame_max_size * 8;
++
++ } else if (tc == tc_nums - 2) {
++ cbs[tc].max_interfrence_size = (port_frame_max_size
++ + cbs[tc + 1].tc_max_sized_frame
++ + port_frame_max_size * (cbs[tc + 1].idle_slope
++ / cbs[tc + 1].send_slope)) * 8;
++ } else {
++ max_interfrence_size = port_frame_max_size;
++ send_slope = 0;
++ for (i = tc + 1; i < tc_nums; i++) {
++ send_slope += cbs[i].send_slope;
++ max_interfrence_size += cbs[i].tc_max_sized_frame;
++ }
++ max_interfrence_size = ((u64)port_transmit_rate
++ * max_interfrence_size) / send_slope;
++ cbs[tc].max_interfrence_size = max_interfrence_size * 8;
++ }
++
++ cbs[tc].hi_credit = cbs[tc].max_interfrence_size * cbs[tc].bw / 100;
++ cbs[tc].lo_credit = cbs[tc].tc_max_sized_frame * (cbs[tc].send_slope
++ / port_transmit_rate);
++ cbs[tc].tc = tc;
++
++ hi_credit = (ENETC_CLK * 100L) * (u64)cbs[tc].hi_credit
++ / port_transmit_rate;
++ enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), hi_credit);
++
++ /* Set bw register and enable this traffic class*/
++ enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc),
++ (cbs[tc].bw & 0x7F) | (1 << 31));
++ cbs[tc].enable = true;
++
++ return 0;
++}
++
++static int enetc_get_cbs(struct net_device *ndev, u8 tc)
++{
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ struct enetc_si *si = priv->si;
++ struct enetc_cbs *ecbs = si->ecbs;
++ struct cbs *cbs;
++
++ if (!ecbs)
++ return -ENOMEM;
++ cbs = ecbs->cbs;
++ if (tc >= ecbs->tc_nums) {
++ dev_err(&ndev->dev, "The maximum of TC is %d\n", ecbs->tc_nums);
++ return -EINVAL;
++ }
++
++ return cbs[tc].bw;
++}
++
++static int enetc_set_tsd(struct net_device *ndev, struct tsn_tsd *ttsd)
++{
++ return 0;
++}
++
++static int enetc_get_tsd(struct net_device *ndev, struct tsn_tsd_status *tts)
++{
++ return 0;
++}
++
++static u32 get_ndev_speed(struct net_device *netdev)
++{
++ struct ethtool_link_ksettings ksettings;
++ int rc = -1;
++
++ if (netdev->ethtool_ops->get_link_ksettings) {
++ if (netdev->ethtool_ops->begin) {
++ rc = netdev->ethtool_ops->begin(netdev);
++ if (rc < 0)
++ return 0;
++ }
++
++ memset(&ksettings, 0, sizeof(ksettings));
++
++ if (!netdev->ethtool_ops->get_link_ksettings)
++ return 0;
++
++ rc = netdev->ethtool_ops->get_link_ksettings(netdev,
++ &ksettings);
++
++ if (netdev->ethtool_ops->complete)
++ netdev->ethtool_ops->complete(netdev);
++ }
++
++ return (rc < 0) ? 0 : ksettings.base.speed;
++}
++
++static void enetc_cbs_init(struct enetc_si *si)
++{
++ struct enetc_ndev_priv *priv = netdev_priv(si->ndev);
++ u8 tc_nums;
++
++ tc_nums = priv->num_tx_rings;
++ si->ecbs = kzalloc(sizeof(*si->ecbs) +
++ sizeof(struct cbs) * tc_nums, GFP_KERNEL);
++ if (!si->ecbs)
++ return;
++
++ si->ecbs->port_max_size_frame = si->ndev->mtu + ETH_HLEN
++ + VLAN_HLEN + ETH_FCS_LEN;
++ si->ecbs->tc_nums = tc_nums;
++ si->ecbs->port_transmit_rate = get_ndev_speed(si->ndev);
++
++ /*This trick is used only for CFP*/
++ if (!si->ecbs->port_transmit_rate)
++ si->ecbs->port_transmit_rate = 1000000000;
++
++ if (!si->ecbs->port_transmit_rate) {
++ dev_err(&si->pdev->dev, "Failure to get port speed for CBS\n");
++ kfree(si->ecbs);
++ si->ecbs = NULL;
++ }
++}
++
++static void enetc_qbv_init(struct enetc_hw *hw)
++{
++ /* Set PSPEED to be 1Gbps */
++ enetc_port_wr(hw, ENETC_PMR,
++ (enetc_port_rd(hw, ENETC_PMR)
++ & (~ENETC_PMR_PSPEED_MASK))
++ | ENETC_PMR_PSPEED_1000M);
++}
++
++void enetc_tsn_init(struct net_device *ndev)
++{
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ struct enetc_si *si = priv->si;
++ u32 capability = 0;
++
++ capability = __enetc_tsn_get_cap(si);
++
++ if (capability & TSN_CAP_CBS)
++ enetc_cbs_init(si);
++
++ if (capability & TSN_CAP_QBV)
++ enetc_qbv_init(&si->hw);
++
++ if (capability & TSN_CAP_QCI)
++ enetc_qci_enable(&si->hw);
++
++ dev_info(&si->pdev->dev, "%s: setup done\n", __func__);
++}
++
++void enetc_tsn_deinit(struct net_device *ndev)
++{
++ struct enetc_ndev_priv *priv = netdev_priv(ndev);
++ struct enetc_si *si = priv->si;
++
++ dev_info(&si->pdev->dev, "%s: release\n", __func__);
++}
++
++static struct tsn_ops enetc_tsn_ops_full = {
++ .device_init = enetc_tsn_init,
++ .device_deinit = enetc_tsn_deinit,
++ .get_capability = enetc_tsn_get_capability,
++ .qbv_set = enetc_qbv_set,
++ .qbv_get = enetc_qbv_get,
++ .qbv_get_status = enetc_qbv_get_status,
++ .cb_streamid_set = enetc_cb_streamid_set,
++ .cb_streamid_get = enetc_cb_streamid_get,
++ .cb_streamid_counters_get = enetc_cb_streamid_counters_get,
++ .qci_get_maxcap = enetc_get_max_cap,
++ .qci_sfi_set = enetc_qci_sfi_set,
++ .qci_sfi_get = enetc_qci_sfi_get,
++ .qci_sfi_counters_get = enetc_qci_sfi_counters_get,
++ .qci_sgi_set = enetc_qci_sgi_set,
++ .qci_sgi_get = enetc_qci_sgi_get,
++ .qci_sgi_status_get = enetc_qci_sgi_status_get,
++ .qci_fmi_set = enetc_qci_fmi_set,
++ .qci_fmi_get = enetc_qci_fmi_get,
++ .qbu_set = enetc_qbu_set,
++ .qbu_get = enetc_qbu_get,
++ .cbs_set = enetc_set_cbs,
++ .cbs_get = enetc_get_cbs,
++ .tsd_set = enetc_set_tsd,
++ .tsd_get = enetc_get_tsd,
++};
++
++static struct tsn_ops enetc_tsn_ops_part = {
++ .device_init = enetc_tsn_init,
++ .device_deinit = enetc_tsn_deinit,
++ .get_capability = enetc_tsn_get_capability,
++ .cb_streamid_set = enetc_cb_streamid_set,
++ .cb_streamid_get = enetc_cb_streamid_get,
++ .cb_streamid_counters_get = enetc_cb_streamid_counters_get,
++ .qci_get_maxcap = enetc_get_max_cap,
++ .qci_sfi_set = enetc_qci_sfi_set,
++ .qci_sfi_get = enetc_qci_sfi_get,
++ .qci_sfi_counters_get = enetc_qci_sfi_counters_get,
++ .qci_sgi_set = enetc_qci_sgi_set,
++ .qci_sgi_get = enetc_qci_sgi_get,
++ .qci_sgi_status_get = enetc_qci_sgi_status_get,
++ .qci_fmi_set = enetc_qci_fmi_set,
++ .qci_fmi_get = enetc_qci_fmi_get,
++};
++
++void enetc_tsn_pf_init(struct net_device *netdev, struct pci_dev *pdev)
++{
++ int port = pdev->devfn & 0x7;
++
++ if (port == 1 || port == 3)
++ tsn_port_register(netdev, &enetc_tsn_ops_part,
++ (u16)pdev->bus->number);
++ else
++ tsn_port_register(netdev, &enetc_tsn_ops_full,
++ (u16)pdev->bus->number);
++}
++
++void enetc_tsn_pf_deinit(struct net_device *netdev)
++{
++ tsn_port_unregister(netdev);
++}
++#endif /* #if IS_ENABLED(CONFIG_ENETC_TSN) */