sh: add enable()/disable()/set_rate() to div6 code

This patch updates the div6 clock helper code to add support
for enable(), disable() and set_rate() callbacks.

Needed by the camera clock enabling board code on Migo-R.

Signed-off-by: Magnus Damm <damm@igel.co.jp>
Signed-off-by: Paul Mundt <lethal@linux-sh.org>

3 files changed, 67 insertions, 0 deletions

diff --git a/arch/sh/include/asm/clock.h b/arch/sh/include/asm/clock.h
index 026b9daa5584..9fe7d7f8af40 100644
--- a/arch/sh/include/asm/clock.h
+++ b/arch/sh/include/asm/clock.h
@@ -119,6 +119,10 @@ long clk_rate_table_round(struct clk *clk,
                           struct cpufreq_frequency_table *freq_table,
                           unsigned long rate);
 
+int clk_rate_table_find(struct clk *clk,
+                        struct cpufreq_frequency_table *freq_table,
+                        unsigned long rate);
+
 #define SH_CLK_MSTP32(_name, _id, _parent, _enable_reg, \
             _enable_bit, _flags)                        \
 {                                                       \
diff --git a/arch/sh/kernel/cpu/clock-cpg.c b/arch/sh/kernel/cpu/clock-cpg.c
index fedc8b84db4c..275942e58e4f 100644
--- a/arch/sh/kernel/cpu/clock-cpg.c
+++ b/arch/sh/kernel/cpu/clock-cpg.c
@@ -68,9 +68,53 @@ static unsigned long sh_clk_div6_recalc(struct clk *clk)
         return clk->freq_table[idx].frequency;
 }
 
+static int sh_clk_div6_set_rate(struct clk *clk,
+                                unsigned long rate, int algo_id)
+{
+        unsigned long value;
+        int idx;
+
+        idx = clk_rate_table_find(clk, clk->freq_table, rate);
+        if (idx < 0)
+                return idx;
+
+        value = __raw_readl(clk->enable_reg);
+        value &= ~0x3f;
+        value |= idx;
+        __raw_writel(value, clk->enable_reg);
+        return 0;
+}
+
+static int sh_clk_div6_enable(struct clk *clk)
+{
+        unsigned long value;
+        int ret;
+
+        ret = sh_clk_div6_set_rate(clk, clk->rate, 0);
+        if (ret == 0) {
+                value = __raw_readl(clk->enable_reg);
+                value &= ~0x100; /* clear stop bit to enable clock */
+                __raw_writel(value, clk->enable_reg);
+        }
+        return ret;
+}
+
+static void sh_clk_div6_disable(struct clk *clk)
+{
+        unsigned long value;
+
+        value = __raw_readl(clk->enable_reg);
+        value |= 0x100; /* stop clock */
+        value |= 0x3f; /* VDIV bits must be non-zero, overwrite divider */
+        __raw_writel(value, clk->enable_reg);
+}
+
 static struct clk_ops sh_clk_div6_clk_ops = {
         .recalc         = sh_clk_div6_recalc,
         .round_rate     = sh_clk_div_round_rate,
+        .set_rate       = sh_clk_div6_set_rate,
+        .enable         = sh_clk_div6_enable,
+        .disable        = sh_clk_div6_disable,
 };
 
 int __init sh_clk_div6_register(struct clk *clks, int nr)
diff --git a/arch/sh/kernel/cpu/clock.c b/arch/sh/kernel/cpu/clock.c
index aa0fd0893585..f3a46be2ae81 100644
--- a/arch/sh/kernel/cpu/clock.c
+++ b/arch/sh/kernel/cpu/clock.c
@@ -111,6 +111,25 @@ long clk_rate_table_round(struct clk *clk,
         return rate_best_fit;
 }
 
+int clk_rate_table_find(struct clk *clk,
+                        struct cpufreq_frequency_table *freq_table,
+                        unsigned long rate)
+{
+        int i;
+
+        for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++) {
+                unsigned long freq = freq_table[i].frequency;
+
+                if (freq == CPUFREQ_ENTRY_INVALID)
+                        continue;
+
+                if (freq == rate)
+                        return i;
+        }
+
+        return -ENOENT;
+}
+
 /* Used for clocks that always have same value as the parent clock */
 unsigned long followparent_recalc(struct clk *clk)
 {