1 files changed, 44 insertions, 6 deletions

diff --git a/drivers/tty/serial/mpc52xx_uart.c b/drivers/tty/serial/mpc52xx_uart.c
index 6345f377a246..97888f4900ec 100644
--- a/drivers/tty/serial/mpc52xx_uart.c
+++ b/drivers/tty/serial/mpc52xx_uart.c
@@ -421,6 +421,7 @@ struct psc_fifoc {
 
 static struct psc_fifoc __iomem *psc_fifoc;
 static unsigned int psc_fifoc_irq;
+static struct clk *psc_fifoc_clk;
 
 static void mpc512x_psc_fifo_init(struct uart_port *port)
 {
@@ -568,36 +569,73 @@ static unsigned int mpc512x_psc_set_baudrate(struct uart_port *port,
 /* Init PSC FIFO Controller */
 static int __init mpc512x_psc_fifoc_init(void)
 {
+        int err;
         struct device_node *np;
+        struct clk *clk;
+
+        /* default error code, potentially overwritten by clock calls */
+        err = -ENODEV;
 
         np = of_find_compatible_node(NULL, NULL,
                                      "fsl,mpc5121-psc-fifo");
         if (!np) {
                 pr_err("%s: Can't find FIFOC node\n", __func__);
-                return -ENODEV;
+                goto out_err;
+        }
+
+        clk = of_clk_get(np, 0);
+        if (IS_ERR(clk)) {
+                /* backwards compat with device trees that lack clock specs */
+                clk = clk_get_sys(np->name, "ipg");
+        }
+        if (IS_ERR(clk)) {
+                pr_err("%s: Can't lookup FIFO clock\n", __func__);
+                err = PTR_ERR(clk);
+                goto out_ofnode_put;
+        }
+        if (clk_prepare_enable(clk)) {
+                pr_err("%s: Can't enable FIFO clock\n", __func__);
+                clk_put(clk);
+                goto out_ofnode_put;
         }
+        psc_fifoc_clk = clk;
 
         psc_fifoc = of_iomap(np, 0);
         if (!psc_fifoc) {
                 pr_err("%s: Can't map FIFOC\n", __func__);
-                of_node_put(np);
-                return -ENODEV;
+                goto out_clk_disable;
         }
 
         psc_fifoc_irq = irq_of_parse_and_map(np, 0);
-        of_node_put(np);
         if (psc_fifoc_irq == 0) {
                 pr_err("%s: Can't get FIFOC irq\n", __func__);
-                iounmap(psc_fifoc);
-                return -ENODEV;
+                goto out_unmap;
         }
 
+        of_node_put(np);
         return 0;
+
+out_unmap:
+        iounmap(psc_fifoc);
+out_clk_disable:
+        clk_disable_unprepare(psc_fifoc_clk);
+        clk_put(psc_fifoc_clk);
+out_ofnode_put:
+        of_node_put(np);
+out_err:
+        return err;
 }
 
 static void __exit mpc512x_psc_fifoc_uninit(void)
 {
         iounmap(psc_fifoc);
+
+        /* disable the clock, errors are not fatal */
+        if (psc_fifoc_clk) {
+                clk_disable_unprepare(psc_fifoc_clk);
+                clk_put(psc_fifoc_clk);
+                psc_fifoc_clk = NULL;
+        }
 }
 
 /* 512x specific interrupt handler. The caller holds the port lock */