2 files changed, 98 insertions, 29 deletions

diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index 92578690f6de..419529a9309d 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -17,6 +17,7 @@
 #include <linux/circ_buf.h>
 #include <linux/slab.h>
 #include <linux/init.h>
+#include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include <linux/netdevice.h>
@@ -203,6 +204,47 @@ static int macb_mdio_reset(struct mii_bus *bus)
         return 0;
 }
 
+/**
+ * macb_set_tx_clk() - Set a clock to a new frequency
+ * @clk         Pointer to the clock to change
+ * @rate        New frequency in Hz
+ * @dev         Pointer to the struct net_device
+ */
+static void macb_set_tx_clk(struct clk *clk, int speed, struct net_device *dev)
+{
+        long ferr, rate, rate_rounded;
+
+        switch (speed) {
+        case SPEED_10:
+                rate = 2500000;
+                break;
+        case SPEED_100:
+                rate = 25000000;
+                break;
+        case SPEED_1000:
+                rate = 125000000;
+                break;
+        default:
+                break;
+        }
+
+        rate_rounded = clk_round_rate(clk, rate);
+        if (rate_rounded < 0)
+                return;
+
+        /* RGMII allows 50 ppm frequency error. Test and warn if this limit
+         * is not satisfied.
+         */
+        ferr = abs(rate_rounded - rate);
+        ferr = DIV_ROUND_UP(ferr, rate / 100000);
+        if (ferr > 5)
+                netdev_warn(dev, "unable to generate target frequency: %ld Hz\n",
+                                rate);
+
+        if (clk_set_rate(clk, rate_rounded))
+                netdev_err(dev, "adjusting tx_clk failed.\n");
+}
+
 static void macb_handle_link_change(struct net_device *dev)
 {
         struct macb *bp = netdev_priv(dev);
@@ -250,6 +292,9 @@ static void macb_handle_link_change(struct net_device *dev)
 
         spin_unlock_irqrestore(&bp->lock, flags);
 
+        if (!IS_ERR(bp->tx_clk))
+                macb_set_tx_clk(bp->tx_clk, phydev->speed, dev);
+
         if (status_change) {
                 if (phydev->link) {
                         netif_carrier_on(dev);
@@ -1790,21 +1835,44 @@ static int __init macb_probe(struct platform_device *pdev)
         spin_lock_init(&bp->lock);
         INIT_WORK(&bp->tx_error_task, macb_tx_error_task);
 
-        bp->pclk = clk_get(&pdev->dev, "pclk");
+        bp->pclk = devm_clk_get(&pdev->dev, "pclk");
         if (IS_ERR(bp->pclk)) {
-                dev_err(&pdev->dev, "failed to get macb_clk\n");
+                err = PTR_ERR(bp->pclk);
+                dev_err(&pdev->dev, "failed to get macb_clk (%u)\n", err);
                 goto err_out_free_dev;
         }
-        clk_prepare_enable(bp->pclk);
 
-        bp->hclk = clk_get(&pdev->dev, "hclk");
+        bp->hclk = devm_clk_get(&pdev->dev, "hclk");
         if (IS_ERR(bp->hclk)) {
-                dev_err(&pdev->dev, "failed to get hclk\n");
-                goto err_out_put_pclk;
+                err = PTR_ERR(bp->hclk);
+                dev_err(&pdev->dev, "failed to get hclk (%u)\n", err);
+                goto err_out_free_dev;
         }
-        clk_prepare_enable(bp->hclk);
 
-        bp->regs = ioremap(regs->start, resource_size(regs));
+        bp->tx_clk = devm_clk_get(&pdev->dev, "tx_clk");
+
+        err = clk_prepare_enable(bp->pclk);
+        if (err) {
+                dev_err(&pdev->dev, "failed to enable pclk (%u)\n", err);
+                goto err_out_free_dev;
+        }
+
+        err = clk_prepare_enable(bp->hclk);
+        if (err) {
+                dev_err(&pdev->dev, "failed to enable hclk (%u)\n", err);
+                goto err_out_disable_pclk;
+        }
+
+        if (!IS_ERR(bp->tx_clk)) {
+                err = clk_prepare_enable(bp->tx_clk);
+                if (err) {
+                        dev_err(&pdev->dev, "failed to enable tx_clk (%u)\n",
+                                        err);
+                        goto err_out_disable_hclk;
+                }
+        }
+
+        bp->regs = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
         if (!bp->regs) {
                 dev_err(&pdev->dev, "failed to map registers, aborting.\n");
                 err = -ENOMEM;
@@ -1812,11 +1880,12 @@ static int __init macb_probe(struct platform_device *pdev)
         }
 
         dev->irq = platform_get_irq(pdev, 0);
-        err = request_irq(dev->irq, macb_interrupt, 0, dev->name, dev);
+        err = devm_request_irq(&pdev->dev, dev->irq, macb_interrupt, 0,
+                        dev->name, dev);
         if (err) {
                 dev_err(&pdev->dev, "Unable to request IRQ %d (error %d)\n",
                         dev->irq, err);
-                goto err_out_iounmap;
+                goto err_out_disable_clocks;
         }
 
         dev->netdev_ops = &macb_netdev_ops;
@@ -1879,7 +1948,7 @@ static int __init macb_probe(struct platform_device *pdev)
         err = register_netdev(dev);
         if (err) {
                 dev_err(&pdev->dev, "Cannot register net device, aborting.\n");
-                goto err_out_free_irq;
+                goto err_out_disable_clocks;
         }
 
         err = macb_mii_init(bp);
@@ -1902,16 +1971,13 @@ static int __init macb_probe(struct platform_device *pdev)
 
 err_out_unregister_netdev:
         unregister_netdev(dev);
-err_out_free_irq:
-        free_irq(dev->irq, dev);
-err_out_iounmap:
-        iounmap(bp->regs);
 err_out_disable_clocks:
+        if (!IS_ERR(bp->tx_clk))
+                clk_disable_unprepare(bp->tx_clk);
+err_out_disable_hclk:
         clk_disable_unprepare(bp->hclk);
-        clk_put(bp->hclk);
+err_out_disable_pclk:
         clk_disable_unprepare(bp->pclk);
-err_out_put_pclk:
-        clk_put(bp->pclk);
 err_out_free_dev:
         free_netdev(dev);
 err_out:
@@ -1933,12 +1999,10 @@ static int __exit macb_remove(struct platform_device *pdev)
                 kfree(bp->mii_bus->irq);
                 mdiobus_free(bp->mii_bus);
                 unregister_netdev(dev);
-                free_irq(dev->irq, dev);
-                iounmap(bp->regs);
+                if (!IS_ERR(bp->tx_clk))
+                        clk_disable_unprepare(bp->tx_clk);
                 clk_disable_unprepare(bp->hclk);
-                clk_put(bp->hclk);
                 clk_disable_unprepare(bp->pclk);
-                clk_put(bp->pclk);
                 free_netdev(dev);
         }
 
@@ -1946,45 +2010,49 @@ static int __exit macb_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_PM
-static int macb_suspend(struct platform_device *pdev, pm_message_t state)
+static int macb_suspend(struct device *dev)
 {
+        struct platform_device *pdev = to_platform_device(dev);
         struct net_device *netdev = platform_get_drvdata(pdev);
         struct macb *bp = netdev_priv(netdev);
 
         netif_carrier_off(netdev);
         netif_device_detach(netdev);
 
+        if (!IS_ERR(bp->tx_clk))
+                clk_disable_unprepare(bp->tx_clk);
         clk_disable_unprepare(bp->hclk);
         clk_disable_unprepare(bp->pclk);
 
         return 0;
 }
 
-static int macb_resume(struct platform_device *pdev)
+static int macb_resume(struct device *dev)
 {
+        struct platform_device *pdev = to_platform_device(dev);
         struct net_device *netdev = platform_get_drvdata(pdev);
         struct macb *bp = netdev_priv(netdev);
 
         clk_prepare_enable(bp->pclk);
         clk_prepare_enable(bp->hclk);
+        if (!IS_ERR(bp->tx_clk))
+                clk_prepare_enable(bp->tx_clk);
 
         netif_device_attach(netdev);
 
         return 0;
 }
-#else
-#define macb_suspend    NULL
-#define macb_resume     NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(macb_pm_ops, macb_suspend, macb_resume);
+
 static struct platform_driver macb_driver = {
         .remove         = __exit_p(macb_remove),
-        .suspend        = macb_suspend,
-        .resume         = macb_resume,
         .driver         = {
                 .name           = "macb",
                 .owner  = THIS_MODULE,
                 .of_match_table = of_match_ptr(macb_dt_ids),
+                .pm     = &macb_pm_ops,
         },
 };
 
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index f4076155bed7..51c02442160a 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -572,6 +572,7 @@ struct macb {
         struct platform_device  *pdev;
         struct clk              *pclk;
         struct clk              *hclk;
+        struct clk              *tx_clk;
         struct net_device       *dev;
         struct napi_struct      napi;
         struct work_struct      tx_error_task;