[RFC] ARM: imx clk: Implement pllv1 set_rate

[Alexander Shiyan]

This patch implements frequency change for i.MX clk v1 PLL.
Selection of the values of the registers is not optimal, since is
made by simple enumeration of all possible values.
Comments are welcome.

Signed-off-by: Alexander Shiyan <shc_work at mail.ru>
---
 arch/arm/mach-imx/clk-imx27.c |   2 +-
 arch/arm/mach-imx/clk-pllv1.c | 109 ++++++++++++++++++++++++++++++++++--------
 arch/arm/mach-imx/mx27.h      |   4 +-
 3 files changed, 94 insertions(+), 21 deletions(-)

diff --git a/arch/arm/mach-imx/clk-imx27.c b/arch/arm/mach-imx/clk-imx27.c
index c3cfa41..2b363d9 100644
--- a/arch/arm/mach-imx/clk-imx27.c
+++ b/arch/arm/mach-imx/clk-imx27.c
@@ -125,7 +125,7 @@ int __init mx27_clocks_init(unsigned long fref)
 	clk[vpu_sel] = imx_clk_mux("vpu_sel", CCM_CSCR, 21, 1, vpu_sel_clks, ARRAY_SIZE(vpu_sel_clks));
 	clk[vpu_div] = imx_clk_divider("vpu_div", "vpu_sel", CCM_PCDR0, 10, 6);
 	clk[usb_div] = imx_clk_divider("usb_div", "spll_gate", CCM_CSCR, 28, 3);
-	clk[cpu_sel] = imx_clk_mux("cpu_sel", CCM_CSCR, 15, 1, cpu_sel_clks, ARRAY_SIZE(cpu_sel_clks));
+	clk[cpu_sel] = imx_clk_mux_flags("cpu_sel", CCM_CSCR, 15, 1, cpu_sel_clks, ARRAY_SIZE(cpu_sel_clks), CLK_SET_RATE_PARENT);
 	clk[clko_sel] = imx_clk_mux("clko_sel", CCM_CCSR, 0, 5, clko_sel_clks, ARRAY_SIZE(clko_sel_clks));
 	if (mx27_revision() >= IMX_CHIP_REVISION_2_0)
 		clk[cpu_div] = imx_clk_divider("cpu_div", "cpu_sel", CCM_CSCR, 12, 2);
diff --git a/arch/arm/mach-imx/clk-pllv1.c b/arch/arm/mach-imx/clk-pllv1.c
index c1eaee3..1a80abf 100644
--- a/arch/arm/mach-imx/clk-pllv1.c
+++ b/arch/arm/mach-imx/clk-pllv1.c
@@ -9,6 +9,8 @@
 #include "common.h"
 #include "hardware.h"
 
+#include "mx27.h"
+
 /**
  * pll v1
  *
@@ -25,17 +27,13 @@ struct clk_pllv1 {
 
 #define to_clk_pllv1(clk) (container_of(clk, struct clk_pllv1, clk))
 
-static unsigned long clk_pllv1_recalc_rate(struct clk_hw *hw,
-		unsigned long parent_rate)
+static unsigned long _clk_pllv1_recalc_rate(unsigned int mfi, unsigned int mfn,
+					    unsigned int mfd, unsigned int pd,
+					    unsigned long parent_rate)
 {
-	struct clk_pllv1 *pll = to_clk_pllv1(hw);
-	long long ll;
-	int mfn_abs;
-	unsigned int mfi, mfn, mfd, pd;
-	u32 reg;
 	unsigned long rate;
-
-	reg = readl(pll->base);
+	int mfn_abs = mfn;
+	long long ll;
 
 	/*
 	 * Get the resulting clock rate from a PLL register value and the input
@@ -47,15 +45,6 @@ static unsigned long clk_pllv1_recalc_rate(struct clk_hw *hw,
 	 *                        pd + 1
 	 */
 
-	mfi = (reg >> 10) & 0xf;
-	mfn = reg & 0x3ff;
-	mfd = (reg >> 16) & 0x3ff;
-	pd =  (reg >> 26) & 0xf;
-
-	mfi = mfi <= 5 ? 5 : mfi;
-
-	mfn_abs = mfn;
-
 	/*
 	 * On all i.MXs except i.MX1 and i.MX21 mfn is a 10bit
 	 * 2's complements number
@@ -78,8 +67,90 @@ static unsigned long clk_pllv1_recalc_rate(struct clk_hw *hw,
 	return ll;
 }
 
+static unsigned long clk_pllv1_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_pllv1 *pll = to_clk_pllv1(hw);
+	unsigned int mfi, mfn, mfd, pd;
+	u32 reg = readl(pll->base);
+
+	/* Avoid i.MX27 TO2 SPCTL0 bug */
+	if (cpu_is_mx27() && (mx27_revision() == IMX_CHIP_REVISION_2_0))
+		writel(reg, pll->base);
+
+	mfi = (reg >> 10) & 0xf;
+	mfn = reg & 0x3ff;
+	mfd = (reg >> 16) & 0x3ff;
+	pd =  (reg >> 26) & 0xf;
+
+	mfi = mfi <= 5 ? 5 : mfi;
+
+	return _clk_pllv1_recalc_rate(mfi, mfn, mfd, pd, parent_rate);
+}
+
+static void _clk_pllv1_set_rate(unsigned long rate, unsigned long parent_rate,
+				unsigned int *reg)
+{
+	unsigned int pd, mfi, mfn, mfd;
+	long res, best = 0;
+
+	pd = (parent_rate * 2 * 5) / rate;
+	if (pd > 15)
+		pd = 15;
+	mfi = rate / (parent_rate * 2 * (pd + 1));
+	if (mfi < 5)
+		mfi = 5;
+	else if (mfi > 15)
+		mfi = 15;
+
+	for (mfd = 1; mfd < 0x400; mfd++)
+		for (mfn = 0; mfn < mfd; mfn++) {
+			res = _clk_pllv1_recalc_rate(mfi, mfn, mfd, pd,
+						     parent_rate);
+			if (!best || (abs(rate - res) < abs(rate - best))) {
+				*reg = mfn;
+				*reg |= mfi << 10;
+				*reg |= mfd << 16;
+				*reg |= pd << 26;
+				if (res == rate)
+					return;
+				best = res;
+			}
+		}
+}
+
+static int clk_pllv1_set_rate(struct clk_hw *hw, unsigned long rate,
+			      unsigned long parent_rate)
+{
+	struct clk_pllv1 *pll = to_clk_pllv1(hw);
+	u32 reg;
+
+	_clk_pllv1_set_rate(rate, parent_rate, &reg);
+	writel(reg, pll->base);
+
+	return 0;
+}
+
+static long clk_pllv1_round_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long *prate)
+{
+	unsigned int pd, mfi, mfn, mfd;
+	u32 reg;
+
+	_clk_pllv1_set_rate(rate, *prate, &reg);
+
+	mfi = (reg >> 10) & 0xf;
+	mfn = reg & 0x3ff;
+	mfd = (reg >> 16) & 0x3ff;
+	pd =  (reg >> 26) & 0xf;
+
+	return _clk_pllv1_recalc_rate(mfi, mfn, mfd, pd, *prate);
+}
+
 static struct clk_ops clk_pllv1_ops = {
-	.recalc_rate = clk_pllv1_recalc_rate,
+	.recalc_rate	= clk_pllv1_recalc_rate,
+	.round_rate	= clk_pllv1_round_rate,
+	.set_rate	= clk_pllv1_set_rate,
 };
 
 struct clk *imx_clk_pllv1(const char *name, const char *parent,
diff --git a/arch/arm/mach-imx/mx27.h b/arch/arm/mach-imx/mx27.h
index 8a65f19..6835e23 100644
--- a/arch/arm/mach-imx/mx27.h
+++ b/arch/arm/mach-imx/mx27.h
@@ -231,8 +231,10 @@
 #define MX27_DMA_REQ_SDHC3	36
 #define MX27_DMA_REQ_NFC	37
 
-#ifndef __ASSEMBLY__
+#ifdef CONFIG_SOC_IMX27
 extern int mx27_revision(void);
+#else
+static int mx27_revision(void) { }
 #endif
 
 #endif /* ifndef __MACH_MX27_H__ */
-- 
1.8.1.5