diff --git a/arch/arm/plat-s3c24xx/clock.c b/arch/arm/plat-s3c24xx/clock.c index 99a44746f8f2..d84167fb33b1 100644 --- a/arch/arm/plat-s3c24xx/clock.c +++ b/arch/arm/plat-s3c24xx/clock.c @@ -332,6 +332,58 @@ static int s3c24xx_dclk_setparent(struct clk *clk, struct clk *parent) return 0; } +static unsigned long s3c24xx_calc_div(struct clk *clk, unsigned long rate) +{ + unsigned long div; + + if ((rate == 0) || !clk->parent) + return 0; + + div = clk_get_rate(clk->parent) / rate; + if (div < 2) + div = 2; + else if (div > 16) + div = 16; + + return div; +} + +static unsigned long s3c24xx_round_dclk_rate(struct clk *clk, + unsigned long rate) +{ + unsigned long div = s3c24xx_calc_div(clk, rate); + + if (div == 0) + return 0; + + return clk_get_rate(clk->parent) / div; +} + +static int s3c24xx_set_dclk_rate(struct clk *clk, unsigned long rate) +{ + unsigned long mask, data, div = s3c24xx_calc_div(clk, rate); + + if (div == 0) + return -EINVAL; + + if (clk == &s3c24xx_dclk0) { + mask = S3C2410_DCLKCON_DCLK0_DIV_MASK | + S3C2410_DCLKCON_DCLK0_CMP_MASK; + data = S3C2410_DCLKCON_DCLK0_DIV(div) | + S3C2410_DCLKCON_DCLK0_CMP((div + 1) / 2); + } else if (clk == &s3c24xx_dclk1) { + mask = S3C2410_DCLKCON_DCLK1_DIV_MASK | + S3C2410_DCLKCON_DCLK1_CMP_MASK; + data = S3C2410_DCLKCON_DCLK1_DIV(div) | + S3C2410_DCLKCON_DCLK1_CMP((div + 1) / 2); + } else + return -EINVAL; + + clk->rate = clk_get_rate(clk->parent) / div; + __raw_writel(((__raw_readl(S3C24XX_DCLKCON) & ~mask) | data), + S3C24XX_DCLKCON); + return clk->rate; +} static int s3c24xx_clkout_setparent(struct clk *clk, struct clk *parent) { @@ -378,6 +430,8 @@ struct clk s3c24xx_dclk0 = { .ctrlbit = S3C2410_DCLKCON_DCLK0EN, .enable = s3c24xx_dclk_enable, .set_parent = s3c24xx_dclk_setparent, + .set_rate = s3c24xx_set_dclk_rate, + .round_rate = s3c24xx_round_dclk_rate, }; struct clk s3c24xx_dclk1 = { @@ -386,6 +440,8 @@ struct clk s3c24xx_dclk1 = { .ctrlbit = S3C2410_DCLKCON_DCLK0EN, .enable = s3c24xx_dclk_enable, .set_parent = s3c24xx_dclk_setparent, + .set_rate = s3c24xx_set_dclk_rate, + .round_rate = s3c24xx_round_dclk_rate, }; struct clk s3c24xx_clkout0 = {