[RFC net-next 1/5] net: stmmac: introduce IEEE 802.1Qbv configuration functionalities
From: Voon Weifeng
Date: Tue Jun 18 2019 - 09:41:14 EST
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;
+
+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