clk: at91: optimization of the set_rate callback

Signed-off-by: Boris BREZILLON <b.brezillon@overkiz.com>
Signed-off-by: Jean-Jacques Hiblot <jjhiblot@traphandler.com>
Signed-off-by: Mike Turquette <mturquette@linaro.org>

diff --git a/drivers/clk/at91/clk-programmable.c b/drivers/clk/at91/clk-programmable.c
index 7bcc725f67afb7c1350e236ece270435314b0b5d..62e2509f9df16d52352da1d4aaa13b06d65b4824 100644 (file)
--- a/drivers/clk/at91/clk-programmable.c
+++ b/drivers/clk/at91/clk-programmable.c
@@ -139,43 +139,21 @@ static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate,
        struct clk_programmable *prog = to_clk_programmable(hw);
        struct at91_pmc *pmc = prog->pmc;
        const struct clk_programmable_layout *layout = prog->layout;
-       unsigned long best_rate = parent_rate;
-       unsigned long best_diff;
-       unsigned long new_diff;
-       unsigned long cur_rate;
+       unsigned long div = parent_rate / rate;
        int shift = 0;
        u32 tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id)) &
                  ~(PROG_PRES_MASK << layout->pres_shift);
 
-       if (rate > parent_rate)
-               return parent_rate;
-       else
-               best_diff = parent_rate - rate;
+       if (!div)
+               return -EINVAL;
 
-       if (!best_diff) {
-               pmc_write(pmc, AT91_PMC_PCKR(prog->id), tmp | shift);
-               return 0;
-       }
-
-       for (shift = 1; shift < PROG_PRES_MASK; shift++) {
-               cur_rate = parent_rate >> shift;
-
-               if (cur_rate > rate)
-                       new_diff = cur_rate - rate;
-               else
-                       new_diff = rate - cur_rate;
-
-               if (!new_diff)
-                       break;
+       shift = fls(div) - 1;
 
-               if (new_diff < best_diff) {
-                       best_diff = new_diff;
-                       best_rate = cur_rate;
-               }
+       if (div != (1<<shift))
+               return -EINVAL;
 
-               if (rate > cur_rate)
-                       break;
-       }
+       if (shift >= PROG_PRES_MASK)
+               return -EINVAL;
 
        pmc_write(pmc, AT91_PMC_PCKR(prog->id),
                  tmp | (shift << layout->pres_shift));