3 files changed, 100 insertions, 13 deletions

diff --git a/arch/sh/include/asm/clock.h b/arch/sh/include/asm/clock.h
index 40cf3c07d7e6..da681de1500a 100644
--- a/arch/sh/include/asm/clock.h
+++ b/arch/sh/include/asm/clock.h
@@ -16,6 +16,7 @@ struct clk_ops {
         int (*set_rate)(struct clk *clk, unsigned long rate, int algo_id);
         int (*set_parent)(struct clk *clk, struct clk *parent);
         long (*round_rate)(struct clk *clk, unsigned long rate);
+        void (*build_rate_table)(struct clk *clk);
 };
 
 struct clk {
diff --git a/arch/sh/kernel/cpu/clock.c b/arch/sh/kernel/cpu/clock.c
index 033f4662b59d..56c6e11fa83b 100644
--- a/arch/sh/kernel/cpu/clock.c
+++ b/arch/sh/kernel/cpu/clock.c
@@ -63,6 +63,9 @@ void propagate_rate(struct clk *tclk)
         list_for_each_entry(clkp, &tclk->children, sibling) {
                 if (clkp->ops && clkp->ops->recalc)
                         clkp->rate = clkp->ops->recalc(clkp);
+                if (clkp->ops && clkp->ops->build_rate_table)
+                        clkp->ops->build_rate_table(clkp);
+
                 propagate_rate(clkp);
         }
 }
diff --git a/arch/sh/kernel/cpu/sh4a/clock-sh7785.c b/arch/sh/kernel/cpu/sh4a/clock-sh7785.c
index 87584dc81926..b7a32dd1b2db 100644
--- a/arch/sh/kernel/cpu/sh4a/clock-sh7785.c
+++ b/arch/sh/kernel/cpu/sh4a/clock-sh7785.c
@@ -13,28 +13,43 @@
 #include <linux/kernel.h>
 #include <linux/clk.h>
 #include <linux/io.h>
+#include <linux/cpufreq.h>
 #include <asm/clock.h>
 #include <asm/freq.h>
 
 static unsigned int div2[] = { 1, 2, 4, 6, 8, 12, 16, 18,
                                24, 32, 36, 48 };
 struct clk_priv {
-        unsigned int shift;
+        unsigned int                    shift;
+
+        /* allowable divisor bitmap */
+        unsigned long                   div_bitmap;
+
+        /* Supportable frequencies + termination entry */
+        struct cpufreq_frequency_table  freq_table[ARRAY_SIZE(div2)+1];
 };
 
-#define FRQMR_CLK_DATA(_name, _shift)   \
-static struct clk_priv _name##_data = { .shift = _shift, }
+#define FRQMR_CLK_DATA(_name, _shift, _div_bitmap)      \
+static struct clk_priv _name##_data = {                 \
+        .shift          = _shift,                       \
+        .div_bitmap     = _div_bitmap,                  \
+                                                        \
+        .freq_table[0]  = {                             \
+                .index = 0,                             \
+                .frequency = CPUFREQ_TABLE_END,         \
+        },                                              \
+}
 
-FRQMR_CLK_DATA(pfc,  0);
-FRQMR_CLK_DATA(s3fc, 4);
-FRQMR_CLK_DATA(s2fc, 8);
-FRQMR_CLK_DATA(mfc, 12);
-FRQMR_CLK_DATA(bfc, 16);
-FRQMR_CLK_DATA(sfc, 20);
-FRQMR_CLK_DATA(ufc, 24);
-FRQMR_CLK_DATA(ifc, 28);
+FRQMR_CLK_DATA(pfc,  0, 0x0f80);
+FRQMR_CLK_DATA(s3fc, 4, 0x0ff0);
+FRQMR_CLK_DATA(s2fc, 8, 0x0030);
+FRQMR_CLK_DATA(mfc, 12, 0x000c);
+FRQMR_CLK_DATA(bfc, 16, 0x0fe0);
+FRQMR_CLK_DATA(sfc, 20, 0x000c);
+FRQMR_CLK_DATA(ufc, 24, 0x000c);
+FRQMR_CLK_DATA(ifc, 28, 0x000e);
 
-static unsigned long frqmr_clk_recalc(struct clk *clk)
+static unsigned long frqmr_recalc(struct clk *clk)
 {
         struct clk_priv *data = clk->priv;
         unsigned int idx;
@@ -49,8 +64,76 @@ static unsigned long frqmr_clk_recalc(struct clk *clk)
         return clk->parent->rate * 36 / div2[idx];
 }
 
+static void frqmr_build_rate_table(struct clk *clk)
+{
+        struct clk_priv *data = clk->priv;
+        int i, entry;
+
+        for (i = entry = 0; i < ARRAY_SIZE(div2); i++) {
+                if ((data->div_bitmap & (1 << i)) == 0)
+                        continue;
+
+                data->freq_table[entry].index = entry;
+                data->freq_table[entry].frequency =
+                        clk->parent->rate * 36 / div2[i];
+
+                entry++;
+        }
+
+        if (entry == 0) {
+                pr_warning("clkfwk: failed to build frequency table "
+                           "for \"%s\" clk!\n", clk->name);
+                return;
+        }
+
+        /* Termination entry */
+        data->freq_table[entry].index = entry;
+        data->freq_table[entry].frequency = CPUFREQ_TABLE_END;
+}
+
+static long frqmr_round_rate(struct clk *clk, unsigned long rate)
+{
+        struct clk_priv *data = clk->priv;
+        unsigned long rate_error, rate_error_prev = ~0UL;
+        unsigned long rate_best_fit = rate;
+        unsigned long highest, lowest;
+        int i;
+
+        highest = lowest = 0;
+
+        for (i = 0; data->freq_table[i].frequency != CPUFREQ_TABLE_END; i++) {
+                unsigned long freq = data->freq_table[i].frequency;
+
+                if (freq == CPUFREQ_ENTRY_INVALID)
+                        continue;
+
+                if (freq > highest)
+                        highest = freq;
+                if (freq < lowest)
+                        lowest = freq;
+
+                rate_error = abs(freq - rate);
+                if (rate_error < rate_error_prev) {
+                        rate_best_fit = freq;
+                        rate_error_prev = rate_error;
+                }
+
+                if (rate_error == 0)
+                        break;
+        }
+
+        if (rate >= highest)
+                rate_best_fit = highest;
+        if (rate <= lowest)
+                rate_best_fit = lowest;
+
+        return rate_best_fit;
+}
+
 static struct clk_ops frqmr_clk_ops = {
-        .recalc         = frqmr_clk_recalc,
+        .recalc                 = frqmr_recalc,
+        .build_rate_table       = frqmr_build_rate_table,
+        .round_rate             = frqmr_round_rate,
 };
 
 /*