sh: shared mstp32 clock code

[pandora-kernel.git] / arch / sh / kernel / cpu / clock-cpg.c

diff --git a/arch/sh/kernel/cpu/clock-cpg.c b/arch/sh/kernel/cpu/clock-cpg.c
index b78c237..72228d2 100644 (file)
--- a/arch/sh/kernel/cpu/clock-cpg.c
+++ b/arch/sh/kernel/cpu/clock-cpg.c
@@ -1,7 +1,42 @@
 #include <linux/clk.h>
 #include <linux/compiler.h>
+#include <linux/io.h>
 #include <asm/clock.h>
 
+static int sh_clk_mstp32_enable(struct clk *clk)
+{
+       __raw_writel(__raw_readl(clk->enable_reg) & ~(1 << clk->enable_bit),
+                    clk->enable_reg);
+       return 0;
+}
+
+static void sh_clk_mstp32_disable(struct clk *clk)
+{
+       __raw_writel(__raw_readl(clk->enable_reg) | (1 << clk->enable_bit),
+                    clk->enable_reg);
+}
+
+static struct clk_ops sh_clk_mstp32_clk_ops = {
+       .enable         = sh_clk_mstp32_enable,
+       .disable        = sh_clk_mstp32_disable,
+       .recalc         = followparent_recalc,
+};
+
+int __init sh_clk_mstp32_register(struct clk *clks, int nr)
+{
+       struct clk *clkp;
+       int ret = 0;
+       int k;
+
+       for (k = 0; !ret && (k < nr); k++) {
+               clkp = clks + k;
+               clkp->ops = &sh_clk_mstp32_clk_ops;
+               ret |= clk_register(clkp);
+       }
+
+       return ret;
+}
+
 #ifdef CONFIG_SH_CLK_CPG_LEGACY
 static struct clk master_clk = {
        .name           = "master_clk",