2 files changed, 99 insertions, 13 deletions

diff --git a/arch/sh/kernel/cpu/clock.c b/arch/sh/kernel/cpu/clock.c
index 033f4662b59..56c6e11fa83 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 87584dc8192..b7a32dd1b2d 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,
 };
 
 /*