5 files changed, 57 insertions, 88 deletions

diff --git a/arch/arm/mach-mx1/clock.c b/arch/arm/mach-mx1/clock.c
index 4bcd1ece55f5..3c464331b870 100644
--- a/arch/arm/mach-mx1/clock.c
+++ b/arch/arm/mach-mx1/clock.c
@@ -87,33 +87,6 @@ static int _clk_parent_set_rate(struct clk *clk, unsigned long rate)
         return clk->parent->set_rate(clk->parent, rate);
 }
 
-/*
- *  get the system pll clock in Hz
- *
- *                  mfi + mfn / (mfd +1)
- *  f = 2 * f_ref * --------------------
- *                        pd + 1
- */
-static unsigned long mx1_decode_pll(unsigned int pll, u32 f_ref)
-{
-        unsigned long long ll;
-        unsigned long quot;
-
-        u32 mfi = (pll >> 10) & 0xf;
-        u32 mfn = pll & 0x3ff;
-        u32 mfd = (pll >> 16) & 0x3ff;
-        u32 pd =  (pll >> 26) & 0xf;
-
-        mfi = mfi <= 5 ? 5 : mfi;
-
-        ll = 2 * (unsigned long long)f_ref *
-                ((mfi << 16) + (mfn << 16) / (mfd + 1));
-        quot = (pd + 1) * (1 << 16);
-        ll += quot / 2;
-        do_div(ll, quot);
-        return (unsigned long)ll;
-}
-
 static unsigned long clk16m_get_rate(struct clk *clk)
 {
         return 16000000;
@@ -188,7 +161,7 @@ static struct clk prem_clk = {
 
 static unsigned long system_clk_get_rate(struct clk *clk)
 {
-        return mx1_decode_pll(__raw_readl(CCM_SPCTL0),
+        return mxc_decode_pll(__raw_readl(CCM_SPCTL0),
                               clk_get_rate(clk->parent));
 }
 
@@ -200,7 +173,7 @@ static struct clk system_clk = {
 
 static unsigned long mcu_clk_get_rate(struct clk *clk)
 {
-        return mx1_decode_pll(__raw_readl(CCM_MPCTL0),
+        return mxc_decode_pll(__raw_readl(CCM_MPCTL0),
                               clk_get_rate(clk->parent));
 }
 
diff --git a/arch/arm/mach-mx2/clock_imx27.c b/arch/arm/mach-mx2/clock_imx27.c
index c69896d011a1..047e71e6ea9a 100644
--- a/arch/arm/mach-mx2/clock_imx27.c
+++ b/arch/arm/mach-mx2/clock_imx27.c
@@ -486,26 +486,8 @@ static struct clk ckil_clk = {
 
 static unsigned long get_mpll_clk(struct clk *clk)
 {
-        uint32_t reg;
-        unsigned long ref_clk;
-        unsigned long mfi = 0, mfn = 0, mfd = 0, pdf = 0;
-        unsigned long long temp;
-
-        ref_clk = clk_get_rate(clk->parent);
-
-        reg = __raw_readl(CCM_MPCTL0);
-        pdf = (reg & CCM_MPCTL0_PD_MASK) >> CCM_MPCTL0_PD_OFFSET;
-        mfd = (reg & CCM_MPCTL0_MFD_MASK) >> CCM_MPCTL0_MFD_OFFSET;
-        mfi = (reg & CCM_MPCTL0_MFI_MASK) >> CCM_MPCTL0_MFI_OFFSET;
-        mfn = (reg & CCM_MPCTL0_MFN_MASK) >> CCM_MPCTL0_MFN_OFFSET;
-
-        mfi = (mfi <= 5) ? 5 : mfi;
-        temp = 2LL * ref_clk * mfn;
-        do_div(temp, mfd + 1);
-        temp = 2LL * ref_clk * mfi + temp;
-        do_div(temp, pdf + 1);
-
-        return (unsigned long)temp;
+        return mxc_decode_pll(__raw_readl(CCM_MPCTL0),
+                        clk_get_rate(clk->parent));
 }
 
 static struct clk mpll_clk = {
@@ -555,28 +537,18 @@ static unsigned long get_spll_clk(struct clk *clk)
 {
         uint32_t reg;
         unsigned long ref_clk;
-        unsigned long mfi = 0, mfn = 0, mfd = 0, pdf = 0;
-        unsigned long long temp;
 
         ref_clk = clk_get_rate(clk->parent);
 
         reg = __raw_readl(CCM_SPCTL0);
-        /*TODO: This is TO2 Bug */
+
+        /* On TO2 we have to write the value back. Otherwise we
+         * read 0 from this register the next time.
+         */
         if (mx27_revision() >= CHIP_REV_2_0)
                 __raw_writel(reg, CCM_SPCTL0);
 
-        pdf = (reg & CCM_SPCTL0_PD_MASK) >> CCM_SPCTL0_PD_OFFSET;
-        mfd = (reg & CCM_SPCTL0_MFD_MASK) >> CCM_SPCTL0_MFD_OFFSET;
-        mfi = (reg & CCM_SPCTL0_MFI_MASK) >> CCM_SPCTL0_MFI_OFFSET;
-        mfn = (reg & CCM_SPCTL0_MFN_MASK) >> CCM_SPCTL0_MFN_OFFSET;
-
-        mfi = (mfi <= 5) ? 5 : mfi;
-        temp = 2LL * ref_clk * mfn;
-        do_div(temp, mfd + 1);
-        temp = 2LL * ref_clk * mfi + temp;
-        do_div(temp, pdf + 1);
-
-        return (unsigned long)temp;
+        return mxc_decode_pll(reg, ref_clk);
 }
 
 static struct clk spll_clk = {
diff --git a/arch/arm/mach-mx3/clock.c b/arch/arm/mach-mx3/clock.c
index b1746aae1f89..8486ea46d6c1 100644
--- a/arch/arm/mach-mx3/clock.c
+++ b/arch/arm/mach-mx3/clock.c
@@ -158,10 +158,8 @@ static int _clk_pll_set_rate(struct clk *clk, unsigned long rate)
 
 static unsigned long _clk_pll_get_rate(struct clk *clk)
 {
-        long mfi, mfn, mfd, pdf, ref_clk, mfn_abs;
         unsigned long reg, ccmr;
-        s64 temp;
-        unsigned int prcs;
+        unsigned int prcs, ref_clk;
 
         ccmr = __raw_readl(MXC_CCM_CCMR);
         prcs = (ccmr & MXC_CCM_CCMR_PRCS_MASK) >> MXC_CCM_CCMR_PRCS_OFFSET;
@@ -185,27 +183,7 @@ static unsigned long _clk_pll_get_rate(struct clk *clk)
                 return 0;
         }
 
-        pdf = (reg & MXC_CCM_PCTL_PD_MASK) >> MXC_CCM_PCTL_PD_OFFSET;
-        mfd = (reg & MXC_CCM_PCTL_MFD_MASK) >> MXC_CCM_PCTL_MFD_OFFSET;
-        mfi = (reg & MXC_CCM_PCTL_MFI_MASK) >> MXC_CCM_PCTL_MFI_OFFSET;
-        mfi = (mfi <= 5) ? 5 : mfi;
-        mfn = mfn_abs = reg & MXC_CCM_PCTL_MFN_MASK;
-
-        if (mfn >= 0x200) {
-                mfn |= 0xFFFFFE00;
-                mfn_abs = -mfn;
-        }
-
-        ref_clk *= 2;
-        ref_clk /= pdf + 1;
-
-        temp = (u64) ref_clk * mfn_abs;
-        do_div(temp, mfd + 1);
-        if (mfn < 0)
-                temp = -temp;
-        temp = (ref_clk * mfi) + temp;
-
-        return temp;
+        return mxc_decode_pll(reg, ref_clk);
 }
 
 static int _clk_usb_pll_enable(struct clk *clk)
diff --git a/arch/arm/plat-mxc/clock.c b/arch/arm/plat-mxc/clock.c
index 0a38f0b396eb..888dd33abf76 100644
--- a/arch/arm/plat-mxc/clock.c
+++ b/arch/arm/plat-mxc/clock.c
@@ -328,3 +328,47 @@ static int __init mxc_setup_proc_entry(void)
 
 late_initcall(mxc_setup_proc_entry);
 #endif
+
+/*
+ * Get the resulting clock rate from a PLL register value and the input
+ * frequency. PLLs with this register layout can at least be found on
+ * MX1, MX21, MX27 and MX31
+ *
+ *                  mfi + mfn / (mfd + 1)
+ *  f = 2 * f_ref * --------------------
+ *                        pd + 1
+ */
+unsigned long mxc_decode_pll(unsigned int reg_val, u32 freq)
+{
+        long long ll;
+        int mfn_abs;
+        unsigned int mfi, mfn, mfd, pd;
+
+        mfi = (reg_val >> 10) & 0xf;
+        mfn = reg_val & 0x3ff;
+        mfd = (reg_val >> 16) & 0x3ff;
+        pd =  (reg_val >> 26) & 0xf;
+
+        mfi = mfi <= 5 ? 5 : mfi;
+
+        mfn_abs = mfn;
+
+#if !defined CONFIG_ARCH_MX1 && !defined CONFIG_ARCH_MX21
+        if (mfn >= 0x200) {
+                mfn |= 0xFFFFFE00;
+                mfn_abs = -mfn;
+        }
+#endif
+
+        freq *= 2;
+        freq /= pd + 1;
+
+        ll = (unsigned long long)freq * mfn_abs;
+
+        do_div(ll, mfd + 1);
+        if (mfn < 0)
+                ll = -ll;
+        ll = (freq * mfi) + ll;
+
+        return ll;
+}
diff --git a/arch/arm/plat-mxc/include/mach/clock.h b/arch/arm/plat-mxc/include/mach/clock.h
index d21f78e78819..b830655514eb 100644
--- a/arch/arm/plat-mxc/include/mach/clock.h
+++ b/arch/arm/plat-mxc/include/mach/clock.h
@@ -63,5 +63,7 @@ struct clk {
 int clk_register(struct clk *clk);
 void clk_unregister(struct clk *clk);
 
+unsigned long mxc_decode_pll(unsigned int pll, u32 f_ref);
+
 #endif /* __ASSEMBLY__ */
 #endif /* __ASM_ARCH_MXC_CLOCK_H__ */