smsc95xx: add wol magic packet support

This patch enables wake from system suspend on magic packet.

Patch updated to replace BUG_ON with WARN_ON_ONCE and return.

Signed-off-by: Steve Glendinning <steve.glendinning@shawell.net>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/usb/smsc95xx.c b/drivers/net/usb/smsc95xx.c
index ba0360fb857f89f90f264898594fe5ad4b0e467b..7479a5761d0d6b35c9a275ae470e49dc959de366 100644 (file)
--- a/drivers/net/usb/smsc95xx.c
+++ b/drivers/net/usb/smsc95xx.c
@@ -46,6 +46,7 @@
 #define SMSC95XX_INTERNAL_PHY_ID       (1)
 #define SMSC95XX_TX_OVERHEAD           (8)
 #define SMSC95XX_TX_OVERHEAD_CSUM      (12)
+#define SUPPORTED_WAKE                 (WAKE_MAGIC)
 
 #define check_warn(ret, fmt, args...) \
        ({ if (ret < 0) netdev_warn(dev->net, fmt, ##args); })
@@ -60,6 +61,7 @@ struct smsc95xx_priv {
        u32 mac_cr;
        u32 hash_hi;
        u32 hash_lo;
+       u32 wolopts;
        spinlock_t mac_cr_lock;
 };
 
@@ -125,6 +127,30 @@ static int __must_check smsc95xx_write_reg(struct usbnet *dev, u32 index,
        return ret;
 }
 
+static int smsc95xx_set_feature(struct usbnet *dev, u32 feature)
+{
+       if (WARN_ON_ONCE(!dev))
+               return -EINVAL;
+
+       cpu_to_le32s(&feature);
+
+       return usb_control_msg(dev->udev, usb_sndctrlpipe(dev->udev, 0),
+               USB_REQ_SET_FEATURE, USB_RECIP_DEVICE, feature, 0, NULL, 0,
+               USB_CTRL_SET_TIMEOUT);
+}
+
+static int smsc95xx_clear_feature(struct usbnet *dev, u32 feature)
+{
+       if (WARN_ON_ONCE(!dev))
+               return -EINVAL;
+
+       cpu_to_le32s(&feature);
+
+       return usb_control_msg(dev->udev, usb_sndctrlpipe(dev->udev, 0),
+               USB_REQ_CLEAR_FEATURE, USB_RECIP_DEVICE, feature, 0, NULL, 0,
+               USB_CTRL_SET_TIMEOUT);
+}
+
 /* Loop until the read is completed with timeout
  * called with phy_mutex held */
 static int __must_check smsc95xx_phy_wait_not_busy(struct usbnet *dev)
@@ -636,6 +662,26 @@ smsc95xx_ethtool_getregs(struct net_device *netdev, struct ethtool_regs *regs,
        }
 }
 
+static void smsc95xx_ethtool_get_wol(struct net_device *net,
+                                    struct ethtool_wolinfo *wolinfo)
+{
+       struct usbnet *dev = netdev_priv(net);
+       struct smsc95xx_priv *pdata = (struct smsc95xx_priv *)(dev->data[0]);
+
+       wolinfo->supported = SUPPORTED_WAKE;
+       wolinfo->wolopts = pdata->wolopts;
+}
+
+static int smsc95xx_ethtool_set_wol(struct net_device *net,
+                                   struct ethtool_wolinfo *wolinfo)
+{
+       struct usbnet *dev = netdev_priv(net);
+       struct smsc95xx_priv *pdata = (struct smsc95xx_priv *)(dev->data[0]);
+
+       pdata->wolopts = wolinfo->wolopts & SUPPORTED_WAKE;
+       return 0;
+}
+
 static const struct ethtool_ops smsc95xx_ethtool_ops = {
        .get_link       = usbnet_get_link,
        .nway_reset     = usbnet_nway_reset,
@@ -649,6 +695,8 @@ static const struct ethtool_ops smsc95xx_ethtool_ops = {
        .set_eeprom     = smsc95xx_ethtool_set_eeprom,
        .get_regs_len   = smsc95xx_ethtool_getregslen,
        .get_regs       = smsc95xx_ethtool_getregs,
+       .get_wol        = smsc95xx_ethtool_get_wol,
+       .set_wol        = smsc95xx_ethtool_set_wol,
 };
 
 static int smsc95xx_ioctl(struct net_device *netdev, struct ifreq *rq, int cmd)
@@ -1021,26 +1069,147 @@ static void smsc95xx_unbind(struct usbnet *dev, struct usb_interface *intf)
 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]);
        int ret;
        u32 val;
 
-       if (WARN_ON_ONCE(!dev))
-               return -EINVAL;
-
        ret = usbnet_suspend(intf, message);
        check_warn_return(ret, "usbnet_suspend error");
 
-       netdev_info(dev->net, "entering SUSPEND2 mode");
+       /* if no wol options set, enter lowest power SUSPEND2 mode */
+       if (!(pdata->wolopts & SUPPORTED_WAKE)) {
+               netdev_info(dev->net, "entering SUSPEND2 mode");
+
+               /* disable energy detect (link up) & wake up events */
+               ret = smsc95xx_read_reg(dev, WUCSR, &val);
+               check_warn_return(ret, "Error reading WUCSR");
+
+               val &= ~(WUCSR_MPEN_ | WUCSR_WAKE_EN_);
+
+               ret = smsc95xx_write_reg(dev, WUCSR, val);
+               check_warn_return(ret, "Error writing WUCSR");
+
+               ret = smsc95xx_read_reg(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(dev, PM_CTRL, val);
+               check_warn_return(ret, "Error writing PM_CTRL");
+
+               /* enter suspend2 mode */
+               ret = smsc95xx_read_reg(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(dev, PM_CTRL, val);
+               check_warn_return(ret, "Error writing PM_CTRL");
+
+               return 0;
+       }
+
+       if (pdata->wolopts & WAKE_MAGIC) {
+               /* clear any pending magic packet status */
+               ret = smsc95xx_read_reg(dev, WUCSR, &val);
+               check_warn_return(ret, "Error reading WUCSR");
+
+               val |= WUCSR_MPR_;
+
+               ret = smsc95xx_write_reg(dev, WUCSR, val);
+               check_warn_return(ret, "Error writing WUCSR");
+       }
+
+       /* enable/disable magic packup wake */
+       ret = smsc95xx_read_reg(dev, WUCSR, &val);
+       check_warn_return(ret, "Error reading WUCSR");
+
+       if (pdata->wolopts & WAKE_MAGIC) {
+               netdev_info(dev->net, "enabling magic packet wakeup");
+               val |= WUCSR_MPEN_;
+       } else {
+               netdev_info(dev->net, "disabling magic packet wakeup");
+               val &= ~WUCSR_MPEN_;
+       }
+
+       ret = smsc95xx_write_reg(dev, WUCSR, val);
+       check_warn_return(ret, "Error writing WUCSR");
+
+       /* enable wol wakeup source */
+       ret = smsc95xx_read_reg(dev, PM_CTRL, &val);
+       check_warn_return(ret, "Error reading PM_CTRL");
+
+       val |= PM_CTL_WOL_EN_;
+
+       ret = smsc95xx_write_reg(dev, PM_CTRL, val);
+       check_warn_return(ret, "Error writing PM_CTRL");
+
+       /* enable receiver */
+       smsc95xx_start_rx_path(dev);
+
+       /* some wol options are enabled, so enter SUSPEND0 */
+       netdev_info(dev->net, "entering SUSPEND0 mode");
 
        ret = smsc95xx_read_reg(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;
+       val &= (~(PM_CTL_SUS_MODE_ | PM_CTL_WUPS_ | PM_CTL_PHY_RST_));
+       val |= PM_CTL_SUS_MODE_0;
 
        ret = smsc95xx_write_reg(dev, PM_CTRL, val);
        check_warn_return(ret, "Error writing PM_CTRL");
 
+       /* clear wol status */
+       val &= ~PM_CTL_WUPS_;
+       val |= PM_CTL_WUPS_WOL_;
+       ret = smsc95xx_write_reg(dev, PM_CTRL, val);
+       check_warn_return(ret, "Error writing PM_CTRL");
+
+       /* read back PM_CTRL */
+       ret = smsc95xx_read_reg(dev, PM_CTRL, &val);
+       check_warn_return(ret, "Error reading PM_CTRL");
+
+       smsc95xx_set_feature(dev, USB_DEVICE_REMOTE_WAKEUP);
+
+       return 0;
+}
+
+static int smsc95xx_resume(struct usb_interface *intf)
+{
+       struct usbnet *dev = usb_get_intfdata(intf);
+       struct smsc95xx_priv *pdata = (struct smsc95xx_priv *)(dev->data[0]);
+       int ret;
+       u32 val;
+
+       BUG_ON(!dev);
+
+       if (pdata->wolopts & WAKE_MAGIC) {
+               smsc95xx_clear_feature(dev, USB_DEVICE_REMOTE_WAKEUP);
+
+               /* Disable magic packup wake */
+               ret = smsc95xx_read_reg(dev, WUCSR, &val);
+               check_warn_return(ret, "Error reading WUCSR");
+
+               val &= ~WUCSR_MPEN_;
+
+               ret = smsc95xx_write_reg(dev, WUCSR, val);
+               check_warn_return(ret, "Error writing WUCSR");
+
+               /* clear wake-up status */
+               ret = smsc95xx_read_reg(dev, PM_CTRL, &val);
+               check_warn_return(ret, "Error reading PM_CTRL");
+
+               val &= ~PM_CTL_WOL_EN_;
+               val |= PM_CTL_WUPS_;
+
+               ret = smsc95xx_write_reg(dev, PM_CTRL, val);
+               check_warn_return(ret, "Error writing PM_CTRL");
+       }
+
+       return usbnet_resume(intf);
+       check_warn_return(ret, "usbnet_resume error");
+
        return 0;
 }
 
@@ -1307,8 +1476,8 @@ static struct usb_driver smsc95xx_driver = {
        .id_table       = products,
        .probe          = usbnet_probe,
        .suspend        = smsc95xx_suspend,
-       .resume         = usbnet_resume,
-       .reset_resume   = usbnet_resume,
+       .resume         = smsc95xx_resume,
+       .reset_resume   = smsc95xx_resume,
        .disconnect     = usbnet_disconnect,
        .disable_hub_initiated_lpm = 1,
 };

diff --git a/drivers/net/usb/smsc95xx.h b/drivers/net/usb/smsc95xx.h
index 89ad9259d6135c78b0a204caa5e845955e8b05e4..2ff9815aa27c5e9e098dd587e8291398f2dce25f 100644 (file)
--- a/drivers/net/usb/smsc95xx.h
+++ b/drivers/net/usb/smsc95xx.h
@@ -205,6 +205,11 @@
 #define WUFF                           (0x128)
 
 #define WUCSR                          (0x12C)
+#define WUCSR_GUE_                     (0x00000200)
+#define WUCSR_WUFR_                    (0x00000040)
+#define WUCSR_MPR_                     (0x00000020)
+#define WUCSR_WAKE_EN_                 (0x00000004)
+#define WUCSR_MPEN_                    (0x00000002)
 
 #define COE_CR                         (0x130)
 #define Tx_COE_EN_                     (0x00010000)