diff --git a/arch/arm/plat-s3c24xx/s3c244x-clock.c b/arch/arm/plat-s3c24xx/s3c244x-clock.c index 0bf5e7fbb2bf31d398828bcde116e44f7f0576e2..faf3e0f9f4e23b9647850038e6312c38b530a4f6 100644 --- a/arch/arm/plat-s3c24xx/s3c244x-clock.c +++ b/arch/arm/plat-s3c24xx/s3c244x-clock.c @@ -44,15 +44,52 @@ #include #include +static int s3c2440_setparent_armclk(struct clk *clk, struct clk *parent) +{ + unsigned long camdivn; + unsigned long dvs; + + if (parent == &clk_f) + dvs = 0; + else if (parent == &clk_h) + dvs = S3C2440_CAMDIVN_DVSEN; + else + return -EINVAL; + + clk->parent = parent; + + camdivn = __raw_readl(S3C2440_CAMDIVN); + camdivn &= ~S3C2440_CAMDIVN_DVSEN; + camdivn |= dvs; + __raw_writel(camdivn, S3C2440_CAMDIVN); + + return 0; +} + +static struct clk clk_arm = { + .name = "armclk", + .id = -1, + .set_parent = s3c2440_setparent_armclk, +}; + static int s3c244x_clk_add(struct sys_device *sysdev) { unsigned long camdivn = __raw_readl(S3C2440_CAMDIVN); unsigned long clkdivn; struct clk *clock_upll; + int ret; printk("S3C244X: Clock Support, DVS %s\n", (camdivn & S3C2440_CAMDIVN_DVSEN) ? "on" : "off"); + clk_arm.parent = (camdivn & S3C2440_CAMDIVN_DVSEN) ? &clk_h : &clk_f; + + ret = s3c24xx_register_clock(&clk_arm); + if (ret < 0) { + printk(KERN_ERR "S3C24XX: Failed to add armclk (%d)\n", ret); + return ret; + } + clock_upll = clk_get(NULL, "upll"); if (IS_ERR(clock_upll)) { printk(KERN_ERR "S3C244X: Failed to get upll clock\n");