net: phy: microchip_t1: SQI support for LAN887x

Add support for measuring Signal Quality Index for LAN887x T1 PHY.
Signal Quality Index (SQI) is measure of Link Channel Quality from
0 to 7, with 7 as the best. By default, a link loss event shall
indicate an SQI of 0.

Signed-off-by: Tarun Alle <Tarun.Alle@microchip.com>
Reviewed-by: Andrew Lunn <andrew@lunn.ch>
Link: https://patch.msgid.link/20241007063943.3233-1-tarun.alle@microchip.com
Signed-off-by: Jakub Kicinski <kuba@kernel.org>

diff --git a/drivers/net/phy/microchip_t1.c b/drivers/net/phy/microchip_t1.c
index f99f37634d5ece3b150be652f4a0ac8c26d251d0..71d6050b2833f7fc67dad35b430702595500c45b 100644 (file)
--- a/drivers/net/phy/microchip_t1.c
+++ b/drivers/net/phy/microchip_t1.c
@@ -6,6 +6,7 @@
 #include <linux/delay.h>
 #include <linux/mii.h>
 #include <linux/phy.h>
+#include <linux/sort.h>
 #include <linux/ethtool.h>
 #include <linux/ethtool_netlink.h>
 #include <linux/bitfield.h>
@@ -238,6 +239,35 @@
 #define LAN887X_MX_CHIP_TOP_ALL_MSK    (LAN887X_INT_MSK_T1_PHY_INT_MSK |\
                                         LAN887X_MX_CHIP_TOP_LINK_MSK)
 
+#define LAN887X_COEFF_PWR_DN_CONFIG_100                0x0404
+#define LAN887X_COEFF_PWR_DN_CONFIG_100_V      0x16d6
+#define LAN887X_SQI_CONFIG_100                 0x042e
+#define LAN887X_SQI_CONFIG_100_V               0x9572
+#define LAN887X_SQI_MSE_100                    0x483
+
+#define LAN887X_POKE_PEEK_100                  0x040d
+#define LAN887X_POKE_PEEK_100_EN               BIT(0)
+
+#define LAN887X_COEFF_MOD_CONFIG               0x080d
+#define LAN887X_COEFF_MOD_CONFIG_DCQ_COEFF_EN  BIT(8)
+
+#define LAN887X_DCQ_SQI_STATUS                 0x08b2
+
+/* SQI raw samples count */
+#define SQI_SAMPLES 200
+
+/* Samples percentage considered for SQI calculation */
+#define SQI_INLINERS_PERCENT 60
+
+/* Samples count considered for SQI calculation */
+#define SQI_INLIERS_NUM (SQI_SAMPLES * SQI_INLINERS_PERCENT / 100)
+
+/* Start offset of samples */
+#define SQI_INLIERS_START ((SQI_SAMPLES - SQI_INLIERS_NUM) / 2)
+
+/* End offset of samples */
+#define SQI_INLIERS_END (SQI_INLIERS_START + SQI_INLIERS_NUM)
+
 #define DRIVER_AUTHOR  "Nisar Sayed <nisar.sayed@microchip.com>"
 #define DRIVER_DESC    "Microchip LAN87XX/LAN937x/LAN887x T1 PHY driver"
 
@@ -1889,6 +1919,145 @@ static int lan887x_cable_test_get_status(struct phy_device *phydev,
        return lan887x_cable_test_report(phydev);
 }
 
+/* Compare block to sort in ascending order */
+static int sqi_compare(const void *a, const void *b)
+{
+       return  *(u16 *)a - *(u16 *)b;
+}
+
+static int lan887x_get_sqi_100M(struct phy_device *phydev)
+{
+       u16 rawtable[SQI_SAMPLES];
+       u32 sqiavg = 0;
+       u8 sqinum = 0;
+       int rc, i;
+
+       /* Configuration of SQI 100M */
+       rc = phy_write_mmd(phydev, MDIO_MMD_VEND1,
+                          LAN887X_COEFF_PWR_DN_CONFIG_100,
+                          LAN887X_COEFF_PWR_DN_CONFIG_100_V);
+       if (rc < 0)
+               return rc;
+
+       rc = phy_write_mmd(phydev, MDIO_MMD_VEND1, LAN887X_SQI_CONFIG_100,
+                          LAN887X_SQI_CONFIG_100_V);
+       if (rc < 0)
+               return rc;
+
+       rc = phy_read_mmd(phydev, MDIO_MMD_VEND1, LAN887X_SQI_CONFIG_100);
+       if (rc != LAN887X_SQI_CONFIG_100_V)
+               return -EINVAL;
+
+       rc = phy_modify_mmd(phydev, MDIO_MMD_VEND1, LAN887X_POKE_PEEK_100,
+                           LAN887X_POKE_PEEK_100_EN,
+                           LAN887X_POKE_PEEK_100_EN);
+       if (rc < 0)
+               return rc;
+
+       /* Required before reading register
+        * otherwise it will return high value
+        */
+       msleep(50);
+
+       /* Link check before raw readings */
+       rc = genphy_c45_read_link(phydev);
+       if (rc < 0)
+               return rc;
+
+       if (!phydev->link)
+               return -ENETDOWN;
+
+       /* Get 200 SQI raw readings */
+       for (i = 0; i < SQI_SAMPLES; i++) {
+               rc = phy_write_mmd(phydev, MDIO_MMD_VEND1,
+                                  LAN887X_POKE_PEEK_100,
+                                  LAN887X_POKE_PEEK_100_EN);
+               if (rc < 0)
+                       return rc;
+
+               rc = phy_read_mmd(phydev, MDIO_MMD_VEND1,
+                                 LAN887X_SQI_MSE_100);
+               if (rc < 0)
+                       return rc;
+
+               rawtable[i] = (u16)rc;
+       }
+
+       /* Link check after raw readings */
+       rc = genphy_c45_read_link(phydev);
+       if (rc < 0)
+               return rc;
+
+       if (!phydev->link)
+               return -ENETDOWN;
+
+       /* Sort SQI raw readings in ascending order */
+       sort(rawtable, SQI_SAMPLES, sizeof(u16), sqi_compare, NULL);
+
+       /* Keep inliers and discard outliers */
+       for (i = SQI_INLIERS_START; i < SQI_INLIERS_END; i++)
+               sqiavg += rawtable[i];
+
+       /* Handle invalid samples */
+       if (sqiavg != 0) {
+               /* Get SQI average */
+               sqiavg /= SQI_INLIERS_NUM;
+
+               if (sqiavg < 75)
+                       sqinum = 7;
+               else if (sqiavg < 94)
+                       sqinum = 6;
+               else if (sqiavg < 119)
+                       sqinum = 5;
+               else if (sqiavg < 150)
+                       sqinum = 4;
+               else if (sqiavg < 189)
+                       sqinum = 3;
+               else if (sqiavg < 237)
+                       sqinum = 2;
+               else if (sqiavg < 299)
+                       sqinum = 1;
+               else
+                       sqinum = 0;
+       }
+
+       return sqinum;
+}
+
+static int lan887x_get_sqi(struct phy_device *phydev)
+{
+       int rc, val;
+
+       if (phydev->speed != SPEED_1000 &&
+           phydev->speed != SPEED_100)
+               return -ENETDOWN;
+
+       if (phydev->speed == SPEED_100)
+               return lan887x_get_sqi_100M(phydev);
+
+       /* Writing DCQ_COEFF_EN to trigger a SQI read */
+       rc = phy_set_bits_mmd(phydev, MDIO_MMD_VEND1,
+                             LAN887X_COEFF_MOD_CONFIG,
+                             LAN887X_COEFF_MOD_CONFIG_DCQ_COEFF_EN);
+       if (rc < 0)
+               return rc;
+
+       /* Wait for DCQ done */
+       rc = phy_read_mmd_poll_timeout(phydev, MDIO_MMD_VEND1,
+                                      LAN887X_COEFF_MOD_CONFIG, val, ((val &
+                                      LAN887X_COEFF_MOD_CONFIG_DCQ_COEFF_EN) !=
+                                      LAN887X_COEFF_MOD_CONFIG_DCQ_COEFF_EN),
+                                      10, 200, true);
+       if (rc < 0)
+               return rc;
+
+       rc = phy_read_mmd(phydev, MDIO_MMD_VEND1, LAN887X_DCQ_SQI_STATUS);
+       if (rc < 0)
+               return rc;
+
+       return FIELD_GET(T1_DCQ_SQI_MSK, rc);
+}
+
 static struct phy_driver microchip_t1_phy_driver[] = {
        {
                PHY_ID_MATCH_MODEL(PHY_ID_LAN87XX),
@@ -1942,6 +2111,8 @@ static struct phy_driver microchip_t1_phy_driver[] = {
                .cable_test_get_status = lan887x_cable_test_get_status,
                .config_intr    = lan887x_config_intr,
                .handle_interrupt = lan887x_handle_interrupt,
+               .get_sqi        = lan887x_get_sqi,
+               .get_sqi_max    = lan87xx_get_sqi_max,
        }
 };