[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,
         },
 };