S3C64XX: Add initial support for ARMCLK

Add support for reconfiguring the clock for the ARM core, enabling
CPUfreq support. Currently only the divider for ARMCLK may be changed,
ARMPLL is left static.

Signed-off-by: Mark Brown <broonie@opensource.wolfsonmicro.com>

1 files changed, 76 insertions, 0 deletions

diff --git a/arch/arm/plat-s3c64xx/s3c6400-clock.c b/arch/arm/plat-s3c64xx/s3c6400-clock.c
index a6f6b91b732..2dc67331c89 100644
--- a/arch/arm/plat-s3c64xx/s3c6400-clock.c
+++ b/arch/arm/plat-s3c64xx/s3c6400-clock.c
@@ -24,6 +24,7 @@
 
 #include <mach/hardware.h>
 #include <mach/map.h>
+#include <mach/cpu.h>
 
 #include <plat/cpu-freq.h>
 
@@ -88,6 +89,80 @@ struct clksrc_clk clk_mout_apll = {
 	.sources	= &clk_src_apll,
 };
 
+static u32 clk_arm_div_mask(void)
+{
+	if (cpu_is_s3c6400())
+		return S3C6400_CLKDIV0_ARM_MASK;
+
+	if (cpu_is_s3c6410())
+		return S3C6410_CLKDIV0_ARM_MASK;
+
+	return 0;
+}
+
+static unsigned long s3c64xx_clk_arm_get_rate(struct clk *clk)
+{
+	unsigned long rate = clk_get_rate(clk->parent);
+	u32 val;
+
+	val = __raw_readl(S3C_CLK_DIV0);
+	val &= clk_arm_div_mask();
+
+	return rate / (val + 1);
+}
+
+static unsigned long s3c64xx_clk_arm_round_rate(struct clk *clk,
+						unsigned long rate)
+{
+	unsigned long parent = clk_get_rate(clk->parent);
+	int div;
+	int max = clk_arm_div_mask() + 1;
+
+	if (parent < rate)
+		return parent;
+
+	div = parent / rate;
+
+	if (div < 1)
+		div = 1;
+	if (div > max)
+		div = max;
+
+	return parent / div;
+}
+
+static int s3c64xx_clk_arm_set_rate(struct clk *clk, unsigned long rate)
+{
+	unsigned int div;
+	u32 val;
+	unsigned long flags;
+
+	rate = clk_round_rate(clk, rate);
+	div = (clk_get_rate(clk->parent) / rate) - 1;
+
+	if (div > clk_arm_div_mask())
+		return -EINVAL;
+
+	val = __raw_readl(S3C_CLK_DIV0);
+	val &= ~clk_arm_div_mask();
+	val |= div;
+
+	local_irq_save(flags);
+	__raw_writel(val, S3C_CLK_DIV0);
+	local_irq_restore(flags);
+
+	return 0;
+}
+
+static struct clk clk_arm = {
+	.name		= "armclk",
+	.id		= -1,
+	.parent		= &clk_mout_apll.clk,
+	.round_rate	= &s3c64xx_clk_arm_round_rate,
+	.get_rate	= s3c64xx_clk_arm_get_rate,
+	.set_rate	= s3c64xx_clk_arm_set_rate,
+};
+
 struct clk clk_fout_epll = {
 	.name		= "fout_epll",
 	.id		= -1,
@@ -633,6 +708,7 @@ static struct clk *clks[] __initdata = {
 	&clk_audio0.clk,
 	&clk_audio1.clk,
 	&clk_irda.clk,
+	&clk_arm,
 };
 
 void __init s3c6400_register_clocks(void)