enetc: update TSN Qbv PSPEED set according to adjust link speed

ENETC has a register PSPEED to indicate the link speed of hardware.
It is need to update accordingly. PSPEED field needs to be updated
with the port speed for QBV scheduling purposes. Or else there is
chance for gate slot not free by frame taking the MAC if PSPEED and
phy speed not match. So update PSPEED when link adjust. This is
implement by the adjust_link.

Signed-off-by: Po Liu <Po.Liu@nxp.com>
Signed-off-by: Claudiu Manoil <claudiu.manoil@nxp.com>
Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/freescale/enetc/enetc.c b/drivers/net/ethernet/freescale/enetc/enetc.c
index d58dbc2c4270eada46dde11c8ced539fef3bae76..f6b00c68451bc227a167905a9549a76414dde913 100644 (file)
--- a/drivers/net/ethernet/freescale/enetc/enetc.c
+++ b/drivers/net/ethernet/freescale/enetc/enetc.c
@@ -742,9 +742,14 @@ void enetc_get_si_caps(struct enetc_si *si)
        si->num_rss = 0;
        val = enetc_rd(hw, ENETC_SIPCAPR0);
        if (val & ENETC_SIPCAPR0_RSS) {
-               val = enetc_rd(hw, ENETC_SIRSSCAPR);
-               si->num_rss = ENETC_SIRSSCAPR_GET_NUM_RSS(val);
+               u32 rss;
+
+               rss = enetc_rd(hw, ENETC_SIRSSCAPR);
+               si->num_rss = ENETC_SIRSSCAPR_GET_NUM_RSS(rss);
        }
+
+       if (val & ENETC_SIPCAPR0_QBV)
+               si->hw_features |= ENETC_SI_F_QBV;
 }
 
 static int enetc_dma_alloc_bdr(struct enetc_bdr *r, size_t bd_size)
@@ -1314,8 +1319,12 @@ static void enetc_disable_interrupts(struct enetc_ndev_priv *priv)
 
 static void adjust_link(struct net_device *ndev)
 {
+       struct enetc_ndev_priv *priv = netdev_priv(ndev);
        struct phy_device *phydev = ndev->phydev;
 
+       if (priv->active_offloads & ENETC_F_QBV)
+               enetc_sched_speed_set(ndev);
+
        phy_print_status(phydev);
 }
 

diff --git a/drivers/net/ethernet/freescale/enetc/enetc.h b/drivers/net/ethernet/freescale/enetc/enetc.h
index 8ca2f97050c822a1d2a614c5c64413782302b7bc..89f23156f330c52e85b127c9b6df02978961551b 100644 (file)
--- a/drivers/net/ethernet/freescale/enetc/enetc.h
+++ b/drivers/net/ethernet/freescale/enetc/enetc.h
@@ -118,6 +118,8 @@ enum enetc_errata {
        ENETC_ERR_UCMCSWP       = BIT(2),
 };
 
+#define ENETC_SI_F_QBV BIT(0)
+
 /* PCI IEP device data */
 struct enetc_si {
        struct pci_dev *pdev;
@@ -133,6 +135,7 @@ struct enetc_si {
        int num_fs_entries;
        int num_rss; /* number of RSS buckets */
        unsigned short pad;
+       int hw_features;
 };
 
 #define ENETC_SI_ALIGN 32
@@ -173,6 +176,7 @@ struct enetc_cls_rule {
 enum enetc_active_offloads {
        ENETC_F_RX_TSTAMP       = BIT(0),
        ENETC_F_TX_TSTAMP       = BIT(1),
+       ENETC_F_QBV             = BIT(2),
 };
 
 struct enetc_ndev_priv {
@@ -188,6 +192,8 @@ struct enetc_ndev_priv {
        u16 msg_enable;
        int active_offloads;
 
+       u32 speed; /* store speed for compare update pspeed */
+
        struct enetc_bdr *tx_ring[16];
        struct enetc_bdr *rx_ring[16];
 
@@ -248,6 +254,8 @@ int enetc_send_cmd(struct enetc_si *si, struct enetc_cbd *cbd);
 
 #ifdef CONFIG_FSL_ENETC_QOS
 int enetc_setup_tc_taprio(struct net_device *ndev, void *type_data);
+void enetc_sched_speed_set(struct net_device *ndev);
 #else
 #define enetc_setup_tc_taprio(ndev, type_data) -EOPNOTSUPP
+#define enetc_sched_speed_set(ndev) (void)0
 #endif

diff --git a/drivers/net/ethernet/freescale/enetc/enetc_hw.h b/drivers/net/ethernet/freescale/enetc/enetc_hw.h
index df6b35dc3534b9ea7ae6f9b7782d59e6ffccbab3..924ddb6d358af90b4ed56df023fa3cb222fb20ee 100644 (file)
--- a/drivers/net/ethernet/freescale/enetc/enetc_hw.h
+++ b/drivers/net/ethernet/freescale/enetc/enetc_hw.h
@@ -149,6 +149,11 @@ enum enetc_bdr_type {TX, RX};
 #define ENETC_PORT_BASE                0x10000
 #define ENETC_PMR              0x0000
 #define ENETC_PMR_EN   GENMASK(18, 16)
+#define ENETC_PMR_PSPEED_MASK GENMASK(11, 8)
+#define ENETC_PMR_PSPEED_10M   0
+#define ENETC_PMR_PSPEED_100M  BIT(8)
+#define ENETC_PMR_PSPEED_1000M BIT(9)
+#define ENETC_PMR_PSPEED_2500M BIT(10)
 #define ENETC_PSR              0x0004 /* RO */
 #define ENETC_PSIPMR           0x0018
 #define ENETC_PSIPMR_SET_UP(n) BIT(n) /* n = SI index */

diff --git a/drivers/net/ethernet/freescale/enetc/enetc_pf.c b/drivers/net/ethernet/freescale/enetc/enetc_pf.c
index 7da79b816416106a439184dc98ef5239a4a6d844..e7482d483b288f55b34f069f01d2810f22b4b7b3 100644 (file)
--- a/drivers/net/ethernet/freescale/enetc/enetc_pf.c
+++ b/drivers/net/ethernet/freescale/enetc/enetc_pf.c
@@ -742,6 +742,9 @@ static void enetc_pf_netdev_setup(struct enetc_si *si, struct net_device *ndev,
 
        ndev->priv_flags |= IFF_UNICAST_FLT;
 
+       if (si->hw_features & ENETC_SI_F_QBV)
+               priv->active_offloads |= ENETC_F_QBV;
+
        /* pick up primary MAC address from SI */
        enetc_get_primary_mac_addr(&si->hw, ndev->dev_addr);
 }

diff --git a/drivers/net/ethernet/freescale/enetc/enetc_qos.c b/drivers/net/ethernet/freescale/enetc/enetc_qos.c
index 84c2ab98fae9780f1bb46ec45d14ff7068429ce2..66a3da61ca16618dd17d63d3df369a6a4761af7b 100644 (file)
--- a/drivers/net/ethernet/freescale/enetc/enetc_qos.c
+++ b/drivers/net/ethernet/freescale/enetc/enetc_qos.c
@@ -11,6 +11,40 @@ static u16 enetc_get_max_gcl_len(struct enetc_hw *hw)
                & ENETC_QBV_MAX_GCL_LEN_MASK;
 }
 
+void enetc_sched_speed_set(struct net_device *ndev)
+{
+       struct enetc_ndev_priv *priv = netdev_priv(ndev);
+       struct phy_device *phydev = ndev->phydev;
+       u32 old_speed = priv->speed;
+       u32 speed, pspeed;
+
+       if (phydev->speed == old_speed)
+               return;
+
+       speed = phydev->speed;
+       switch (speed) {
+       case SPEED_1000:
+               pspeed = ENETC_PMR_PSPEED_1000M;
+               break;
+       case SPEED_2500:
+               pspeed = ENETC_PMR_PSPEED_2500M;
+               break;
+       case SPEED_100:
+               pspeed = ENETC_PMR_PSPEED_100M;
+               break;
+       case SPEED_10:
+       default:
+               pspeed = ENETC_PMR_PSPEED_10M;
+               netdev_err(ndev, "Qbv PSPEED set speed link down.\n");
+       }
+
+       priv->speed = speed;
+       enetc_port_wr(&priv->si->hw, ENETC_PMR,
+                     (enetc_port_rd(&priv->si->hw, ENETC_PMR)
+                     & (~ENETC_PMR_PSPEED_MASK))
+                     | pspeed);
+}
+
 static int enetc_setup_taprio(struct net_device *ndev,
                              struct tc_taprio_qopt_offload *admin_conf)
 {