diff options
Diffstat (limited to 'drivers/clk/renesas/clk-div6.c')
| -rw-r--r-- | drivers/clk/renesas/clk-div6.c | 80 |
1 files changed, 54 insertions, 26 deletions
diff --git a/drivers/clk/renesas/clk-div6.c b/drivers/clk/renesas/clk-div6.c index 8fb68e703a6b..3abd6e5400ad 100644 --- a/drivers/clk/renesas/clk-div6.c +++ b/drivers/clk/renesas/clk-div6.c @@ -28,8 +28,7 @@ * @hw: handle between common and hardware-specific interfaces * @reg: IO-remapped register * @div: divisor value (1-64) - * @src_shift: Shift to access the register bits to select the parent clock - * @src_width: Number of register bits to select the parent clock (may be 0) + * @src_mask: Bitmask covering the register bits to select the parent clock * @nb: Notifier block to save/restore clock state for system resume * @parents: Array to map from valid parent clocks indices to hardware indices */ @@ -37,8 +36,7 @@ struct div6_clock { struct clk_hw hw; void __iomem *reg; unsigned int div; - u32 src_shift; - u32 src_width; + u32 src_mask; struct notifier_block nb; u8 parents[]; }; @@ -99,15 +97,52 @@ static unsigned int cpg_div6_clock_calc_div(unsigned long rate, rate = 1; div = DIV_ROUND_CLOSEST(parent_rate, rate); - return clamp_t(unsigned int, div, 1, 64); + return clamp(div, 1U, 64U); } -static long cpg_div6_clock_round_rate(struct clk_hw *hw, unsigned long rate, - unsigned long *parent_rate) +static int cpg_div6_clock_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) { - unsigned int div = cpg_div6_clock_calc_div(rate, *parent_rate); + unsigned long prate, calc_rate, diff, best_rate, best_prate; + unsigned int num_parents = clk_hw_get_num_parents(hw); + struct clk_hw *parent, *best_parent = NULL; + unsigned int i, min_div, max_div, div; + unsigned long min_diff = ULONG_MAX; + + for (i = 0; i < num_parents; i++) { + parent = clk_hw_get_parent_by_index(hw, i); + if (!parent) + continue; + + prate = clk_hw_get_rate(parent); + if (!prate) + continue; + + min_div = max(DIV_ROUND_UP(prate, req->max_rate), 1UL); + max_div = req->min_rate ? min(prate / req->min_rate, 64UL) : 64; + if (max_div < min_div) + continue; + + div = cpg_div6_clock_calc_div(req->rate, prate); + div = clamp(div, min_div, max_div); + calc_rate = prate / div; + diff = calc_rate > req->rate ? calc_rate - req->rate + : req->rate - calc_rate; + if (diff < min_diff) { + best_rate = calc_rate; + best_parent = parent; + best_prate = prate; + min_diff = diff; + } + } + + if (!best_parent) + return -EINVAL; - return *parent_rate / div; + req->best_parent_rate = best_prate; + req->best_parent_hw = best_parent; + req->rate = best_rate; + return 0; } static int cpg_div6_clock_set_rate(struct clk_hw *hw, unsigned long rate, @@ -133,11 +168,11 @@ static u8 cpg_div6_clock_get_parent(struct clk_hw *hw) unsigned int i; u8 hw_index; - if (clock->src_width == 0) + if (clock->src_mask == 0) return 0; - hw_index = (readl(clock->reg) >> clock->src_shift) & - (BIT(clock->src_width) - 1); + hw_index = (readl(clock->reg) & clock->src_mask) >> + __ffs(clock->src_mask); for (i = 0; i < clk_hw_get_num_parents(hw); i++) { if (clock->parents[i] == hw_index) return i; @@ -151,18 +186,13 @@ static u8 cpg_div6_clock_get_parent(struct clk_hw *hw) static int cpg_div6_clock_set_parent(struct clk_hw *hw, u8 index) { struct div6_clock *clock = to_div6_clock(hw); - u8 hw_index; - u32 mask; + u32 src; if (index >= clk_hw_get_num_parents(hw)) return -EINVAL; - mask = ~((BIT(clock->src_width) - 1) << clock->src_shift); - hw_index = clock->parents[index]; - - writel((readl(clock->reg) & mask) | (hw_index << clock->src_shift), - clock->reg); - + src = clock->parents[index] << __ffs(clock->src_mask); + writel((readl(clock->reg) & ~clock->src_mask) | src, clock->reg); return 0; } @@ -173,7 +203,7 @@ static const struct clk_ops cpg_div6_clock_ops = { .get_parent = cpg_div6_clock_get_parent, .set_parent = cpg_div6_clock_set_parent, .recalc_rate = cpg_div6_clock_recalc_rate, - .round_rate = cpg_div6_clock_round_rate, + .determine_rate = cpg_div6_clock_determine_rate, .set_rate = cpg_div6_clock_set_rate, }; @@ -236,17 +266,15 @@ struct clk * __init cpg_div6_register(const char *name, switch (num_parents) { case 1: /* fixed parent clock */ - clock->src_shift = clock->src_width = 0; + clock->src_mask = 0; break; case 4: /* clock with EXSRC bits 6-7 */ - clock->src_shift = 6; - clock->src_width = 2; + clock->src_mask = GENMASK(7, 6); break; case 8: /* VCLK with EXSRC bits 12-14 */ - clock->src_shift = 12; - clock->src_width = 3; + clock->src_mask = GENMASK(14, 12); break; default: pr_err("%s: invalid number of parents for DIV6 clock %s\n", |