From mboxrd@z Thu Jan 1 00:00:00 1970 Return-Path: X-Spam-Checker-Version: SpamAssassin 3.4.0 (2014-02-07) on aws-us-west-2-korg-lkml-1.web.codeaurora.org X-Spam-Level: X-Spam-Status: No, score=-6.7 required=3.0 tests=HEADER_FROM_DIFFERENT_DOMAINS, INCLUDES_PATCH,MAILING_LIST_MULTI,SIGNED_OFF_BY,SPF_HELO_NONE,SPF_PASS, URIBL_BLOCKED autolearn=unavailable autolearn_force=no version=3.4.0 Received: from mail.kernel.org (mail.kernel.org [198.145.29.99]) by smtp.lore.kernel.org (Postfix) with ESMTP id F0E8CC31E5E for ; Wed, 19 Jun 2019 18:12:51 +0000 (UTC) Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by mail.kernel.org (Postfix) with ESMTP id B0B2A21721 for ; Wed, 19 Jun 2019 18:12:51 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S1727244AbfFSSMs (ORCPT ); Wed, 19 Jun 2019 14:12:48 -0400 Received: from mga06.intel.com ([134.134.136.31]:39934 "EHLO mga06.intel.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S1726197AbfFSSMr (ORCPT ); Wed, 19 Jun 2019 14:12:47 -0400 X-Amp-Result: SKIPPED(no attachment in message) X-Amp-File-Uploaded: False Received: from fmsmga006.fm.intel.com ([10.253.24.20]) by orsmga104.jf.intel.com with ESMTP/TLS/DHE-RSA-AES256-GCM-SHA384; 19 Jun 2019 11:12:42 -0700 X-ExtLoop1: 1 X-IronPort-AV: E=Sophos;i="5.63,393,1557212400"; d="scan'208";a="358690762" Received: from ellie.jf.intel.com (HELO ellie) ([10.54.70.22]) by fmsmga006.fm.intel.com with ESMTP; 19 Jun 2019 11:12:41 -0700 From: Vinicius Costa Gomes To: Voon Weifeng , "David S. Miller" , Maxime Coquelin Cc: netdev@vger.kernel.org, linux-kernel@vger.kernel.org, Jose Abreu , Giuseppe Cavallaro , Andrew Lunn , Florian Fainelli , Alexandre Torgue , Ong Boon Leong , Voon Weifeng 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: netdev-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: netdev@vger.kernel.org Hi, Voon Weifeng writes: > From: Ong Boon Leong > > 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 > Reviewed-by: Voon Weifeng > Reviewed-by: Kweh Hock Leong > Signed-off-by: Ong Boon Leong > Signed-off-by: Voon Weifeng > --- > 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 > #include > +#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 > #include > +#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