[ARM] 4778/1: S3C2412: Add armclk and init from DVS state

Add armclk to the S3C2412 to indicate the current clock connected to
the ARM core.

Signed-off-by: Ben Dooks <ben-linux@fluff.org>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

1 files changed, 50 insertions, 0 deletions

diff --git a/arch/arm/mach-s3c2412/clock.c b/arch/arm/mach-s3c2412/clock.c
index 458993601897..42ccb5eb6042 100644
--- a/arch/arm/mach-s3c2412/clock.c
+++ b/arch/arm/mach-s3c2412/clock.c
@@ -234,6 +234,45 @@ static struct clk clk_msysclk = {
         .set_parent     = s3c2412_setparent_msysclk,
 };
 
+static int s3c2412_setparent_armclk(struct clk *clk, struct clk *parent)
+{
+        unsigned long flags;
+        unsigned long clkdiv;
+        unsigned long dvs;
+
+        /* Note, we current equate fclk andf msysclk for S3C2412 */
+
+        if (parent == &clk_msysclk || parent == &clk_f)
+                dvs = 0;
+        else if (parent == &clk_h)
+                dvs = S3C2412_CLKDIVN_DVSEN;
+        else
+                return -EINVAL;
+
+        clk->parent = parent;
+
+        /* update this under irq lockdown, clkdivn is not protected
+         * by the clock system. */
+
+        local_irq_save(flags);
+
+        clkdiv  = __raw_readl(S3C2410_CLKDIVN);
+        clkdiv &= ~S3C2412_CLKDIVN_DVSEN;
+        clkdiv |= dvs;
+        __raw_writel(clkdiv, S3C2410_CLKDIVN);
+
+        local_irq_restore(flags);
+
+        return 0;
+}
+
+static struct clk clk_armclk = {
+        .name           = "armclk",
+        .id             = -1,
+        .parent         = &clk_msysclk,
+        .set_parent     = s3c2412_setparent_armclk,
+};
+
 /* these next clocks have an divider immediately after them,
  * so we can register them with their divider and leave out the
  * intermediate clock stage
@@ -630,11 +669,13 @@ static struct clk *clks[] __initdata = {
         &clk_erefclk,
         &clk_urefclk,
         &clk_mrefclk,
+        &clk_armclk,
 };
 
 int __init s3c2412_baseclk_add(void)
 {
         unsigned long clkcon  = __raw_readl(S3C2410_CLKCON);
+        unsigned int dvs;
         struct clk *clkp;
         int ret;
         int ptr;
@@ -655,6 +696,15 @@ int __init s3c2412_baseclk_add(void)
                 }
         }
 
+        /* set the dvs state according to what we got at boot time */
+
+        dvs = __raw_readl(S3C2410_CLKDIVN) & S3C2412_CLKDIVN_DVSEN;
+
+        if (dvs)
+                clk_armclk.parent = &clk_h;
+
+        printk(KERN_INFO "S3C2412: DVS is %s\n", dvs ? "on" : "off");
+
         /* ensure usb bus clock is within correct rate of 48MHz */
 
         if (clk_get_rate(&clk_usb_bus) != (48 * 1000 * 1000)) {