[ARM] MXC: Use a single function for decoding a PLL

We had 3 versions of this function in clock support for MX1/2/3
Use a single one instead. I picked the one from the MX3 as it seems
to calculate more accurate as the other ones. Also, on MX27 and MX31 mfn
can be negative, this hasn't been handled correctly on MX27 since now.

This patch has been tested on MX27 and MX31 and produces the same clock
frequencies for me.

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

5 files changed, 57 insertions, 88 deletions

diff --git a/arch/arm/mach-mx1/clock.c b/arch/arm/mach-mx1/clock.c
index 4bcd1ece55f..3c464331b87 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 c69896d011a..047e71e6ea9 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 b1746aae1f8..8486ea46d6c 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 0a38f0b396e..888dd33abf7 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 d21f78e7881..b830655514e 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__ */