1 files changed, 119 insertions, 26 deletions

diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index bba87775419d..9bb6220663b2 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -188,6 +188,9 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define FEC_MMFR_RA(v)          ((v & 0x1f) << 18)
 #define FEC_MMFR_TA             (2 << 16)
 #define FEC_MMFR_DATA(v)        (v & 0xffff)
+/* FEC ECR bits definition */
+#define FEC_ECR_MAGICEN         (1 << 2)
+#define FEC_ECR_SLEEP           (1 << 3)
 
 #define FEC_MII_TIMEOUT         30000 /* us */
 
@@ -196,6 +199,9 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 
 #define FEC_PAUSE_FLAG_AUTONEG  0x1
 #define FEC_PAUSE_FLAG_ENABLE   0x2
+#define FEC_WOL_HAS_MAGIC_PACKET        (0x1 << 0)
+#define FEC_WOL_FLAG_ENABLE             (0x1 << 1)
+#define FEC_WOL_FLAG_SLEEP_ON           (0x1 << 2)
 
 #define COPYBREAK_DEFAULT       256
 
@@ -1090,7 +1096,9 @@ static void
 fec_stop(struct net_device *ndev)
 {
         struct fec_enet_private *fep = netdev_priv(ndev);
+        struct fec_platform_data *pdata = fep->pdev->dev.platform_data;
         u32 rmii_mode = readl(fep->hwp + FEC_R_CNTRL) & (1 << 8);
+        u32 val;
 
         /* We cannot expect a graceful transmit stop without link !!! */
         if (fep->link) {
@@ -1104,17 +1112,28 @@ fec_stop(struct net_device *ndev)
          * For i.MX6SX SOC, enet use AXI bus, we use disable MAC
          * instead of reset MAC itself.
          */
-        if (fep->quirks & FEC_QUIRK_HAS_AVB) {
-                writel(0, fep->hwp + FEC_ECNTRL);
+        if (!(fep->wol_flag & FEC_WOL_FLAG_SLEEP_ON)) {
+                if (fep->quirks & FEC_QUIRK_HAS_AVB) {
+                        writel(0, fep->hwp + FEC_ECNTRL);
+                } else {
+                        writel(1, fep->hwp + FEC_ECNTRL);
+                        udelay(10);
+                }
+                writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
         } else {
-                writel(1, fep->hwp + FEC_ECNTRL);
-                udelay(10);
+                writel(FEC_DEFAULT_IMASK | FEC_ENET_WAKEUP, fep->hwp + FEC_IMASK);
+                val = readl(fep->hwp + FEC_ECNTRL);
+                val |= (FEC_ECR_MAGICEN | FEC_ECR_SLEEP);
+                writel(val, fep->hwp + FEC_ECNTRL);
+
+                if (pdata && pdata->sleep_mode_enable)
+                        pdata->sleep_mode_enable(true);
         }
         writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
-        writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
 
         /* We have to keep ENET enabled to have MII interrupt stay working */
-        if (fep->quirks & FEC_QUIRK_ENET_MAC) {
+        if (fep->quirks & FEC_QUIRK_ENET_MAC &&
+                !(fep->wol_flag & FEC_WOL_FLAG_SLEEP_ON)) {
                 writel(2, fep->hwp + FEC_ECNTRL);
                 writel(rmii_mode, fep->hwp + FEC_R_CNTRL);
         }
@@ -1170,12 +1189,13 @@ static void
 fec_enet_tx_queue(struct net_device *ndev, u16 queue_id)
 {
         struct  fec_enet_private *fep;
-        struct bufdesc *bdp;
+        struct bufdesc *bdp, *bdp_t;
         unsigned short status;
         struct  sk_buff *skb;
         struct fec_enet_priv_tx_q *txq;
         struct netdev_queue *nq;
         int     index = 0;
+        int     i, bdnum;
         int     entries_free;
 
         fep = netdev_priv(ndev);
@@ -1196,18 +1216,29 @@ fec_enet_tx_queue(struct net_device *ndev, u16 queue_id)
                 if (bdp == txq->cur_tx)
                         break;
 
-                index = fec_enet_get_bd_index(txq->tx_bd_base, bdp, fep);
-
+                bdp_t = bdp;
+                bdnum = 1;
+                index = fec_enet_get_bd_index(txq->tx_bd_base, bdp_t, fep);
                 skb = txq->tx_skbuff[index];
-                txq->tx_skbuff[index] = NULL;
-                if (!IS_TSO_HEADER(txq, bdp->cbd_bufaddr))
-                        dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
-                                        bdp->cbd_datlen, DMA_TO_DEVICE);
-                bdp->cbd_bufaddr = 0;
-                if (!skb) {
-                        bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
-                        continue;
+                while (!skb) {
+                        bdp_t = fec_enet_get_nextdesc(bdp_t, fep, queue_id);
+                        index = fec_enet_get_bd_index(txq->tx_bd_base, bdp_t, fep);
+                        skb = txq->tx_skbuff[index];
+                        bdnum++;
                 }
+                if (skb_shinfo(skb)->nr_frags &&
+                    (status = bdp_t->cbd_sc) & BD_ENET_TX_READY)
+                        break;
+
+                for (i = 0; i < bdnum; i++) {
+                        if (!IS_TSO_HEADER(txq, bdp->cbd_bufaddr))
+                                dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
+                                                 bdp->cbd_datlen, DMA_TO_DEVICE);
+                        bdp->cbd_bufaddr = 0;
+                        if (i < bdnum - 1)
+                                bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
+                }
+                txq->tx_skbuff[index] = NULL;
 
                 /* Check for errors. */
                 if (status & (BD_ENET_TX_HB | BD_ENET_TX_LC |
@@ -2428,6 +2459,44 @@ static int fec_enet_set_tunable(struct net_device *netdev,
         return ret;
 }
 
+static void
+fec_enet_get_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
+{
+        struct fec_enet_private *fep = netdev_priv(ndev);
+
+        if (fep->wol_flag & FEC_WOL_HAS_MAGIC_PACKET) {
+                wol->supported = WAKE_MAGIC;
+                wol->wolopts = fep->wol_flag & FEC_WOL_FLAG_ENABLE ? WAKE_MAGIC : 0;
+        } else {
+                wol->supported = wol->wolopts = 0;
+        }
+}
+
+static int
+fec_enet_set_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
+{
+        struct fec_enet_private *fep = netdev_priv(ndev);
+
+        if (!(fep->wol_flag & FEC_WOL_HAS_MAGIC_PACKET))
+                return -EINVAL;
+
+        if (wol->wolopts & ~WAKE_MAGIC)
+                return -EINVAL;
+
+        device_set_wakeup_enable(&ndev->dev, wol->wolopts & WAKE_MAGIC);
+        if (device_may_wakeup(&ndev->dev)) {
+                fep->wol_flag |= FEC_WOL_FLAG_ENABLE;
+                if (fep->irq[0] > 0)
+                        enable_irq_wake(fep->irq[0]);
+        } else {
+                fep->wol_flag &= (~FEC_WOL_FLAG_ENABLE);
+                if (fep->irq[0] > 0)
+                        disable_irq_wake(fep->irq[0]);
+        }
+
+        return 0;
+}
+
 static const struct ethtool_ops fec_enet_ethtool_ops = {
         .get_settings           = fec_enet_get_settings,
         .set_settings           = fec_enet_set_settings,
@@ -2446,6 +2515,8 @@ static const struct ethtool_ops fec_enet_ethtool_ops = {
         .get_ts_info            = fec_enet_get_ts_info,
         .get_tunable            = fec_enet_get_tunable,
         .set_tunable            = fec_enet_set_tunable,
+        .get_wol                = fec_enet_get_wol,
+        .set_wol                = fec_enet_set_wol,
 };
 
 static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
@@ -2525,12 +2596,9 @@ static void fec_enet_free_queue(struct net_device *ndev)
                 }
 
         for (i = 0; i < fep->num_rx_queues; i++)
-                if (fep->rx_queue[i])
-                        kfree(fep->rx_queue[i]);
-
+                kfree(fep->rx_queue[i]);
         for (i = 0; i < fep->num_tx_queues; i++)
-                if (fep->tx_queue[i])
-                        kfree(fep->tx_queue[i]);
+                kfree(fep->tx_queue[i]);
 }
 
 static int fec_enet_alloc_queue(struct net_device *ndev)
@@ -2706,6 +2774,9 @@ fec_enet_open(struct net_device *ndev)
         phy_start(fep->phy_dev);
         netif_tx_start_all_queues(ndev);
 
+        device_set_wakeup_enable(&ndev->dev, fep->wol_flag &
+                                 FEC_WOL_FLAG_ENABLE);
+
         return 0;
 
 err_enet_mii_probe:
@@ -3155,6 +3226,9 @@ fec_probe(struct platform_device *pdev)
 
         platform_set_drvdata(pdev, ndev);
 
+        if (of_get_property(np, "fsl,magic-packet", NULL))
+                fep->wol_flag |= FEC_WOL_HAS_MAGIC_PACKET;
+
         phy_node = of_parse_phandle(np, "phy-handle", 0);
         if (!phy_node && of_phy_is_fixed_link(np)) {
                 ret = of_phy_register_fixed_link(np);
@@ -3249,6 +3323,8 @@ fec_probe(struct platform_device *pdev)
                                        0, pdev->name, ndev);
                 if (ret)
                         goto failed_irq;
+
+                fep->irq[i] = irq;
         }
 
         init_completion(&fep->mdio_done);
@@ -3265,6 +3341,9 @@ fec_probe(struct platform_device *pdev)
         if (ret)
                 goto failed_register;
 
+        device_init_wakeup(&ndev->dev, fep->wol_flag &
+                           FEC_WOL_HAS_MAGIC_PACKET);
+
         if (fep->bufdesc_ex && fep->ptp_clock)
                 netdev_info(ndev, "registered PHC device %d\n", fep->dev_id);
 
@@ -3318,6 +3397,8 @@ static int __maybe_unused fec_suspend(struct device *dev)
 
         rtnl_lock();
         if (netif_running(ndev)) {
+                if (fep->wol_flag & FEC_WOL_FLAG_ENABLE)
+                        fep->wol_flag |= FEC_WOL_FLAG_SLEEP_ON;
                 phy_stop(fep->phy_dev);
                 napi_disable(&fep->napi);
                 netif_tx_lock_bh(ndev);
@@ -3325,11 +3406,12 @@ static int __maybe_unused fec_suspend(struct device *dev)
                 netif_tx_unlock_bh(ndev);
                 fec_stop(ndev);
                 fec_enet_clk_enable(ndev, false);
-                pinctrl_pm_select_sleep_state(&fep->pdev->dev);
+                if (!(fep->wol_flag & FEC_WOL_FLAG_ENABLE))
+                        pinctrl_pm_select_sleep_state(&fep->pdev->dev);
         }
         rtnl_unlock();
 
-        if (fep->reg_phy)
+        if (fep->reg_phy && !(fep->wol_flag & FEC_WOL_FLAG_ENABLE))
                 regulator_disable(fep->reg_phy);
 
         /* SOC supply clock to phy, when clock is disabled, phy link down
@@ -3345,9 +3427,11 @@ static int __maybe_unused fec_resume(struct device *dev)
 {
         struct net_device *ndev = dev_get_drvdata(dev);
         struct fec_enet_private *fep = netdev_priv(ndev);
+        struct fec_platform_data *pdata = fep->pdev->dev.platform_data;
         int ret;
+        int val;
 
-        if (fep->reg_phy) {
+        if (fep->reg_phy && !(fep->wol_flag & FEC_WOL_FLAG_ENABLE)) {
                 ret = regulator_enable(fep->reg_phy);
                 if (ret)
                         return ret;
@@ -3355,12 +3439,21 @@ static int __maybe_unused fec_resume(struct device *dev)
 
         rtnl_lock();
         if (netif_running(ndev)) {
-                pinctrl_pm_select_default_state(&fep->pdev->dev);
                 ret = fec_enet_clk_enable(ndev, true);
                 if (ret) {
                         rtnl_unlock();
                         goto failed_clk;
                 }
+                if (fep->wol_flag & FEC_WOL_FLAG_ENABLE) {
+                        if (pdata && pdata->sleep_mode_enable)
+                                pdata->sleep_mode_enable(false);
+                        val = readl(fep->hwp + FEC_ECNTRL);
+                        val &= ~(FEC_ECR_MAGICEN | FEC_ECR_SLEEP);
+                        writel(val, fep->hwp + FEC_ECNTRL);
+                        fep->wol_flag &= ~FEC_WOL_FLAG_SLEEP_ON;
+                } else {
+                        pinctrl_pm_select_default_state(&fep->pdev->dev);
+                }
                 fec_restart(ndev);
                 netif_tx_lock_bh(ndev);
                 netif_device_attach(ndev);