[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>

diff --git a/arch/arm/plat-s3c/pwm-clock.c b/arch/arm/plat-s3c/pwm-clock.c
index 3fad68a..b0fcbc1 100644 (file)
--- 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,
        },
 };