[RFC Patch net-next 3/3] net: phy: lan87xx: added ethtool SQI support

[Arun Ramadoss <arun.ramadoss@microchip.com>]

This patch add the support for measuring Signal Quality Index for
LAN87xx and LAN937x T1 Phy. To get better accuracy of the SQI,
readings are measured and its average is calculated. If the link is
down, then routine return immediately.

Signed-off-by: Arun Ramadoss <arun.ramadoss@microchip.com>
---
 drivers/net/phy/microchip_t1.c | 132 +++++++++++++++++++++++++++++++++
 1 file changed, 132 insertions(+)

diff --git a/drivers/net/phy/microchip_t1.c b/drivers/net/phy/microchip_t1.c
index 2742d71a469f..ebbf1e8067f6 100644
--- a/drivers/net/phy/microchip_t1.c
+++ b/drivers/net/phy/microchip_t1.c
@@ -9,6 +9,7 @@
 #include <linux/ethtool.h>
 #include <linux/ethtool_netlink.h>
 #include <linux/bitfield.h>
+#include <linux/sort.h>
 
 #define PHY_ID_LAN87XX				0x0007c150
 #define PHY_ID_LAN937X				0x0007c180
@@ -68,7 +69,14 @@
 #define T1_POST_LCK_MUFACT_CFG_REG	0x1C
 #define T1_TX_RX_FIFO_CFG_REG		0x02
 #define T1_TX_LPF_FIR_CFG_REG		0x55
+#define T1_COEF_CLK_PWR_DN_CFG		0x04
+#define T1_COEF_RW_CTL_CFG		0x0D
 #define T1_SQI_CONFIG_REG		0x2E
+#define T1_SQI_CONFIG2_REG		0x4A
+#define T1_DCQ_MSE_REG			0xC1
+#define T1_MSE_VLD_MSK			BIT(9)
+#define T1_DCQ_SQI_REG			0xC3
+#define T1_DCQ_SQI_MSK			GENMASK(3, 1)
 #define T1_MDIO_CONTROL2_REG		0x10
 #define T1_INTERRUPT_SOURCE_REG		0x18
 #define T1_INTERRUPT2_SOURCE_REG	0x08
@@ -82,6 +90,12 @@
 #define T1_MODE_STAT_REG		0x11
 #define T1_LINK_UP_MSK			BIT(0)
 
+/* SQI defines */
+#define LAN87XX_MAX_SQI			0x07
+#define LAN87XX_SQI_ENTRY		200
+#define SQI_AVG_MIN			40
+#define SQI_AVG_MAX			160
+
 #define DRIVER_AUTHOR	"Nisar Sayed <nisar.sayed@microchip.com>"
 #define DRIVER_DESC	"Microchip LAN87XX/LAN937x T1 PHY driver"
 
@@ -740,6 +754,120 @@ static int lan87xx_config_aneg(struct phy_device *phydev)
 	return rc;
 }
 
+static int lan87xx_sqi_cmp(const void *a, const void *b)
+{
+	return *(u16 *)a - *(u16 *)b;
+}
+
+static int lan87xx_get_sqi(struct phy_device *phydev)
+{
+	u16 sqi_value[LAN87XX_SQI_ENTRY];
+	u32 sqi_avg = 0;
+	int rc;
+	u8 i;
+
+	rc = lan87xx_update_link(phydev);
+	if (rc < 0)
+		return rc;
+
+	if (phydev->link == 0)
+		return sqi_avg;
+
+	rc = access_ereg(phydev, PHYACC_ATTR_MODE_WRITE,
+			 PHYACC_ATTR_BANK_DSP, T1_COEF_CLK_PWR_DN_CFG, 0x16d6);
+	if (rc < 0)
+		return rc;
+
+	/* Enable SQI measurement */
+	rc = access_ereg(phydev, PHYACC_ATTR_MODE_WRITE,
+			 PHYACC_ATTR_BANK_DSP, T1_SQI_CONFIG_REG, 0x9572);
+	if (rc < 0)
+		return rc;
+
+	/* Enable SQI Method 5 */
+	rc = access_ereg(phydev, PHYACC_ATTR_MODE_WRITE,
+			 PHYACC_ATTR_BANK_DSP, T1_SQI_CONFIG2_REG, 0x0001);
+	if (rc < 0)
+		return rc;
+
+	/* Below effectively throws away first reading
+	 * required delay before reading DSP.
+	 */
+	rc = access_ereg(phydev, PHYACC_ATTR_MODE_WRITE,
+			 PHYACC_ATTR_BANK_DSP, T1_COEF_RW_CTL_CFG, 0x0301);
+	if (rc < 0)
+		return rc;
+
+	usleep_range(40, 50);
+
+	for (i = 0; i < LAN87XX_SQI_ENTRY; i++) {
+		rc = lan87xx_update_link(phydev);
+		if (rc < 0)
+			return rc;
+
+		if (phydev->link == 0)
+			return sqi_avg;
+
+		rc = access_ereg(phydev, PHYACC_ATTR_MODE_WRITE,
+				 PHYACC_ATTR_BANK_DSP,
+				 T1_COEF_RW_CTL_CFG, 0x0301);
+		if (rc < 0)
+			return rc;
+
+		rc = access_ereg(phydev, PHYACC_ATTR_MODE_READ,
+				 PHYACC_ATTR_BANK_DSP, T1_DCQ_SQI_REG, 0x0);
+		if (rc < 0)
+			return rc;
+
+		sqi_value[i] = FIELD_GET(T1_DCQ_SQI_MSK, rc);
+
+		rc = access_ereg(phydev, PHYACC_ATTR_MODE_READ,
+				 PHYACC_ATTR_BANK_DSP, T1_DCQ_MSE_REG, 0x0);
+		if (rc < 0)
+			return rc;
+
+		/* Check valid value. 0 - valid, 1 - Invalid
+		 * if invalid, re-read the value after 250ms
+		 */
+		if (FIELD_GET(T1_MSE_VLD_MSK, rc) == 1) {
+			rc = access_ereg(phydev, PHYACC_ATTR_MODE_WRITE,
+					 PHYACC_ATTR_BANK_DSP,
+					 T1_COEF_RW_CTL_CFG, 0x0301);
+			if (rc < 0)
+				return rc;
+
+			msleep(250);
+
+			rc = access_ereg(phydev, PHYACC_ATTR_MODE_READ,
+					 PHYACC_ATTR_BANK_DSP,
+					 T1_DCQ_SQI_REG, 0x0);
+			if (rc < 0)
+				return rc;
+
+			sqi_value[i] = FIELD_GET(T1_DCQ_SQI_MSK, rc);
+		}
+	}
+
+	/* Sorting SQI values */
+	sort(sqi_value, LAN87XX_SQI_ENTRY, sizeof(u16), lan87xx_sqi_cmp, NULL);
+
+	/* Discarding outliers */
+	for (i = 0; i < LAN87XX_SQI_ENTRY; i++) {
+		if (i >= SQI_AVG_MIN && i <= SQI_AVG_MAX)
+			sqi_avg += sqi_value[i];
+	}
+
+	/* Calculating SQI number */
+	sqi_avg = DIV_ROUND_UP(sqi_avg, (SQI_AVG_MAX - SQI_AVG_MIN + 1));
+
+	return sqi_avg;
+}
+
+static int lan87xx_get_sqi_max(struct phy_device *phydev)
+{
+	return LAN87XX_MAX_SQI;
+}
+
 static struct phy_driver microchip_t1_phy_driver[] = {
 	{
 		PHY_ID_MATCH_MODEL(PHY_ID_LAN87XX),
@@ -753,6 +881,8 @@ static struct phy_driver microchip_t1_phy_driver[] = {
 		.resume         = genphy_resume,
 		.config_aneg    = lan87xx_config_aneg,
 		.read_status	= lan87xx_read_status,
+		.get_sqi	= lan87xx_get_sqi,
+		.get_sqi_max	= lan87xx_get_sqi_max,
 		.cable_test_start = lan87xx_cable_test_start,
 		.cable_test_get_status = lan87xx_cable_test_get_status,
 	},
@@ -766,6 +896,8 @@ static struct phy_driver microchip_t1_phy_driver[] = {
 		.resume		= genphy_resume,
 		.config_aneg    = lan87xx_config_aneg,
 		.read_status	= lan87xx_read_status,
+		.get_sqi	= lan87xx_get_sqi,
+		.get_sqi_max	= lan87xx_get_sqi_max,
 		.cable_test_start = lan87xx_cable_test_start,
 		.cable_test_get_status = lan87xx_cable_test_get_status,
 	}
-- 
2.33.0