smsc95xx: refactor entering suspend modes

This patch splits out the logic for entering suspend modes
to separate functions, to reduce the complexity of the
smsc95xx_suspend function.
Signed-off-by: Steve Glendinning <steve.glendinning@shawell.net>
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/usb/smsc95xx.c

@@ -1130,85 +1130,62 @@ static int smsc95xx_link_ok_nopm(struct usbnet *dev)
 	return !!(ret & BMSR_LSTATUS);
 }
 
-static int smsc95xx_suspend(struct usb_interface *intf, pm_message_t message)
+static int smsc95xx_enter_suspend0(struct usbnet *dev)
 {
-	struct usbnet *dev = usb_get_intfdata(intf);
 	struct smsc95xx_priv *pdata = (struct smsc95xx_priv *)(dev->data[0]);
-	u32 val, link_up;
+	u32 val;
 	int ret;
 
-	ret = usbnet_suspend(intf, message);
-	check_warn_return(ret, "usbnet_suspend error");
-
-	/* determine if link is up using only _nopm functions */
-	link_up = smsc95xx_link_ok_nopm(dev);
-
-	/* if no wol options set, or if link is down and we're not waking on
-	 * PHY activity, enter lowest power SUSPEND2 mode
-	 */
-	if (!(pdata->wolopts & SUPPORTED_WAKE) ||
-		!(link_up || (pdata->wolopts & WAKE_PHY))) {
-		netdev_info(dev->net, "entering SUSPEND2 mode");
-
-		/* disable energy detect (link up) & wake up events */
-		ret = smsc95xx_read_reg_nopm(dev, WUCSR, &val);
-		check_warn_return(ret, "Error reading WUCSR");
-
-		val &= ~(WUCSR_MPEN_ | WUCSR_WAKE_EN_);
-
-		ret = smsc95xx_write_reg_nopm(dev, WUCSR, val);
-		check_warn_return(ret, "Error writing WUCSR");
-
 	ret = smsc95xx_read_reg_nopm(dev, PM_CTRL, &val);
 	check_warn_return(ret, "Error reading PM_CTRL");
 
-		val &= ~(PM_CTL_ED_EN_ | PM_CTL_WOL_EN_);
+	val &= (~(PM_CTL_SUS_MODE_ | PM_CTL_WUPS_ | PM_CTL_PHY_RST_));
+	val |= PM_CTL_SUS_MODE_0;
 
 	ret = smsc95xx_write_reg_nopm(dev, PM_CTRL, val);
 	check_warn_return(ret, "Error writing PM_CTRL");
 
-		/* enter suspend2 mode */
-		ret = smsc95xx_read_reg_nopm(dev, PM_CTRL, &val);
-		check_warn_return(ret, "Error reading PM_CTRL");
+	/* clear wol status */
+	val &= ~PM_CTL_WUPS_;
+	val |= PM_CTL_WUPS_WOL_;
 
-		val &= ~(PM_CTL_SUS_MODE_ | PM_CTL_WUPS_ | PM_CTL_PHY_RST_);
-		val |= PM_CTL_SUS_MODE_2;
+	/* enable energy detection */
+	if (pdata->wolopts & WAKE_PHY)
+		val |= PM_CTL_WUPS_ED_;
 
 	ret = smsc95xx_write_reg_nopm(dev, PM_CTRL, val);
 	check_warn_return(ret, "Error writing PM_CTRL");
 
-		return 0;
-	}
+	/* read back PM_CTRL */
+	ret = smsc95xx_read_reg_nopm(dev, PM_CTRL, &val);
+	check_warn_return(ret, "Error reading PM_CTRL");
 
-	if (pdata->wolopts & WAKE_PHY) {
-		ret = smsc95xx_enable_phy_wakeup_interrupts(dev,
-			(PHY_INT_MASK_ANEG_COMP_ | PHY_INT_MASK_LINK_DOWN_));
-		check_warn_return(ret, "error enabling PHY wakeup ints");
+	smsc95xx_set_feature(dev, USB_DEVICE_REMOTE_WAKEUP);
 
-		/* if link is down then configure EDPD and enter SUSPEND1,
-		 * otherwise enter SUSPEND0 below
-		 */
-		if (!link_up) {
+	return 0;
+}
+
+static int smsc95xx_enter_suspend1(struct usbnet *dev)
+{
+	struct smsc95xx_priv *pdata = (struct smsc95xx_priv *)(dev->data[0]);
 	struct mii_if_info *mii = &dev->mii;
-			netdev_info(dev->net, "entering SUSPEND1 mode");
+	u32 val;
+	int ret;
 
 	/* reconfigure link pulse detection timing for
 	 * compatibility with non-standard link partners
 	 */
 	if (pdata->features & FEATURE_PHY_NLP_CROSSOVER)
-				smsc95xx_mdio_write_nopm(dev->net, mii->phy_id,
-					PHY_EDPD_CONFIG,
+		smsc95xx_mdio_write_nopm(dev->net, mii->phy_id,	PHY_EDPD_CONFIG,
 			PHY_EDPD_CONFIG_DEFAULT);
 
 	/* enable energy detect power-down mode */
-			ret = smsc95xx_mdio_read_nopm(dev->net, mii->phy_id,
-				PHY_MODE_CTRL_STS);
+	ret = smsc95xx_mdio_read_nopm(dev->net, mii->phy_id, PHY_MODE_CTRL_STS);
 	check_warn_return(ret, "Error reading PHY_MODE_CTRL_STS");
 
 	ret |= MODE_CTRL_STS_EDPWRDOWN_;
 
-			smsc95xx_mdio_write_nopm(dev->net, mii->phy_id,
-				PHY_MODE_CTRL_STS, ret);
+	smsc95xx_mdio_write_nopm(dev->net, mii->phy_id, PHY_MODE_CTRL_STS, ret);
 
 	/* enter SUSPEND1 mode */
 	ret = smsc95xx_read_reg_nopm(dev, PM_CTRL, &val);
@@ -1230,6 +1207,76 @@ static int smsc95xx_suspend(struct usb_interface *intf, pm_message_t message)
 	smsc95xx_set_feature(dev, USB_DEVICE_REMOTE_WAKEUP);
 
 	return 0;
+}
+
+static int smsc95xx_enter_suspend2(struct usbnet *dev)
+{
+	u32 val;
+	int ret;
+
+	ret = smsc95xx_read_reg_nopm(dev, PM_CTRL, &val);
+	check_warn_return(ret, "Error reading PM_CTRL");
+
+	val &= ~(PM_CTL_SUS_MODE_ | PM_CTL_WUPS_ | PM_CTL_PHY_RST_);
+	val |= PM_CTL_SUS_MODE_2;
+
+	ret = smsc95xx_write_reg_nopm(dev, PM_CTRL, val);
+	check_warn_return(ret, "Error writing PM_CTRL");
+
+	return 0;
+}
+
+static int smsc95xx_suspend(struct usb_interface *intf, pm_message_t message)
+{
+	struct usbnet *dev = usb_get_intfdata(intf);
+	struct smsc95xx_priv *pdata = (struct smsc95xx_priv *)(dev->data[0]);
+	u32 val, link_up;
+	int ret;
+
+	ret = usbnet_suspend(intf, message);
+	check_warn_return(ret, "usbnet_suspend error");
+
+	/* determine if link is up using only _nopm functions */
+	link_up = smsc95xx_link_ok_nopm(dev);
+
+	/* if no wol options set, or if link is down and we're not waking on
+	 * PHY activity, enter lowest power SUSPEND2 mode
+	 */
+	if (!(pdata->wolopts & SUPPORTED_WAKE) ||
+		!(link_up || (pdata->wolopts & WAKE_PHY))) {
+		netdev_info(dev->net, "entering SUSPEND2 mode");
+
+		/* disable energy detect (link up) & wake up events */
+		ret = smsc95xx_read_reg_nopm(dev, WUCSR, &val);
+		check_warn_return(ret, "Error reading WUCSR");
+
+		val &= ~(WUCSR_MPEN_ | WUCSR_WAKE_EN_);
+
+		ret = smsc95xx_write_reg_nopm(dev, WUCSR, val);
+		check_warn_return(ret, "Error writing WUCSR");
+
+		ret = smsc95xx_read_reg_nopm(dev, PM_CTRL, &val);
+		check_warn_return(ret, "Error reading PM_CTRL");
+
+		val &= ~(PM_CTL_ED_EN_ | PM_CTL_WOL_EN_);
+
+		ret = smsc95xx_write_reg_nopm(dev, PM_CTRL, val);
+		check_warn_return(ret, "Error writing PM_CTRL");
+
+		return smsc95xx_enter_suspend2(dev);
+	}
+
+	if (pdata->wolopts & WAKE_PHY) {
+		ret = smsc95xx_enable_phy_wakeup_interrupts(dev,
+			(PHY_INT_MASK_ANEG_COMP_ | PHY_INT_MASK_LINK_DOWN_));
+		check_warn_return(ret, "error enabling PHY wakeup ints");
+
+		/* if link is down then configure EDPD and enter SUSPEND1,
+		 * otherwise enter SUSPEND0 below
+		 */
+		if (!link_up) {
+			netdev_info(dev->net, "entering SUSPEND1 mode");
+			return smsc95xx_enter_suspend1(dev);
 		}
 	}
 
@@ -1383,34 +1430,7 @@ static int smsc95xx_suspend(struct usb_interface *intf, pm_message_t message)
 
 	/* some wol options are enabled, so enter SUSPEND0 */
 	netdev_info(dev->net, "entering SUSPEND0 mode");
-
-	ret = smsc95xx_read_reg_nopm(dev, PM_CTRL, &val);
-	check_warn_return(ret, "Error reading PM_CTRL");
-
-	val &= (~(PM_CTL_SUS_MODE_ | PM_CTL_WUPS_ | PM_CTL_PHY_RST_));
-	val |= PM_CTL_SUS_MODE_0;
-
-	ret = smsc95xx_write_reg_nopm(dev, PM_CTRL, val);
-	check_warn_return(ret, "Error writing PM_CTRL");
-
-	/* clear wol status */
-	val &= ~PM_CTL_WUPS_;
-	val |= PM_CTL_WUPS_WOL_;
-
-	/* enable energy detection */
-	if (pdata->wolopts & WAKE_PHY)
-		val |= PM_CTL_WUPS_ED_;
-
-	ret = smsc95xx_write_reg_nopm(dev, PM_CTRL, val);
-	check_warn_return(ret, "Error writing PM_CTRL");
-
-	/* read back PM_CTRL */
-	ret = smsc95xx_read_reg_nopm(dev, PM_CTRL, &val);
-	check_warn_return(ret, "Error reading PM_CTRL");
-
-	smsc95xx_set_feature(dev, USB_DEVICE_REMOTE_WAKEUP);
-
-	return 0;
+	return smsc95xx_enter_suspend0(dev);
 }
 
 static int smsc95xx_resume(struct usb_interface *intf)