net: davinci_emac: Fix rollback of emac_dev_open()

[~andy/linux] / drivers / net / ethernet / ti / davinci_emac.c

diff --git a/drivers/net/ethernet/ti/davinci_emac.c b/drivers/net/ethernet/ti/davinci_emac.c
index cd9b164a0434acb3a51066b5d0e17262a4bdc0dd..8f0e69ce07ca3e03cfbf4cf1b22c92c9af27beeb 100644 (file)
--- a/drivers/net/ethernet/ti/davinci_emac.c
+++ b/drivers/net/ethernet/ti/davinci_emac.c
@@ -1532,9 +1532,9 @@ static int emac_dev_open(struct net_device *ndev)
        struct device *emac_dev = &ndev->dev;
        u32 cnt;
        struct resource *res;
-       int ret;
+       int q, m, ret;
+       int res_num = 0, irq_num = 0;
        int i = 0;
-       int k = 0;
        struct emac_priv *priv = netdev_priv(ndev);
 
        pm_runtime_get(&priv->pdev->dev);
@@ -1564,15 +1564,24 @@ static int emac_dev_open(struct net_device *ndev)
        }
 
        /* Request IRQ */
+       while ((res = platform_get_resource(priv->pdev, IORESOURCE_IRQ,
+                                           res_num))) {
+               for (irq_num = res->start; irq_num <= res->end; irq_num++) {
+                       dev_err(emac_dev, "Request IRQ %d\n", irq_num);
+                       if (request_irq(irq_num, emac_irq, 0, ndev->name,
+                                       ndev)) {
+                               dev_err(emac_dev,
+                                       "DaVinci EMAC: request_irq() failed\n");
+                               ret = -EBUSY;
 
-       while ((res = platform_get_resource(priv->pdev, IORESOURCE_IRQ, k))) {
-               for (i = res->start; i <= res->end; i++) {
-                       if (devm_request_irq(&priv->pdev->dev, i, emac_irq,
-                                            0, ndev->name, ndev))
                                goto rollback;
+                       }
                }
-               k++;
+               res_num++;
        }
+       /* prepare counters for rollback in case of an error */
+       res_num--;
+       irq_num--;
 
        /* Start/Enable EMAC hardware */
        emac_hw_enable(priv);
@@ -1639,11 +1648,23 @@ static int emac_dev_open(struct net_device *ndev)
 
        return 0;
 
-rollback:
-
-       dev_err(emac_dev, "DaVinci EMAC: devm_request_irq() failed");
-       ret = -EBUSY;
 err:
+       emac_int_disable(priv);
+       napi_disable(&priv->napi);
+
+rollback:
+       for (q = res_num; q >= 0; q--) {
+               res = platform_get_resource(priv->pdev, IORESOURCE_IRQ, q);
+               /* at the first iteration, irq_num is already set to the
+                * right value
+                */
+               if (q != res_num)
+                       irq_num = res->end;
+
+               for (m = irq_num; m >= res->start; m--)
+                       free_irq(m, ndev);
+       }
+       cpdma_ctlr_stop(priv->dma);
        pm_runtime_put(&priv->pdev->dev);
        return ret;
 }
@@ -1659,6 +1680,9 @@ err:
  */
 static int emac_dev_stop(struct net_device *ndev)
 {
+       struct resource *res;
+       int i = 0;
+       int irq_num;
        struct emac_priv *priv = netdev_priv(ndev);
        struct device *emac_dev = &ndev->dev;
 
@@ -1674,6 +1698,13 @@ static int emac_dev_stop(struct net_device *ndev)
        if (priv->phydev)
                phy_disconnect(priv->phydev);
 
+       /* Free IRQ */
+       while ((res = platform_get_resource(priv->pdev, IORESOURCE_IRQ, i))) {
+               for (irq_num = res->start; irq_num <= res->end; irq_num++)
+                       free_irq(irq_num, priv->ndev);
+               i++;
+       }
+
        if (netif_msg_drv(priv))
                dev_notice(emac_dev, "DaVinci EMAC: %s stopped\n", ndev->name);