clk: Add __clk_mux_determine_rate_closest

Some clock drivers want to find the closest rate on the input of
a mux instead of a rate that's less than or equal to the desired
rate. Add a generic mux function to support this.

Signed-off-by: Stephen Boyd <sboyd@codeaurora.org>
Tested-by: Kenneth Westfield <kwestfie@codeaurora.org>
Signed-off-by: Michael Turquette <mturquette@linaro.org>

1 files changed, 38 insertions, 9 deletions

diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index 30fff56e1f15..9fc209abcb48 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -690,14 +690,20 @@ struct clk *__clk_lookup(const char *name)
         return NULL;
 }
 
-/*
- * Helper for finding best parent to provide a given frequency. This can be used
- * directly as a determine_rate callback (e.g. for a mux), or from a more
- * complex clock that may combine a mux with other operations.
- */
-long __clk_mux_determine_rate(struct clk_hw *hw, unsigned long rate,
-                              unsigned long *best_parent_rate,
-                              struct clk_hw **best_parent_p)
+static bool mux_is_better_rate(unsigned long rate, unsigned long now,
+                           unsigned long best, unsigned long flags)
+{
+        if (flags & CLK_MUX_ROUND_CLOSEST)
+                return abs(now - rate) < abs(best - rate);
+
+        return now <= rate && now > best;
+}
+
+static long
+clk_mux_determine_rate_flags(struct clk_hw *hw, unsigned long rate,
+                             unsigned long *best_parent_rate,
+                             struct clk_hw **best_parent_p,
+                             unsigned long flags)
 {
         struct clk *clk = hw->clk, *parent, *best_parent = NULL;
         int i, num_parents;
@@ -725,7 +731,7 @@ long __clk_mux_determine_rate(struct clk_hw *hw, unsigned long rate,
                         parent_rate = __clk_round_rate(parent, rate);
                 else
                         parent_rate = __clk_get_rate(parent);
-                if (parent_rate <= rate && parent_rate > best) {
+                if (mux_is_better_rate(rate, parent_rate, best, flags)) {
                         best_parent = parent;
                         best = parent_rate;
                 }
@@ -738,8 +744,31 @@ out:
 
         return best;
 }
+
+/*
+ * Helper for finding best parent to provide a given frequency. This can be used
+ * directly as a determine_rate callback (e.g. for a mux), or from a more
+ * complex clock that may combine a mux with other operations.
+ */
+long __clk_mux_determine_rate(struct clk_hw *hw, unsigned long rate,
+                              unsigned long *best_parent_rate,
+                              struct clk_hw **best_parent_p)
+{
+        return clk_mux_determine_rate_flags(hw, rate, best_parent_rate,
+                                            best_parent_p, 0);
+}
 EXPORT_SYMBOL_GPL(__clk_mux_determine_rate);
 
+long __clk_mux_determine_rate_closest(struct clk_hw *hw, unsigned long rate,
+                              unsigned long *best_parent_rate,
+                              struct clk_hw **best_parent_p)
+{
+        return clk_mux_determine_rate_flags(hw, rate, best_parent_rate,
+                                            best_parent_p,
+                                            CLK_MUX_ROUND_CLOSEST);
+}
+EXPORT_SYMBOL_GPL(__clk_mux_determine_rate_closest);
+
 /***        clk api        ***/
 
 void __clk_unprepare(struct clk *clk)