diff options
Diffstat (limited to 'drivers/clk/at91/clk-programmable.c')
| -rw-r--r-- | drivers/clk/at91/clk-programmable.c | 38 |
1 files changed, 8 insertions, 30 deletions
diff --git a/drivers/clk/at91/clk-programmable.c b/drivers/clk/at91/clk-programmable.c index 7bcc725f67af..62e2509f9df1 100644 --- 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)); |