Received-SPF: pass (google.com: best guess record for domain of linux-kernel-owner@vger.kernel.org designates 209.132.180.67 as permitted sender) client-ip=209.132.180.67;
From:   Vinicius Costa Gomes <vinicius.gomes@intel.com>
To:     Voon Weifeng <weifeng.voon@intel.com>,
        "David S. Miller" <davem@davemloft.net>,
        Maxime Coquelin <mcoquelin.stm32@gmail.com>
Cc:     netdev@vger.kernel.org, linux-kernel@vger.kernel.org,
        Jose Abreu <joabreu@synopsys.com>,
        Giuseppe Cavallaro <peppe.cavallaro@st.com>,
        Andrew Lunn <andrew@lunn.ch>,
        Florian Fainelli <f.fainelli@gmail.com>,
        Alexandre Torgue <alexandre.torgue@st.com>,
        Ong Boon Leong <boon.leong.ong@intel.com>,
        Voon Weifeng <weifeng.voon@intel.com>
Subject: Re: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities
In-Reply-To: <1560893778-6838-2-git-send-email-weifeng.voon@intel.com>
References: <1560893778-6838-1-git-send-email-weifeng.voon@intel.com> <1560893778-6838-2-git-send-email-weifeng.voon@intel.com>
Date:   Wed, 19 Jun 2019 11:12:41 -0700
Message-ID: <874l4lsaue.fsf@intel.com>
MIME-Version: 1.0
Content-Type: text/plain
Sender: linux-kernel-owner@vger.kernel.org
Precedence: bulk

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