omap2 clock: omap2 clock.c: Consolidate wait-for-lock code

omap2 clock.c contains two nearly identical wait-for-clock-enable routines.
Consolidate them into one omap2_wait_clock_ready() function.

Signed-off-by: Paul Walmsley <paul@pwsan.com>
Signed-off-by: Tony Lindgren <tony@atomide.com>

diff --git a/arch/arm/mach-omap2/clock.c b/arch/arm/mach-omap2/clock.c
index a3d0f1222fca0d89c7e62d14ccb9aa7d3147fc64..ede0dd5e9450b4098127ad12e801b7e463492423 100644 (file)
--- a/arch/arm/mach-omap2/clock.c
+++ b/arch/arm/mach-omap2/clock.c
@@ -42,6 +42,8 @@
 #define CLKSEL_VLYNQ_96MHZ             0
 #define CLKSEL_VLYNQ_CORECLK_16                0x10
 
+#define MAX_PLL_LOCK_WAIT              100000
+
 //#define DOWN_VARIABLE_DPLL 1                 /* Experimental */
 
 static struct prcm_config *curr_prcm_set;
@@ -117,10 +119,37 @@ static void omap2_set_osc_ck(int enable)
                              OMAP24XX_PRCM_CLKSRC_CTRL);
 }
 
+/*
+ * omap2_wait_clock_ready - wait for PLL to lock
+ *
+ * Returns 1 if the PLL locked, 0 if it failed to lock.
+ */
+static int omap2_wait_clock_ready(void __iomem *reg, u32 cval, const char *name)
+{
+       int i = 0;
+
+       /* Wait for lock */
+       while (!(cm_read_reg(reg) & cval)) {
+               ++i;
+               udelay(1);
+               if (i == MAX_PLL_LOCK_WAIT) {
+                       printk(KERN_ERR "Clock %s didn't lock in %d tries\n",
+                              name, MAX_PLL_LOCK_WAIT);
+                       break;
+               }
+       }
+
+       if (i)
+               pr_debug("Clock %s stable after %d loops\n", name, i);
+
+       return (i < MAX_PLL_LOCK_WAIT) ? 1 : 0;
+};
+
+
 /* Enable an APLL if off */
 static void omap2_clk_fixed_enable(struct clk *clk)
 {
-       u32 cval, i=0;
+       u32 cval;
 
        cval = cm_read_mod_reg(PLL_MOD, CM_CLKEN);
 
@@ -136,35 +165,27 @@ static void omap2_clk_fixed_enable(struct clk *clk)
        else if (clk == &apll54_ck)
                cval = OMAP24XX_ST_54M_CLK;
 
-       /* Wait for lock */
-       while (!(cm_read_mod_reg(PLL_MOD, CM_IDLEST) & cval)) {
-               ++i;
-               udelay(1);
-               if (i == 100000) {
-                       printk(KERN_ERR "Clock %s didn't lock\n", clk->name);
-                       break;
-               }
-       }
+       omap2_wait_clock_ready(OMAP_CM_REGADDR(PLL_MOD, CM_IDLEST), cval,
+                           clk->name);
 }
 
 static void omap2_clk_wait_ready(struct clk *clk)
 {
-       unsigned long reg, other_reg, st_reg;
+       void __iomem *reg, *other_reg, *st_reg;
        u32 bit;
-       int i;
-
-       reg = (unsigned long) clk->enable_reg;
-       if (reg == (unsigned long)OMAP_CM_REGADDR(CORE_MOD, CM_FCLKEN1) ||
-           reg == (unsigned long)OMAP_CM_REGADDR(CORE_MOD, OMAP24XX_CM_FCLKEN2))
-               other_reg = (reg & ~0xf0) | 0x10; /* CM_ICLKEN* */
-       else if (reg == (unsigned long)OMAP_CM_REGADDR(CORE_MOD, CM_ICLKEN1) ||
-                reg == (unsigned long)OMAP_CM_REGADDR(CORE_MOD, CM_ICLKEN2))
-               other_reg = (reg & ~0xf0) | 0x00; /* CM_FCLKEN* */
+
+       reg = clk->enable_reg;
+       if (reg == OMAP_CM_REGADDR(CORE_MOD, CM_FCLKEN1) ||
+           reg == OMAP_CM_REGADDR(CORE_MOD, OMAP24XX_CM_FCLKEN2))
+               other_reg = (void __iomem *)(((u32)reg & ~0xf0) | 0x10); /* CM_ICLKEN* */
+       else if (reg == OMAP_CM_REGADDR(CORE_MOD, CM_ICLKEN1) ||
+                reg == OMAP_CM_REGADDR(CORE_MOD, CM_ICLKEN2))
+               other_reg = (void __iomem *)(((u32)reg & ~0xf0) | 0x00); /* CM_FCLKEN* */
        else
                return;
 
        /* No check for DSS or cam clocks */
-       if ((reg & 0x0f) == 0) {
+       if (((u32)reg & 0x0f) == 0) { /* CM_{F,I}CLKEN1 */
                if (clk->enable_bit <= 1 || clk->enable_bit == 31)
                        return;
        }
@@ -172,19 +193,11 @@ static void omap2_clk_wait_ready(struct clk *clk)
        /* Check if both functional and interface clocks
         * are running. */
        bit = 1 << clk->enable_bit;
-       if (!(cm_read_reg((void __iomem *)other_reg) & bit))
+       if (!(cm_read_reg(other_reg) & bit))
                return;
-       st_reg = (other_reg & ~0xf0) | 0x20; /* CM_IDLEST* */
-       i = 0;
-       while (!(cm_read_reg((void __iomem *)st_reg) & bit)) {
-               i++;
-               if (i == 100000) {
-                       printk(KERN_ERR "Timeout enabling clock %s\n", clk->name);
-                       break;
-               }
-       }
-       if (i)
-               pr_debug("Clock %s stable after %d loops\n", clk->name, i);
+       st_reg = (void __iomem *)(((u32)other_reg & ~0xf0) | 0x20); /* CM_IDLEST* */
+
+       omap2_wait_clock_ready(st_reg, bit, clk->name);
 }
 
 /* Enables clock without considering parent dependencies or use count