2 files changed, 48 insertions, 0 deletions

diff --git a/arch/sh/include/asm/clock.h b/arch/sh/include/asm/clock.h
index aa9480d4aa0..f43d3e72d26 100644
--- a/arch/sh/include/asm/clock.h
+++ b/arch/sh/include/asm/clock.h
@@ -117,4 +117,17 @@ long clk_rate_table_round(struct clk *clk,
                           struct cpufreq_frequency_table *freq_table,
                           unsigned long rate);
 
+#define SH_CLK_MSTP32(_name, _id, _parent, _enable_reg, \
+            _enable_bit, _flags)                        \
+{                                                       \
+        .name           = _name,                        \
+        .id             = _id,                          \
+        .parent         = _parent,                      \
+        .enable_reg     = (void __iomem *)_enable_reg,  \
+        .enable_bit     = _enable_bit,                  \
+        .flags          = _flags,                       \
+}
+
+int sh_clk_mstp32_register(struct clk *clks, int nr);
+
 #endif /* __ASM_SH_CLOCK_H */
diff --git a/arch/sh/kernel/cpu/clock-cpg.c b/arch/sh/kernel/cpu/clock-cpg.c
index b78c237ab36..72228d2945a 100644
--- 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",