[ARM] S3C: Add set_rate/round_rate methods for pwm-scaler clock

Add the set_rate and round_rate methods for the pwm-scaler
clock for use with the time code.

Signed-off-by: Ben Dooks <ben-linux@fluff.org>

1 files changed, 47 insertions, 4 deletions

diff --git a/arch/arm/plat-s3c/pwm-clock.c b/arch/arm/plat-s3c/pwm-clock.c
index 3fad68a1e6bc..b0fcbc1fb64b 100644
--- a/arch/arm/plat-s3c/pwm-clock.c
+++ b/arch/arm/plat-s3c/pwm-clock.c
@@ -73,7 +73,7 @@
  * tclk -------------------------/
 */
 
-static unsigned long clk_pwm_scaler_getrate(struct clk *clk)
+static unsigned long clk_pwm_scaler_get_rate(struct clk *clk)
 {
 	unsigned long tcfg0 = __raw_readl(S3C2410_TCFG0);
 
@@ -87,18 +87,61 @@ static unsigned long clk_pwm_scaler_getrate(struct clk *clk)
 	return clk_get_rate(clk->parent) / (tcfg0 + 1);
 }
 
-/* TODO - add set rate calls. */
+static unsigned long clk_pwm_scaler_round_rate(struct clk *clk,
+					       unsigned long rate)
+{
+	unsigned long parent_rate = clk_get_rate(clk->parent);
+	unsigned long divisor = parent_rate / rate;
+
+	if (divisor > 256)
+		divisor = 256;
+	else if (divisor < 2)
+		divisor = 2;
+
+	return parent_rate / divisor;
+}
+
+static int clk_pwm_scaler_set_rate(struct clk *clk, unsigned long rate)
+{
+	unsigned long round = clk_pwm_scaler_round_rate(clk, rate);
+	unsigned long tcfg0;
+	unsigned long divisor;
+	unsigned long flags;
+
+	divisor = clk_get_rate(clk->parent) / round;
+	divisor--;
+
+	local_irq_save(flags);
+	tcfg0 = __raw_readl(S3C2410_TCFG0);
+
+	if (clk->id == 1) {
+		tcfg0 &= ~S3C2410_TCFG_PRESCALER1_MASK;
+		tcfg0 |= divisor << S3C2410_TCFG_PRESCALER1_SHIFT;
+	} else {
+		tcfg0 &= ~S3C2410_TCFG_PRESCALER0_MASK;
+		tcfg0 |= divisor;
+	}
+
+	__raw_writel(tcfg0, S3C2410_TCFG0);
+	local_irq_restore(flags);
+
+	return 0;
+}
 
 static struct clk clk_timer_scaler[] = {
 	[0]	= {
 		.name		= "pwm-scaler0",
 		.id		= -1,
-		.get_rate	= clk_pwm_scaler_getrate,
+		.get_rate	= clk_pwm_scaler_get_rate,
+		.set_rate	= clk_pwm_scaler_set_rate,
+		.round_rate	= clk_pwm_scaler_round_rate,
 	},
 	[1]	= {
 		.name		= "pwm-scaler1",
 		.id		= -1,
-		.get_rate	= clk_pwm_scaler_getrate,
+		.get_rate	= clk_pwm_scaler_get_rate,
+		.set_rate	= clk_pwm_scaler_set_rate,
+		.round_rate	= clk_pwm_scaler_round_rate,
 	},
 };