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 {
@@ -261,3 +292,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
@@ -241,10 +242,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
@@ -298,6 +309,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] */
@@ -456,6 +476,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)
{
@@ -483,6 +504,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;
@@ -629,21 +652,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;
@@ -660,13 +969,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];
@@ -681,11 +1032,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);
}
@@ -942,6 +949,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:
@@ -974,6 +983,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) */