Re: [RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities
From: Vinicius Costa Gomes
Date: Wed Jun 19 2019 - 14:17:58 EST
Hi,
Voon Weifeng <weifeng.voon@xxxxxxxxx> writes:
> From: Ong Boon Leong <boon.leong.ong@xxxxxxxxx>
>
> 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@xxxxxxxxx>
> Reviewed-by: Voon Weifeng <weifeng.voon@xxxxxxxxx>
> Reviewed-by: Kweh Hock Leong <hock.leong.kweh@xxxxxxxxx>
> Signed-off-by: Ong Boon Leong <boon.leong.ong@xxxxxxxxx>
> Signed-off-by: Voon Weifeng <weifeng.voon@xxxxxxxxx>
> ---
> 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