sh: clkfwk: Abstract rate rounding helper.

Presently the only assisted rate rounding is frequency table backed, but
there are cases where it's impractical to use a frequency table for
certain clocks (such as the FSIDIV case, which supports 65535 divisors),
and we wish to reuse the same rate rounding algorithm.

This breaks out the core of the rate rounding logic in to its own helper
routine and shuffles the frequency table logic around, switching to using
an iterator for the generic helper routine.

Signed-off-by: Paul Mundt <lethal@linux-sh.org>

2 files changed, 53 insertions, 14 deletions

diff --git a/drivers/sh/clk.c b/drivers/sh/clk.c
index 813d97cdad49..3ac6fa0005b9 100644
--- a/drivers/sh/clk.c
+++ b/drivers/sh/clk.c
@@ -45,6 +45,8 @@ void clk_rate_table_build(struct clk *clk,
         unsigned long freq;
         int i;
 
+        clk->nr_freqs = nr_freqs;
+
         for (i = 0; i < nr_freqs; i++) {
                 div = 1;
                 mult = 1;
@@ -69,30 +71,39 @@ void clk_rate_table_build(struct clk *clk,
         freq_table[i].frequency = CPUFREQ_TABLE_END;
 }
 
-long clk_rate_table_round(struct clk *clk,
-                          struct cpufreq_frequency_table *freq_table,
-                          unsigned long rate)
+struct clk_rate_round_data;
+
+struct clk_rate_round_data {
+        unsigned long rate;
+        unsigned int min, max;
+        long (*func)(unsigned int pos, struct clk_rate_round_data *arg);
+        void *arg;
+};
+
+#define for_each_frequency(pos, r, freq)                        \
+        for (pos = r->min, freq = r->func(pos, r->arg);         \
+             pos < r->max; pos++, freq = r->func(pos, r))       \
+                if (unlikely(freq == 0))                        \
+                        ;                                       \
+                else
+
+static long clk_rate_round_helper(struct clk_rate_round_data *rounder)
 {
         unsigned long rate_error, rate_error_prev = ~0UL;
-        unsigned long rate_best_fit = rate;
-        unsigned long highest, lowest;
+        unsigned long rate_best_fit = rounder->rate;
+        unsigned long highest, lowest, freq;
         int i;
 
         highest = 0;
         lowest = ~0UL;
 
-        for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++) {
-                unsigned long freq = freq_table[i].frequency;
-
-                if (freq == CPUFREQ_ENTRY_INVALID)
-                        continue;
-
+        for_each_frequency(i, rounder, freq) {
                 if (freq > highest)
                         highest = freq;
                 if (freq < lowest)
                         lowest = freq;
 
-                rate_error = abs(freq - rate);
+                rate_error = abs(freq - rounder->rate);
                 if (rate_error < rate_error_prev) {
                         rate_best_fit = freq;
                         rate_error_prev = rate_error;
@@ -102,14 +113,41 @@ long clk_rate_table_round(struct clk *clk,
                         break;
         }
 
-        if (rate >= highest)
+        if (rounder->rate >= highest)
                 rate_best_fit = highest;
-        if (rate <= lowest)
+        if (rounder->rate <= lowest)
                 rate_best_fit = lowest;
 
         return rate_best_fit;
 }
 
+static long clk_rate_table_iter(unsigned int pos,
+                                struct clk_rate_round_data *rounder)
+{
+        struct cpufreq_frequency_table *freq_table = rounder->arg;
+        unsigned long freq = freq_table[pos].frequency;
+
+        if (freq == CPUFREQ_ENTRY_INVALID)
+                freq = 0;
+
+        return freq;
+}
+
+long clk_rate_table_round(struct clk *clk,
+                          struct cpufreq_frequency_table *freq_table,
+                          unsigned long rate)
+{
+        struct clk_rate_round_data table_round = {
+                .min    = 0,
+                .max    = clk->nr_freqs,
+                .func   = clk_rate_table_iter,
+                .arg    = freq_table,
+                .rate   = rate,
+        };
+
+        return clk_rate_round_helper(&table_round);
+}
+
 int clk_rate_table_find(struct clk *clk,
                         struct cpufreq_frequency_table *freq_table,
                         unsigned long rate)
diff --git a/include/linux/sh_clk.h b/include/linux/sh_clk.h
index 8ae37707a4a4..49f6e9b6eda2 100644
--- a/include/linux/sh_clk.h
+++ b/include/linux/sh_clk.h
@@ -53,6 +53,7 @@ struct clk {
         struct dentry           *dentry;
         struct clk_mapping      *mapping;
         struct cpufreq_frequency_table *freq_table;
+        unsigned int            nr_freqs;
 };
 
 #define CLK_ENABLE_ON_INIT      (1 << 0)