@@ -27,6 +27,9 @@ enum rgmii_rx_clock_delay {
/* Microsemi VSC85xx PHY registers */
/* IEEE 802. Std Registers */
+#define MSCC_PHY_BYPASS_CONTROL 18
+#define DISABLE_PAIR_SWAP_CORR_MASK 0x0020
+
#define MSCC_PHY_EXT_PHY_CNTL_1 23
#define MAC_IF_SELECTION_MASK 0x1800
#define MAC_IF_SELECTION_GMII 0
@@ -35,6 +38,9 @@ enum rgmii_rx_clock_delay {
#define MAC_IF_SELECTION_POS 11
#define FAR_END_LOOPBACK_MODE_MASK 0x0008
+#define MSCC_PHY_EXT_PHY_CNTL_2 24
+#define CONNECTOR_LOOPBACK_MASK 0x0001
+
#define MII_VSC85XX_INT_MASK 25
#define MII_VSC85XX_INT_MASK_MASK 0xa000
#define MII_VSC85XX_INT_MASK_WOL 0x0040
@@ -110,6 +116,114 @@ static int vsc85xx_phy_page_set(struct phy_device *phydev, u8 page)
return rc;
}
+static int vsc85xx_loopback_get(struct phy_device *phydev, u8 *type)
+{
+ u16 reg_val;
+
+ reg_val = phy_read(phydev, MII_BMCR);
+ if (BMCR_LOOPBACK & reg_val) {
+ *type = ETHTOOL_PHY_LOOPBACK_NEAR;
+ goto out;
+ }
+
+ reg_val = phy_read(phydev, MSCC_PHY_EXT_PHY_CNTL_1);
+ if (FAR_END_LOOPBACK_MODE_MASK & reg_val) {
+ *type = ETHTOOL_PHY_LOOPBACK_FAR;
+ goto out;
+ }
+
+ reg_val = phy_read(phydev, MSCC_PHY_EXT_PHY_CNTL_2);
+ if (CONNECTOR_LOOPBACK_MASK & reg_val) {
+ *type = ETHTOOL_PHY_LOOPBACK_EXTN;
+ goto out;
+ }
+ *type = ETHTOOL_PHY_LOOPBACK_DISABLE;
+
+out:
+ return 0;
+}
+
+static int vsc85xx_loopback_set(struct phy_device *phydev, u8 type)
+{
+ int rc;
+ u16 reg_val;
+
+ /* Clear/Disable all Loopbacks first */
+ /* Disable Near End Loopback */
+ reg_val = phy_read(phydev, MII_BMCR);
+ if (reg_val & BMCR_LOOPBACK && type != ETHTOOL_PHY_LOOPBACK_NEAR) {
+ reg_val &= ~BMCR_LOOPBACK;
+ rc = phy_write(phydev, MII_BMCR, reg_val);
+ if (rc != 0)
+ goto out;
+ }
+
+ /* Disable Far End Loopback */
+ reg_val = phy_read(phydev, MSCC_PHY_EXT_PHY_CNTL_1);
+ if (reg_val & FAR_END_LOOPBACK_MODE_MASK &&
+ type != ETHTOOL_PHY_LOOPBACK_FAR) {
+ reg_val &= ~FAR_END_LOOPBACK_MODE_MASK;
+ rc = phy_write(phydev, MSCC_PHY_EXT_PHY_CNTL_1, reg_val);
+ if (rc != 0)
+ goto out;
+ }
+
+ /* Disable Connector End Loopback */
+ reg_val = phy_read(phydev, MSCC_PHY_EXT_PHY_CNTL_2);
+ if (reg_val & CONNECTOR_LOOPBACK_MASK &&
+ type != ETHTOOL_PHY_LOOPBACK_EXTN) {
+ reg_val &= ~CONNECTOR_LOOPBACK_MASK;
+ rc = phy_write(phydev, MSCC_PHY_EXT_PHY_CNTL_2, reg_val);
+ if (rc != 0)
+ goto out;
+ reg_val = phy_read(phydev, MSCC_PHY_BYPASS_CONTROL);
+ reg_val &= ~DISABLE_PAIR_SWAP_CORR_MASK;
+ rc = phy_write(phydev, MSCC_PHY_BYPASS_CONTROL, reg_val);
+ if (rc != 0)
+ goto out;
+ }
+
+ switch (type) {
+ case ETHTOOL_PHY_LOOPBACK_NEAR:
+ reg_val = phy_read(phydev, MII_BMCR);
+ reg_val |= BMCR_LOOPBACK;
+ rc = phy_write(phydev, MII_BMCR, reg_val);
+ if (rc != 0)
+ goto out;
+ break;
+ case ETHTOOL_PHY_LOOPBACK_FAR:
+ reg_val = phy_read(phydev, MSCC_PHY_EXT_PHY_CNTL_1);
+ reg_val |= FAR_END_LOOPBACK_MODE_MASK;
+ rc = phy_write(phydev, MSCC_PHY_EXT_PHY_CNTL_1, reg_val);
+ if (rc != 0)
+ goto out;
+ break;
+ case ETHTOOL_PHY_LOOPBACK_EXTN:
+ reg_val = phy_read(phydev, MSCC_PHY_EXT_PHY_CNTL_2);
+ reg_val |= CONNECTOR_LOOPBACK_MASK;
+ rc = phy_write(phydev, MSCC_PHY_EXT_PHY_CNTL_2, reg_val);
+ if (rc != 0)
+ goto out;
+ reg_val = phy_read(phydev, MSCC_PHY_BYPASS_CONTROL);
+ reg_val |= DISABLE_PAIR_SWAP_CORR_MASK;
+ rc = phy_write(phydev, MSCC_PHY_BYPASS_CONTROL, reg_val);
+ if (rc != 0)
+ goto out;
+ break;
+ case ETHTOOL_PHY_LOOPBACK_DISABLE:
+ /* Already disable all Loopbacks */
+ break;
+ default:
+ phydev_err(phydev, "Invalid Loopback Type (valid only off|near|far|extn)\n");
+ rc = -ERANGE;
+ break;
+ }
+
+out:
+
+ return rc;
+}
+
static int vsc85xx_downshift_get(struct phy_device *phydev, u8 *count)
{
int rc;
@@ -398,6 +512,8 @@ static int vsc85xx_get_tunable(struct phy_device *phydev,
switch (tuna->id) {
case ETHTOOL_PHY_DOWNSHIFT:
return vsc85xx_downshift_get(phydev, (u8 *)data);
+ case ETHTOOL_PHY_LOOPBACK:
+ return vsc85xx_loopback_get(phydev, (u8 *)data);
default:
return -EINVAL;
}
@@ -410,6 +526,8 @@ static int vsc85xx_set_tunable(struct phy_device *phydev,
switch (tuna->id) {
case ETHTOOL_PHY_DOWNSHIFT:
return vsc85xx_downshift_set(phydev, *(u8 *)data);
+ case ETHTOOL_PHY_LOOPBACK:
+ return vsc85xx_loopback_set(phydev, *(u8 *)data);
default:
return -EINVAL;
}