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>

diff --git a/arch/sh/include/asm/clock.h b/arch/sh/include/asm/clock.h
index 026b9da..9fe7d7f 100644 (file)
--- 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 fedc8b8..275942e 100644 (file)
--- 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 aa0fd08..f3a46be 100644 (file)
--- 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)
 {