[PATCH net-next] net: phy: marvell10g: enable WoL for mv2110

@@ -28,6 +28,7 @@
 #include <linux/marvell_phy.h>
 #include <linux/phy.h>
 #include <linux/sfp.h>
+#include <linux/netdevice.h>
 
 #define MV_PHY_ALASKA_NBT_QUIRK_MASK	0xfffffffe
 #define MV_PHY_ALASKA_NBT_QUIRK_REV	(MARVELL_PHY_ID_88X3310 | 0xa)

@@ -106,6 +107,17 @@ enum {
 	MV_V2_TEMP_CTRL_DISABLE	= 0xc000,
 	MV_V2_TEMP		= 0xf08c,
 	MV_V2_TEMP_UNKNOWN	= 0x9600, /* unknown function */
+	MV_V2_MAGIC_PKT_WORD0	= 0xf06b,
+	MV_V2_MAGIC_PKT_WORD1	= 0xf06c,
+	MV_V2_MAGIC_PKT_WORD2	= 0xf06d,
+	/* Wake on LAN registers */
+	MV_V2_WOL_CTRL		= 0xf06e,
+	MV_V2_WOL_STS		= 0xf06f,
+	MV_V2_WOL_CLEAR_STS	= BIT(15),
+	MV_V2_WOL_MAGIC_PKT_EN	= BIT(0),
+	MV_V2_PORT_INTR_STS	= 0xf040,
+	MV_V2_PORT_INTR_MASK	= 0xf043,
+	MV_V2_WOL_INTR_EN	= BIT(8),
 };
 
 struct mv3310_chip {

@@ -991,6 +1003,94 @@ static int mv2111_match_phy_device(struct phy_device *phydev)
 	return mv211x_match_phy_device(phydev, false);
 }
 
+static void mv2110_get_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
+{
+	int ret = 0;
+
+	wol->supported = WAKE_MAGIC;
+	wol->wolopts = 0;
+
+	ret = phy_read_mmd(phydev, MDIO_MMD_VEND2, MV_V2_WOL_CTRL);
+
+	if (ret & MV_V2_WOL_MAGIC_PKT_EN)
+		wol->wolopts |= WAKE_MAGIC;
+}
+
+static int mv2110_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
+{
+	int ret = 0;
+
+	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_WOL_INTR_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_MAGIC_PKT_EN |
+				       MV_V2_WOL_CLEAR_STS);
+
+		if (ret < 0)
+			return ret;
+
+		/* Reset the clear WOL status bit as it does not self-clear */
+		ret = phy_clear_bits_mmd(phydev, MDIO_MMD_VEND2,
+					 MV_V2_WOL_CTRL,
+					 MV_V2_WOL_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_MAGIC_PKT_EN,
+				     MV_V2_WOL_CLEAR_STS);
+
+		if (ret < 0)
+			return ret;
+
+		ret = phy_clear_bits_mmd(phydev, MDIO_MMD_VEND2,
+					 MV_V2_WOL_CTRL,
+					 MV_V2_WOL_CLEAR_STS);
+
+		if (ret < 0)
+			return ret;
+	}
+
+	return ret;
+}
+
 static struct phy_driver mv3310_drivers[] = {
 	{
 		.phy_id		= MARVELL_PHY_ID_88X3310,

@@ -1045,6 +1145,8 @@ static struct phy_driver mv3310_drivers[] = {
 		.set_tunable	= mv3310_set_tunable,
 		.remove		= mv3310_remove,
 		.set_loopback	= genphy_c45_loopback,
+		.get_wol	= mv2110_get_wol,
+		.set_wol	= mv2110_set_wol,
 	},
 	{
 		.phy_id		= MARVELL_PHY_ID_88E2110,