arm/mxs: add cpu clock set functions

Add clock set routines, ported from the kernel.

Signed-off-by: Juergen Beisert <jbe@pengutronix.de>
Signed-off-by: Robert Schwebel <r.schwebel@pengutronix.de>

2 files changed, 104 insertions, 0 deletions

diff --git a/arch/arm/mach-mxs/include/mach/clock-imx23.h b/arch/arm/mach-mxs/include/mach/clock-imx23.h
index 723f3435..f8848269 100644
--- a/arch/arm/mach-mxs/include/mach/clock-imx23.h
+++ b/arch/arm/mach-mxs/include/mach/clock-imx23.h
@@ -17,7 +17,9 @@ unsigned imx_get_mpllclk(void);
 unsigned imx_get_emiclk(void);
 unsigned imx_get_ioclk(void);
 unsigned imx_get_armclk(void);
+unsigned imx_set_armclk(unsigned);
 unsigned imx_get_hclk(void);
+unsigned imx_set_hclk(unsigned);
 unsigned imx_get_xclk(void);
 unsigned imx_get_sspclk(unsigned);
 unsigned imx_set_sspclk(unsigned, unsigned, int);
diff --git a/arch/arm/mach-mxs/speed-imx23.c b/arch/arm/mach-mxs/speed-imx23.c
index a31139d7..a32e17ea 100644
--- a/arch/arm/mach-mxs/speed-imx23.c
+++ b/arch/arm/mach-mxs/speed-imx23.c
@@ -21,6 +21,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  * MA 02111-1307 USA
  */
+#define DEBUG
 
 #include <common.h>
 #include <init.h>
@@ -32,8 +33,12 @@
 #define HW_CLKCTRL_PLLCTRL0 0x000
 #define HW_CLKCTRL_PLLCTRL1 0x010
 #define HW_CLKCTRL_CPU 0x20
+# define CLKCTRL_CPU_BUSY_REF_XTAL (1 << 29)
+# define CLKCTRL_CPU_BUSY_REF_CPU (1 << 28)
 # define GET_CPU_XTAL_DIV(x) (((x) >> 16) & 0x3ff)
+# define SET_CPU_XTAL_DIV(x) (((x) & 0x3ff) << 16)
 # define GET_CPU_PLL_DIV(x) ((x) & 0x3f)
+# define SET_CPU_PLL_DIV(x) ((x) & 0x3f)
 #define HW_CLKCTRL_HBUS 0x30
 #define HW_CLKCTRL_XBUS 0x40
 #define HW_CLKCTRL_XTAL 0x050
@@ -68,6 +73,7 @@
 # define GET_EMIFRAC(x) (((x) >> 8) & 0x3f)
 # define CLKCTRL_FRAC_CLKGATECPU (1 << 7)
 # define GET_CPUFRAC(x) ((x) & 0x3f)
+# define SET_CPUFRAC(x) ((x) & 0x3f)
 #define HW_CLKCTRL_FRAC1 0x100
 #define HW_CLKCTRL_CLKSEQ 0x110
 # define CLKCTRL_CLKSEQ_BYPASS_ETM (1 << 8)
@@ -177,6 +183,92 @@ unsigned imx_get_armclk(void)
 			* 1000;
 }
 
+/**
+ * @param nc New frequency in [Hz]
+ */
+unsigned imx_set_armclk(unsigned nc)
+{
+	unsigned reg, bm_busy, d, f, div, frac;
+	unsigned long diff, parent_rate, calc_rate;
+	int i;
+	unsigned reg_offset;
+#define PARENT_RATE_SHIFT     8
+
+	if (nc <= imx_get_xtalclk()) {
+		pr_debug("Setting crystal based CPU clock\n");
+		bm_busy = CLKCTRL_CPU_BUSY_REF_XTAL;
+		reg_offset = BIT_SET;
+		div = DIV_ROUND_UP(imx_get_xtalclk(), nc);
+		if (div == 0 || div > 0x3ff)
+			return 0;
+
+		reg = readl(IMX_CCM_BASE + HW_CLKCTRL_CPU);
+		reg &= ~SET_CPU_XTAL_DIV(0x3ff);
+		reg |= SET_CPU_XTAL_DIV(div);
+		writel(reg, IMX_CCM_BASE + HW_CLKCTRL_CPU);
+	} else {
+		pr_debug("Setting PLL based CPU clock\n");
+		/* we assume here the PLL is already running */
+		parent_rate = imx_get_mpllclk();
+		bm_busy = CLKCTRL_CPU_BUSY_REF_CPU;
+		reg_offset = BIT_CLR;
+		nc >>= PARENT_RATE_SHIFT;
+		parent_rate >>= PARENT_RATE_SHIFT;
+		diff = parent_rate;
+		div = frac = 1;
+		for (d = 1; d <= 0x3f; d++) {
+			f = parent_rate * 18 / d / nc;
+			if ((parent_rate * 18 / d) % nc)
+				f++;
+			if (f < 18 || f > 35)
+				continue;
+
+			calc_rate = parent_rate * 18 / f / d;
+			if (calc_rate > nc)
+				continue;
+
+			if (nc - calc_rate < diff) {
+				frac = f;
+				div = d;
+				diff = nc - calc_rate;
+			}
+
+			if (diff == 0)
+				break;
+		}
+
+		if (diff == parent_rate)
+			return 0;
+
+		pr_debug("frac: %u, div: %u\n", frac, div);
+		reg = readl(IMX_CCM_BASE + HW_CLKCTRL_FRAC);
+		reg &= ~SET_CPUFRAC(0x3f);
+		reg |= SET_CPUFRAC(frac);
+		reg &= ~CLKCTRL_FRAC_CLKGATECPU;
+		writel(reg, IMX_CCM_BASE + HW_CLKCTRL_FRAC);
+
+		reg = readl(IMX_CCM_BASE + HW_CLKCTRL_CPU);
+		reg &= ~SET_CPU_PLL_DIV(0x3f);
+		reg |= SET_CPU_PLL_DIV(div);
+		writel(reg, IMX_CCM_BASE + HW_CLKCTRL_CPU);
+	}
+
+	for (i = 10000; i; i--) {
+		if (!(readl(IMX_CCM_BASE + HW_CLKCTRL_CPU) & bm_busy))
+			break;
+	}
+	if (!i)	{
+		pr_err("%s: divider writing timeout\n", __func__);
+		return 0;
+	}
+
+	pr_debug("Changing CPU's clock source...");
+	writel(CLKCTRL_CLKSEQ_BYPASS_CPU, IMX_CCM_BASE + HW_CLKCTRL_CLKSEQ + reg_offset);
+	pr_debug("done\n");
+
+	return imx_get_armclk();
+}
+
 /* this is the AHB and APBH bus clock */
 unsigned imx_get_hclk(void)
 {
@@ -190,6 +282,16 @@ unsigned imx_get_hclk(void)
 	return rate * 1000;
 }
 
+unsigned imx_set_hclk(unsigned nc)
+{
+	unsigned reg = readl(IMX_CCM_BASE + HW_CLKCTRL_HBUS) & ~0x3f;
+
+	reg |= nc & 0x1f;
+	writel(reg, IMX_CCM_BASE + HW_CLKCTRL_HBUS);
+
+	return imx_get_hclk();
+}
+
 /*
  * Source of UART, debug UART, audio, PWM, dri, timer, digctl
  */