[PATCH v6 net-next 09/14] net: phy: marvell-88q2xxx: add cable test support

[Dimitri Fedrau <dima.fedrau@gmail.com>]

Add cable test support for Marvell 88Q222x devices. Reported distance
granularity is 1m.

1m cable, open:
  Cable test started for device eth0.
  Cable test completed for device eth0.
  Pair A code Open Circuit
  Pair A, fault length: 1.00m

1m cable, shorted:
  Cable test started for device eth0.
  Cable test completed for device eth0.
  Pair A code Short within Pair
  Pair A, fault length: 1.00m

6m cable, open:
  Cable test started for device eth0.
  Cable test completed for device eth0.
  Pair A code Open Circuit
  Pair A, fault length: 6.00m

6m cable, shorted:
  Cable test started for device eth0.
  Cable test completed for device eth0.
  Pair A code Short within Pair
  Pair A, fault length: 6.00m

Signed-off-by: Dimitri Fedrau <dima.fedrau@gmail.com>
---
 drivers/net/phy/marvell-88q2xxx.c | 97 +++++++++++++++++++++++++++++++
 1 file changed, 97 insertions(+)

diff --git a/drivers/net/phy/marvell-88q2xxx.c b/drivers/net/phy/marvell-88q2xxx.c
index 2ca1b47e8f8f..93a6a9dff722 100644
--- a/drivers/net/phy/marvell-88q2xxx.c
+++ b/drivers/net/phy/marvell-88q2xxx.c
@@ -72,6 +72,26 @@
 
 #define MDIO_MMD_PCS_MV_RX_STAT			33328
 
+#define MDIO_MMD_PCS_MV_TDR_RESET			65226
+#define MDIO_MMD_PCS_MV_TDR_RESET_TDR_RST		0x1000
+
+#define MDIO_MMD_PCS_MV_TDR_OFF_SHORT_CABLE		65241
+
+#define MDIO_MMD_PCS_MV_TDR_OFF_LONG_CABLE		65242
+
+#define MDIO_MMD_PCS_MV_TDR_STATUS			65245
+#define MDIO_MMD_PCS_MV_TDR_STATUS_OFF			0x0001
+#define MDIO_MMD_PCS_MV_TDR_STATUS_ON			0x0002
+#define MDIO_MMD_PCS_MV_TDR_STATUS_DIST_MASK		0xff00
+#define MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_MASK	0x00f0
+#define MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_SHORT	0x0030
+#define MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_OPEN	0x00e0
+#define MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_OK		0x0070
+#define MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_IN_PROGR	0x0080
+#define MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_NOISE	0x0050
+
+#define MDIO_MMD_PCS_MV_TDR_OFF_CUTOFF			65246
+
 struct mmd_val {
 	int devad;
 	u32 regnum;
@@ -715,6 +735,80 @@ static int mv88q222x_revb0_config_init(struct phy_device *phydev)
 	return 0;
 }
 
+static int mv88q222x_cable_test_start(struct phy_device *phydev)
+{
+	int ret;
+
+	ret = phy_write_mmd(phydev, MDIO_MMD_PCS,
+			    MDIO_MMD_PCS_MV_TDR_OFF_CUTOFF, 0x0058);
+	if (ret < 0)
+		return ret;
+
+	ret = phy_write_mmd(phydev, MDIO_MMD_PCS,
+			    MDIO_MMD_PCS_MV_TDR_OFF_LONG_CABLE, 0x00eb);
+	if (ret < 0)
+		return ret;
+
+	ret = phy_write_mmd(phydev, MDIO_MMD_PCS,
+			    MDIO_MMD_PCS_MV_TDR_OFF_SHORT_CABLE, 0x010e);
+	if (ret < 0)
+		return ret;
+
+	ret = phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_MMD_PCS_MV_TDR_RESET,
+			    0x0d90);
+	if (ret < 0)
+		return ret;
+
+	return phy_write_mmd(phydev, MDIO_MMD_PCS, MDIO_MMD_PCS_MV_TDR_STATUS,
+			     MDIO_MMD_PCS_MV_TDR_STATUS_ON);
+}
+
+static int mv88q222x_cable_test_get_status(struct phy_device *phydev,
+					   bool *finished)
+{
+	int ret;
+	u32 dist;
+
+	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_MMD_PCS_MV_TDR_STATUS);
+	if (ret < 0)
+		return ret;
+
+	*finished = true;
+	/* fault length in meters */
+	dist = FIELD_GET(MDIO_MMD_PCS_MV_TDR_STATUS_DIST_MASK, ret) * 100;
+	switch (ret & MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_MASK) {
+	case MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_OPEN:
+		ethnl_cable_test_result(phydev, ETHTOOL_A_CABLE_PAIR_A,
+					ETHTOOL_A_CABLE_RESULT_CODE_OPEN);
+		ethnl_cable_test_fault_length(phydev, ETHTOOL_A_CABLE_PAIR_A,
+					      dist);
+		break;
+	case MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_SHORT:
+		ethnl_cable_test_result(phydev, ETHTOOL_A_CABLE_PAIR_A,
+					ETHTOOL_A_CABLE_RESULT_CODE_SAME_SHORT);
+		ethnl_cable_test_fault_length(phydev, ETHTOOL_A_CABLE_PAIR_A,
+					      dist);
+		break;
+	case MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_OK:
+		ethnl_cable_test_result(phydev, ETHTOOL_A_CABLE_PAIR_A,
+					ETHTOOL_A_CABLE_RESULT_CODE_OK);
+		break;
+	case MDIO_MMD_PCS_MV_TDR_STATUS_VCT_STAT_IN_PROGR:
+		*finished = false;
+		break;
+	default:
+		ethnl_cable_test_result(phydev, ETHTOOL_A_CABLE_PAIR_A,
+					ETHTOOL_A_CABLE_RESULT_CODE_UNSPEC);
+	}
+
+	if (*finished)
+		return phy_write_mmd(phydev, MDIO_MMD_PCS,
+				     MDIO_MMD_PCS_MV_TDR_RESET,
+				     MDIO_MMD_PCS_MV_TDR_RESET_TDR_RST | 0xd90);
+
+	return 0;
+}
+
 static struct phy_driver mv88q2xxx_driver[] = {
 	{
 		.phy_id			= MARVELL_PHY_ID_88Q2110,
@@ -732,6 +826,7 @@ static struct phy_driver mv88q2xxx_driver[] = {
 	{
 		PHY_ID_MATCH_EXACT(PHY_ID_88Q2220_REVB0),
 		.name			= "mv88q2220",
+		.flags			= PHY_POLL_CABLE_TEST,
 		.probe			= mv88q2xxx_probe,
 		.get_features		= mv88q2xxx_get_features,
 		.config_aneg		= mv88q222x_config_aneg,
@@ -742,6 +837,8 @@ static struct phy_driver mv88q2xxx_driver[] = {
 		.config_intr		= mv88q2xxx_config_intr,
 		.handle_interrupt	= mv88q2xxx_handle_interrupt,
 		.set_loopback		= genphy_c45_loopback,
+		.cable_test_start	= mv88q222x_cable_test_start,
+		.cable_test_get_status	= mv88q222x_cable_test_get_status,
 		.get_sqi		= mv88q2xxx_get_sqi,
 		.get_sqi_max		= mv88q2xxx_get_sqi_max,
 		.suspend		= mv88q2xxx_suspend,
-- 
2.39.2