1 files changed, 100 insertions, 12 deletions

diff --git a/drivers/sh/clk/core.c b/drivers/sh/clk/core.c
index fd0d1b98901c..3f5e387ed564 100644
--- a/drivers/sh/clk/core.c
+++ b/drivers/sh/clk/core.c
@@ -90,8 +90,8 @@ struct clk_rate_round_data {
 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 = rounder->rate;
         unsigned long highest, lowest, freq;
+        long rate_best_fit = -ENOENT;
         int i;
 
         highest = 0;
@@ -146,7 +146,7 @@ long clk_rate_table_round(struct clk *clk,
         };
 
         if (clk->nr_freqs < 1)
-                return 0;
+                return -ENOSYS;
 
         return clk_rate_round_helper(&table_round);
 }
@@ -418,8 +418,11 @@ int clk_register(struct clk *clk)
                 list_add(&clk->sibling, &root_clks);
 
         list_add(&clk->node, &clock_list);
+
+#ifdef CONFIG_SH_CLK_CPG_LEGACY
         if (clk->ops && clk->ops->init)
                 clk->ops->init(clk);
+#endif
 
 out_unlock:
         mutex_unlock(&clock_list_sem);
@@ -455,19 +458,13 @@ EXPORT_SYMBOL_GPL(clk_get_rate);
 
 int clk_set_rate(struct clk *clk, unsigned long rate)
 {
-        return clk_set_rate_ex(clk, rate, 0);
-}
-EXPORT_SYMBOL_GPL(clk_set_rate);
-
-int clk_set_rate_ex(struct clk *clk, unsigned long rate, int algo_id)
-{
         int ret = -EOPNOTSUPP;
         unsigned long flags;
 
         spin_lock_irqsave(&clock_lock, flags);
 
         if (likely(clk->ops && clk->ops->set_rate)) {
-                ret = clk->ops->set_rate(clk, rate, algo_id);
+                ret = clk->ops->set_rate(clk, rate);
                 if (ret != 0)
                         goto out_unlock;
         } else {
@@ -485,7 +482,7 @@ out_unlock:
 
         return ret;
 }
-EXPORT_SYMBOL_GPL(clk_set_rate_ex);
+EXPORT_SYMBOL_GPL(clk_set_rate);
 
 int clk_set_parent(struct clk *clk, struct clk *parent)
 {
@@ -541,6 +538,98 @@ long clk_round_rate(struct clk *clk, unsigned long rate)
 }
 EXPORT_SYMBOL_GPL(clk_round_rate);
 
+long clk_round_parent(struct clk *clk, unsigned long target,
+                      unsigned long *best_freq, unsigned long *parent_freq,
+                      unsigned int div_min, unsigned int div_max)
+{
+        struct cpufreq_frequency_table *freq, *best = NULL;
+        unsigned long error = ULONG_MAX, freq_high, freq_low, div;
+        struct clk *parent = clk_get_parent(clk);
+
+        if (!parent) {
+                *parent_freq = 0;
+                *best_freq = clk_round_rate(clk, target);
+                return abs(target - *best_freq);
+        }
+
+        for (freq = parent->freq_table; freq->frequency != CPUFREQ_TABLE_END;
+             freq++) {
+                if (freq->frequency == CPUFREQ_ENTRY_INVALID)
+                        continue;
+
+                if (unlikely(freq->frequency / target <= div_min - 1)) {
+                        unsigned long freq_max;
+
+                        freq_max = (freq->frequency + div_min / 2) / div_min;
+                        if (error > target - freq_max) {
+                                error = target - freq_max;
+                                best = freq;
+                                if (best_freq)
+                                        *best_freq = freq_max;
+                        }
+
+                        pr_debug("too low freq %u, error %lu\n", freq->frequency,
+                                 target - freq_max);
+
+                        if (!error)
+                                break;
+
+                        continue;
+                }
+
+                if (unlikely(freq->frequency / target >= div_max)) {
+                        unsigned long freq_min;
+
+                        freq_min = (freq->frequency + div_max / 2) / div_max;
+                        if (error > freq_min - target) {
+                                error = freq_min - target;
+                                best = freq;
+                                if (best_freq)
+                                        *best_freq = freq_min;
+                        }
+
+                        pr_debug("too high freq %u, error %lu\n", freq->frequency,
+                                 freq_min - target);
+
+                        if (!error)
+                                break;
+
+                        continue;
+                }
+
+                div = freq->frequency / target;
+                freq_high = freq->frequency / div;
+                freq_low = freq->frequency / (div + 1);
+
+                if (freq_high - target < error) {
+                        error = freq_high - target;
+                        best = freq;
+                        if (best_freq)
+                                *best_freq = freq_high;
+                }
+
+                if (target - freq_low < error) {
+                        error = target - freq_low;
+                        best = freq;
+                        if (best_freq)
+                                *best_freq = freq_low;
+                }
+
+                pr_debug("%u / %lu = %lu, / %lu = %lu, best %lu, parent %u\n",
+                         freq->frequency, div, freq_high, div + 1, freq_low,
+                         *best_freq, best->frequency);
+
+                if (!error)
+                        break;
+        }
+
+        if (parent_freq)
+                *parent_freq = best->frequency;
+
+        return error;
+}
+EXPORT_SYMBOL_GPL(clk_round_parent);
+
 #ifdef CONFIG_PM
 static int clks_sysdev_suspend(struct sys_device *dev, pm_message_t state)
 {
@@ -561,8 +650,7 @@ static int clks_sysdev_suspend(struct sys_device *dev, pm_message_t state)
                                         clkp->ops->set_parent(clkp,
                                                 clkp->parent);
                                 if (likely(clkp->ops->set_rate))
-                                        clkp->ops->set_rate(clkp,
-                                                rate, NO_CHANGE);
+                                        clkp->ops->set_rate(clkp, rate);
                                 else if (likely(clkp->ops->recalc))
                                         clkp->rate = clkp->ops->recalc(clkp);
                         }