diff --git a/arch/arm/mach-imx/clk-pllv3.c b/arch/arm/mach-imx/clk-pllv3.c index c9ca19184420..df1736232961 100644 --- a/arch/arm/mach-imx/clk-pllv3.c +++ b/arch/arm/mach-imx/clk-pllv3.c @@ -46,10 +46,30 @@ struct clk_pllv3 { #define to_clk_pllv3(_hw) container_of(_hw, struct clk_pllv3, hw) +static int clk_pllv3_wait_lock(struct clk_pllv3 *pll) +{ + unsigned long timeout = jiffies + msecs_to_jiffies(10); + u32 val = readl_relaxed(pll->base) & BM_PLL_POWER; + + /* No need to wait for lock when pll is not powered up */ + if ((pll->powerup_set && !val) || (!pll->powerup_set && val)) + return 0; + + /* Wait for PLL to lock */ + do { + if (readl_relaxed(pll->base) & BM_PLL_LOCK) + break; + if (time_after(jiffies, timeout)) + break; + usleep_range(50, 500); + } while (1); + + return readl_relaxed(pll->base) & BM_PLL_LOCK ? 0 : -ETIMEDOUT; +} + static int clk_pllv3_prepare(struct clk_hw *hw) { struct clk_pllv3 *pll = to_clk_pllv3(hw); - unsigned long timeout; u32 val; val = readl_relaxed(pll->base); @@ -60,20 +80,7 @@ static int clk_pllv3_prepare(struct clk_hw *hw) val &= ~BM_PLL_POWER; writel_relaxed(val, pll->base); - timeout = jiffies + msecs_to_jiffies(10); - /* Wait for PLL to lock */ - do { - if (readl_relaxed(pll->base) & BM_PLL_LOCK) - break; - if (time_after(jiffies, timeout)) - break; - usleep_range(50, 500); - } while (1); - - if (readl_relaxed(pll->base) & BM_PLL_LOCK) - return 0; - else - return -ETIMEDOUT; + return clk_pllv3_wait_lock(pll); } static void clk_pllv3_unprepare(struct clk_hw *hw) @@ -148,7 +155,7 @@ static int clk_pllv3_set_rate(struct clk_hw *hw, unsigned long rate, val |= div; writel_relaxed(val, pll->base); - return 0; + return clk_pllv3_wait_lock(pll); } static const struct clk_ops clk_pllv3_ops = { @@ -204,7 +211,7 @@ static int clk_pllv3_sys_set_rate(struct clk_hw *hw, unsigned long rate, val |= div; writel_relaxed(val, pll->base); - return 0; + return clk_pllv3_wait_lock(pll); } static const struct clk_ops clk_pllv3_sys_ops = { @@ -278,7 +285,7 @@ static int clk_pllv3_av_set_rate(struct clk_hw *hw, unsigned long rate, writel_relaxed(mfn, pll->base + PLL_NUM_OFFSET); writel_relaxed(mfd, pll->base + PLL_DENOM_OFFSET); - return 0; + return clk_pllv3_wait_lock(pll); } static const struct clk_ops clk_pllv3_av_ops = {