From: Tarun Alle <Tarun.Alle@microchip.com>

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>
---
v3 -> v4
- Added check to handle invalid samples.
- Added macro for ARRAY_SIZE(rawtable).

v2 -> v3
- Replaced hard-coded values with ARRAY_SIZE(rawtable).

v1 -> v2
- Replaced hard-coded 200 with ARRAY_SIZE(rawtable).
- Replaced return value -EINVAL with -ENETDOWN.
- Changed link checks.
---
 drivers/net/phy/microchip_t1.c | 161 +++++++++++++++++++++++++++++++++
 1 file changed, 161 insertions(+)

diff --git a/drivers/net/phy/microchip_t1.c b/drivers/net/phy/microchip_t1.c
index a5ef8fe50704..e5cc4c8b2265 100644
--- 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>
@@ -226,6 +227,22 @@
 #define MICROCHIP_CABLE_MAX_TIME_DIFF	\
 	(MICROCHIP_CABLE_MIN_TIME_DIFF + MICROCHIP_CABLE_TIME_MARGIN)
 
+#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
+
+#define SQI100M_SAMPLE_INIT(x, t)	(ARRAY_SIZE(t) / (x))
+
 #define DRIVER_AUTHOR	"Nisar Sayed <nisar.sayed@microchip.com>"
 #define DRIVER_DESC	"Microchip LAN87XX/LAN937x/LAN887x T1 PHY driver"
 
@@ -1830,6 +1847,148 @@ 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 data_compare(const void *a, const void *b)
+{
+	return  *(u16 *)a - *(u16 *)b;
+}
+
+static int lan887x_get_sqi_100M(struct phy_device *phydev)
+{
+	u16 rawtable[200];
+	u32 sqiavg = 0;
+	u8 sqinum = 0;
+	int rc;
+
+	/* 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 (int i = 0; i < ARRAY_SIZE(rawtable); 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, ARRAY_SIZE(rawtable), sizeof(u16), data_compare, NULL);
+
+	/* Keep inliers and discard outliers */
+	for (int i = SQI100M_SAMPLE_INIT(5, rawtable);
+	     i < SQI100M_SAMPLE_INIT(5, rawtable) * 4; i++)
+		sqiavg += rawtable[i];
+
+	/* Handle invalid samples */
+	if (sqiavg != 0) {
+		/* Get SQI average */
+		sqiavg /= SQI100M_SAMPLE_INIT(5, rawtable) * 4 -
+				SQI100M_SAMPLE_INIT(5, rawtable);
+
+		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),
@@ -1881,6 +2040,8 @@ static struct phy_driver microchip_t1_phy_driver[] = {
 		.read_status	= genphy_c45_read_status,
 		.cable_test_start = lan887x_cable_test_start,
 		.cable_test_get_status = lan887x_cable_test_get_status,
+		.get_sqi	= lan887x_get_sqi,
+		.get_sqi_max	= lan87xx_get_sqi_max,
 	}
 };
 
-- 
2.34.1

On Tue, Sep 17, 2024 at 05:26:57PM +0530, Tarun Alle wrote:
> From: Tarun Alle <Tarun.Alle@microchip.com>
> 
> 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>

Please note that the merge window is open, which means that net-next is
currently closed. Thus, patches should be submitted as RFC.

> ---
> v3 -> v4
> - Added check to handle invalid samples.
> - Added macro for ARRAY_SIZE(rawtable).
> 
> v2 -> v3
> - Replaced hard-coded values with ARRAY_SIZE(rawtable).
> 
> v1 -> v2
> - Replaced hard-coded 200 with ARRAY_SIZE(rawtable).

Hmm. We've been through several iterations trying to clean this up
into something more easily readable, but I fear there'll be another
iteration.

Maybe the following would be nicer:

enum {
	SQI_SAMPLES = 200,
	/* Look at samples of the middle 60% */
	SQI_INLIERS_NUM = SQI_SAMPLES * 60 / 100,
	SQI_INLIERS_START = (SQI_SAMPLES - SQI_INLIERS_NUM) / 2,
	SQI_INLIERS_END = SQI_INLIERS_START + SQI_INLIERS_NUM,
};

> +static int lan887x_get_sqi_100M(struct phy_device *phydev)
> +{
> +	u16 rawtable[200];

	u16 rawtable[SQI_SAMPLES];

> +	u32 sqiavg = 0;
> +	u8 sqinum = 0;
> +	int rc;

Since you use "i" multiple times, declare it at the beginning of the
function rather than in each for loop.

	int 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 (int i = 0; i < ARRAY_SIZE(rawtable); i++) {

	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, ARRAY_SIZE(rawtable), sizeof(u16), data_compare, NULL);

	sort(rawtable, SQI_SAMPLES, sizeof(u16), data_compare, NULL);

Although renaming data_compare to sqi_compare would be even more
descriptive of what it's doing.

> +
> +	/* Keep inliers and discard outliers */
> +	for (int i = SQI100M_SAMPLE_INIT(5, rawtable);
> +	     i < SQI100M_SAMPLE_INIT(5, rawtable) * 4; i++)

	for (i = SQI_INLIERS_START; i < SQI_INLIERS_END; i++)

> +		sqiavg += rawtable[i];
> +
> +	/* Handle invalid samples */
> +	if (sqiavg != 0) {
> +		/* Get SQI average */
> +		sqiavg /= SQI100M_SAMPLE_INIT(5, rawtable) * 4 -
> +				SQI100M_SAMPLE_INIT(5, rawtable);

		sqiavg /= SQI_INLIERS_NUM;

Overall, I think this is better rather than the SQI100M_SAMPLE_INIT()
macro... for which I'm not sure what the _INIT() bit actually means.

I think my suggestion has the advantage that it makes it clear what
these various calculations are doing, because the result of the
calculations is described in the enum name.

Thanks.

-- 
RMK's Patch system: https://www.armlinux.org.uk/developer/patches/
FTTP is here! 80Mbps down 10Mbps up. Decent connectivity at last!