[RFC net-next 0/5] net: stmmac: Introducing IEEE802.1Qbv feature

[RFC net-next 0/5] net: stmmac: Introducing IEEE802.1Qbv feature

[Voon Weifeng]

Enabling IEEE 802.1Qbv Enhancements for Scheduled Traffics (EST) which
is available in EQoS ver5.xx. The EST features can be configured using
tc taprio hw offload which proposed by Vinicius. A few hw tunable data
are configured through platform data.

Ong Boon Leong (1):
  net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

Vinicius Costa Gomes (1):
  taprio: Add support for hardware offloading

Voon Weifeng (3):
  net: stmmac: gcl errors reporting and its interrupt handling
  net: stmmac: enable HW offloading for tc taprio
  net: stmmac: Set TSN HW tunable after tsn setup

 drivers/net/ethernet/stmicro/stmmac/Makefile      |   2 +-
 drivers/net/ethernet/stmicro/stmmac/common.h      |   4 +
 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c  | 913 ++++++++++++++++++++++
 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h  | 218 ++++++
 drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c |  16 +
 drivers/net/ethernet/stmicro/stmmac/hwif.h        |  66 ++
 drivers/net/ethernet/stmicro/stmmac/stmmac_main.c |  71 ++
 drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c   |  96 +++
 include/linux/netdevice.h                         |   1 +
 include/linux/stmmac.h                            |   4 +
 include/net/pkt_sched.h                           |  18 +
 include/uapi/linux/pkt_sched.h                    |   4 +
 net/sched/sch_taprio.c                            | 263 ++++++-
 13 files changed, 1673 insertions(+), 3 deletions(-)
 create mode 100644 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
 create mode 100644 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h

-- 
1.9.1

[RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Voon Weifeng]

From: Ong Boon Leong <boon.leong.ong@intel.com>

IEEE 802.1Qbv Enhancements for Scheduled Traffics (EST) is available in
EQoS ver5.xx. The change adds basic EST functionalities:

a) EST initialization with hardware capabilities detection.
b) Setting Gate Control List (GCL), i.e. gate open/close & time intervals,
   and all GC Related Registers (GCRR), e.g., base time (BTR), cycle time
   (CTR), time extension (TER) and GC List Length (LLR).
c) Setting time interval left shift (TILS), PTP time offset (PTOV) and
   current time offset (CTOV).
d) Enable/disable EST.
e) Getting TSN hardware capabilities.
f) Getting Gate Control configuration either from driver data store or
   hardware.

We extend the main driver logic to include basic TSN capability discovery,
and setup. We also add EST feature enable/disable control.

Reviewed-by: Chuah Kim Tatt <kim.tatt.chuah@intel.com>
Reviewed-by: Voon Weifeng <weifeng.voon@intel.com>
Reviewed-by: Kweh Hock Leong <hock.leong.kweh@intel.com>
Signed-off-by: Ong Boon Leong <boon.leong.ong@intel.com>
Signed-off-by: Voon Weifeng <weifeng.voon@intel.com>
---
 drivers/net/ethernet/stmicro/stmmac/Makefile      |   2 +-
 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c  | 790 ++++++++++++++++++++++
 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h  | 173 +++++
 drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c |  13 +
 drivers/net/ethernet/stmicro/stmmac/hwif.h        |  52 ++
 drivers/net/ethernet/stmicro/stmmac/stmmac_main.c |  46 ++
 include/linux/stmmac.h                            |   1 +
 7 files changed, 1076 insertions(+), 1 deletion(-)
 create mode 100644 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
 create mode 100644 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h

diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile
index c59926d96bcc..76fb36cb4da7 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Makefile
+++ b/drivers/net/ethernet/stmicro/stmmac/Makefile
@@ -6,7 +6,7 @@ stmmac-objs:= stmmac_main.o stmmac_ethtool.o stmmac_mdio.o ring_mode.o	\
 	      mmc_core.o stmmac_hwtstamp.o stmmac_ptp.o dwmac4_descs.o	\
 	      dwmac4_dma.o dwmac4_lib.o dwmac4_core.o dwmac5.o hwif.o \
 	      stmmac_tc.o dwxgmac2_core.o dwxgmac2_dma.o dwxgmac2_descs.o \
-	      $(stmmac-y)
+	      dw_tsn_lib.o $(stmmac-y)
 
 stmmac-$(CONFIG_STMMAC_SELFTESTS) += stmmac_selftests.o
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
new file mode 100644
index 000000000000..cba27c604cb1
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
@@ -0,0 +1,790 @@
+// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
+/* Copyright (c) 2019, Intel Corporation.
+ * dw_tsn_lib.c: DW EQoS v5.00 TSN capabilities
+ */
+
+#include "dwmac4.h"
+#include "dwmac5.h"
+#include "dw_tsn_lib.h"
+
+static struct tsn_hw_cap dw_tsn_hwcap;
+static bool dw_tsn_feat_en[TSN_FEAT_ID_MAX];
+static unsigned int dw_tsn_hwtunable[TSN_HWTUNA_MAX];
+static struct est_gc_config dw_est_gc_config;
+
+static unsigned int est_get_gcl_depth(unsigned int hw_cap)
+{
+	unsigned int estdep = (hw_cap & GMAC_HW_FEAT_ESTDEP)
+			>> GMAC_HW_FEAT_ESTDEP_SHIFT;
+	unsigned int depth;
+
+	switch (estdep) {
+	case 1:
+		depth = 64;
+		break;
+	case 2:
+		depth = 128;
+		break;
+	case 3:
+		depth = 256;
+		break;
+	case 4:
+		depth = 512;
+		break;
+	case 5:
+		depth = 1024;
+		break;
+	default:
+		depth = 0;
+	}
+
+	return depth;
+}
+
+static unsigned int est_get_ti_width(unsigned int hw_cap)
+{
+	unsigned int estwid = (hw_cap & GMAC_HW_FEAT_ESTWID)
+			>> GMAC_HW_FEAT_ESTWID_SHIFT;
+	unsigned int width;
+
+	switch (estwid) {
+	case 1:
+		width = 16;
+		break;
+	case 2:
+		width = 20;
+		break;
+	case 3:
+		width = 24;
+		break;
+	default:
+		width = 0;
+	}
+
+	return width;
+}
+
+static int est_poll_srwo(void *ioaddr)
+{
+	/* Poll until the EST GCL Control[SRWO] bit clears.
+	 * Total wait = 12 x 50ms ~= 0.6s.
+	 */
+	unsigned int retries = 12;
+	unsigned int value;
+
+	do {
+		value = TSN_RD32(ioaddr + MTL_EST_GCL_CTRL);
+		if (!(value & MTL_EST_GCL_CTRL_SRWO))
+			return 0;
+		msleep(50);
+	} while (--retries);
+
+	return -ETIMEDOUT;
+}
+
+static int est_set_gcl_addr(void *ioaddr, unsigned int addr,
+			    unsigned int gcrr, unsigned int rwops,
+			    unsigned int dbgb, unsigned int dbgm)
+{
+	unsigned int value;
+
+	value = MTL_EST_GCL_CTRL_ADDR_VAL(addr) & MTL_EST_GCL_CTRL_ADDR;
+
+	if (dbgm) {
+		if (dbgb)
+			value |= MTL_EST_GCL_CTRL_DBGB1;
+
+		value |= MTL_EST_GCL_CTRL_DBGM;
+	}
+
+	if (gcrr)
+		value |= MTL_EST_GCL_CTRL_GCRR;
+
+	/* This is the only place SRWO is set and driver polls SRWO
+	 * for self-cleared before exit. Therefore, caller should
+	 * check return status for possible time out error.
+	 */
+	value |= (rwops | MTL_EST_GCL_CTRL_SRWO);
+
+	TSN_WR32(value, ioaddr + MTL_EST_GCL_CTRL);
+
+	return est_poll_srwo(ioaddr);
+}
+
+static int est_write_gcl_config(void *ioaddr, unsigned int data,
+				unsigned int addr, unsigned int gcrr,
+				unsigned int dbgb, unsigned int dbgm)
+{
+	TSN_WR32(data, ioaddr + MTL_EST_GCL_DATA);
+
+	return est_set_gcl_addr(ioaddr, addr, gcrr, GCL_OPS_W, dbgb, dbgm);
+}
+
+static int est_read_gcl_config(void *ioaddr, unsigned int *data,
+			       unsigned int addr, unsigned int gcrr,
+			       unsigned int dbgb, unsigned int dbgm)
+{
+	int ret;
+
+	ret = est_set_gcl_addr(ioaddr, addr, gcrr, GCL_OPS_R, dbgb, dbgm);
+	if (ret)
+		return ret;
+
+	*data = TSN_RD32(ioaddr + MTL_EST_GCL_DATA);
+
+	return ret;
+}
+
+static int est_read_gce(void *ioaddr, unsigned int row,
+			unsigned int *gates, unsigned int *ti_nsec,
+			unsigned int dbgb, unsigned int dbgm)
+{
+	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
+	unsigned int ti_wid = cap->ti_wid;
+	unsigned int gates_mask;
+	unsigned int ti_mask;
+	unsigned int value;
+	int ret;
+
+	gates_mask = (1 << cap->txqcnt) - 1;
+	ti_mask = (1 << ti_wid) - 1;
+
+	ret = est_read_gcl_config(ioaddr, &value, row, 0, dbgb, dbgm);
+	if (ret) {
+		TSN_ERR("Read GCE failed! row=%u\n", row);
+
+		return ret;
+	}
+	*ti_nsec = value & ti_mask;
+	*gates = (value >> ti_wid) & gates_mask;
+
+	return ret;
+}
+
+static unsigned int est_get_gcl_total_intervals_nsec(unsigned int bank,
+						     unsigned int gcl_len)
+{
+	struct est_gc_entry *gcl = dw_est_gc_config.gcb[bank].gcl;
+	unsigned int nsec = 0;
+	unsigned int row;
+
+	for (row = 0; row < gcl_len; row++) {
+		nsec += gcl->ti_nsec;
+		gcl++;
+	}
+
+	return nsec;
+}
+
+static int est_set_tils(void *ioaddr, const unsigned int tils)
+{
+	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
+	unsigned int value;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	if (tils > cap->tils_max) {
+		TSN_WARN("EST: invalid tils(%u), max=%u\n",
+			 tils, cap->tils_max);
+
+		return -EINVAL;
+	}
+
+	/* Ensure that HW is not in the midst of GCL transition */
+	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
+	value &= ~MTL_EST_CTRL_SSWL;
+
+	/* MTL_EST_CTRL value has been read earlier, if TILS value
+	 * differs, we update here.
+	 */
+	if (tils != dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_TILS]) {
+		value &= ~MTL_EST_CTRL_TILS;
+		value |= (tils << MTL_EST_CTRL_TILS_SHIFT);
+
+		TSN_WR32(value, ioaddr + MTL_EST_CTRL);
+		dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_TILS] = tils;
+	}
+
+	return 0;
+}
+
+static int est_set_ov(void *ioaddr,
+		      const unsigned int *ptov,
+		      const unsigned int *ctov)
+{
+	unsigned int value;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
+	value &= ~MTL_EST_CTRL_SSWL;
+
+	if (ptov) {
+		if (*ptov > EST_PTOV_MAX) {
+			TSN_WARN("EST: invalid PTOV(%u), max=%u\n",
+				 *ptov, EST_PTOV_MAX);
+
+			return -EINVAL;
+		} else if (*ptov !=
+			   dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_PTOV]) {
+			value &= ~MTL_EST_CTRL_PTOV;
+			value |= (*ptov << MTL_EST_CTRL_PTOV_SHIFT);
+			dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_PTOV] = *ptov;
+		}
+	}
+
+	if (ctov) {
+		if (*ctov > EST_CTOV_MAX) {
+			TSN_WARN("EST: invalid CTOV(%u), max=%u\n",
+				 *ctov, EST_CTOV_MAX);
+
+			return -EINVAL;
+		} else if (*ctov != dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_CTOV]) {
+			value &= ~MTL_EST_CTRL_CTOV;
+			value |= (*ctov << MTL_EST_CTRL_CTOV_SHIFT);
+			dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_CTOV] = *ctov;
+		}
+	}
+
+	TSN_WR32(value, ioaddr + MTL_EST_CTRL);
+
+	return 0;
+}
+
+void dwmac_tsn_init(void *ioaddr)
+{
+	unsigned int hwid = TSN_RD32(ioaddr + GMAC4_VERSION) & TSN_VER_MASK;
+	unsigned int hw_cap2 = TSN_RD32(ioaddr + GMAC_HW_FEATURE2);
+	unsigned int hw_cap3 = TSN_RD32(ioaddr + GMAC_HW_FEATURE3);
+	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
+	unsigned int gcl_depth;
+	unsigned int tils_max;
+	unsigned int ti_wid;
+
+	memset(cap, 0, sizeof(*cap));
+
+	if (hwid < TSN_CORE_VER) {
+		TSN_WARN_NA("IP v5.00 does not support TSN\n");
+		return;
+	}
+
+	if (!(hw_cap3 & GMAC_HW_FEAT_ESTSEL)) {
+		TSN_WARN_NA("EST NOT supported\n");
+		cap->est_support = 0;
+
+		return;
+	}
+
+	gcl_depth = est_get_gcl_depth(hw_cap3);
+	ti_wid = est_get_ti_width(hw_cap3);
+
+	cap->ti_wid = ti_wid;
+	cap->gcl_depth = gcl_depth;
+
+	tils_max = (hw_cap3 & GMAC_HW_FEAT_ESTSEL ? 3 : 0);
+	tils_max = (1 << tils_max) - 1;
+	cap->tils_max = tils_max;
+
+	cap->ext_max = EST_TIWID_TO_EXTMAX(ti_wid);
+	cap->txqcnt = ((hw_cap2 & GMAC_HW_FEAT_TXQCNT) >> 6) + 1;
+	cap->est_support = 1;
+
+	TSN_INFO("EST: depth=%u, ti_wid=%u, tils_max=%u tqcnt=%u\n",
+		 gcl_depth, ti_wid, tils_max, cap->txqcnt);
+}
+
+void dwmac_get_tsn_hwcap(struct tsn_hw_cap **tsn_hwcap)
+{
+	*tsn_hwcap = &dw_tsn_hwcap;
+}
+
+void dwmac_set_est_gcb(struct est_gc_entry *gcl, unsigned int bank)
+{
+	if (bank >= 0 && bank < EST_GCL_BANK_MAX)
+		dw_est_gc_config.gcb[bank].gcl = gcl;
+}
+
+void dwmac_set_tsn_feat(enum tsn_feat_id featid, bool enable)
+{
+	if (featid < TSN_FEAT_ID_MAX)
+		dw_tsn_feat_en[featid] = enable;
+}
+
+int dwmac_set_tsn_hwtunable(void *ioaddr,
+			    enum tsn_hwtunable_id id,
+			    const unsigned int *data)
+{
+	int ret = 0;
+
+	switch (id) {
+	case TSN_HWTUNA_TX_EST_TILS:
+		ret = est_set_tils(ioaddr, *data);
+		break;
+	case TSN_HWTUNA_TX_EST_PTOV:
+		ret = est_set_ov(ioaddr, data, NULL);
+		break;
+	case TSN_HWTUNA_TX_EST_CTOV:
+		ret = est_set_ov(ioaddr, NULL, data);
+		break;
+	default:
+		ret = -EINVAL;
+	};
+
+	return ret;
+}
+
+int dwmac_get_tsn_hwtunable(enum tsn_hwtunable_id id, unsigned int *data)
+{
+	if (id >= TSN_HWTUNA_MAX)
+		return -EINVAL;
+
+	*data = dw_tsn_hwtunable[id];
+
+	return 0;
+}
+
+int dwmac_get_est_bank(void *ioaddr, unsigned int own)
+{
+	int swol;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	swol = TSN_RD32(ioaddr + MTL_EST_STATUS);
+
+	swol = ((swol & MTL_EST_STATUS_SWOL) >>
+		MTL_EST_STATUS_SWOL_SHIFT);
+
+	if (own)
+		return swol;
+	else
+		return (~swol & 0x1);
+}
+
+int dwmac_set_est_gce(void *ioaddr,
+		      struct est_gc_entry *gce, unsigned int row,
+		      unsigned int dbgb, unsigned int dbgm)
+{
+	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
+	unsigned int ti_nsec = gce->ti_nsec;
+	unsigned int gates = gce->gates;
+	struct est_gc_entry *gcl;
+	unsigned int gates_mask;
+	unsigned int ti_wid;
+	unsigned int ti_max;
+	unsigned int value;
+	unsigned int bank;
+	int ret;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	if (dbgb >= EST_GCL_BANK_MAX)
+		return -EINVAL;
+
+	if (dbgm) {
+		bank = dbgb;
+	} else {
+		value = TSN_RD32(ioaddr + MTL_EST_STATUS);
+		bank = (value & MTL_EST_STATUS_SWOL) >>
+		       MTL_EST_STATUS_SWOL_SHIFT;
+	}
+
+	if (!cap->gcl_depth || row > cap->gcl_depth) {
+		TSN_WARN("EST: row(%u) > GCL depth(%u)\n",
+			 row, cap->gcl_depth);
+
+		return -EINVAL;
+	}
+
+	ti_wid = cap->ti_wid;
+	ti_max = (1 << ti_wid) - 1;
+	if (ti_nsec > ti_max) {
+		TSN_WARN("EST: ti_nsec(%u) > upper limit(%u)\n",
+			 ti_nsec, ti_max);
+
+		return -EINVAL;
+	}
+
+	gates_mask = (1 << cap->txqcnt) - 1;
+	value = ((gates & gates_mask) << ti_wid) | ti_nsec;
+
+	ret = est_write_gcl_config(ioaddr, value, row, 0, dbgb, dbgm);
+	if (ret) {
+		TSN_ERR("EST: GCE write failed: bank=%u row=%u.\n",
+			bank, row);
+
+		return ret;
+	}
+
+	TSN_INFO("EST: GCE write: dbgm=%u bank=%u row=%u, gc=0x%x.\n",
+		 dbgm, bank, row, value);
+
+	/* Since GC write is successful, update GCL copy of the driver */
+	gcl = dw_est_gc_config.gcb[bank].gcl + row;
+	gcl->gates = gates;
+	gcl->ti_nsec = ti_nsec;
+
+	return ret;
+}
+
+int dwmac_get_est_gcrr_llr(void *ioaddr, unsigned int *gcl_len,
+			   unsigned int dbgb, unsigned int dbgm)
+{
+	unsigned int bank, value;
+	int ret;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	if (dbgb >= EST_GCL_BANK_MAX)
+		return -EINVAL;
+
+	if (dbgm) {
+		bank = dbgb;
+	} else {
+		value = TSN_RD32(ioaddr + MTL_EST_STATUS);
+		bank = (value & MTL_EST_STATUS_SWOL) >>
+		       MTL_EST_STATUS_SWOL_SHIFT;
+	}
+
+	ret = est_read_gcl_config(ioaddr, &value,
+				  GCL_CTRL_ADDR_LLR, 1,
+				  dbgb, dbgm);
+	if (ret) {
+		TSN_ERR("read LLR fail at bank=%u\n", bank);
+
+			return ret;
+	}
+
+	*gcl_len = value;
+
+	return 0;
+}
+
+int dwmac_set_est_gcrr_llr(void *ioaddr, unsigned int gcl_len,
+			   unsigned int dbgb, unsigned int dbgm)
+{
+	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
+	unsigned int bank, value;
+	struct est_gcrr *bgcrr;
+	int ret = 0;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	if (dbgb >= EST_GCL_BANK_MAX)
+		return -EINVAL;
+
+	if (dbgm) {
+		bank = dbgb;
+	} else {
+		value = TSN_RD32(ioaddr + MTL_EST_STATUS);
+		bank = (value & MTL_EST_STATUS_SWOL) >>
+		       MTL_EST_STATUS_SWOL_SHIFT;
+	}
+
+	if (gcl_len > cap->gcl_depth) {
+		TSN_WARN("EST: GCL length(%u) > depth(%u)\n",
+			 gcl_len, cap->gcl_depth);
+
+		return -EINVAL;
+	}
+
+	bgcrr = &dw_est_gc_config.gcb[bank].gcrr;
+
+	if (gcl_len != bgcrr->llr) {
+		ret = est_write_gcl_config(ioaddr, gcl_len,
+					   GCL_CTRL_ADDR_LLR, 1,
+					   dbgb, dbgm);
+		if (ret) {
+			TSN_ERR_NA("EST: GCRR programming failure!\n");
+
+			return ret;
+		}
+		bgcrr->llr = gcl_len;
+	}
+
+	return 0;
+}
+
+int dwmac_set_est_gcrr_times(void *ioaddr,
+			     struct est_gcrr *gcrr,
+			     unsigned int dbgb, unsigned int dbgm)
+{
+	unsigned int cycle_nsec = gcrr->cycle_nsec;
+	unsigned int cycle_sec = gcrr->cycle_sec;
+	unsigned int base_nsec = gcrr->base_nsec;
+	unsigned int base_sec = gcrr->base_sec;
+	unsigned int ext_nsec = gcrr->ter_nsec;
+	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
+	unsigned int gcl_len, tti_ns, value;
+	struct est_gcrr *bgcrr;
+	u64 val_ns, sys_ns;
+	unsigned int bank;
+	int ret = 0;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	if (dbgb >= EST_GCL_BANK_MAX)
+		return -EINVAL;
+
+	if (dbgm) {
+		bank = dbgb;
+	} else {
+		value = TSN_RD32(ioaddr + MTL_EST_STATUS);
+		bank = (value & MTL_EST_STATUS_SWOL) >>
+		       MTL_EST_STATUS_SWOL_SHIFT;
+	}
+
+	if (base_nsec > 1000000000ULL || cycle_nsec > 1000000000ULL) {
+		TSN_WARN("EST: base(%u) or cycle(%u) nsec > 1s !\n",
+			 base_nsec, cycle_nsec);
+
+		return -EINVAL;
+	}
+
+	/* Ensure base time is later than MAC system time */
+	val_ns = (u64)base_nsec;
+	val_ns += (u64)(base_sec * 1000000000ULL);
+
+	/* Get the MAC system time */
+	sys_ns = TSN_RD32(ioaddr + TSN_PTP_STNSR);
+	sys_ns += TSN_RD32(ioaddr + TSN_PTP_STSR) * 1000000000ULL;
+
+	if (val_ns <= sys_ns) {
+		TSN_WARN("EST: base time(%llu) <= system time(%llu)\n",
+			 val_ns, sys_ns);
+
+		return -EINVAL;
+	}
+
+	if (cycle_sec > EST_CTR_HI_MAX) {
+		TSN_WARN("EST: cycle time(%u) > 255 seconds\n", cycle_sec);
+
+		return -EINVAL;
+	}
+
+	if (ext_nsec > cap->ext_max) {
+		TSN_WARN("EST: invalid time extension(%u), max=%u\n",
+			 ext_nsec, cap->ext_max);
+
+		return -EINVAL;
+	}
+
+	bgcrr = &dw_est_gc_config.gcb[bank].gcrr;
+	gcl_len = bgcrr->llr;
+
+	/* Sanity test on GCL total time intervals against cycle time.
+	 * a) For GC length = 1, if its time interval is equal or greater
+	 *    than cycle time, it is a constant gate error.
+	 * b) If total time interval > cycle time, irregardless of GC
+	 *    length, it is not considered an error that GC list is
+	 *    truncated. In this case, giving a warning message is
+	 *    sufficient.
+	 * c) If total time interval < cycle time, irregardless of GC
+	 *    length, all GATES are OPEN after the last GC is processed
+	 *    until cycle time lapses. This is potentially due to poor
+	 *    GCL configuration but is not an error, so we inform user
+	 *    about it.
+	 */
+	tti_ns = est_get_gcl_total_intervals_nsec(bank, gcl_len);
+	val_ns = (u64)cycle_nsec;
+	val_ns += (u64)(cycle_sec * 1000000000ULL);
+	if (gcl_len == 1 && tti_ns >= val_ns) {
+		TSN_WARN_NA("EST: Constant gate error!\n");
+
+		return -EINVAL;
+	}
+
+	if (tti_ns > val_ns)
+		TSN_WARN_NA("EST: GCL is truncated!\n");
+
+	if (tti_ns < val_ns) {
+		TSN_INFO("EST: All GCs OPEN at %u of %llu-ns cycle\n",
+			 tti_ns, val_ns);
+	}
+
+	/* Finally, start programming GCL related registers if the value
+	 * differs from the driver copy for efficiency.
+	 */
+
+	if (base_nsec != bgcrr->base_nsec)
+		ret |= est_write_gcl_config(ioaddr, base_nsec,
+					    GCL_CTRL_ADDR_BTR_LO, 1,
+					    dbgb, dbgm);
+
+	if (base_sec != bgcrr->base_sec)
+		ret |= est_write_gcl_config(ioaddr, base_sec,
+					    GCL_CTRL_ADDR_BTR_HI, 1,
+					    dbgb, dbgm);
+
+	if (cycle_nsec != bgcrr->cycle_nsec)
+		ret |= est_write_gcl_config(ioaddr, cycle_nsec,
+					    GCL_CTRL_ADDR_CTR_LO, 1,
+					    dbgb, dbgm);
+
+	if (cycle_sec != bgcrr->cycle_sec)
+		ret |= est_write_gcl_config(ioaddr, cycle_sec,
+					    GCL_CTRL_ADDR_CTR_HI, 1,
+					    dbgb, dbgm);
+
+	if (ext_nsec != bgcrr->ter_nsec)
+		ret |= est_write_gcl_config(ioaddr, ext_nsec,
+					    GCL_CTRL_ADDR_TER, 1,
+					    dbgb, dbgm);
+
+	if (ret) {
+		TSN_ERR_NA("EST: GCRR programming failure!\n");
+
+		return ret;
+	}
+
+	/* Finally, we are ready to switch SWOL now. */
+	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
+	value |= MTL_EST_CTRL_SSWL;
+	TSN_WR32(value, ioaddr + MTL_EST_CTRL);
+
+	/* Update driver copy */
+	bgcrr->base_sec = base_sec;
+	bgcrr->base_nsec = base_nsec;
+	bgcrr->cycle_sec = cycle_sec;
+	bgcrr->cycle_nsec = cycle_nsec;
+	bgcrr->ter_nsec = ext_nsec;
+
+	TSN_INFO_NA("EST: gcrr set successful\n");
+
+	return 0;
+}
+
+int dwmac_set_est_enable(void *ioaddr, bool enable)
+{
+	unsigned int value;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
+	value &= ~(MTL_EST_CTRL_SSWL | MTL_EST_CTRL_EEST);
+	value |= (enable & MTL_EST_CTRL_EEST);
+	TSN_WR32(value, ioaddr + MTL_EST_CTRL);
+	dw_est_gc_config.enable = enable;
+
+	return 0;
+}
+
+int dwmac_get_est_gcc(void *ioaddr,
+		      struct est_gc_config **gcc, bool frmdrv)
+{
+	struct est_gc_config *pgcc;
+	unsigned int bank;
+	unsigned int value;
+	int ret;
+
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	/* Get GC config from driver */
+	if (frmdrv) {
+		*gcc = &dw_est_gc_config;
+
+		TSN_INFO_NA("EST: read GCL from driver copy done.\n");
+
+		return 0;
+	}
+
+	/* Get GC config from HW */
+	pgcc = &dw_est_gc_config;
+
+	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
+	pgcc->enable = value & MTL_EST_CTRL_EEST;
+
+	for (bank = 0; bank < EST_GCL_BANK_MAX; bank++) {
+		unsigned int llr, row;
+		struct est_gc_bank *gcbc = &pgcc->gcb[bank];
+
+		ret = est_read_gcl_config(ioaddr, &value,
+					  GCL_CTRL_ADDR_BTR_LO, 1,
+					  bank, 1);
+		if (ret) {
+			TSN_ERR("read BTR(low) fail at bank=%u\n", bank);
+
+			return ret;
+		}
+		gcbc->gcrr.base_nsec = value;
+
+		ret = est_read_gcl_config(ioaddr, &value,
+					  GCL_CTRL_ADDR_BTR_HI, 1,
+					  bank, 1);
+		if (ret) {
+			TSN_ERR("read BTR(high) fail at bank=%u\n", bank);
+
+			return ret;
+		}
+		gcbc->gcrr.base_sec = value;
+
+		ret = est_read_gcl_config(ioaddr, &value,
+					  GCL_CTRL_ADDR_CTR_LO, 1,
+					  bank, 1);
+		if (ret) {
+			TSN_ERR("read CTR(low) fail at bank=%u\n", bank);
+
+			return ret;
+		}
+		gcbc->gcrr.cycle_nsec = value;
+
+		ret = est_read_gcl_config(ioaddr, &value,
+					  GCL_CTRL_ADDR_CTR_HI, 1,
+					  bank, 1);
+		if (ret) {
+			TSN_ERR("read CTR(high) fail at bank=%u\n", bank);
+
+			return ret;
+		}
+		gcbc->gcrr.cycle_sec = value;
+
+		ret = est_read_gcl_config(ioaddr, &value,
+					  GCL_CTRL_ADDR_TER, 1,
+					  bank, 1);
+		if (ret) {
+			TSN_ERR("read TER fail at bank=%u\n", bank);
+
+			return ret;
+		}
+		gcbc->gcrr.ter_nsec = value;
+
+		ret = est_read_gcl_config(ioaddr, &value,
+					  GCL_CTRL_ADDR_LLR, 1,
+					  bank, 1);
+		if (ret) {
+			TSN_ERR("read LLR fail at bank=%u\n", bank);
+
+			return ret;
+		}
+		gcbc->gcrr.llr = value;
+		llr = value;
+
+		for (row = 0; row < llr; row++) {
+			unsigned int gates, ti_nsec;
+			struct est_gc_entry *gce = gcbc->gcl + row;
+
+			ret = est_read_gce(ioaddr, row, &gates, &ti_nsec,
+					   bank, 1);
+			if (ret) {
+				TSN_ERR("read GCE fail at bank=%u\n", bank);
+
+				return ret;
+			}
+			gce->gates = gates;
+			gce->ti_nsec = ti_nsec;
+		}
+	}
+
+	*gcc = pgcc;
+	TSN_INFO_NA("EST: read GCL from HW done.\n");
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h
new file mode 100644
index 000000000000..feb71f7e7031
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h
@@ -0,0 +1,173 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (c) 2019, Intel Corporation.
+ * dw_tsn_lib.h: DW EQoS v5.00 TSN capabilities header
+ */
+
+#ifndef __DW_TSN_LIB_H__
+#define __DW_TSN_LIB_H__
+
+#include "linux/printk.h"
+
+/* DWMAC v5.xx supports the following Time Sensitive Networking protocols:
+ * 1) IEEE 802.1Qbv Enhancements for Scheduled Traffic (EST)
+ */
+
+/* MAC HW features3 bitmap */
+#define GMAC_HW_FEAT_ESTWID		GENMASK(21, 20)
+#define GMAC_HW_FEAT_ESTWID_SHIFT	20
+#define GMAC_HW_FEAT_ESTDEP		GENMASK(19, 17)
+#define GMAC_HW_FEAT_ESTDEP_SHIFT	17
+#define GMAC_HW_FEAT_ESTSEL		BIT(16)
+
+/* MTL EST control register */
+#define MTL_EST_CTRL			0x00000c50
+#define MTL_EST_CTRL_PTOV		GENMASK(31, 24)
+#define MTL_EST_CTRL_PTOV_SHIFT		24
+#define MTL_EST_CTRL_CTOV		GENMASK(23, 12)
+#define MTL_EST_CTRL_CTOV_SHIFT		12
+#define MTL_EST_CTRL_TILS		GENMASK(10, 8)
+#define MTL_EST_CTRL_TILS_SHIFT		8
+#define MTL_EST_CTRL_SSWL		BIT(1)	/* Switch to SWOL */
+#define MTL_EST_CTRL_EEST		BIT(0)	/* Enable EST */
+
+/* MTL EST status register */
+#define MTL_EST_STATUS			0x00000c58
+#define MTL_EST_STATUS_BTRL		GENMASK(11, 8)	/* BTR ERR loop cnt */
+#define MTL_EST_STATUS_BTRL_SHIFT	8
+#define MTL_EST_STATUS_BTRL_MAX		(0xF << 8)
+#define MTL_EST_STATUS_SWOL		BIT(7)	/* SW owned list */
+#define MTL_EST_STATUS_SWOL_SHIFT	7
+#define MTL_EST_STATUS_BTRE		BIT(1)	/* BTR Error */
+#define MTL_EST_STATUS_SWLC		BIT(0)	/* Switch to SWOL complete */
+
+/* MTL EST GCL control register */
+#define MTL_EST_GCL_CTRL		0x00000c80
+#define MTL_EST_GCL_CTRL_ADDR		GENMASK(10, 8)	/* GCL Address */
+#define MTL_EST_GCL_CTRL_ADDR_VAL(addr)	(addr << 8)
+#define GCL_CTRL_ADDR_BTR_LO		0x0
+#define GCL_CTRL_ADDR_BTR_HI		0x1
+#define GCL_CTRL_ADDR_CTR_LO		0x2
+#define GCL_CTRL_ADDR_CTR_HI		0x3
+#define GCL_CTRL_ADDR_TER		0x4
+#define GCL_CTRL_ADDR_LLR		0x5
+#define MTL_EST_GCL_CTRL_DBGB1		BIT(5)	/* Debug Mode Bank Select */
+#define MTL_EST_GCL_CTRL_DBGM		BIT(4)	/* Debug Mode */
+#define MTL_EST_GCL_CTRL_GCRR		BIT(2)	/* GC Related Registers */
+#define MTL_EST_GCL_CTRL_R1W0		BIT(1)	/* Read / Write Operation */
+#define GCL_OPS_R			BIT(1)
+#define GCL_OPS_W			0
+#define MTL_EST_GCL_CTRL_SRWO		BIT(0)	/* Start R/W Operation */
+
+/* MTL EST GCL data register */
+#define MTL_EST_GCL_DATA		0x00000c84
+
+/* EST Global defines */
+#define EST_CTR_HI_MAX			0xff	/* CTR Hi is 8-bit only */
+#define EST_PTOV_MAX			0xff	/* Max PTP time offset */
+#define EST_CTOV_MAX			0xfff	/* Max Current time offset */
+#define EST_TIWID_TO_EXTMAX(ti_wid)	((1 << (ti_wid + 7)) - 1)
+#define EST_GCL_BANK_MAX	(2)
+
+/* MAC Core Version */
+#define TSN_VER_MASK		0xFF
+#define TSN_CORE_VER		0x50
+
+/* MAC PTP clock registers */
+#define TSN_PTP_STSR		0x08
+#define TSN_PTP_STNSR		0x0c
+
+/* Hardware Tunable Enum */
+enum tsn_hwtunable_id {
+	TSN_HWTUNA_TX_EST_TILS = 0,
+	TSN_HWTUNA_TX_EST_PTOV,
+	TSN_HWTUNA_TX_EST_CTOV,
+	TSN_HWTUNA_MAX,
+};
+
+/* TSN Feature Enabled List */
+enum tsn_feat_id {
+	TSN_FEAT_ID_EST = 0,
+	TSN_FEAT_ID_MAX,
+};
+
+/* HW register read & write macros */
+#define TSN_RD32(__addr)		readl(__addr)
+#define TSN_WR32(__val, __addr)		writel(__val, __addr)
+
+/* Logging macros with no args */
+#define DRVNAME "stmmac"
+#define TSN_INFO_NA(__msg)	printk(KERN_INFO DRVNAME ":" __msg)
+#define TSN_WARN_NA(__msg)	printk(KERN_WARNING DRVNAME ":" __msg)
+#define TSN_ERR_NA(__msg)	printk(KERN_ERR DRVNAME ":" __msg)
+
+/* Logging macros with args */
+#define TSN_INFO(__msg, __arg0, __args...) \
+	printk(KERN_INFO DRVNAME ":" __msg, (__arg0), ##__args)
+#define TSN_WARN(__msg, __arg0, __args...) \
+	printk(KERN_WARNING DRVNAME ":" __msg, (__arg0), ##__args)
+#define TSN_ERR(__msg, __arg0, __args...) \
+	printk(KERN_ERR DRVNAME ":" __msg, (__arg0), ##__args)
+
+/* TSN HW Capabilities */
+struct tsn_hw_cap {
+	bool est_support;		/* 1: supported */
+	unsigned int txqcnt;		/* Number of TxQ (control gate) */
+	unsigned int gcl_depth;		/* GCL depth. */
+	unsigned int ti_wid;		/* time interval width */
+	unsigned int tils_max;		/* Max time interval left shift */
+	unsigned int ext_max;		/* Max time extension */
+};
+
+/* EST Gate Control Entry */
+struct est_gc_entry {
+	unsigned int gates;		/* gate control: 0: closed,
+					 *               1: open.
+					 */
+	unsigned int ti_nsec;		/* time interval in nsec */
+};
+
+/* EST GCL Related Registers */
+struct est_gcrr {
+	unsigned int base_nsec;		/* base time denominator (nsec) */
+	unsigned int base_sec;		/* base time numerator (sec) */
+	unsigned int cycle_nsec;	/* cycle time denominator (nsec) */
+	unsigned int cycle_sec;		/* cycle time numerator sec)*/
+	unsigned int ter_nsec;		/* time extension (nsec) */
+	unsigned int llr;		/* GC list length */
+};
+
+/* EST Gate Control bank */
+struct est_gc_bank {
+	struct est_gc_entry *gcl;	/* Gate Control List */
+	struct est_gcrr gcrr;		/* GCL Related Registers */
+};
+
+/* EST Gate Control Configuration */
+struct est_gc_config {
+	struct est_gc_bank gcb[EST_GCL_BANK_MAX];
+	bool enable;			/* 1: enabled */
+};
+
+/* TSN functions */
+void dwmac_tsn_init(void *ioaddr);
+void dwmac_get_tsn_hwcap(struct tsn_hw_cap **tsn_hwcap);
+void dwmac_set_est_gcb(struct est_gc_entry *gcl, unsigned int bank);
+void dwmac_set_tsn_feat(enum tsn_feat_id featid, bool enable);
+int dwmac_set_tsn_hwtunable(void *ioaddr, enum tsn_hwtunable_id id,
+			    const unsigned int *data);
+int dwmac_get_tsn_hwtunable(enum tsn_hwtunable_id id, unsigned int *data);
+int dwmac_get_est_bank(void *ioaddr, unsigned int own);
+int dwmac_set_est_gce(void *ioaddr,
+		      struct est_gc_entry *gce, unsigned int row,
+		      unsigned int dbgb, unsigned int dbgm);
+int dwmac_get_est_gcrr_llr(void *ioaddr, unsigned int *gcl_len,
+			   unsigned int dbgb, unsigned int dbgm);
+int dwmac_set_est_gcrr_llr(void *ioaddr, unsigned int gcl_len,
+			   unsigned int dbgb, unsigned int dbgm);
+int dwmac_set_est_gcrr_times(void *ioaddr,
+			     struct est_gcrr *gcrr,
+			     unsigned int dbgb, unsigned int dbgm);
+int dwmac_set_est_enable(void *ioaddr, bool enable);
+int dwmac_get_est_gcc(void *ioaddr,
+		      struct est_gc_config **gcc, bool frmdrv);
+#endif /* __DW_TSN_LIB_H__ */
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
index 8d9f6cda4012..1361807fe802 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
@@ -817,6 +817,19 @@ static void dwmac4_set_mac_loopback(void __iomem *ioaddr, bool enable)
 	.pcs_get_adv_lp = dwmac4_get_adv_lp,
 	.debug = dwmac4_debug,
 	.set_filter = dwmac4_set_filter,
+	.tsn_init = dwmac_tsn_init,
+	.get_tsn_hwcap = dwmac_get_tsn_hwcap,
+	.set_est_gcb = dwmac_set_est_gcb,
+	.set_tsn_feat = dwmac_set_tsn_feat,
+	.set_tsn_hwtunable = dwmac_set_tsn_hwtunable,
+	.get_tsn_hwtunable = dwmac_get_tsn_hwtunable,
+	.get_est_bank = dwmac_get_est_bank,
+	.set_est_gce = dwmac_set_est_gce,
+	.get_est_gcrr_llr = dwmac_get_est_gcrr_llr,
+	.set_est_gcrr_llr = dwmac_set_est_gcrr_llr,
+	.set_est_gcrr_times = dwmac_set_est_gcrr_times,
+	.set_est_enable = dwmac_set_est_enable,
+	.get_est_gcc = dwmac_get_est_gcc,
 	.safety_feat_config = dwmac5_safety_feat_config,
 	.safety_feat_irq_status = dwmac5_safety_feat_irq_status,
 	.safety_feat_dump = dwmac5_safety_feat_dump,
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index 2acfbc70e3c8..518a72805185 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -7,6 +7,7 @@
 
 #include <linux/netdevice.h>
 #include <linux/stmmac.h>
+#include "dw_tsn_lib.h"
 
 #define stmmac_do_void_callback(__priv, __module, __cname,  __arg0, __args...) \
 ({ \
@@ -311,6 +312,31 @@ struct stmmac_ops {
 			     bool loopback);
 	void (*pcs_rane)(void __iomem *ioaddr, bool restart);
 	void (*pcs_get_adv_lp)(void __iomem *ioaddr, struct rgmii_adv *adv);
+	/* TSN functions */
+	void (*tsn_init)(void __iomem *ioaddr);
+	void (*get_tsn_hwcap)(struct tsn_hw_cap **tsn_hwcap);
+	void (*set_est_gcb)(struct est_gc_entry *gcl,
+			    u32 bank);
+	void (*set_tsn_feat)(enum tsn_feat_id featid, bool enable);
+	int (*set_tsn_hwtunable)(void __iomem *ioaddr,
+				 enum tsn_hwtunable_id id,
+				 const unsigned int *data);
+	int (*get_tsn_hwtunable)(enum tsn_hwtunable_id id,
+				 unsigned int *data);
+	int (*get_est_bank)(void __iomem *ioaddr, u32 own);
+	int (*set_est_gce)(void __iomem *ioaddr,
+			   struct est_gc_entry *gce, u32 row,
+			   u32 dbgb, u32 dbgm);
+	int (*get_est_gcrr_llr)(void __iomem *ioaddr, u32 *gcl_len,
+				u32 dbgb, u32 dbgm);
+	int (*set_est_gcrr_llr)(void __iomem *ioaddr, u32 gcl_len,
+				u32 dbgb, u32 dbgm);
+	int (*set_est_gcrr_times)(void __iomem *ioaddr,
+				  struct est_gcrr *gcrr,
+				  u32 dbgb, u32 dbgm);
+	int (*set_est_enable)(void __iomem *ioaddr, bool enable);
+	int (*get_est_gcc)(void __iomem *ioaddr,
+			   struct est_gc_config **gcc, bool frmdrv);
 	/* Safety Features */
 	int (*safety_feat_config)(void __iomem *ioaddr, unsigned int asp);
 	int (*safety_feat_irq_status)(struct net_device *ndev,
@@ -385,6 +411,32 @@ struct stmmac_ops {
 	stmmac_do_void_callback(__priv, mac, pcs_rane, __args)
 #define stmmac_pcs_get_adv_lp(__priv, __args...) \
 	stmmac_do_void_callback(__priv, mac, pcs_get_adv_lp, __args)
+#define stmmac_tsn_init(__priv, __args...) \
+	stmmac_do_void_callback(__priv, mac, tsn_init, __args)
+#define stmmac_get_tsn_hwcap(__priv, __args...) \
+	stmmac_do_void_callback(__priv, mac, get_tsn_hwcap, __args)
+#define stmmac_set_est_gcb(__priv, __args...) \
+	stmmac_do_void_callback(__priv, mac, set_est_gcb, __args)
+#define stmmac_set_tsn_feat(__priv, __args...) \
+	stmmac_do_void_callback(__priv, mac, set_tsn_feat, __args)
+#define stmmac_set_tsn_hwtunable(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, set_tsn_hwtunable, __args)
+#define stmmac_get_tsn_hwtunable(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, get_tsn_hwtunable, __args)
+#define stmmac_get_est_bank(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, get_est_bank, __args)
+#define stmmac_set_est_gce(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, set_est_gce, __args)
+#define stmmac_get_est_gcrr_llr(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, get_est_gcrr_llr, __args)
+#define stmmac_set_est_gcrr_llr(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, set_est_gcrr_llr, __args)
+#define stmmac_set_est_gcrr_times(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, set_est_gcrr_times, __args)
+#define stmmac_set_est_enable(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, set_est_enable, __args)
+#define stmmac_get_est_gcc(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, get_est_gcc, __args)
 #define stmmac_safety_feat_config(__priv, __args...) \
 	stmmac_do_callback(__priv, mac, safety_feat_config, __args)
 #define stmmac_safety_feat_irq_status(__priv, __args...) \
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index a48751989fa6..91213cd3a668 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -41,6 +41,7 @@
 #include "stmmac.h"
 #include <linux/reset.h>
 #include <linux/of_mdio.h>
+#include "dw_tsn_lib.h"
 #include "dwmac1000.h"
 #include "dwxgmac2.h"
 #include "hwif.h"
@@ -3621,6 +3622,8 @@ static int stmmac_set_features(struct net_device *netdev,
 	 */
 	stmmac_rx_ipc(priv, priv->hw);
 
+	netdev->features = features;
+
 	return 0;
 }
 
@@ -4070,6 +4073,8 @@ static void stmmac_service_task(struct work_struct *work)
  */
 static int stmmac_hw_init(struct stmmac_priv *priv)
 {
+	struct tsn_hw_cap *tsn_hwcap;
+	int gcl_depth = 0;
 	int ret;
 
 	/* dwmac-sun8i only work in chain mode */
@@ -4082,6 +4087,38 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 	if (ret)
 		return ret;
 
+	/* Initialize TSN capability */
+	stmmac_tsn_init(priv, priv->ioaddr);
+	stmmac_get_tsn_hwcap(priv, &tsn_hwcap);
+	if (tsn_hwcap)
+		gcl_depth = tsn_hwcap->gcl_depth;
+	if (gcl_depth > 0) {
+		u32 bank;
+		struct est_gc_entry *gcl[EST_GCL_BANK_MAX];
+
+		for (bank = 0; bank < EST_GCL_BANK_MAX; bank++) {
+			gcl[bank] = devm_kzalloc(priv->device,
+						 (sizeof(*gcl) * gcl_depth),
+						 GFP_KERNEL);
+			if (!gcl[bank]) {
+				ret = -ENOMEM;
+				break;
+			}
+			stmmac_set_est_gcb(priv, gcl[bank], bank);
+		}
+		if (ret) {
+			int i;
+
+			for (i = bank - 1; i >= 0; i--) {
+				devm_kfree(priv->device, gcl[i]);
+				stmmac_set_est_gcb(priv, NULL, bank);
+			}
+			dev_warn(priv->device, "EST: GCL -ENOMEM\n");
+
+			return ret;
+		}
+	}
+
 	/* Get the HW capability (new GMAC newer than 3.50a) */
 	priv->hw_cap_support = stmmac_get_hw_features(priv);
 	if (priv->hw_cap_support) {
@@ -4168,6 +4205,7 @@ int stmmac_dvr_probe(struct device *device,
 		     struct stmmac_resources *res)
 {
 	struct net_device *ndev = NULL;
+	struct tsn_hw_cap *tsn_hwcap;
 	struct stmmac_priv *priv;
 	u32 queue, maxq;
 	int ret = 0;
@@ -4254,6 +4292,14 @@ int stmmac_dvr_probe(struct device *device,
 	}
 	ndev->features |= ndev->hw_features | NETIF_F_HIGHDMA;
 	ndev->watchdog_timeo = msecs_to_jiffies(watchdog);
+
+	/* TSN HW feature setup */
+	stmmac_get_tsn_hwcap(priv, &tsn_hwcap);
+	if (tsn_hwcap && tsn_hwcap->est_support && priv->plat->tsn_est_en) {
+		stmmac_set_tsn_feat(priv, TSN_FEAT_ID_EST, true);
+		dev_info(priv->device, "EST feature enabled\n");
+	}
+
 #ifdef STMMAC_VLAN_TAG_USED
 	/* Both mac100 and gmac support receive VLAN tag detection */
 	ndev->features |= NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_STAG_RX;
diff --git a/include/linux/stmmac.h b/include/linux/stmmac.h
index 7d06241582dd..d4a90f48e49b 100644
--- a/include/linux/stmmac.h
+++ b/include/linux/stmmac.h
@@ -172,6 +172,7 @@ struct plat_stmmacenet_data {
 	int has_gmac4;
 	bool has_sun8i;
 	bool tso_en;
+	bool tsn_est_en;
 	int mac_port_sel_speed;
 	bool en_tx_lpi_clockgating;
 	int has_xgmac;
-- 
1.9.1

Re: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Andrew Lunn]

On Wed, Jun 19, 2019 at 05:36:14AM +0800, Voon Weifeng wrote:

Hi Voon

> +static int est_poll_srwo(void *ioaddr)
> +{
> +	/* Poll until the EST GCL Control[SRWO] bit clears.
> +	 * Total wait = 12 x 50ms ~= 0.6s.
> +	 */
> +	unsigned int retries = 12;
> +	unsigned int value;
> +
> +	do {
> +		value = TSN_RD32(ioaddr + MTL_EST_GCL_CTRL);
> +		if (!(value & MTL_EST_GCL_CTRL_SRWO))
> +			return 0;
> +		msleep(50);
> +	} while (--retries);
> +
> +	return -ETIMEDOUT;

Maybe use one of the readx_poll_timeout() macros?

> +static int est_read_gce(void *ioaddr, unsigned int row,
> +			unsigned int *gates, unsigned int *ti_nsec,
> +			unsigned int dbgb, unsigned int dbgm)
> +{
> +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
> +	unsigned int ti_wid = cap->ti_wid;
> +	unsigned int gates_mask;
> +	unsigned int ti_mask;
> +	unsigned int value;
> +	int ret;
> +
> +	gates_mask = (1 << cap->txqcnt) - 1;
> +	ti_mask = (1 << ti_wid) - 1;
> +
> +	ret = est_read_gcl_config(ioaddr, &value, row, 0, dbgb, dbgm);
> +	if (ret) {
> +		TSN_ERR("Read GCE failed! row=%u\n", row);

It is generally not a good idea to put wrappers around the kernel
print functions. It would be better if all these functions took struct
stmmac_priv *priv rather than ioaddr, so you could then do

	netdev_err(priv->dev, "Read GCE failed! row=%u\n", row);

> +	/* Ensure that HW is not in the midst of GCL transition */
> +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);

Also, don't put wrapper around readl()/writel().

> +	value &= ~MTL_EST_CTRL_SSWL;
> +
> +	/* MTL_EST_CTRL value has been read earlier, if TILS value
> +	 * differs, we update here.
> +	 */
> +	if (tils != dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_TILS]) {
> +		value &= ~MTL_EST_CTRL_TILS;
> +		value |= (tils << MTL_EST_CTRL_TILS_SHIFT);
> +
> +		TSN_WR32(value, ioaddr + MTL_EST_CTRL);
> +		dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_TILS] = tils;
> +	}
> +
> +	return 0;
> +}
> +
> +static int est_set_ov(void *ioaddr,
> +		      const unsigned int *ptov,
> +		      const unsigned int *ctov)
> +{
> +	unsigned int value;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
> +	value &= ~MTL_EST_CTRL_SSWL;
> +
> +	if (ptov) {
> +		if (*ptov > EST_PTOV_MAX) {
> +			TSN_WARN("EST: invalid PTOV(%u), max=%u\n",
> +				 *ptov, EST_PTOV_MAX);

It looks like most o the TSN_WARN should actually be netdev_dbg().

   Andrew

RE: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Voon, Weifeng]

> > +static int est_poll_srwo(void *ioaddr) {
> > +	/* Poll until the EST GCL Control[SRWO] bit clears.
> > +	 * Total wait = 12 x 50ms ~= 0.6s.
> > +	 */
> > +	unsigned int retries = 12;
> > +	unsigned int value;
> > +
> > +	do {
> > +		value = TSN_RD32(ioaddr + MTL_EST_GCL_CTRL);
> > +		if (!(value & MTL_EST_GCL_CTRL_SRWO))
> > +			return 0;
> > +		msleep(50);
> > +	} while (--retries);
> > +
> > +	return -ETIMEDOUT;
> 
> Maybe use one of the readx_poll_timeout() macros?
> 
> > +static int est_read_gce(void *ioaddr, unsigned int row,
> > +			unsigned int *gates, unsigned int *ti_nsec,
> > +			unsigned int dbgb, unsigned int dbgm) {
> > +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
> > +	unsigned int ti_wid = cap->ti_wid;
> > +	unsigned int gates_mask;
> > +	unsigned int ti_mask;
> > +	unsigned int value;
> > +	int ret;
> > +
> > +	gates_mask = (1 << cap->txqcnt) - 1;
> > +	ti_mask = (1 << ti_wid) - 1;
> > +
> > +	ret = est_read_gcl_config(ioaddr, &value, row, 0, dbgb, dbgm);
> > +	if (ret) {
> > +		TSN_ERR("Read GCE failed! row=%u\n", row);
> 
> It is generally not a good idea to put wrappers around the kernel print
> functions. It would be better if all these functions took struct
> stmmac_priv *priv rather than ioaddr, so you could then do
> 
> 	netdev_err(priv->dev, "Read GCE failed! row=%u\n", row);
> 
> > +	/* Ensure that HW is not in the midst of GCL transition */
> > +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
> 
> Also, don't put wrapper around readl()/writel().
> 
> > +	value &= ~MTL_EST_CTRL_SSWL;
> > +
> > +	/* MTL_EST_CTRL value has been read earlier, if TILS value
> > +	 * differs, we update here.
> > +	 */
> > +	if (tils != dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_TILS]) {
> > +		value &= ~MTL_EST_CTRL_TILS;
> > +		value |= (tils << MTL_EST_CTRL_TILS_SHIFT);
> > +
> > +		TSN_WR32(value, ioaddr + MTL_EST_CTRL);
> > +		dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_TILS] = tils;
> > +	}
> > +
> > +	return 0;
> > +}
> > +
> > +static int est_set_ov(void *ioaddr,
> > +		      const unsigned int *ptov,
> > +		      const unsigned int *ctov)
> > +{
> > +	unsigned int value;
> > +
> > +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> > +		return -ENOTSUPP;
> > +
> > +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
> > +	value &= ~MTL_EST_CTRL_SSWL;
> > +
> > +	if (ptov) {
> > +		if (*ptov > EST_PTOV_MAX) {
> > +			TSN_WARN("EST: invalid PTOV(%u), max=%u\n",
> > +				 *ptov, EST_PTOV_MAX);
> 
> It looks like most o the TSN_WARN should actually be netdev_dbg().
> 
>    Andrew

Hi Andrew,
This file is targeted for dual licensing which is GPL-2.0 OR BSD-3-Clause.
This is the reason why we are using wrappers around the functions so that
all the function call is generic.
	
Regards,
Weifeng

Re: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Andrew Lunn]

> > It looks like most o the TSN_WARN should actually be netdev_dbg().
> > 
> >    Andrew
> 
> Hi Andrew,
> This file is targeted for dual licensing which is GPL-2.0 OR BSD-3-Clause.
> This is the reason why we are using wrappers around the functions so that
> all the function call is generic.

I don't see why dual licenses should require wrappers. Please explain.

  Thanks
	Andrew

RE: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Ong, Boon Leong]

>> > It looks like most o the TSN_WARN should actually be netdev_dbg().
>> >
>> >    Andrew
>>
>> Hi Andrew,
>> This file is targeted for dual licensing which is GPL-2.0 OR BSD-3-Clause.
>> This is the reason why we are using wrappers around the functions so that
>> all the function call is generic.
>
>I don't see why dual licenses should require wrappers. Please explain.
>
>  Thanks
>	Andrew
Agree with the Andrew. We can change those wrapper functions that have
serve the internal development needs for multiple OS scaling reason. 
We will update the kernel codes as suggsted.  

Re: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Vinicius Costa Gomes]

Hi,

Voon Weifeng <weifeng.voon@intel.com> writes:

> From: Ong Boon Leong <boon.leong.ong@intel.com>
>
> IEEE 802.1Qbv Enhancements for Scheduled Traffics (EST) is available in
> EQoS ver5.xx. The change adds basic EST functionalities:
>
> a) EST initialization with hardware capabilities detection.
> b) Setting Gate Control List (GCL), i.e. gate open/close & time intervals,
>    and all GC Related Registers (GCRR), e.g., base time (BTR), cycle time
>    (CTR), time extension (TER) and GC List Length (LLR).
> c) Setting time interval left shift (TILS), PTP time offset (PTOV) and
>    current time offset (CTOV).
> d) Enable/disable EST.
> e) Getting TSN hardware capabilities.
> f) Getting Gate Control configuration either from driver data store or
>    hardware.
>
> We extend the main driver logic to include basic TSN capability discovery,
> and setup. We also add EST feature enable/disable control.
>
> Reviewed-by: Chuah Kim Tatt <kim.tatt.chuah@intel.com>
> Reviewed-by: Voon Weifeng <weifeng.voon@intel.com>
> Reviewed-by: Kweh Hock Leong <hock.leong.kweh@intel.com>
> Signed-off-by: Ong Boon Leong <boon.leong.ong@intel.com>
> Signed-off-by: Voon Weifeng <weifeng.voon@intel.com>
> ---
>  drivers/net/ethernet/stmicro/stmmac/Makefile      |   2 +-
>  drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c  | 790 ++++++++++++++++++++++
>  drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h  | 173 +++++
>  drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c |  13 +
>  drivers/net/ethernet/stmicro/stmmac/hwif.h        |  52 ++
>  drivers/net/ethernet/stmicro/stmmac/stmmac_main.c |  46 ++
>  include/linux/stmmac.h                            |   1 +
>  7 files changed, 1076 insertions(+), 1 deletion(-)
>  create mode 100644 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
>  create mode 100644 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h
>
> diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile
> index c59926d96bcc..76fb36cb4da7 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/Makefile
> +++ b/drivers/net/ethernet/stmicro/stmmac/Makefile
> @@ -6,7 +6,7 @@ stmmac-objs:= stmmac_main.o stmmac_ethtool.o stmmac_mdio.o ring_mode.o	\
>  	      mmc_core.o stmmac_hwtstamp.o stmmac_ptp.o dwmac4_descs.o	\
>  	      dwmac4_dma.o dwmac4_lib.o dwmac4_core.o dwmac5.o hwif.o \
>  	      stmmac_tc.o dwxgmac2_core.o dwxgmac2_dma.o dwxgmac2_descs.o \
> -	      $(stmmac-y)
> +	      dw_tsn_lib.o $(stmmac-y)
>  
>  stmmac-$(CONFIG_STMMAC_SELFTESTS) += stmmac_selftests.o
>  
> diff --git a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
> new file mode 100644
> index 000000000000..cba27c604cb1
> --- /dev/null
> +++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
> @@ -0,0 +1,790 @@
> +// SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
> +/* Copyright (c) 2019, Intel Corporation.
> + * dw_tsn_lib.c: DW EQoS v5.00 TSN capabilities
> + */
> +
> +#include "dwmac4.h"
> +#include "dwmac5.h"
> +#include "dw_tsn_lib.h"
> +
> +static struct tsn_hw_cap dw_tsn_hwcap;
> +static bool dw_tsn_feat_en[TSN_FEAT_ID_MAX];
> +static unsigned int dw_tsn_hwtunable[TSN_HWTUNA_MAX];
> +static struct est_gc_config dw_est_gc_config;

If it's at all possible to have more than one of these devices in a
system, this should be moved to a per-device structure. That
mac_device_info struct perhaps?

> +
> +static unsigned int est_get_gcl_depth(unsigned int hw_cap)
> +{
> +	unsigned int estdep = (hw_cap & GMAC_HW_FEAT_ESTDEP)
> +			>> GMAC_HW_FEAT_ESTDEP_SHIFT;
> +	unsigned int depth;
> +
> +	switch (estdep) {
> +	case 1:
> +		depth = 64;
> +		break;
> +	case 2:
> +		depth = 128;
> +		break;
> +	case 3:
> +		depth = 256;
> +		break;
> +	case 4:
> +		depth = 512;
> +		break;
> +	case 5:
> +		depth = 1024;
> +		break;
> +	default:
> +		depth = 0;
> +	}
> +
> +	return depth;
> +}
> +
> +static unsigned int est_get_ti_width(unsigned int hw_cap)
> +{
> +	unsigned int estwid = (hw_cap & GMAC_HW_FEAT_ESTWID)
> +			>> GMAC_HW_FEAT_ESTWID_SHIFT;
> +	unsigned int width;
> +
> +	switch (estwid) {
> +	case 1:
> +		width = 16;
> +		break;
> +	case 2:
> +		width = 20;
> +		break;
> +	case 3:
> +		width = 24;
> +		break;
> +	default:
> +		width = 0;
> +	}
> +
> +	return width;
> +}
> +
> +static int est_poll_srwo(void *ioaddr)
> +{
> +	/* Poll until the EST GCL Control[SRWO] bit clears.
> +	 * Total wait = 12 x 50ms ~= 0.6s.
> +	 */
> +	unsigned int retries = 12;
> +	unsigned int value;
> +
> +	do {
> +		value = TSN_RD32(ioaddr + MTL_EST_GCL_CTRL);
> +		if (!(value & MTL_EST_GCL_CTRL_SRWO))
> +			return 0;
> +		msleep(50);
> +	} while (--retries);
> +
> +	return -ETIMEDOUT;
> +}
> +
> +static int est_set_gcl_addr(void *ioaddr, unsigned int addr,
> +			    unsigned int gcrr, unsigned int rwops,
> +			    unsigned int dbgb, unsigned int dbgm)
> +{
> +	unsigned int value;
> +
> +	value = MTL_EST_GCL_CTRL_ADDR_VAL(addr) & MTL_EST_GCL_CTRL_ADDR;
> +
> +	if (dbgm) {
> +		if (dbgb)
> +			value |= MTL_EST_GCL_CTRL_DBGB1;
> +
> +		value |= MTL_EST_GCL_CTRL_DBGM;
> +	}
> +
> +	if (gcrr)
> +		value |= MTL_EST_GCL_CTRL_GCRR;
> +
> +	/* This is the only place SRWO is set and driver polls SRWO
> +	 * for self-cleared before exit. Therefore, caller should
> +	 * check return status for possible time out error.
> +	 */
> +	value |= (rwops | MTL_EST_GCL_CTRL_SRWO);
> +
> +	TSN_WR32(value, ioaddr + MTL_EST_GCL_CTRL);
> +
> +	return est_poll_srwo(ioaddr);
> +}
> +
> +static int est_write_gcl_config(void *ioaddr, unsigned int data,
> +				unsigned int addr, unsigned int gcrr,
> +				unsigned int dbgb, unsigned int dbgm)
> +{
> +	TSN_WR32(data, ioaddr + MTL_EST_GCL_DATA);
> +
> +	return est_set_gcl_addr(ioaddr, addr, gcrr, GCL_OPS_W, dbgb, dbgm);
> +}
> +
> +static int est_read_gcl_config(void *ioaddr, unsigned int *data,
> +			       unsigned int addr, unsigned int gcrr,
> +			       unsigned int dbgb, unsigned int dbgm)
> +{
> +	int ret;
> +
> +	ret = est_set_gcl_addr(ioaddr, addr, gcrr, GCL_OPS_R, dbgb, dbgm);
> +	if (ret)
> +		return ret;
> +
> +	*data = TSN_RD32(ioaddr + MTL_EST_GCL_DATA);
> +
> +	return ret;
> +}
> +
> +static int est_read_gce(void *ioaddr, unsigned int row,
> +			unsigned int *gates, unsigned int *ti_nsec,
> +			unsigned int dbgb, unsigned int dbgm)
> +{
> +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
> +	unsigned int ti_wid = cap->ti_wid;
> +	unsigned int gates_mask;
> +	unsigned int ti_mask;
> +	unsigned int value;
> +	int ret;
> +
> +	gates_mask = (1 << cap->txqcnt) - 1;
> +	ti_mask = (1 << ti_wid) - 1;
> +
> +	ret = est_read_gcl_config(ioaddr, &value, row, 0, dbgb, dbgm);
> +	if (ret) {
> +		TSN_ERR("Read GCE failed! row=%u\n", row);
> +
> +		return ret;
> +	}
> +	*ti_nsec = value & ti_mask;
> +	*gates = (value >> ti_wid) & gates_mask;
> +
> +	return ret;
> +}
> +
> +static unsigned int est_get_gcl_total_intervals_nsec(unsigned int bank,
> +						     unsigned int gcl_len)
> +{
> +	struct est_gc_entry *gcl = dw_est_gc_config.gcb[bank].gcl;
> +	unsigned int nsec = 0;
> +	unsigned int row;
> +
> +	for (row = 0; row < gcl_len; row++) {
> +		nsec += gcl->ti_nsec;
> +		gcl++;
> +	}
> +
> +	return nsec;
> +}
> +
> +static int est_set_tils(void *ioaddr, const unsigned int tils)
> +{
> +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
> +	unsigned int value;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	if (tils > cap->tils_max) {
> +		TSN_WARN("EST: invalid tils(%u), max=%u\n",
> +			 tils, cap->tils_max);
> +
> +		return -EINVAL;
> +	}
> +
> +	/* Ensure that HW is not in the midst of GCL transition */
> +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
> +	value &= ~MTL_EST_CTRL_SSWL;
> +
> +	/* MTL_EST_CTRL value has been read earlier, if TILS value
> +	 * differs, we update here.
> +	 */
> +	if (tils != dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_TILS]) {
> +		value &= ~MTL_EST_CTRL_TILS;
> +		value |= (tils << MTL_EST_CTRL_TILS_SHIFT);
> +
> +		TSN_WR32(value, ioaddr + MTL_EST_CTRL);
> +		dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_TILS] = tils;
> +	}
> +
> +	return 0;
> +}
> +
> +static int est_set_ov(void *ioaddr,
> +		      const unsigned int *ptov,
> +		      const unsigned int *ctov)
> +{
> +	unsigned int value;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
> +	value &= ~MTL_EST_CTRL_SSWL;
> +
> +	if (ptov) {
> +		if (*ptov > EST_PTOV_MAX) {
> +			TSN_WARN("EST: invalid PTOV(%u), max=%u\n",
> +				 *ptov, EST_PTOV_MAX);
> +
> +			return -EINVAL;
> +		} else if (*ptov !=
> +			   dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_PTOV]) {
> +			value &= ~MTL_EST_CTRL_PTOV;
> +			value |= (*ptov << MTL_EST_CTRL_PTOV_SHIFT);
> +			dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_PTOV] = *ptov;
> +		}
> +	}
> +
> +	if (ctov) {
> +		if (*ctov > EST_CTOV_MAX) {
> +			TSN_WARN("EST: invalid CTOV(%u), max=%u\n",
> +				 *ctov, EST_CTOV_MAX);
> +
> +			return -EINVAL;
> +		} else if (*ctov != dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_CTOV]) {
> +			value &= ~MTL_EST_CTRL_CTOV;
> +			value |= (*ctov << MTL_EST_CTRL_CTOV_SHIFT);
> +			dw_tsn_hwtunable[TSN_HWTUNA_TX_EST_CTOV] = *ctov;
> +		}
> +	}
> +
> +	TSN_WR32(value, ioaddr + MTL_EST_CTRL);
> +
> +	return 0;
> +}
> +     
> +void dwmac_tsn_init(void *ioaddr)

Perhaps this should return an error if TSN is not supported. It may help
simplify the initialization below.

> +{
> +	unsigned int hwid = TSN_RD32(ioaddr + GMAC4_VERSION) & TSN_VER_MASK;
> +	unsigned int hw_cap2 = TSN_RD32(ioaddr + GMAC_HW_FEATURE2);
> +	unsigned int hw_cap3 = TSN_RD32(ioaddr + GMAC_HW_FEATURE3);
> +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
> +	unsigned int gcl_depth;
> +	unsigned int tils_max;
> +	unsigned int ti_wid;
> +
> +	memset(cap, 0, sizeof(*cap));
> +
> +	if (hwid < TSN_CORE_VER) {
> +		TSN_WARN_NA("IP v5.00 does not support TSN\n");
> +		return;
> +	}
> +
> +	if (!(hw_cap3 & GMAC_HW_FEAT_ESTSEL)) {
> +		TSN_WARN_NA("EST NOT supported\n");
> +		cap->est_support = 0;
> +
> +		return;
> +	}
> +
> +	gcl_depth = est_get_gcl_depth(hw_cap3);
> +	ti_wid = est_get_ti_width(hw_cap3);
> +
> +	cap->ti_wid = ti_wid;
> +	cap->gcl_depth = gcl_depth;
> +
> +	tils_max = (hw_cap3 & GMAC_HW_FEAT_ESTSEL ? 3 : 0);
> +	tils_max = (1 << tils_max) - 1;
> +	cap->tils_max = tils_max;
> +
> +	cap->ext_max = EST_TIWID_TO_EXTMAX(ti_wid);
> +	cap->txqcnt = ((hw_cap2 & GMAC_HW_FEAT_TXQCNT) >> 6) + 1;
> +	cap->est_support = 1;
> +
> +	TSN_INFO("EST: depth=%u, ti_wid=%u, tils_max=%u tqcnt=%u\n",
> +		 gcl_depth, ti_wid, tils_max, cap->txqcnt);
> +}
> +
> +void dwmac_get_tsn_hwcap(struct tsn_hw_cap **tsn_hwcap)
> +{
> +	*tsn_hwcap = &dw_tsn_hwcap;
> +}
> +
> +void dwmac_set_est_gcb(struct est_gc_entry *gcl, unsigned int bank)
> +{
> +	if (bank >= 0 && bank < EST_GCL_BANK_MAX)
> +		dw_est_gc_config.gcb[bank].gcl = gcl;
> +}
> +
> +void dwmac_set_tsn_feat(enum tsn_feat_id featid, bool enable)
> +{
> +	if (featid < TSN_FEAT_ID_MAX)
> +		dw_tsn_feat_en[featid] = enable;
> +}
> +
> +int dwmac_set_tsn_hwtunable(void *ioaddr,
> +			    enum tsn_hwtunable_id id,
> +			    const unsigned int *data)
> +{
> +	int ret = 0;
> +
> +	switch (id) {
> +	case TSN_HWTUNA_TX_EST_TILS:
> +		ret = est_set_tils(ioaddr, *data);
> +		break;
> +	case TSN_HWTUNA_TX_EST_PTOV:
> +		ret = est_set_ov(ioaddr, data, NULL);
> +		break;
> +	case TSN_HWTUNA_TX_EST_CTOV:
> +		ret = est_set_ov(ioaddr, NULL, data);
> +		break;
> +	default:
> +		ret = -EINVAL;
> +	};
> +
> +	return ret;
> +}
> +
> +int dwmac_get_tsn_hwtunable(enum tsn_hwtunable_id id, unsigned int *data)
> +{
> +	if (id >= TSN_HWTUNA_MAX)
> +		return -EINVAL;
> +
> +	*data = dw_tsn_hwtunable[id];
> +
> +	return 0;
> +}
> +
> +int dwmac_get_est_bank(void *ioaddr, unsigned int own)
> +{
> +	int swol;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	swol = TSN_RD32(ioaddr + MTL_EST_STATUS);
> +
> +	swol = ((swol & MTL_EST_STATUS_SWOL) >>
> +		MTL_EST_STATUS_SWOL_SHIFT);
> +
> +	if (own)
> +		return swol;
> +	else
> +		return (~swol & 0x1);
> +}
> +
> +int dwmac_set_est_gce(void *ioaddr,
> +		      struct est_gc_entry *gce, unsigned int row,
> +		      unsigned int dbgb, unsigned int dbgm)
> +{
> +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
> +	unsigned int ti_nsec = gce->ti_nsec;
> +	unsigned int gates = gce->gates;
> +	struct est_gc_entry *gcl;
> +	unsigned int gates_mask;
> +	unsigned int ti_wid;
> +	unsigned int ti_max;
> +	unsigned int value;
> +	unsigned int bank;
> +	int ret;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	if (dbgb >= EST_GCL_BANK_MAX)
> +		return -EINVAL;
> +
> +	if (dbgm) {
> +		bank = dbgb;
> +	} else {
> +		value = TSN_RD32(ioaddr + MTL_EST_STATUS);
> +		bank = (value & MTL_EST_STATUS_SWOL) >>
> +		       MTL_EST_STATUS_SWOL_SHIFT;
> +	}
> +
> +	if (!cap->gcl_depth || row > cap->gcl_depth) {
> +		TSN_WARN("EST: row(%u) > GCL depth(%u)\n",
> +			 row, cap->gcl_depth);
> +
> +		return -EINVAL;
> +	}
> +
> +	ti_wid = cap->ti_wid;
> +	ti_max = (1 << ti_wid) - 1;
> +	if (ti_nsec > ti_max) {
> +		TSN_WARN("EST: ti_nsec(%u) > upper limit(%u)\n",
> +			 ti_nsec, ti_max);
> +
> +		return -EINVAL;
> +	}
> +
> +	gates_mask = (1 << cap->txqcnt) - 1;
> +	value = ((gates & gates_mask) << ti_wid) | ti_nsec;
> +
> +	ret = est_write_gcl_config(ioaddr, value, row, 0, dbgb, dbgm);
> +	if (ret) {
> +		TSN_ERR("EST: GCE write failed: bank=%u row=%u.\n",
> +			bank, row);
> +
> +		return ret;
> +	}
> +
> +	TSN_INFO("EST: GCE write: dbgm=%u bank=%u row=%u, gc=0x%x.\n",
> +		 dbgm, bank, row, value);
> +
> +	/* Since GC write is successful, update GCL copy of the driver */
> +	gcl = dw_est_gc_config.gcb[bank].gcl + row;
> +	gcl->gates = gates;
> +	gcl->ti_nsec = ti_nsec;
> +
> +	return ret;
> +}
> +
> +int dwmac_get_est_gcrr_llr(void *ioaddr, unsigned int *gcl_len,
> +			   unsigned int dbgb, unsigned int dbgm)
> +{
> +	unsigned int bank, value;
> +	int ret;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	if (dbgb >= EST_GCL_BANK_MAX)
> +		return -EINVAL;
> +
> +	if (dbgm) {
> +		bank = dbgb;
> +	} else {
> +		value = TSN_RD32(ioaddr + MTL_EST_STATUS);
> +		bank = (value & MTL_EST_STATUS_SWOL) >>
> +		       MTL_EST_STATUS_SWOL_SHIFT;
> +	}
> +
> +	ret = est_read_gcl_config(ioaddr, &value,
> +				  GCL_CTRL_ADDR_LLR, 1,
> +				  dbgb, dbgm);
> +	if (ret) {
> +		TSN_ERR("read LLR fail at bank=%u\n", bank);
> +
> +			return ret;
> +	}
> +
> +	*gcl_len = value;
> +
> +	return 0;
> +}
> +
> +int dwmac_set_est_gcrr_llr(void *ioaddr, unsigned int gcl_len,
> +			   unsigned int dbgb, unsigned int dbgm)
> +{
> +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
> +	unsigned int bank, value;
> +	struct est_gcrr *bgcrr;
> +	int ret = 0;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	if (dbgb >= EST_GCL_BANK_MAX)
> +		return -EINVAL;
> +
> +	if (dbgm) {
> +		bank = dbgb;
> +	} else {
> +		value = TSN_RD32(ioaddr + MTL_EST_STATUS);
> +		bank = (value & MTL_EST_STATUS_SWOL) >>
> +		       MTL_EST_STATUS_SWOL_SHIFT;
> +	}
> +
> +	if (gcl_len > cap->gcl_depth) {
> +		TSN_WARN("EST: GCL length(%u) > depth(%u)\n",
> +			 gcl_len, cap->gcl_depth);
> +
> +		return -EINVAL;
> +	}
> +
> +	bgcrr = &dw_est_gc_config.gcb[bank].gcrr;
> +
> +	if (gcl_len != bgcrr->llr) {
> +		ret = est_write_gcl_config(ioaddr, gcl_len,
> +					   GCL_CTRL_ADDR_LLR, 1,
> +					   dbgb, dbgm);
> +		if (ret) {
> +			TSN_ERR_NA("EST: GCRR programming failure!\n");
> +
> +			return ret;
> +		}
> +		bgcrr->llr = gcl_len;
> +	}
> +
> +	return 0;
> +}
> +
> +int dwmac_set_est_gcrr_times(void *ioaddr,
> +			     struct est_gcrr *gcrr,
> +			     unsigned int dbgb, unsigned int dbgm)
> +{
> +	unsigned int cycle_nsec = gcrr->cycle_nsec;
> +	unsigned int cycle_sec = gcrr->cycle_sec;
> +	unsigned int base_nsec = gcrr->base_nsec;
> +	unsigned int base_sec = gcrr->base_sec;
> +	unsigned int ext_nsec = gcrr->ter_nsec;
> +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
> +	unsigned int gcl_len, tti_ns, value;
> +	struct est_gcrr *bgcrr;
> +	u64 val_ns, sys_ns;
> +	unsigned int bank;
> +	int ret = 0;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	if (dbgb >= EST_GCL_BANK_MAX)
> +		return -EINVAL;
> +
> +	if (dbgm) {
> +		bank = dbgb;
> +	} else {
> +		value = TSN_RD32(ioaddr + MTL_EST_STATUS);
> +		bank = (value & MTL_EST_STATUS_SWOL) >>
> +		       MTL_EST_STATUS_SWOL_SHIFT;
> +	}
> +
> +	if (base_nsec > 1000000000ULL || cycle_nsec > 1000000000ULL) {
> +		TSN_WARN("EST: base(%u) or cycle(%u) nsec > 1s !\n",
> +			 base_nsec, cycle_nsec);
> +
> +		return -EINVAL;
> +	}
> +
> +	/* Ensure base time is later than MAC system time */
> +	val_ns = (u64)base_nsec;
> +	val_ns += (u64)(base_sec * 1000000000ULL);
> +
> +	/* Get the MAC system time */
> +	sys_ns = TSN_RD32(ioaddr + TSN_PTP_STNSR);
> +	sys_ns += TSN_RD32(ioaddr + TSN_PTP_STSR) * 1000000000ULL;
> +
> +	if (val_ns <= sys_ns) {
> +		TSN_WARN("EST: base time(%llu) <= system time(%llu)\n",
> +			 val_ns, sys_ns);
> +
> +		return -EINVAL;
> +	}
> +
> +	if (cycle_sec > EST_CTR_HI_MAX) {
> +		TSN_WARN("EST: cycle time(%u) > 255 seconds\n", cycle_sec);
> +
> +		return -EINVAL;
> +	}
> +
> +	if (ext_nsec > cap->ext_max) {
> +		TSN_WARN("EST: invalid time extension(%u), max=%u\n",
> +			 ext_nsec, cap->ext_max);
> +
> +		return -EINVAL;
> +	}
> +
> +	bgcrr = &dw_est_gc_config.gcb[bank].gcrr;
> +	gcl_len = bgcrr->llr;
> +
> +	/* Sanity test on GCL total time intervals against cycle time.
> +	 * a) For GC length = 1, if its time interval is equal or greater
> +	 *    than cycle time, it is a constant gate error.
> +	 * b) If total time interval > cycle time, irregardless of GC
> +	 *    length, it is not considered an error that GC list is
> +	 *    truncated. In this case, giving a warning message is
> +	 *    sufficient.
> +	 * c) If total time interval < cycle time, irregardless of GC
> +	 *    length, all GATES are OPEN after the last GC is processed
> +	 *    until cycle time lapses. This is potentially due to poor
> +	 *    GCL configuration but is not an error, so we inform user
> +	 *    about it.
> +	 */
> +	tti_ns = est_get_gcl_total_intervals_nsec(bank, gcl_len);
> +	val_ns = (u64)cycle_nsec;
> +	val_ns += (u64)(cycle_sec * 1000000000ULL);
> +	if (gcl_len == 1 && tti_ns >= val_ns) {
> +		TSN_WARN_NA("EST: Constant gate error!\n");
> +
> +		return -EINVAL;
> +	}
> +
> +	if (tti_ns > val_ns)
> +		TSN_WARN_NA("EST: GCL is truncated!\n");
> +
> +	if (tti_ns < val_ns) {
> +		TSN_INFO("EST: All GCs OPEN at %u of %llu-ns cycle\n",
> +			 tti_ns, val_ns);
> +	}
> +
> +	/* Finally, start programming GCL related registers if the value
> +	 * differs from the driver copy for efficiency.
> +	 */
> +
> +	if (base_nsec != bgcrr->base_nsec)
> +		ret |= est_write_gcl_config(ioaddr, base_nsec,
> +					    GCL_CTRL_ADDR_BTR_LO, 1,
> +					    dbgb, dbgm);
> +
> +	if (base_sec != bgcrr->base_sec)
> +		ret |= est_write_gcl_config(ioaddr, base_sec,
> +					    GCL_CTRL_ADDR_BTR_HI, 1,
> +					    dbgb, dbgm);
> +
> +	if (cycle_nsec != bgcrr->cycle_nsec)
> +		ret |= est_write_gcl_config(ioaddr, cycle_nsec,
> +					    GCL_CTRL_ADDR_CTR_LO, 1,
> +					    dbgb, dbgm);
> +
> +	if (cycle_sec != bgcrr->cycle_sec)
> +		ret |= est_write_gcl_config(ioaddr, cycle_sec,
> +					    GCL_CTRL_ADDR_CTR_HI, 1,
> +					    dbgb, dbgm);
> +
> +	if (ext_nsec != bgcrr->ter_nsec)
> +		ret |= est_write_gcl_config(ioaddr, ext_nsec,
> +					    GCL_CTRL_ADDR_TER, 1,
> +					    dbgb, dbgm);
> +
> +	if (ret) {
> +		TSN_ERR_NA("EST: GCRR programming failure!\n");
> +
> +		return ret;
> +	}
> +
> +	/* Finally, we are ready to switch SWOL now. */
> +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
> +	value |= MTL_EST_CTRL_SSWL;
> +	TSN_WR32(value, ioaddr + MTL_EST_CTRL);
> +
> +	/* Update driver copy */
> +	bgcrr->base_sec = base_sec;
> +	bgcrr->base_nsec = base_nsec;
> +	bgcrr->cycle_sec = cycle_sec;
> +	bgcrr->cycle_nsec = cycle_nsec;
> +	bgcrr->ter_nsec = ext_nsec;
> +
> +	TSN_INFO_NA("EST: gcrr set successful\n");
> +
> +	return 0;
> +}
> +
> +int dwmac_set_est_enable(void *ioaddr, bool enable)
> +{
> +	unsigned int value;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
> +	value &= ~(MTL_EST_CTRL_SSWL | MTL_EST_CTRL_EEST);
> +	value |= (enable & MTL_EST_CTRL_EEST);
> +	TSN_WR32(value, ioaddr + MTL_EST_CTRL);
> +	dw_est_gc_config.enable = enable;
> +
> +	return 0;
> +}
> +
> +int dwmac_get_est_gcc(void *ioaddr,
> +		      struct est_gc_config **gcc, bool frmdrv)
> +{
> +	struct est_gc_config *pgcc;
> +	unsigned int bank;
> +	unsigned int value;
> +	int ret;
> +
> +	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
> +		return -ENOTSUPP;
> +
> +	/* Get GC config from driver */
> +	if (frmdrv) {
> +		*gcc = &dw_est_gc_config;
> +
> +		TSN_INFO_NA("EST: read GCL from driver copy done.\n");
> +
> +		return 0;
> +	}
> +
> +	/* Get GC config from HW */
> +	pgcc = &dw_est_gc_config;
> +
> +	value = TSN_RD32(ioaddr + MTL_EST_CTRL);
> +	pgcc->enable = value & MTL_EST_CTRL_EEST;
> +
> +	for (bank = 0; bank < EST_GCL_BANK_MAX; bank++) {
> +		unsigned int llr, row;
> +		struct est_gc_bank *gcbc = &pgcc->gcb[bank];
> +
> +		ret = est_read_gcl_config(ioaddr, &value,
> +					  GCL_CTRL_ADDR_BTR_LO, 1,
> +					  bank, 1);
> +		if (ret) {
> +			TSN_ERR("read BTR(low) fail at bank=%u\n", bank);
> +
> +			return ret;
> +		}
> +		gcbc->gcrr.base_nsec = value;
> +
> +		ret = est_read_gcl_config(ioaddr, &value,
> +					  GCL_CTRL_ADDR_BTR_HI, 1,
> +					  bank, 1);
> +		if (ret) {
> +			TSN_ERR("read BTR(high) fail at bank=%u\n", bank);
> +
> +			return ret;
> +		}
> +		gcbc->gcrr.base_sec = value;
> +
> +		ret = est_read_gcl_config(ioaddr, &value,
> +					  GCL_CTRL_ADDR_CTR_LO, 1,
> +					  bank, 1);
> +		if (ret) {
> +			TSN_ERR("read CTR(low) fail at bank=%u\n", bank);
> +
> +			return ret;
> +		}
> +		gcbc->gcrr.cycle_nsec = value;
> +
> +		ret = est_read_gcl_config(ioaddr, &value,
> +					  GCL_CTRL_ADDR_CTR_HI, 1,
> +					  bank, 1);
> +		if (ret) {
> +			TSN_ERR("read CTR(high) fail at bank=%u\n", bank);
> +
> +			return ret;
> +		}
> +		gcbc->gcrr.cycle_sec = value;
> +
> +		ret = est_read_gcl_config(ioaddr, &value,
> +					  GCL_CTRL_ADDR_TER, 1,
> +					  bank, 1);
> +		if (ret) {
> +			TSN_ERR("read TER fail at bank=%u\n", bank);
> +
> +			return ret;
> +		}
> +		gcbc->gcrr.ter_nsec = value;
> +
> +		ret = est_read_gcl_config(ioaddr, &value,
> +					  GCL_CTRL_ADDR_LLR, 1,
> +					  bank, 1);
> +		if (ret) {
> +			TSN_ERR("read LLR fail at bank=%u\n", bank);
> +
> +			return ret;
> +		}
> +		gcbc->gcrr.llr = value;
> +		llr = value;
> +
> +		for (row = 0; row < llr; row++) {
> +			unsigned int gates, ti_nsec;
> +			struct est_gc_entry *gce = gcbc->gcl + row;
> +
> +			ret = est_read_gce(ioaddr, row, &gates, &ti_nsec,
> +					   bank, 1);
> +			if (ret) {
> +				TSN_ERR("read GCE fail at bank=%u\n", bank);
> +
> +				return ret;
> +			}
> +			gce->gates = gates;
> +			gce->ti_nsec = ti_nsec;
> +		}
> +	}
> +
> +	*gcc = pgcc;
> +	TSN_INFO_NA("EST: read GCL from HW done.\n");
> +
> +	return 0;
> +}
> diff --git a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h
> new file mode 100644
> index 000000000000..feb71f7e7031
> --- /dev/null
> +++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h
> @@ -0,0 +1,173 @@
> +/* SPDX-License-Identifier: GPL-2.0 */
> +/* Copyright (c) 2019, Intel Corporation.
> + * dw_tsn_lib.h: DW EQoS v5.00 TSN capabilities header
> + */
> +
> +#ifndef __DW_TSN_LIB_H__
> +#define __DW_TSN_LIB_H__
> +
> +#include "linux/printk.h"
> +
> +/* DWMAC v5.xx supports the following Time Sensitive Networking protocols:
> + * 1) IEEE 802.1Qbv Enhancements for Scheduled Traffic (EST)
> + */
> +
> +/* MAC HW features3 bitmap */
> +#define GMAC_HW_FEAT_ESTWID		GENMASK(21, 20)
> +#define GMAC_HW_FEAT_ESTWID_SHIFT	20
> +#define GMAC_HW_FEAT_ESTDEP		GENMASK(19, 17)
> +#define GMAC_HW_FEAT_ESTDEP_SHIFT	17
> +#define GMAC_HW_FEAT_ESTSEL		BIT(16)
> +
> +/* MTL EST control register */
> +#define MTL_EST_CTRL			0x00000c50
> +#define MTL_EST_CTRL_PTOV		GENMASK(31, 24)
> +#define MTL_EST_CTRL_PTOV_SHIFT		24
> +#define MTL_EST_CTRL_CTOV		GENMASK(23, 12)
> +#define MTL_EST_CTRL_CTOV_SHIFT		12
> +#define MTL_EST_CTRL_TILS		GENMASK(10, 8)
> +#define MTL_EST_CTRL_TILS_SHIFT		8
> +#define MTL_EST_CTRL_SSWL		BIT(1)	/* Switch to SWOL */
> +#define MTL_EST_CTRL_EEST		BIT(0)	/* Enable EST */
> +
> +/* MTL EST status register */
> +#define MTL_EST_STATUS			0x00000c58
> +#define MTL_EST_STATUS_BTRL		GENMASK(11, 8)	/* BTR ERR loop cnt */
> +#define MTL_EST_STATUS_BTRL_SHIFT	8
> +#define MTL_EST_STATUS_BTRL_MAX		(0xF << 8)
> +#define MTL_EST_STATUS_SWOL		BIT(7)	/* SW owned list */
> +#define MTL_EST_STATUS_SWOL_SHIFT	7
> +#define MTL_EST_STATUS_BTRE		BIT(1)	/* BTR Error */
> +#define MTL_EST_STATUS_SWLC		BIT(0)	/* Switch to SWOL complete */
> +
> +/* MTL EST GCL control register */
> +#define MTL_EST_GCL_CTRL		0x00000c80
> +#define MTL_EST_GCL_CTRL_ADDR		GENMASK(10, 8)	/* GCL Address */
> +#define MTL_EST_GCL_CTRL_ADDR_VAL(addr)	(addr << 8)
> +#define GCL_CTRL_ADDR_BTR_LO		0x0
> +#define GCL_CTRL_ADDR_BTR_HI		0x1
> +#define GCL_CTRL_ADDR_CTR_LO		0x2
> +#define GCL_CTRL_ADDR_CTR_HI		0x3
> +#define GCL_CTRL_ADDR_TER		0x4
> +#define GCL_CTRL_ADDR_LLR		0x5
> +#define MTL_EST_GCL_CTRL_DBGB1		BIT(5)	/* Debug Mode Bank Select */
> +#define MTL_EST_GCL_CTRL_DBGM		BIT(4)	/* Debug Mode */
> +#define MTL_EST_GCL_CTRL_GCRR		BIT(2)	/* GC Related Registers */
> +#define MTL_EST_GCL_CTRL_R1W0		BIT(1)	/* Read / Write Operation */
> +#define GCL_OPS_R			BIT(1)
> +#define GCL_OPS_W			0
> +#define MTL_EST_GCL_CTRL_SRWO		BIT(0)	/* Start R/W Operation */
> +
> +/* MTL EST GCL data register */
> +#define MTL_EST_GCL_DATA		0x00000c84
> +
> +/* EST Global defines */
> +#define EST_CTR_HI_MAX			0xff	/* CTR Hi is 8-bit only */
> +#define EST_PTOV_MAX			0xff	/* Max PTP time offset */
> +#define EST_CTOV_MAX			0xfff	/* Max Current time offset */
> +#define EST_TIWID_TO_EXTMAX(ti_wid)	((1 << (ti_wid + 7)) - 1)
> +#define EST_GCL_BANK_MAX	(2)
> +
> +/* MAC Core Version */
> +#define TSN_VER_MASK		0xFF
> +#define TSN_CORE_VER		0x50
> +
> +/* MAC PTP clock registers */
> +#define TSN_PTP_STSR		0x08
> +#define TSN_PTP_STNSR		0x0c
> +
> +/* Hardware Tunable Enum */
> +enum tsn_hwtunable_id {
> +	TSN_HWTUNA_TX_EST_TILS = 0,
> +	TSN_HWTUNA_TX_EST_PTOV,
> +	TSN_HWTUNA_TX_EST_CTOV,
> +	TSN_HWTUNA_MAX,
> +};
> +
> +/* TSN Feature Enabled List */
> +enum tsn_feat_id {
> +	TSN_FEAT_ID_EST = 0,
> +	TSN_FEAT_ID_MAX,
> +};
> +
> +/* HW register read & write macros */
> +#define TSN_RD32(__addr)		readl(__addr)
> +#define TSN_WR32(__val, __addr)		writel(__val, __addr)
> +
> +/* Logging macros with no args */
> +#define DRVNAME "stmmac"
> +#define TSN_INFO_NA(__msg)	printk(KERN_INFO DRVNAME ":" __msg)
> +#define TSN_WARN_NA(__msg)	printk(KERN_WARNING DRVNAME ":" __msg)
> +#define TSN_ERR_NA(__msg)	printk(KERN_ERR DRVNAME ":" __msg)
> +
> +/* Logging macros with args */
> +#define TSN_INFO(__msg, __arg0, __args...) \
> +	printk(KERN_INFO DRVNAME ":" __msg, (__arg0), ##__args)
> +#define TSN_WARN(__msg, __arg0, __args...) \
> +	printk(KERN_WARNING DRVNAME ":" __msg, (__arg0), ##__args)
> +#define TSN_ERR(__msg, __arg0, __args...) \
> +	printk(KERN_ERR DRVNAME ":" __msg, (__arg0), ##__args)
> +
> +/* TSN HW Capabilities */
> +struct tsn_hw_cap {
> +	bool est_support;		/* 1: supported */
> +	unsigned int txqcnt;		/* Number of TxQ (control gate) */
> +	unsigned int gcl_depth;		/* GCL depth. */
> +	unsigned int ti_wid;		/* time interval width */
> +	unsigned int tils_max;		/* Max time interval left shift */
> +	unsigned int ext_max;		/* Max time extension */
> +};
> +
> +/* EST Gate Control Entry */
> +struct est_gc_entry {
> +	unsigned int gates;		/* gate control: 0: closed,
> +					 *               1: open.
> +					 */
> +	unsigned int ti_nsec;		/* time interval in nsec */
> +};
> +
> +/* EST GCL Related Registers */
> +struct est_gcrr {
> +	unsigned int base_nsec;		/* base time denominator (nsec) */
> +	unsigned int base_sec;		/* base time numerator (sec) */
> +	unsigned int cycle_nsec;	/* cycle time denominator (nsec) */
> +	unsigned int cycle_sec;		/* cycle time numerator sec)*/
> +	unsigned int ter_nsec;		/* time extension (nsec) */
> +	unsigned int llr;		/* GC list length */
> +};
> +
> +/* EST Gate Control bank */
> +struct est_gc_bank {
> +	struct est_gc_entry *gcl;	/* Gate Control List */
> +	struct est_gcrr gcrr;		/* GCL Related Registers */
> +};
> +
> +/* EST Gate Control Configuration */
> +struct est_gc_config {
> +	struct est_gc_bank gcb[EST_GCL_BANK_MAX];
> +	bool enable;			/* 1: enabled */
> +};
> +
> +/* TSN functions */
> +void dwmac_tsn_init(void *ioaddr);
> +void dwmac_get_tsn_hwcap(struct tsn_hw_cap **tsn_hwcap);
> +void dwmac_set_est_gcb(struct est_gc_entry *gcl, unsigned int bank);
> +void dwmac_set_tsn_feat(enum tsn_feat_id featid, bool enable);
> +int dwmac_set_tsn_hwtunable(void *ioaddr, enum tsn_hwtunable_id id,
> +			    const unsigned int *data);
> +int dwmac_get_tsn_hwtunable(enum tsn_hwtunable_id id, unsigned int *data);
> +int dwmac_get_est_bank(void *ioaddr, unsigned int own);
> +int dwmac_set_est_gce(void *ioaddr,
> +		      struct est_gc_entry *gce, unsigned int row,
> +		      unsigned int dbgb, unsigned int dbgm);
> +int dwmac_get_est_gcrr_llr(void *ioaddr, unsigned int *gcl_len,
> +			   unsigned int dbgb, unsigned int dbgm);
> +int dwmac_set_est_gcrr_llr(void *ioaddr, unsigned int gcl_len,
> +			   unsigned int dbgb, unsigned int dbgm);
> +int dwmac_set_est_gcrr_times(void *ioaddr,
> +			     struct est_gcrr *gcrr,
> +			     unsigned int dbgb, unsigned int dbgm);
> +int dwmac_set_est_enable(void *ioaddr, bool enable);
> +int dwmac_get_est_gcc(void *ioaddr,
> +		      struct est_gc_config **gcc, bool frmdrv);
> +#endif /* __DW_TSN_LIB_H__ */
> diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
> index 8d9f6cda4012..1361807fe802 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
> +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
> @@ -817,6 +817,19 @@ static void dwmac4_set_mac_loopback(void __iomem *ioaddr, bool enable)
>  	.pcs_get_adv_lp = dwmac4_get_adv_lp,
>  	.debug = dwmac4_debug,
>  	.set_filter = dwmac4_set_filter,
> +	.tsn_init = dwmac_tsn_init,
> +	.get_tsn_hwcap = dwmac_get_tsn_hwcap,
> +	.set_est_gcb = dwmac_set_est_gcb,
> +	.set_tsn_feat = dwmac_set_tsn_feat,
> +	.set_tsn_hwtunable = dwmac_set_tsn_hwtunable,
> +	.get_tsn_hwtunable = dwmac_get_tsn_hwtunable,
> +	.get_est_bank = dwmac_get_est_bank,
> +	.set_est_gce = dwmac_set_est_gce,
> +	.get_est_gcrr_llr = dwmac_get_est_gcrr_llr,
> +	.set_est_gcrr_llr = dwmac_set_est_gcrr_llr,
> +	.set_est_gcrr_times = dwmac_set_est_gcrr_times,
> +	.set_est_enable = dwmac_set_est_enable,
> +	.get_est_gcc = dwmac_get_est_gcc,
>  	.safety_feat_config = dwmac5_safety_feat_config,
>  	.safety_feat_irq_status = dwmac5_safety_feat_irq_status,
>  	.safety_feat_dump = dwmac5_safety_feat_dump,
> diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
> index 2acfbc70e3c8..518a72805185 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
> +++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
> @@ -7,6 +7,7 @@
>  
>  #include <linux/netdevice.h>
>  #include <linux/stmmac.h>
> +#include "dw_tsn_lib.h"
>  
>  #define stmmac_do_void_callback(__priv, __module, __cname,  __arg0, __args...) \
>  ({ \
> @@ -311,6 +312,31 @@ struct stmmac_ops {
>  			     bool loopback);
>  	void (*pcs_rane)(void __iomem *ioaddr, bool restart);
>  	void (*pcs_get_adv_lp)(void __iomem *ioaddr, struct rgmii_adv *adv);
> +	/* TSN functions */
> +	void (*tsn_init)(void __iomem *ioaddr);
> +	void (*get_tsn_hwcap)(struct tsn_hw_cap **tsn_hwcap);
> +	void (*set_est_gcb)(struct est_gc_entry *gcl,
> +			    u32 bank);
> +	void (*set_tsn_feat)(enum tsn_feat_id featid, bool enable);
> +	int (*set_tsn_hwtunable)(void __iomem *ioaddr,
> +				 enum tsn_hwtunable_id id,
> +				 const unsigned int *data);
> +	int (*get_tsn_hwtunable)(enum tsn_hwtunable_id id,
> +				 unsigned int *data);
> +	int (*get_est_bank)(void __iomem *ioaddr, u32 own);
> +	int (*set_est_gce)(void __iomem *ioaddr,
> +			   struct est_gc_entry *gce, u32 row,
> +			   u32 dbgb, u32 dbgm);
> +	int (*get_est_gcrr_llr)(void __iomem *ioaddr, u32 *gcl_len,
> +				u32 dbgb, u32 dbgm);
> +	int (*set_est_gcrr_llr)(void __iomem *ioaddr, u32 gcl_len,
> +				u32 dbgb, u32 dbgm);
> +	int (*set_est_gcrr_times)(void __iomem *ioaddr,
> +				  struct est_gcrr *gcrr,
> +				  u32 dbgb, u32 dbgm);
> +	int (*set_est_enable)(void __iomem *ioaddr, bool enable);
> +	int (*get_est_gcc)(void __iomem *ioaddr,
> +			   struct est_gc_config **gcc, bool frmdrv);

These functions do not seem to be consistent with the rest of the
stmmac_ops: most of the operations already there receive an
mac_device_info as first argument, which seem much less error prone than
a void* ioaddr.

>  	/* Safety Features */
>  	int (*safety_feat_config)(void __iomem *ioaddr, unsigned int asp);
>  	int (*safety_feat_irq_status)(struct net_device *ndev,
> @@ -385,6 +411,32 @@ struct stmmac_ops {
>  	stmmac_do_void_callback(__priv, mac, pcs_rane, __args)
>  #define stmmac_pcs_get_adv_lp(__priv, __args...) \
>  	stmmac_do_void_callback(__priv, mac, pcs_get_adv_lp, __args)
> +#define stmmac_tsn_init(__priv, __args...) \
> +	stmmac_do_void_callback(__priv, mac, tsn_init, __args)
> +#define stmmac_get_tsn_hwcap(__priv, __args...) \
> +	stmmac_do_void_callback(__priv, mac, get_tsn_hwcap, __args)
> +#define stmmac_set_est_gcb(__priv, __args...) \
> +	stmmac_do_void_callback(__priv, mac, set_est_gcb, __args)
> +#define stmmac_set_tsn_feat(__priv, __args...) \
> +	stmmac_do_void_callback(__priv, mac, set_tsn_feat, __args)
> +#define stmmac_set_tsn_hwtunable(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, set_tsn_hwtunable, __args)
> +#define stmmac_get_tsn_hwtunable(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, get_tsn_hwtunable, __args)
> +#define stmmac_get_est_bank(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, get_est_bank, __args)
> +#define stmmac_set_est_gce(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, set_est_gce, __args)
> +#define stmmac_get_est_gcrr_llr(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, get_est_gcrr_llr, __args)
> +#define stmmac_set_est_gcrr_llr(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, set_est_gcrr_llr, __args)
> +#define stmmac_set_est_gcrr_times(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, set_est_gcrr_times, __args)
> +#define stmmac_set_est_enable(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, set_est_enable, __args)
> +#define stmmac_get_est_gcc(__priv, __args...) \
> +	stmmac_do_callback(__priv, mac, get_est_gcc, __args)
>  #define stmmac_safety_feat_config(__priv, __args...) \
>  	stmmac_do_callback(__priv, mac, safety_feat_config, __args)
>  #define stmmac_safety_feat_irq_status(__priv, __args...) \
> diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
> index a48751989fa6..91213cd3a668 100644
> --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
> +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
> @@ -41,6 +41,7 @@
>  #include "stmmac.h"
>  #include <linux/reset.h>
>  #include <linux/of_mdio.h>
> +#include "dw_tsn_lib.h"
>  #include "dwmac1000.h"
>  #include "dwxgmac2.h"
>  #include "hwif.h"
> @@ -3621,6 +3622,8 @@ static int stmmac_set_features(struct net_device *netdev,
>  	 */
>  	stmmac_rx_ipc(priv, priv->hw);
>  
> +	netdev->features = features;
> +
>  	return 0;
>  }
>  
> @@ -4070,6 +4073,8 @@ static void stmmac_service_task(struct work_struct *work)
>   */
>  static int stmmac_hw_init(struct stmmac_priv *priv)
>  {
> +	struct tsn_hw_cap *tsn_hwcap;
> +	int gcl_depth = 0;
>  	int ret;
>  
>  	/* dwmac-sun8i only work in chain mode */
> @@ -4082,6 +4087,38 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
>  	if (ret)
>  		return ret;
>  
> +	/* Initialize TSN capability */
> +	stmmac_tsn_init(priv, priv->ioaddr);
> +	stmmac_get_tsn_hwcap(priv, &tsn_hwcap);
> +	if (tsn_hwcap)
> +		gcl_depth = tsn_hwcap->gcl_depth;
> +	if (gcl_depth > 0) {
> +		u32 bank;
> +		struct est_gc_entry *gcl[EST_GCL_BANK_MAX];
> +
> +		for (bank = 0; bank < EST_GCL_BANK_MAX; bank++) {
> +			gcl[bank] = devm_kzalloc(priv->device,
> +						 (sizeof(*gcl) * gcl_depth),
> +						 GFP_KERNEL);
> +			if (!gcl[bank]) {
> +				ret = -ENOMEM;
> +				break;
> +			}
> +			stmmac_set_est_gcb(priv, gcl[bank], bank);
> +		}
> +		if (ret) {
> +			int i;
> +
> +			for (i = bank - 1; i >= 0; i--) {
> +				devm_kfree(priv->device, gcl[i]);
> +				stmmac_set_est_gcb(priv, NULL, bank);
> +			}
> +			dev_warn(priv->device, "EST: GCL -ENOMEM\n");
> +
> +			return ret;
> +		}
> +	}
> +
>  	/* Get the HW capability (new GMAC newer than 3.50a) */
>  	priv->hw_cap_support = stmmac_get_hw_features(priv);
>  	if (priv->hw_cap_support) {
> @@ -4168,6 +4205,7 @@ int stmmac_dvr_probe(struct device *device,
>  		     struct stmmac_resources *res)
>  {
>  	struct net_device *ndev = NULL;
> +	struct tsn_hw_cap *tsn_hwcap;
>  	struct stmmac_priv *priv;
>  	u32 queue, maxq;
>  	int ret = 0;
> @@ -4254,6 +4292,14 @@ int stmmac_dvr_probe(struct device *device,
>  	}
>  	ndev->features |= ndev->hw_features | NETIF_F_HIGHDMA;
>  	ndev->watchdog_timeo = msecs_to_jiffies(watchdog);
> +
> +	/* TSN HW feature setup */
> +	stmmac_get_tsn_hwcap(priv, &tsn_hwcap);
> +	if (tsn_hwcap && tsn_hwcap->est_support && priv->plat->tsn_est_en) {
> +		stmmac_set_tsn_feat(priv, TSN_FEAT_ID_EST, true);
> +		dev_info(priv->device, "EST feature enabled\n");
> +	}
> +
>  #ifdef STMMAC_VLAN_TAG_USED
>  	/* Both mac100 and gmac support receive VLAN tag detection */
>  	ndev->features |= NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_STAG_RX;
> diff --git a/include/linux/stmmac.h b/include/linux/stmmac.h
> index 7d06241582dd..d4a90f48e49b 100644
> --- a/include/linux/stmmac.h
> +++ b/include/linux/stmmac.h
> @@ -172,6 +172,7 @@ struct plat_stmmacenet_data {
>  	int has_gmac4;
>  	bool has_sun8i;
>  	bool tso_en;
> +	bool tsn_est_en;
>  	int mac_port_sel_speed;
>  	bool en_tx_lpi_clockgating;
>  	int has_xgmac;
> -- 
> 1.9.1

RE: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Ong, Boon Leong]

>-----Original Message-----
>From: Gomes, Vinicius
>> +++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
>> @@ -0,0 +1,790 @@
>> +
>> +static struct tsn_hw_cap dw_tsn_hwcap;
>> +static bool dw_tsn_feat_en[TSN_FEAT_ID_MAX];
>> +static unsigned int dw_tsn_hwtunable[TSN_HWTUNA_MAX];
>> +static struct est_gc_config dw_est_gc_config;
>
>If it's at all possible to have more than one of these devices in a
>system, this should be moved to a per-device structure. That
>mac_device_info struct perhaps?
I do see value in scaling the code to more than one device there.
Thanks.

>> +void dwmac_tsn_init(void *ioaddr)
>
>Perhaps this should return an error if TSN is not supported. It may help
>simplify the initialization below.
Thanks for the input. It may not be apparent because this code does not
include Qbu detection yet. The thinking here is to avoid caller function
not need to handle and IP configuration difference, i.e. SoC-1 may have only
Qbv and SoC-2 have both. 

>
>> +{
>> +	unsigned int hwid = TSN_RD32(ioaddr + GMAC4_VERSION) &
>TSN_VER_MASK;
>> +	unsigned int hw_cap2 = TSN_RD32(ioaddr + GMAC_HW_FEATURE2);
>> +	unsigned int hw_cap3 = TSN_RD32(ioaddr + GMAC_HW_FEATURE3);
>> +	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
>> +	unsigned int gcl_depth;
>> +	unsigned int tils_max;
>> +	unsigned int ti_wid;
>> +
>> +	memset(cap, 0, sizeof(*cap));
>> +
>> +	if (hwid < TSN_CORE_VER) {
>> +		TSN_WARN_NA("IP v5.00 does not support TSN\n");
Perhaps, we just print info here instead of warning because SoC with EQoS v5
can be built without Qbv. 

>> +		return;
>> +	}
>> +
>> +	if (!(hw_cap3 & GMAC_HW_FEAT_ESTSEL)) {
>> +		TSN_WARN_NA("EST NOT supported\n");
>> +		cap->est_support = 0;
Same here. 

>> +
>> +		return;
>> +	}
>> +
>> +	gcl_depth = est_get_gcl_depth(hw_cap3);
>> +	ti_wid = est_get_ti_width(hw_cap3);
>> +
>> +	cap->ti_wid = ti_wid;
>> +	cap->gcl_depth = gcl_depth;
>> +
>> +	tils_max = (hw_cap3 & GMAC_HW_FEAT_ESTSEL ? 3 : 0);
>> +	tils_max = (1 << tils_max) - 1;
>> +	cap->tils_max = tils_max;
>> +
>> +	cap->ext_max = EST_TIWID_TO_EXTMAX(ti_wid);
>> +	cap->txqcnt = ((hw_cap2 & GMAC_HW_FEAT_TXQCNT) >> 6) + 1;
>> +	cap->est_support = 1;
>> +
>> +	TSN_INFO("EST: depth=%u, ti_wid=%u, tils_max=%u tqcnt=%u\n",
>> +		 gcl_depth, ti_wid, tils_max, cap->txqcnt);
>> +}

>> diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h
>b/drivers/net/ethernet/stmicro/stmmac/hwif.h
>> index 2acfbc70e3c8..518a72805185 100644
>> --- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
>> +++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
>> @@ -7,6 +7,7 @@
>>
>>  #include <linux/netdevice.h>
>>  #include <linux/stmmac.h>
>> +#include "dw_tsn_lib.h"
>>
>>  #define stmmac_do_void_callback(__priv, __module, __cname,  __arg0,
>__args...) \
>>  ({ \
>> @@ -311,6 +312,31 @@ struct stmmac_ops {
>>  			     bool loopback);
>>  	void (*pcs_rane)(void __iomem *ioaddr, bool restart);
>>  	void (*pcs_get_adv_lp)(void __iomem *ioaddr, struct rgmii_adv *adv);
>> +	/* TSN functions */
>> +	void (*tsn_init)(void __iomem *ioaddr);
>> +	void (*get_tsn_hwcap)(struct tsn_hw_cap **tsn_hwcap);
>> +	void (*set_est_gcb)(struct est_gc_entry *gcl,
>> +			    u32 bank);
>> +	void (*set_tsn_feat)(enum tsn_feat_id featid, bool enable);
>> +	int (*set_tsn_hwtunable)(void __iomem *ioaddr,
>> +				 enum tsn_hwtunable_id id,
>> +				 const unsigned int *data);
>> +	int (*get_tsn_hwtunable)(enum tsn_hwtunable_id id,
>> +				 unsigned int *data);
>> +	int (*get_est_bank)(void __iomem *ioaddr, u32 own);
>> +	int (*set_est_gce)(void __iomem *ioaddr,
>> +			   struct est_gc_entry *gce, u32 row,
>> +			   u32 dbgb, u32 dbgm);
>> +	int (*get_est_gcrr_llr)(void __iomem *ioaddr, u32 *gcl_len,
>> +				u32 dbgb, u32 dbgm);
>> +	int (*set_est_gcrr_llr)(void __iomem *ioaddr, u32 gcl_len,
>> +				u32 dbgb, u32 dbgm);
>> +	int (*set_est_gcrr_times)(void __iomem *ioaddr,
>> +				  struct est_gcrr *gcrr,
>> +				  u32 dbgb, u32 dbgm);
>> +	int (*set_est_enable)(void __iomem *ioaddr, bool enable);
>> +	int (*get_est_gcc)(void __iomem *ioaddr,
>> +			   struct est_gc_config **gcc, bool frmdrv);
>
>These functions do not seem to be consistent with the rest of the
>stmmac_ops: most of the operations already there receive an
>mac_device_info as first argument, which seem much less error prone than
>a void* ioaddr.
Thanks for the input. We will look into this together with mac_device_info
and adjust accordingly. 

RE: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Jose Abreu]

From: Voon Weifeng <weifeng.voon@intel.com>

> diff --git a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
> new file mode 100644
> index 000000000000..cba27c604cb1
> --- /dev/null
> +++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c

XGMAC also supports TSN features so I think more abstraction is needed 
on this because the XGMAC implementation is very similar (only reg 
offsets and bitfields changes).

I would rather:
	- Implement HW specific handling in dwmac4_core.c / dwmac4_dma.c and 
add the callbacks in hwif table;
	- Let TSN logic in this file but call it stmmac_tsn.c.

> @@ -3621,6 +3622,8 @@ static int stmmac_set_features(struct net_device *netdev,
>  	 */
>  	stmmac_rx_ipc(priv, priv->hw);
>  
> +	netdev->features = features;

Isn't this a fix ?

RE: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities

[Ong, Boon Leong]

>-----Original Message-----
>From: Jose Abreu [mailto:Jose.Abreu@synopsys.com]
>>From: Voon Weifeng <weifeng.voon@intel.com>
>> diff --git a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
>b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
>> new file mode 100644
>> index 000000000000..cba27c604cb1
>> --- /dev/null
>> +++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
>
>XGMAC also supports TSN features so I think more abstraction is needed
>on this because the XGMAC implementation is very similar (only reg
>offsets and bitfields changes).
>
>I would rather:
>	- Implement HW specific handling in dwmac4_core.c / dwmac4_dma.c
>and
>add the callbacks in hwif table;
>	- Let TSN logic in this file but call it stmmac_tsn.c.
OK. Thanks for above feedback.
>
>> @@ -3621,6 +3622,8 @@ static int stmmac_set_features(struct net_device
>*netdev,
>>  	 */
>>  	stmmac_rx_ipc(priv, priv->hw);
>>
>> +	netdev->features = features;
>
>Isn't this a fix ?
Yup. We will split this out as a patch and send separately.

[RFC net-next 2/5] net: stmmac: gcl errors reporting and its interrupt handling

[Voon Weifeng]

Enabled interrupt for Constant Gate Control Error (CGCE), Head-of-Line
Blocking due to scheduling error (HLBS) and Head-of-Line Blocking due to
frame size error (HLBF).

CGCE should not happen as the driver has already implemented a check
before applying the settings. CGCE handling is added as a safety
check so that we can catch it if there is such error being fired. For
HLBS, the user will get the info of all the queues that shows this
error. For HLBF, the user will get the info of all the queue with the
latest frame size which causes the error. Frame size 0 indicates no
error.

This patch also added functionality to get and clear the gcl errors.

The ISR handling takes place when EST feature is enabled by user.

Signed-off-by: Voon Weifeng <weifeng.voon@intel.com>
Signed-off-by: Ong Boon Leong <boon.leong.ong@intel.com>
---
 drivers/net/ethernet/stmicro/stmmac/common.h      |   4 +
 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c  | 123 ++++++++++++++++++++++
 drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h  |  45 ++++++++
 drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c |   3 +
 drivers/net/ethernet/stmicro/stmmac/hwif.h        |   9 ++
 drivers/net/ethernet/stmicro/stmmac/stmmac_main.c |   7 ++
 6 files changed, 191 insertions(+)

diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index ad9e9368535d..9723b0a12110 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -407,6 +407,9 @@ struct mii_regs {
 	unsigned int clk_csr_mask;
 };
 
+/* tsn capability,  meant for mac_device_info->tsn_cap */
+#define TSN_CAP_EST			BIT(0)
+
 struct mac_device_info {
 	const struct stmmac_ops *mac;
 	const struct stmmac_desc_ops *desc;
@@ -425,6 +428,7 @@ struct mac_device_info {
 	unsigned int pcs;
 	unsigned int pmt;
 	unsigned int ps;
+	u32 tsn_cap;
 };
 
 struct stmmac_rx_routing {
diff --git a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
index cba27c604cb1..f14e86fcc93c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.c
@@ -11,6 +11,7 @@
 static bool dw_tsn_feat_en[TSN_FEAT_ID_MAX];
 static unsigned int dw_tsn_hwtunable[TSN_HWTUNA_MAX];
 static struct est_gc_config dw_est_gc_config;
+static struct tsn_err_stat dw_err_stat;
 
 static unsigned int est_get_gcl_depth(unsigned int hw_cap)
 {
@@ -295,6 +296,24 @@ void dwmac_tsn_init(void *ioaddr)
 		 gcl_depth, ti_wid, tils_max, cap->txqcnt);
 }
 
+/* dwmac_tsn_setup is called within stmmac_hw_setup() after
+ * stmmac_init_dma_engine() which resets MAC controller.
+ * This is so-that MAC registers are not cleared.
+ */
+void dwmac_tsn_setup(void *ioaddr)
+{
+	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
+	unsigned int value;
+
+	if (cap->est_support) {
+		/* Enable EST interrupts */
+		value = (MTL_EST_INT_EN_CGCE | MTL_EST_INT_EN_IEHS |
+			 MTL_EST_INT_EN_IEHF | MTL_EST_INT_EN_IEBE |
+			 MTL_EST_INT_EN_IECC);
+		TSN_WR32(value, ioaddr + MTL_EST_INT_EN);
+	}
+}
+
 void dwmac_get_tsn_hwcap(struct tsn_hw_cap **tsn_hwcap)
 {
 	*tsn_hwcap = &dw_tsn_hwcap;
@@ -788,3 +807,107 @@ int dwmac_get_est_gcc(void *ioaddr,
 
 	return 0;
 }
+
+int dwmac_est_irq_status(void *ioaddr)
+{
+	struct tsn_err_stat *err_stat = &dw_err_stat;
+	struct tsn_hw_cap *cap = &dw_tsn_hwcap;
+	unsigned int txqcnt_mask = 0;
+	unsigned int status = 0;
+	unsigned int value = 0;
+	unsigned int feqn = 0;
+	unsigned int hbfq = 0;
+	unsigned int hbfs = 0;
+
+	txqcnt_mask = (1 << cap->txqcnt) - 1;
+	status = TSN_RD32(ioaddr + MTL_EST_STATUS);
+
+	value = (MTL_EST_STATUS_CGCE | MTL_EST_STATUS_HLBS |
+		 MTL_EST_STATUS_HLBF | MTL_EST_STATUS_BTRE |
+		 MTL_EST_STATUS_SWLC);
+
+	/* Return if there is no error */
+	if (!(status & value))
+		return 0;
+
+	/* spin_lock() is not needed here because of BTRE and SWLC
+	 * bit will not be altered. Both of the bit will be
+	 * polled in dwmac_set_est_gcrr_times()
+	 */
+	if (status & MTL_EST_STATUS_CGCE) {
+		/* Clear Interrupt */
+		TSN_WR32(MTL_EST_STATUS_CGCE, ioaddr + MTL_EST_STATUS);
+
+		err_stat->cgce_n++;
+	}
+
+	if (status & MTL_EST_STATUS_HLBS) {
+		value = TSN_RD32(ioaddr + MTL_EST_SCH_ERR);
+		value &= txqcnt_mask;
+
+		/* Clear Interrupt */
+		TSN_WR32(value, ioaddr + MTL_EST_SCH_ERR);
+
+		/* Collecting info to shows all the queues that has HLBS */
+		/* issue. The only way to clear this is to clear the     */
+		/* statistic  */
+		err_stat->hlbs_q |= value;
+	}
+
+	if (status & MTL_EST_STATUS_HLBF) {
+		value = TSN_RD32(ioaddr + MTL_EST_FRM_SZ_ERR);
+		feqn = value & txqcnt_mask;
+
+		value = TSN_RD32(ioaddr + MTL_EST_FRM_SZ_CAP);
+		hbfq = (value & MTL_EST_FRM_SZ_CAP_HBFQ_MASK(cap->txqcnt))
+			>> MTL_EST_FRM_SZ_CAP_HBFQ_SHIFT;
+		hbfs = value & MTL_EST_FRM_SZ_CAP_HBFS_MASK;
+
+		/* Clear Interrupt */
+		TSN_WR32(feqn, ioaddr + MTL_EST_FRM_SZ_ERR);
+
+		err_stat->hlbf_sz[hbfq] = hbfs;
+	}
+
+	if (status & MTL_EST_STATUS_BTRE) {
+		if ((status & MTL_EST_STATUS_BTRL) ==
+		    MTL_EST_STATUS_BTRL_MAX)
+			err_stat->btre_max_n++;
+		else
+			err_stat->btre_n++;
+
+		err_stat->btrl = (status & MTL_EST_STATUS_BTRL) >>
+					MTL_EST_STATUS_BTRL_SHIFT;
+
+		TSN_WR32(MTL_EST_STATUS_BTRE, ioaddr +
+		       MTL_EST_STATUS);
+	}
+
+	if (status & MTL_EST_STATUS_SWLC) {
+		TSN_WR32(MTL_EST_STATUS_SWLC, ioaddr +
+			 MTL_EST_STATUS);
+		TSN_INFO_NA("SWOL has been switched\n");
+	}
+
+	return status;
+}
+
+int dwmac_get_est_err_stat(struct tsn_err_stat **err_stat)
+{
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	*err_stat = &dw_err_stat;
+
+	return 0;
+}
+
+int dwmac_clr_est_err_stat(void *ioaddr)
+{
+	if (!dw_tsn_feat_en[TSN_FEAT_ID_EST])
+		return -ENOTSUPP;
+
+	memset(&dw_err_stat, 0, sizeof(dw_err_stat));
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h
index feb71f7e7031..fa9a06c51a04 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dw_tsn_lib.h
@@ -37,9 +37,30 @@
 #define MTL_EST_STATUS_BTRL_MAX		(0xF << 8)
 #define MTL_EST_STATUS_SWOL		BIT(7)	/* SW owned list */
 #define MTL_EST_STATUS_SWOL_SHIFT	7
+#define MTL_EST_STATUS_CGCE		BIT(4)	/* Constant gate ctrl err */
+#define MTL_EST_STATUS_HLBS		BIT(3)	/* HLB due to scheduling */
+#define MTL_EST_STATUS_HLBF		BIT(2)	/* HLB due to frame size */
 #define MTL_EST_STATUS_BTRE		BIT(1)	/* BTR Error */
 #define MTL_EST_STATUS_SWLC		BIT(0)	/* Switch to SWOL complete */
 
+/* MTL EST Scheduling error */
+#define MTL_EST_SCH_ERR			0x00000c60
+#define MTL_EST_FRM_SZ_ERR		0x00000c64
+#define MTL_EST_FRM_SZ_CAP		0x00000c68
+#define MTL_EST_FRM_SZ_CAP_HBFS_MASK	GENMASK(14, 0)
+#define MTL_EST_FRM_SZ_CAP_HBFQ_SHIFT	16
+#define MTL_EST_FRM_SZ_CAP_HBFQ_MASK(x)	(x > 4 ? GENMASK(18, 16) : \
+						 x > 2 ? GENMASK(17, 16) : \
+						 BIT(16))
+
+/* MTL EST interrupt enable */
+#define MTL_EST_INT_EN			0x00000c70
+#define MTL_EST_INT_EN_CGCE		BIT(4)
+#define MTL_EST_INT_EN_IEHS		BIT(3)
+#define MTL_EST_INT_EN_IEHF		BIT(2)
+#define MTL_EST_INT_EN_IEBE		BIT(1)
+#define MTL_EST_INT_EN_IECC		BIT(0)
+
 /* MTL EST GCL control register */
 #define MTL_EST_GCL_CTRL		0x00000c80
 #define MTL_EST_GCL_CTRL_ADDR		GENMASK(10, 8)	/* GCL Address */
@@ -118,6 +139,26 @@ struct tsn_hw_cap {
 	unsigned int ext_max;		/* Max time extension */
 };
 
+/* TSN Error Status */
+struct tsn_err_stat {
+	unsigned int cgce_n;			/* Constant gate error
+						 * count.
+						 */
+	unsigned int hlbs_q;			/* Queue with HLB due to
+						 * Scheduling
+						 */
+	unsigned int hlbf_sz[MTL_MAX_TX_QUEUES];/* Frame size that causes
+						 * HLB
+						 */
+	unsigned int btre_n;			/* BTR error with BTR
+						 * renewal
+						 */
+	unsigned int btre_max_n;		/* BTR error with BTR
+						 * renewal fail count
+						 */
+	unsigned int btrl;			/* BTR error loop count */
+};
+
 /* EST Gate Control Entry */
 struct est_gc_entry {
 	unsigned int gates;		/* gate control: 0: closed,
@@ -150,6 +191,7 @@ struct est_gc_config {
 
 /* TSN functions */
 void dwmac_tsn_init(void *ioaddr);
+void dwmac_tsn_setup(void *ioaddr);
 void dwmac_get_tsn_hwcap(struct tsn_hw_cap **tsn_hwcap);
 void dwmac_set_est_gcb(struct est_gc_entry *gcl, unsigned int bank);
 void dwmac_set_tsn_feat(enum tsn_feat_id featid, bool enable);
@@ -170,4 +212,7 @@ int dwmac_set_est_gcrr_times(void *ioaddr,
 int dwmac_set_est_enable(void *ioaddr, bool enable);
 int dwmac_get_est_gcc(void *ioaddr,
 		      struct est_gc_config **gcc, bool frmdrv);
+int dwmac_est_irq_status(void *ioaddr);
+int dwmac_get_est_err_stat(struct tsn_err_stat **err_stat);
+int dwmac_clr_est_err_stat(void *ioaddr);
 #endif /* __DW_TSN_LIB_H__ */
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
index 1361807fe802..39a35f7eb91d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c
@@ -830,6 +830,9 @@ static void dwmac4_set_mac_loopback(void __iomem *ioaddr, bool enable)
 	.set_est_gcrr_times = dwmac_set_est_gcrr_times,
 	.set_est_enable = dwmac_set_est_enable,
 	.get_est_gcc = dwmac_get_est_gcc,
+	.est_irq_status = dwmac_est_irq_status,
+	.get_est_err_stat = dwmac_get_est_err_stat,
+	.clr_est_err_stat = dwmac_clr_est_err_stat,
 	.safety_feat_config = dwmac5_safety_feat_config,
 	.safety_feat_irq_status = dwmac5_safety_feat_irq_status,
 	.safety_feat_dump = dwmac5_safety_feat_dump,
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index 518a72805185..dec9b1f5c557 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -337,6 +337,9 @@ struct stmmac_ops {
 	int (*set_est_enable)(void __iomem *ioaddr, bool enable);
 	int (*get_est_gcc)(void __iomem *ioaddr,
 			   struct est_gc_config **gcc, bool frmdrv);
+	int (*est_irq_status)(void __iomem *ioaddr);
+	int (*get_est_err_stat)(struct tsn_err_stat **err_stat);
+	int (*clr_est_err_stat)(void __iomem *ioaddr);
 	/* Safety Features */
 	int (*safety_feat_config)(void __iomem *ioaddr, unsigned int asp);
 	int (*safety_feat_irq_status)(struct net_device *ndev,
@@ -437,6 +440,12 @@ struct stmmac_ops {
 	stmmac_do_callback(__priv, mac, set_est_enable, __args)
 #define stmmac_get_est_gcc(__priv, __args...) \
 	stmmac_do_callback(__priv, mac, get_est_gcc, __args)
+#define stmmac_est_irq_status(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, est_irq_status, __args)
+#define stmmac_get_est_err_stat(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, get_est_err_stat, __args)
+#define stmmac_clr_est_err_stat(__priv, __args...) \
+	stmmac_do_callback(__priv, mac, clr_est_err_stat, __args)
 #define stmmac_safety_feat_config(__priv, __args...) \
 	stmmac_do_callback(__priv, mac, safety_feat_config, __args)
 #define stmmac_safety_feat_irq_status(__priv, __args...) \
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 91213cd3a668..c28b5e69f2cd 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -2530,6 +2530,9 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
 	/* Start the ball rolling... */
 	stmmac_start_all_dma(priv);
 
+	/* Setup for TSN capability */
+	dwmac_tsn_setup(priv->ioaddr);
+
 	return 0;
 }
 
@@ -3693,6 +3696,9 @@ static irqreturn_t stmmac_interrupt(int irq, void *dev_id)
 						       queue);
 		}
 
+		if (priv->hw->tsn_cap & TSN_CAP_EST)
+			stmmac_est_irq_status(priv, priv->ioaddr);
+
 		/* PCS link status */
 		if (priv->hw->pcs) {
 			if (priv->xstats.pcs_link)
@@ -4297,6 +4303,7 @@ int stmmac_dvr_probe(struct device *device,
 	stmmac_get_tsn_hwcap(priv, &tsn_hwcap);
 	if (tsn_hwcap && tsn_hwcap->est_support && priv->plat->tsn_est_en) {
 		stmmac_set_tsn_feat(priv, TSN_FEAT_ID_EST, true);
+		priv->hw->tsn_cap |= TSN_CAP_EST;
 		dev_info(priv->device, "EST feature enabled\n");
 	}
 
-- 
1.9.1

[RFC net-next 3/5] taprio: Add support for hardware offloading

[Voon Weifeng]

From: Vinicius Costa Gomes <vinicius.gomes@intel.com>

This allows taprio to offload the schedule enforcement to capable
network cards, resulting in more precise windows and less CPU usage.

The important detail here is the difference between the gate_mask in
taprio and gate_mask for the network driver. For the driver, each bit
in gate_mask references a transmission queue: bit 0 for queue 0, bit 1
for queue 1, and so on. This is done so the driver doesn't need to
know about traffic classes.

Signed-off-by: Vinicius Costa Gomes <vinicius.gomes@intel.com>
Signed-off-by: Voon Weifeng <weifeng.voon@intel.com>
---
 include/linux/netdevice.h      |   1 +
 include/net/pkt_sched.h        |  18 +++
 include/uapi/linux/pkt_sched.h |   4 +
 net/sched/sch_taprio.c         | 263 ++++++++++++++++++++++++++++++++++++++++-
 4 files changed, 284 insertions(+), 2 deletions(-)

diff --git a/include/linux/netdevice.h b/include/linux/netdevice.h
index eeacebd7debb..e6bf0ede8cfa 100644
--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -845,6 +845,7 @@ enum tc_setup_type {
 	TC_SETUP_QDISC_ETF,
 	TC_SETUP_ROOT_QDISC,
 	TC_SETUP_QDISC_GRED,
+	TC_SETUP_QDISC_TAPRIO,
 };
 
 /* These structures hold the attributes of bpf state that are being passed
diff --git a/include/net/pkt_sched.h b/include/net/pkt_sched.h
index a16fbe9a2a67..3333c107f920 100644
--- a/include/net/pkt_sched.h
+++ b/include/net/pkt_sched.h
@@ -161,4 +161,22 @@ struct tc_etf_qopt_offload {
 	s32 queue;
 };
 
+struct tc_taprio_sched_entry {
+	u8 command; /* TC_TAPRIO_CMD_* */
+
+	/* The gate_mask in the offloading side refers to HW queues */
+	u32 gate_mask;
+	u32 interval;
+};
+
+struct tc_taprio_qopt_offload {
+	u8 enable;
+	ktime_t base_time;
+	u64 cycle_time;
+	u64 cycle_time_extension;
+
+	size_t num_entries;
+	struct tc_taprio_sched_entry entries[0];
+};
+
 #endif
diff --git a/include/uapi/linux/pkt_sched.h b/include/uapi/linux/pkt_sched.h
index 8b2f993cbb77..08a260fd7843 100644
--- a/include/uapi/linux/pkt_sched.h
+++ b/include/uapi/linux/pkt_sched.h
@@ -1158,6 +1158,9 @@ enum {
  *       [TCA_TAPRIO_ATTR_SCHED_ENTRY_INTERVAL]
  */
 
+#define TCA_TAPRIO_ATTR_OFFLOAD_FLAG_FULL_OFFLOAD 0x1
+#define TCA_TAPRIO_ATTR_OFFLOAD_FLAG_TXTIME_OFFLOAD 0x2
+
 enum {
 	TCA_TAPRIO_ATTR_UNSPEC,
 	TCA_TAPRIO_ATTR_PRIOMAP, /* struct tc_mqprio_qopt */
@@ -1169,6 +1172,7 @@ enum {
 	TCA_TAPRIO_ATTR_ADMIN_SCHED, /* The admin sched, only used in dump */
 	TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME, /* s64 */
 	TCA_TAPRIO_ATTR_SCHED_CYCLE_TIME_EXTENSION, /* s64 */
+	TCA_TAPRIO_ATTR_OFFLOAD_FLAGS, /* u32 */
 	__TCA_TAPRIO_ATTR_MAX,
 };
 
diff --git a/net/sched/sch_taprio.c b/net/sched/sch_taprio.c
index 9ecfb8f5902a..9e8f066a2474 100644
--- a/net/sched/sch_taprio.c
+++ b/net/sched/sch_taprio.c
@@ -26,6 +26,9 @@
 static DEFINE_SPINLOCK(taprio_list_lock);
 
 #define TAPRIO_ALL_GATES_OPEN -1
+#define FULL_OFFLOAD_IS_ON(flags) ((flags) & TCA_TAPRIO_ATTR_OFFLOAD_FLAG_FULL_OFFLOAD)
+#define TXTIME_OFFLOAD_IS_ON(flags) ((flags) & TCA_TAPRIO_ATTR_OFFLOAD_FLAG_TXTIME_OFFLOAD)
+#define VALID_OFFLOAD(flags) ((flags) != U32_MAX)
 
 struct sched_entry {
 	struct list_head list;
@@ -55,6 +58,8 @@ struct sched_gate_list {
 struct taprio_sched {
 	struct Qdisc **qdiscs;
 	struct Qdisc *root;
+	struct tc_mqprio_qopt mqprio;
+	u32 offload_flags;
 	int clockid;
 	atomic64_t picos_per_byte; /* Using picoseconds because for 10Gbps+
 				    * speeds it's sub-nanoseconds per byte
@@ -66,6 +71,8 @@ struct taprio_sched {
 	struct sched_gate_list __rcu *oper_sched;
 	struct sched_gate_list __rcu *admin_sched;
 	ktime_t (*get_time)(void);
+	struct sk_buff *(*dequeue)(struct Qdisc *sch);
+	struct sk_buff *(*peek)(struct Qdisc *sch);
 	struct hrtimer advance_timer;
 	struct list_head taprio_list;
 };
@@ -143,7 +150,30 @@ static int taprio_enqueue(struct sk_buff *skb, struct Qdisc *sch,
 	return qdisc_enqueue(skb, child, to_free);
 }
 
-static struct sk_buff *taprio_peek(struct Qdisc *sch)
+static struct sk_buff *taprio_peek_offload(struct Qdisc *sch)
+{
+	struct taprio_sched *q = qdisc_priv(sch);
+	struct net_device *dev = qdisc_dev(sch);
+	struct sk_buff *skb;
+	int i;
+
+	for (i = 0; i < dev->num_tx_queues; i++) {
+		struct Qdisc *child = q->qdiscs[i];
+
+		if (unlikely(!child))
+			continue;
+
+		skb = child->ops->peek(child);
+		if (!skb)
+			continue;
+
+		return skb;
+	}
+
+	return NULL;
+}
+
+static struct sk_buff *taprio_peek_soft(struct Qdisc *sch)
 {
 	struct taprio_sched *q = qdisc_priv(sch);
 	struct net_device *dev = qdisc_dev(sch);
@@ -184,6 +214,13 @@ static struct sk_buff *taprio_peek(struct Qdisc *sch)
 	return NULL;
 }
 
+static struct sk_buff *taprio_peek(struct Qdisc *sch)
+{
+	struct taprio_sched *q = qdisc_priv(sch);
+
+	return q->peek(sch);
+}
+
 static inline int length_to_duration(struct taprio_sched *q, int len)
 {
 	return div_u64(len * atomic64_read(&q->picos_per_byte), 1000);
@@ -196,7 +233,7 @@ static void taprio_set_budget(struct taprio_sched *q, struct sched_entry *entry)
 			     atomic64_read(&q->picos_per_byte)));
 }
 
-static struct sk_buff *taprio_dequeue(struct Qdisc *sch)
+static struct sk_buff *taprio_dequeue_soft(struct Qdisc *sch)
 {
 	struct taprio_sched *q = qdisc_priv(sch);
 	struct net_device *dev = qdisc_dev(sch);
@@ -275,6 +312,40 @@ static struct sk_buff *taprio_dequeue(struct Qdisc *sch)
 	return skb;
 }
 
+static struct sk_buff *taprio_dequeue_offload(struct Qdisc *sch)
+{
+	struct taprio_sched *q = qdisc_priv(sch);
+	struct net_device *dev = qdisc_dev(sch);
+	struct sk_buff *skb;
+	int i;
+
+	for (i = 0; i < dev->num_tx_queues; i++) {
+		struct Qdisc *child = q->qdiscs[i];
+
+		if (unlikely(!child))
+			continue;
+
+		skb = child->ops->dequeue(child);
+		if (unlikely(!skb))
+			continue;
+
+		qdisc_bstats_update(sch, skb);
+		qdisc_qstats_backlog_dec(sch, skb);
+		sch->q.qlen--;
+
+		return skb;
+	}
+
+	return NULL;
+}
+
+static struct sk_buff *taprio_dequeue(struct Qdisc *sch)
+{
+	struct taprio_sched *q = qdisc_priv(sch);
+
+	return q->dequeue(sch);
+}
+
 static bool should_restart_cycle(const struct sched_gate_list *oper,
 				 const struct sched_entry *entry)
 {
@@ -707,6 +778,165 @@ static int taprio_dev_notifier(struct notifier_block *nb, unsigned long event,
 	return NOTIFY_DONE;
 }
 
+static u32 tc_mask_to_queue_mask(const struct tc_mqprio_qopt *mqprio,
+				 u32 tc_mask)
+{
+	u32 i, queue_mask = 0;
+
+	for (i = 0; i < mqprio->num_tc; i++) {
+		u32 offset, count;
+
+		if (!(tc_mask & BIT(i)))
+			continue;
+
+		offset = mqprio->offset[i];
+		count = mqprio->count[i];
+
+		queue_mask |= GENMASK(offset + count - 1, offset);
+	}
+
+	return queue_mask;
+}
+
+static void taprio_sched_to_offload(struct taprio_sched *q,
+				    struct sched_gate_list *sched,
+				    struct tc_taprio_qopt_offload *taprio)
+{
+	struct sched_entry *entry;
+	int i = 0;
+
+	taprio->base_time = sched->base_time;
+
+	list_for_each_entry(entry, &sched->entries, list) {
+		struct tc_taprio_sched_entry *e = &taprio->entries[i];
+
+		e->command = entry->command;
+		e->interval = entry->interval;
+
+		/* We do this transformation because the NIC
+		 * has no knowledge of traffic classes, but it
+		 * knows about queues.
+		 */
+		e->gate_mask = tc_mask_to_queue_mask(&q->mqprio,
+						     entry->gate_mask);
+		i++;
+	}
+
+	taprio->num_entries = i;
+}
+
+static void taprio_disable_offload(struct net_device *dev,
+				   struct taprio_sched *q)
+{
+	const struct net_device_ops *ops = dev->netdev_ops;
+	struct tc_taprio_qopt_offload taprio = { };
+	int err;
+
+	if (!q->offload_flags)
+		return;
+
+	if (!ops->ndo_setup_tc)
+		return;
+
+	taprio.enable = 0;
+
+	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, &taprio);
+	if (err < 0)
+		return;
+
+	/* Just to be sure to keep the function pointers in a
+	 * consistent state always.
+	 */
+	q->dequeue = taprio_dequeue_soft;
+	q->peek = taprio_peek_soft;
+
+	q->advance_timer.function = advance_sched;
+
+	q->offload_flags = 0;
+}
+
+static enum hrtimer_restart next_sched(struct hrtimer *timer)
+{
+	struct taprio_sched *q = container_of(timer, struct taprio_sched,
+					      advance_timer);
+	struct sched_gate_list *oper, *admin;
+
+	spin_lock(&q->current_entry_lock);
+	oper = rcu_dereference_protected(q->oper_sched,
+					 lockdep_is_held(&q->current_entry_lock));
+	admin = rcu_dereference_protected(q->admin_sched,
+					  lockdep_is_held(&q->current_entry_lock));
+
+	rcu_assign_pointer(q->oper_sched, admin);
+	rcu_assign_pointer(q->admin_sched, NULL);
+
+	if (oper)
+		call_rcu(&oper->rcu, taprio_free_sched_cb);
+
+	spin_unlock(&q->current_entry_lock);
+
+	return HRTIMER_NORESTART;
+}
+
+static int taprio_enable_offload(struct net_device *dev,
+				 struct tc_mqprio_qopt *mqprio,
+				 struct taprio_sched *q,
+				 struct sched_gate_list *sched,
+				 struct netlink_ext_ack *extack,
+				 u32 offload_flags)
+{
+	const struct net_device_ops *ops = dev->netdev_ops;
+	struct tc_taprio_qopt_offload *taprio;
+	size_t size;
+	int err = 0;
+
+	if (!FULL_OFFLOAD_IS_ON(offload_flags)) {
+		NL_SET_ERR_MSG(extack, "Offload mode is not supported");
+		return -EOPNOTSUPP;
+	}
+
+	if (!ops->ndo_setup_tc) {
+		NL_SET_ERR_MSG(extack, "Specified device does not support taprio offload");
+		return -EOPNOTSUPP;
+	}
+
+	size = sizeof(*taprio) +
+		sched->num_entries * sizeof(struct tc_taprio_sched_entry);
+
+	taprio = kzalloc(size, GFP_ATOMIC);
+	if (!taprio) {
+		NL_SET_ERR_MSG(extack, "Not enough memory for enabling offload mode");
+		return -ENOMEM;
+	}
+
+	taprio->enable = 1;
+	taprio_sched_to_offload(q, sched, taprio);
+
+	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_TAPRIO, taprio);
+	if (err < 0) {
+		NL_SET_ERR_MSG(extack, "Specified device failed to setup taprio hardware offload");
+		goto done;
+	}
+
+	q->dequeue = taprio_dequeue_offload;
+	q->peek = taprio_peek_offload;
+
+	/* This function will only serve to keep the pointers to the
+	 * "oper" and "admin" schedules valid in relation to their
+	 * base times, so when calling dump() the users looks at the
+	 * right schedules.
+	 */
+	q->advance_timer.function = next_sched;
+
+done:
+	kfree(taprio);
+
+	if (err == 0)
+		q->offload_flags = offload_flags;
+
+	return err;
+}
+
 static int taprio_change(struct Qdisc *sch, struct nlattr *opt,
 			 struct netlink_ext_ack *extack)
 {
@@ -715,6 +945,7 @@ static int taprio_change(struct Qdisc *sch, struct nlattr *opt,
 	struct taprio_sched *q = qdisc_priv(sch);
 	struct net_device *dev = qdisc_dev(sch);
 	struct tc_mqprio_qopt *mqprio = NULL;
+	u32 offload_flags = U32_MAX;
 	int i, err, clockid;
 	unsigned long flags;
 	ktime_t start;
@@ -731,6 +962,9 @@ static int taprio_change(struct Qdisc *sch, struct nlattr *opt,
 	if (err < 0)
 		return err;
 
+	if (tb[TCA_TAPRIO_ATTR_OFFLOAD_FLAGS])
+		offload_flags = nla_get_u32(tb[TCA_TAPRIO_ATTR_OFFLOAD_FLAGS]);
+
 	new_admin = kzalloc(sizeof(*new_admin), GFP_KERNEL);
 	if (!new_admin) {
 		NL_SET_ERR_MSG(extack, "Not enough memory for a new schedule");
@@ -749,6 +983,12 @@ static int taprio_change(struct Qdisc *sch, struct nlattr *opt,
 		goto free_sched;
 	}
 
+	if (offload_flags != U32_MAX && (oper || admin)) {
+		NL_SET_ERR_MSG(extack, "Changing 'offload' of a running schedule is not supported");
+		err = -ENOTSUPP;
+		goto free_sched;
+	}
+
 	err = parse_taprio_schedule(tb, new_admin, extack);
 	if (err < 0)
 		goto free_sched;
@@ -802,6 +1042,8 @@ static int taprio_change(struct Qdisc *sch, struct nlattr *opt,
 		for (i = 0; i < TC_BITMASK + 1; i++)
 			netdev_set_prio_tc_map(dev, i,
 					       mqprio->prio_tc_map[i]);
+
+		memcpy(&q->mqprio, mqprio, sizeof(q->mqprio));
 	}
 
 	switch (q->clockid) {
@@ -823,6 +1065,15 @@ static int taprio_change(struct Qdisc *sch, struct nlattr *opt,
 		goto unlock;
 	}
 
+	if (!offload_flags) {
+		taprio_disable_offload(dev, q);
+	} else if (VALID_OFFLOAD(offload_flags) || q->offload_flags) {
+		err = taprio_enable_offload(dev, mqprio, q,
+					    new_admin, extack, offload_flags);
+		if (err)
+			goto unlock;
+	}
+
 	err = taprio_get_start_time(sch, new_admin, &start);
 	if (err < 0) {
 		NL_SET_ERR_MSG(extack, "Internal error: failed get start time");
@@ -866,6 +1117,8 @@ static void taprio_destroy(struct Qdisc *sch)
 
 	hrtimer_cancel(&q->advance_timer);
 
+	taprio_disable_offload(dev, q);
+
 	if (q->qdiscs) {
 		for (i = 0; i < dev->num_tx_queues && q->qdiscs[i]; i++)
 			qdisc_put(q->qdiscs[i]);
@@ -895,6 +1148,9 @@ static int taprio_init(struct Qdisc *sch, struct nlattr *opt,
 	hrtimer_init(&q->advance_timer, CLOCK_TAI, HRTIMER_MODE_ABS);
 	q->advance_timer.function = advance_sched;
 
+	q->dequeue = taprio_dequeue_soft;
+	q->peek = taprio_peek_soft;
+
 	q->root = sch;
 
 	/* We only support static clockids. Use an invalid value as default
@@ -1080,6 +1336,9 @@ static int taprio_dump(struct Qdisc *sch, struct sk_buff *skb)
 	if (nla_put_s32(skb, TCA_TAPRIO_ATTR_SCHED_CLOCKID, q->clockid))
 		goto options_error;
 
+	if (nla_put_u32(skb, TCA_TAPRIO_ATTR_OFFLOAD_FLAGS, q->offload_flags))
+		goto options_error;
+
 	if (oper && dump_schedule(skb, oper))
 		goto options_error;
 
-- 
1.9.1

[RFC net-next 4/5] net: stmmac: enable HW offloading for tc taprio

[Voon Weifeng]

This patch enable iproute2's tc taprio to run IEEE802.1Qbv using HW.
tc taprio manual can refer to:
http://man7.org/linux/man-pages/man8/tc-taprio.8.html

To enable HW offloading, an extra argument need to be added:
offload 1

Example to run:
$ tc qdisc add dev IFACE parent root handle 100 taprio \
      num_tc 3 \
      map 2 2 1 0 2 2 2 2 2 2 2 2 2 2 2 2 \
      queues 1@0 1@1 2@2 \
      base-time 10000000 \
      sched-entry S 03 300000 \
      sched-entry S 02 300000 \
      sched-entry S 06 400000 \
      clockid CLOCK_TAI
      offload 1

Signed-off-by: Voon Weifeng <weifeng.voon@intel.com>
---
 drivers/net/ethernet/stmicro/stmmac/hwif.h        |  5 ++
 drivers/net/ethernet/stmicro/stmmac/stmmac_main.c |  2 +
 drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c   | 96 +++++++++++++++++++++++
 3 files changed, 103 insertions(+)

diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index dec9b1f5c557..c9fa60934710 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -514,6 +514,7 @@ struct stmmac_mode_ops {
 struct stmmac_priv;
 struct tc_cls_u32_offload;
 struct tc_cbs_qopt_offload;
+struct tc_taprio_qopt_offload;
 
 struct stmmac_tc_ops {
 	int (*init)(struct stmmac_priv *priv);
@@ -521,6 +522,8 @@ struct stmmac_tc_ops {
 			     struct tc_cls_u32_offload *cls);
 	int (*setup_cbs)(struct stmmac_priv *priv,
 			 struct tc_cbs_qopt_offload *qopt);
+	int (*setup_taprio)(struct stmmac_priv *priv,
+			    struct tc_taprio_qopt_offload *qopt);
 };
 
 #define stmmac_tc_init(__priv, __args...) \
@@ -529,6 +532,8 @@ struct stmmac_tc_ops {
 	stmmac_do_callback(__priv, tc, setup_cls_u32, __args)
 #define stmmac_tc_setup_cbs(__priv, __args...) \
 	stmmac_do_callback(__priv, tc, setup_cbs, __args)
+#define stmmac_tc_setup_taprio(__priv, __args...) \
+	stmmac_do_callback(__priv, tc, setup_taprio, __args)
 
 struct stmmac_counters;
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index c28b5e69f2cd..a443c42fa58b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -3811,6 +3811,8 @@ static int stmmac_setup_tc(struct net_device *ndev, enum tc_setup_type type,
 		return stmmac_setup_tc_block(priv, type_data);
 	case TC_SETUP_QDISC_CBS:
 		return stmmac_tc_setup_cbs(priv, priv, type_data);
+	case TC_SETUP_QDISC_TAPRIO:
+		return stmmac_tc_setup_taprio(priv, priv, type_data);
 	default:
 		return -EOPNOTSUPP;
 	}
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
index 58ea18af9813..d118e3636d50 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c
@@ -7,10 +7,13 @@
 #include <net/pkt_cls.h>
 #include <net/tc_act/tc_gact.h>
 #include "common.h"
+#include "dw_tsn_lib.h"
 #include "dwmac4.h"
 #include "dwmac5.h"
 #include "stmmac.h"
 
+#define ONE_SEC_IN_NANOSEC 1000000000ULL
+
 static void tc_fill_all_pass_entry(struct stmmac_tc_entry *entry)
 {
 	memset(entry, 0, sizeof(*entry));
@@ -349,8 +352,101 @@ static int tc_setup_cbs(struct stmmac_priv *priv,
 	return 0;
 }
 
+static int tc_setup_taprio(struct stmmac_priv *priv,
+			   struct tc_taprio_qopt_offload *qopt)
+{
+	u64 time_extension = qopt->cycle_time_extension;
+	u64 base_time = ktime_to_ns(qopt->base_time);
+	u64 cycle_time = qopt->cycle_time;
+	struct est_gcrr egcrr;
+	u32 extension_ns;
+	u32 extension_s;
+	u32 cycle_ns;
+	u32 cycle_s;
+	u32 base_ns;
+	u32 base_s;
+	int ret;
+	int i;
+
+	if (qopt->enable) {
+		stmmac_set_est_enable(priv, priv->ioaddr, 1);
+		dev_info(priv->device, "taprio: EST enabled\n");
+	} else {
+		stmmac_set_est_enable(priv, priv->ioaddr, 0);
+		dev_info(priv->device, "taprio: EST disabled\n");
+		return 0;
+	}
+
+	dev_dbg(priv->device, "taprio: time_extension %lld, base_time %lld, cycle_time %lld\n",
+		qopt->cycle_time_extension, qopt->base_time, qopt->cycle_time);
+
+	for (i = 0; i < qopt->num_entries; i++) {
+		struct est_gc_entry sgce;
+
+		sgce.gates = qopt->entries[i].gate_mask;
+		sgce.ti_nsec = qopt->entries[i].interval;
+
+		/* cycle_time will be sum of all time interval
+		 * of the entries in the schedule if the
+		 * cycle_time is not provided
+		 */
+		if (!qopt->cycle_time)
+			cycle_time += qopt->entries[i].interval;
+
+		dev_dbg(priv->device, "taprio: gates 0x%x, ti_ns %d, cycle_ns %d\n",
+			sgce.gates, sgce.ti_nsec, cycle_ns);
+
+		ret = stmmac_set_est_gce(priv, priv->ioaddr, &sgce, i, 0, 0);
+
+		if (ret) {
+			dev_err(priv->device,
+				"taprio: fail to program GC entry(%d).\n", i);
+
+			return ret;
+		}
+	}
+
+	ret = stmmac_set_est_gcrr_llr(priv, priv->ioaddr,
+				      qopt->num_entries,
+				      0, 0);
+	if (ret) {
+		dev_err(priv->device,
+			"taprio: fail to program GC into HW\n");
+	}
+
+	/* set est_info */
+	base_ns = do_div(base_time, ONE_SEC_IN_NANOSEC);
+	base_s = base_time;
+	dev_dbg(priv->device, "taprio: base_s %d, base_ns %d\n",
+		base_s, base_ns);
+
+	cycle_ns = do_div(cycle_time, ONE_SEC_IN_NANOSEC);
+	cycle_s = cycle_time;
+	dev_dbg(priv->device, "taprio: cycle_s %d, cycle_ns %d\n",
+		cycle_s, cycle_ns);
+
+	extension_ns = do_div(time_extension, ONE_SEC_IN_NANOSEC);
+	extension_s = time_extension;
+	dev_dbg(priv->device, "taprio: extension_s %d, extension_ns %d\n",
+		extension_s, extension_ns);
+
+	if (extension_s) {
+		dev_err(priv->device, "taprio: extension in seconds not supported.\n");
+		return -EINVAL;
+	}
+
+	egcrr.cycle_sec = cycle_s;
+	egcrr.cycle_nsec = cycle_ns;
+	egcrr.base_sec = base_s;
+	egcrr.base_nsec = base_ns;
+	egcrr.ter_nsec = extension_ns;
+
+	return stmmac_set_est_gcrr_times(priv, priv->ioaddr, &egcrr, 0, 0);
+}
+
 const struct stmmac_tc_ops dwmac510_tc_ops = {
 	.init = tc_init,
 	.setup_cls_u32 = tc_setup_cls_u32,
 	.setup_cbs = tc_setup_cbs,
+	.setup_taprio = tc_setup_taprio,
 };
-- 
1.9.1

[RFC net-next 5/5] net: stmmac: Set TSN HW tunable after tsn setup

[Voon Weifeng]

TSN HW tunable data for PTP Time Offset Value(PTOV),
Current Time Offset Value(CTOV) and Time Interval Shift
Amount(TILS) are added as platform data. These platform
data are set after tsn setup.

Signed-off-by: Voon Weifeng <weifeng.voon@intel.com>
---
 drivers/net/ethernet/stmicro/stmmac/stmmac_main.c | 16 ++++++++++++++++
 include/linux/stmmac.h                            |  3 +++
 2 files changed, 19 insertions(+)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index a443c42fa58b..d3ce86abdc69 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -2533,6 +2533,22 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
 	/* Setup for TSN capability */
 	dwmac_tsn_setup(priv->ioaddr);
 
+	/* Set TSN HW tunable */
+	if (priv->plat->ptov)
+		stmmac_set_tsn_hwtunable(priv, priv->ioaddr,
+					 TSN_HWTUNA_TX_EST_PTOV,
+					 &priv->plat->ptov);
+
+	if (priv->plat->ctov)
+		stmmac_set_tsn_hwtunable(priv, priv->ioaddr,
+					 TSN_HWTUNA_TX_EST_CTOV,
+					 &priv->plat->ctov);
+
+	if (priv->plat->tils)
+		stmmac_set_tsn_hwtunable(priv, priv->ioaddr,
+					 TSN_HWTUNA_TX_EST_TILS,
+					 &priv->plat->tils);
+
 	return 0;
 }
 
diff --git a/include/linux/stmmac.h b/include/linux/stmmac.h
index d4a90f48e49b..792aa8b3e138 100644
--- a/include/linux/stmmac.h
+++ b/include/linux/stmmac.h
@@ -176,5 +176,8 @@ struct plat_stmmacenet_data {
 	int mac_port_sel_speed;
 	bool en_tx_lpi_clockgating;
 	int has_xgmac;
+	unsigned int ptov;
+	unsigned int ctov;
+	unsigned int tils;
 };
 #endif
-- 
1.9.1