+static void mv3110_get_wol(struct phy_device *phydev,
+ struct ethtool_wolinfo *wol)
+{
+ int ret;
+
+ wol->supported = WAKE_MAGIC;
+ wol->wolopts = 0;
+
+ ret = phy_read_mmd(phydev, MDIO_MMD_VEND2, MV_V2_WOL_CTRL);
+ if (ret < 0)
+ return;
+
+ if (ret & MV_V2_WOL_CTRL_MAGIC_PKT_EN)
+ wol->wolopts |= WAKE_MAGIC;
+}
+
+static int mv3110_set_wol(struct phy_device *phydev,
+ struct ethtool_wolinfo *wol)
+{
+ int ret;
+
+ if (wol->wolopts & WAKE_MAGIC) {
+ /* Enable the WOL interrupt */
+ ret = phy_set_bits_mmd(phydev, MDIO_MMD_VEND2,
+ MV_V2_PORT_INTR_MASK,
+ MV_V2_PORT_INTR_STS_WOL_EN);
+ if (ret < 0)
+ return ret;
+
+ /* Store the device address for the magic packet */
+ ret = phy_write_mmd(phydev, MDIO_MMD_VEND2,
+ MV_V2_MAGIC_PKT_WORD2,
+ ((phydev->attached_dev->dev_addr[5] << 8) |
+ phydev->attached_dev->dev_addr[4]));
+ if (ret < 0)
+ return ret;
+
+ ret = phy_write_mmd(phydev, MDIO_MMD_VEND2,
+ MV_V2_MAGIC_PKT_WORD1,
+ ((phydev->attached_dev->dev_addr[3] << 8) |
+ phydev->attached_dev->dev_addr[2]));
+ if (ret < 0)
+ return ret;
+
+ ret = phy_write_mmd(phydev, MDIO_MMD_VEND2,
+ MV_V2_MAGIC_PKT_WORD0,
+ ((phydev->attached_dev->dev_addr[1] << 8) |
+ phydev->attached_dev->dev_addr[0]));
+ if (ret < 0)
+ return ret;
+
+ /* Clear WOL status and enable magic packet matching */
+ ret = phy_set_bits_mmd(phydev, MDIO_MMD_VEND2,
+ MV_V2_WOL_CTRL,
+ MV_V2_WOL_CTRL_MAGIC_PKT_EN |
+ MV_V2_WOL_CTRL_CLEAR_STS);
+ if (ret < 0)
+ return ret;
+ } else {
+ /* Disable magic packet matching & reset WOL status bit */
+ ret = phy_modify_mmd(phydev, MDIO_MMD_VEND2,
+ MV_V2_WOL_CTRL,
+ MV_V2_WOL_CTRL_MAGIC_PKT_EN,
+ MV_V2_WOL_CTRL_CLEAR_STS);
+ if (ret < 0)
+ return ret;
+ }
+
+ /* Reset the clear WOL status bit as it does not self-clear */
+ return phy_clear_bits_mmd(phydev, MDIO_MMD_VEND2,
+ MV_V2_WOL_CTRL,
+ MV_V2_WOL_CTRL_CLEAR_STS);
+}
+