[PATCH V3 net] net: stmmac: fix MAC WoL unwork if PHY doesn't support WoL

[Joakim Zhang <qiangqing.zhang@nxp.com>]

Both get and set WoL will check device_can_wakeup(), if MAC supports PMT,
it will set device wakeup capability. After commit 1d8e5b0f3f2c ("net:
stmmac: Support WOL with phy"), device wakeup capability will be overwrite
in stmmac_init_phy() according to phy's Wol feature. If phy doesn't support
WoL, then MAC will lose wakeup capability.

This patch combines WoL capabilities both MAC and PHY from stmmac_get_wol(),
set wakeup capability and give WoL priority to the PHY in stmmac_set_wol()
when enable WoL. What PHYs do implement is WAKE_MAGIC, WAKE_UCAST, WAKE_MAGICSECURE
and WAKE_BCAST.

Fixes: commit 1d8e5b0f3f2c ("net: stmmac: Support WOL with phy")
Suggested-by: Andrew Lunn <andrew@lunn.ch>
Signed-off-by: Joakim Zhang <qiangqing.zhang@nxp.com>
---
ChangeLogs:
V1->V2:
	* combine WoL capabilities both MAC and PHY
V2->V3:
	* give WoL priority to the PHY.
---
 .../ethernet/stmicro/stmmac/stmmac_ethtool.c  | 51 ++++++++++++-------
 .../net/ethernet/stmicro/stmmac/stmmac_main.c |  8 +--
 2 files changed, 34 insertions(+), 25 deletions(-)

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
index c5642985ef95..30358c1892f1 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
@@ -630,34 +630,46 @@ static void stmmac_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 static void stmmac_get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
-
-	if (!priv->plat->pmt)
-		return phylink_ethtool_get_wol(priv->phylink, wol);
+	struct ethtool_wolinfo wol_phy = { .cmd = ETHTOOL_GWOL };
 
 	mutex_lock(&priv->lock);
-	if (device_can_wakeup(priv->device)) {
+	if (priv->plat->pmt) {
 		wol->supported = WAKE_MAGIC | WAKE_UCAST;
 		if (priv->hw_cap_support && !priv->dma_cap.pmt_magic_frame)
 			wol->supported &= ~WAKE_MAGIC;
-		wol->wolopts = priv->wolopts;
 	}
+
+	phylink_ethtool_get_wol(priv->phylink, &wol_phy);
+
+	/* Combine WoL capabilities both PHY and MAC */
+	wol->supported |= wol_phy.supported;
+	wol->wolopts = priv->wolopts;
+
 	mutex_unlock(&priv->lock);
 }
 
 static int stmmac_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 {
 	struct stmmac_priv *priv = netdev_priv(dev);
-	u32 support = WAKE_MAGIC | WAKE_UCAST;
+	struct ethtool_wolinfo wol_phy = { .cmd = ETHTOOL_GWOL };
+	u32 support = WAKE_MAGIC | WAKE_UCAST | WAKE_MAGICSECURE | WAKE_BCAST;
 
-	if (!device_can_wakeup(priv->device))
-		return -EOPNOTSUPP;
+	if (wol->wolopts & ~support)
+		return -EINVAL;
 
-	if (!priv->plat->pmt) {
+	/* First check if can WoL from PHY */
+	phylink_ethtool_get_wol(priv->phylink, &wol_phy);
+	if (wol->wolopts & wol_phy.supported) {
 		int ret = phylink_ethtool_set_wol(priv->phylink, wol);
 
-		if (!ret)
-			device_set_wakeup_enable(priv->device, !!wol->wolopts);
-		return ret;
+		if (!ret) {
+			pr_info("stmmac: phy wakeup enable\n");
+			device_set_wakeup_capable(priv->device, 1);
+			device_set_wakeup_enable(priv->device, 1);
+			goto wolopts_update;
+		} else {
+			return ret;
+		}
 	}
 
 	/* By default almost all GMAC devices support the WoL via
@@ -666,18 +678,21 @@ static int stmmac_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 	if ((priv->hw_cap_support) && (!priv->dma_cap.pmt_magic_frame))
 		wol->wolopts &= ~WAKE_MAGIC;
 
-	if (wol->wolopts & ~support)
-		return -EINVAL;
-
-	if (wol->wolopts) {
-		pr_info("stmmac: wakeup enable\n");
+	if (priv->plat->pmt && wol->wolopts) {
+		pr_info("stmmac: mac wakeup enable\n");
+		device_set_wakeup_capable(priv->device, 1);
 		device_set_wakeup_enable(priv->device, 1);
 		enable_irq_wake(priv->wol_irq);
-	} else {
+		goto wolopts_update;
+	}
+
+	if (!wol->wolopts) {
+		device_set_wakeup_capable(priv->device, 0);
 		device_set_wakeup_enable(priv->device, 0);
 		disable_irq_wake(priv->wol_irq);
 	}
 
+wolopts_update:
 	mutex_lock(&priv->lock);
 	priv->wolopts = wol->wolopts;
 	mutex_unlock(&priv->lock);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index c6f24abf6432..d62d8c28463d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -1076,7 +1076,6 @@ static void stmmac_check_pcs_mode(struct stmmac_priv *priv)
  */
 static int stmmac_init_phy(struct net_device *dev)
 {
-	struct ethtool_wolinfo wol = { .cmd = ETHTOOL_GWOL };
 	struct stmmac_priv *priv = netdev_priv(dev);
 	struct device_node *node;
 	int ret;
@@ -1102,9 +1101,6 @@ static int stmmac_init_phy(struct net_device *dev)
 		ret = phylink_connect_phy(priv->phylink, phydev);
 	}
 
-	phylink_ethtool_get_wol(priv->phylink, &wol);
-	device_set_wakeup_capable(priv->device, !!wol.supported);
-
 	return ret;
 }
 
@@ -4787,10 +4783,8 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 	if (priv->plat->tx_coe)
 		dev_info(priv->device, "TX Checksum insertion supported\n");
 
-	if (priv->plat->pmt) {
+	if (priv->plat->pmt)
 		dev_info(priv->device, "Wake-Up On Lan supported\n");
-		device_set_wakeup_capable(priv->device, 1);
-	}
 
 	if (priv->dma_cap.tsoen)
 		dev_info(priv->device, "TSO supported\n");
-- 
2.17.1