2 files changed, 56 insertions, 39 deletions

diff --git a/arch/arm/mach-imx/clk-pllv2.c b/arch/arm/mach-imx/clk-pllv2.c
index 4685919deb63..0440379e3628 100644
--- a/arch/arm/mach-imx/clk-pllv2.c
+++ b/arch/arm/mach-imx/clk-pllv2.c
@@ -74,30 +74,15 @@ struct clk_pllv2 {
         void __iomem    *base;
 };
 
-static unsigned long clk_pllv2_recalc_rate(struct clk_hw *hw,
-                unsigned long parent_rate)
+static unsigned long __clk_pllv2_recalc_rate(unsigned long parent_rate,
+                u32 dp_ctl, u32 dp_op, u32 dp_mfd, u32 dp_mfn)
 {
         long mfi, mfn, mfd, pdf, ref_clk, mfn_abs;
-        unsigned long dp_op, dp_mfd, dp_mfn, dp_ctl, pll_hfsm, dbl;
-        void __iomem *pllbase;
+        unsigned long dbl;
         s64 temp;
-        struct clk_pllv2 *pll = to_clk_pllv2(hw);
-
-        pllbase = pll->base;
 
-        dp_ctl = __raw_readl(pllbase + MXC_PLL_DP_CTL);
-        pll_hfsm = dp_ctl & MXC_PLL_DP_CTL_HFSM;
         dbl = dp_ctl & MXC_PLL_DP_CTL_DPDCK0_2_EN;
 
-        if (pll_hfsm == 0) {
-                dp_op = __raw_readl(pllbase + MXC_PLL_DP_OP);
-                dp_mfd = __raw_readl(pllbase + MXC_PLL_DP_MFD);
-                dp_mfn = __raw_readl(pllbase + MXC_PLL_DP_MFN);
-        } else {
-                dp_op = __raw_readl(pllbase + MXC_PLL_DP_HFS_OP);
-                dp_mfd = __raw_readl(pllbase + MXC_PLL_DP_HFS_MFD);
-                dp_mfn = __raw_readl(pllbase + MXC_PLL_DP_HFS_MFN);
-        }
         pdf = dp_op & MXC_PLL_DP_OP_PDF_MASK;
         mfi = (dp_op & MXC_PLL_DP_OP_MFI_MASK) >> MXC_PLL_DP_OP_MFI_OFFSET;
         mfi = (mfi <= 5) ? 5 : mfi;
@@ -123,18 +108,30 @@ static unsigned long clk_pllv2_recalc_rate(struct clk_hw *hw,
         return temp;
 }
 
-static int clk_pllv2_set_rate(struct clk_hw *hw, unsigned long rate,
+static unsigned long clk_pllv2_recalc_rate(struct clk_hw *hw,
                 unsigned long parent_rate)
 {
+        u32 dp_op, dp_mfd, dp_mfn, dp_ctl;
+        void __iomem *pllbase;
         struct clk_pllv2 *pll = to_clk_pllv2(hw);
+
+        pllbase = pll->base;
+
+        dp_ctl = __raw_readl(pllbase + MXC_PLL_DP_CTL);
+        dp_op = __raw_readl(pllbase + MXC_PLL_DP_OP);
+        dp_mfd = __raw_readl(pllbase + MXC_PLL_DP_MFD);
+        dp_mfn = __raw_readl(pllbase + MXC_PLL_DP_MFN);
+
+        return __clk_pllv2_recalc_rate(parent_rate, dp_ctl, dp_op, dp_mfd, dp_mfn);
+}
+
+static int __clk_pllv2_set_rate(unsigned long rate, unsigned long parent_rate,
+                u32 *dp_op, u32 *dp_mfd, u32 *dp_mfn)
+{
         u32 reg;
-        void __iomem *pllbase;
         long mfi, pdf, mfn, mfd = 999999;
         s64 temp64;
         unsigned long quad_parent_rate;
-        unsigned long pll_hfsm, dp_ctl;
-
-        pllbase = pll->base;
 
         quad_parent_rate = 4 * parent_rate;
         pdf = mfi = -1;
@@ -144,25 +141,41 @@ static int clk_pllv2_set_rate(struct clk_hw *hw, unsigned long rate,
                 return -EINVAL;
         pdf--;
 
-        temp64 = rate * (pdf+1) - quad_parent_rate * mfi;
-        do_div(temp64, quad_parent_rate/1000000);
+        temp64 = rate * (pdf + 1) - quad_parent_rate * mfi;
+        do_div(temp64, quad_parent_rate / 1000000);
         mfn = (long)temp64;
 
+        reg = mfi << 4 | pdf;
+
+        *dp_op = reg;
+        *dp_mfd = mfd;
+        *dp_mfn = mfn;
+
+        return 0;
+}
+
+static int clk_pllv2_set_rate(struct clk_hw *hw, unsigned long rate,
+                unsigned long parent_rate)
+{
+        struct clk_pllv2 *pll = to_clk_pllv2(hw);
+        void __iomem *pllbase;
+        u32 dp_ctl, dp_op, dp_mfd, dp_mfn;
+        int ret;
+
+        pllbase = pll->base;
+
+
+        ret = __clk_pllv2_set_rate(rate, parent_rate, &dp_op, &dp_mfd, &dp_mfn);
+        if (ret)
+                return ret;
+
         dp_ctl = __raw_readl(pllbase + MXC_PLL_DP_CTL);
         /* use dpdck0_2 */
         __raw_writel(dp_ctl | 0x1000L, pllbase + MXC_PLL_DP_CTL);
-        pll_hfsm = dp_ctl & MXC_PLL_DP_CTL_HFSM;
-        if (pll_hfsm == 0) {
-                reg = mfi << 4 | pdf;
-                __raw_writel(reg, pllbase + MXC_PLL_DP_OP);
-                __raw_writel(mfd, pllbase + MXC_PLL_DP_MFD);
-                __raw_writel(mfn, pllbase + MXC_PLL_DP_MFN);
-        } else {
-                reg = mfi << 4 | pdf;
-                __raw_writel(reg, pllbase + MXC_PLL_DP_HFS_OP);
-                __raw_writel(mfd, pllbase + MXC_PLL_DP_HFS_MFD);
-                __raw_writel(mfn, pllbase + MXC_PLL_DP_HFS_MFN);
-        }
+
+        __raw_writel(dp_op, pllbase + MXC_PLL_DP_OP);
+        __raw_writel(dp_mfd, pllbase + MXC_PLL_DP_MFD);
+        __raw_writel(dp_mfn, pllbase + MXC_PLL_DP_MFN);
 
         return 0;
 }
@@ -170,7 +183,11 @@ static int clk_pllv2_set_rate(struct clk_hw *hw, unsigned long rate,
 static long clk_pllv2_round_rate(struct clk_hw *hw, unsigned long rate,
                 unsigned long *prate)
 {
-        return rate;
+        u32 dp_op, dp_mfd, dp_mfn;
+
+        __clk_pllv2_set_rate(rate, *prate, &dp_op, &dp_mfd, &dp_mfn);
+        return __clk_pllv2_recalc_rate(*prate, MXC_PLL_DP_CTL_DPDCK0_2_EN,
+                        dp_op, dp_mfd, dp_mfn);
 }
 
 static int clk_pllv2_prepare(struct clk_hw *hw)
diff --git a/arch/arm/mach-imx/crm-regs-imx5.h b/arch/arm/mach-imx/crm-regs-imx5.h
index 5e11ba7daee2..5e3f1f0f4cab 100644
--- a/arch/arm/mach-imx/crm-regs-imx5.h
+++ b/arch/arm/mach-imx/crm-regs-imx5.h
@@ -23,7 +23,7 @@
 #define MX53_DPLL1_BASE         MX53_IO_ADDRESS(MX53_PLL1_BASE_ADDR)
 #define MX53_DPLL2_BASE         MX53_IO_ADDRESS(MX53_PLL2_BASE_ADDR)
 #define MX53_DPLL3_BASE         MX53_IO_ADDRESS(MX53_PLL3_BASE_ADDR)
-#define MX53_DPLL4_BASE         MX53_IO_ADDRESS(MX53_PLL3_BASE_ADDR)
+#define MX53_DPLL4_BASE         MX53_IO_ADDRESS(MX53_PLL4_BASE_ADDR)
 
 /* PLL Register Offsets */
 #define MXC_PLL_DP_CTL                  0x00