diff --git a/drivers/clk/sunxi-ng/ccu_mp.c b/drivers/clk/sunxi-ng/ccu_mp.c index 5d0af4051737..0357349eb767 100644 --- a/drivers/clk/sunxi-ng/ccu_mp.c +++ b/drivers/clk/sunxi-ng/ccu_mp.c @@ -40,6 +40,61 @@ static void ccu_mp_find_best(unsigned long parent, unsigned long rate, *p = best_p; } +static unsigned long ccu_mp_find_best_with_parent_adj(struct clk_hw *hw, + unsigned long *parent, + unsigned long rate, + unsigned int max_m, + unsigned int max_p) +{ + unsigned long parent_rate_saved; + unsigned long parent_rate, now; + unsigned long best_rate = 0; + unsigned int _m, _p, div; + unsigned long maxdiv; + + parent_rate_saved = *parent; + + /* + * The maximum divider we can use without overflowing + * unsigned long in rate * m * p below + */ + maxdiv = max_m * max_p; + maxdiv = min(ULONG_MAX / rate, maxdiv); + + for (_p = 1; _p <= max_p; _p <<= 1) { + for (_m = 1; _m <= max_m; _m++) { + div = _m * _p; + + if (div > maxdiv) + break; + + if (rate * div == parent_rate_saved) { + /* + * It's the most ideal case if the requested + * rate can be divided from parent clock without + * needing to change parent rate, so return the + * divider immediately. + */ + *parent = parent_rate_saved; + return rate; + } + + parent_rate = clk_hw_round_rate(hw, rate * div); + now = parent_rate / div; + + if (now <= rate && now > best_rate) { + best_rate = now; + *parent = parent_rate; + + if (now == rate) + return rate; + } + } + } + + return best_rate; +} + static unsigned long ccu_mp_round_rate(struct ccu_mux_internal *mux, struct clk_hw *hw, unsigned long *parent_rate, @@ -56,8 +111,13 @@ static unsigned long ccu_mp_round_rate(struct ccu_mux_internal *mux, max_m = cmp->m.max ?: 1 << cmp->m.width; max_p = cmp->p.max ?: 1 << ((1 << cmp->p.width) - 1); - ccu_mp_find_best(*parent_rate, rate, max_m, max_p, &m, &p); - rate = *parent_rate / p / m; + if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { + ccu_mp_find_best(*parent_rate, rate, max_m, max_p, &m, &p); + rate = *parent_rate / p / m; + } else { + rate = ccu_mp_find_best_with_parent_adj(hw, parent_rate, rate, + max_m, max_p); + } if (cmp->common.features & CCU_FEATURE_FIXED_POSTDIV) rate /= cmp->fixed_post_div;