1 // SPDX-License-Identifier: GPL-2.0
3 * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
6 #include <linux/bitops.h>
7 #include <linux/clk-provider.h>
9 #include <linux/export.h>
10 #include <linux/kernel.h>
12 #include <linux/slab.h>
14 static inline u32 clk_mult_readl(struct clk_multiplier *mult)
16 if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
17 return ioread32be(mult->reg);
19 return readl(mult->reg);
22 static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)
24 if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
25 iowrite32be(val, mult->reg);
27 writel(val, mult->reg);
30 static unsigned long __get_mult(struct clk_multiplier *mult,
32 unsigned long parent_rate)
34 if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
35 return DIV_ROUND_CLOSEST(rate, parent_rate);
37 return rate / parent_rate;
40 static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
41 unsigned long parent_rate)
43 struct clk_multiplier *mult = to_clk_multiplier(hw);
46 val = clk_mult_readl(mult) >> mult->shift;
47 val &= GENMASK(mult->width - 1, 0);
49 if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
52 return parent_rate * val;
55 static bool __is_best_rate(unsigned long rate, unsigned long new,
56 unsigned long best, unsigned long flags)
58 if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
59 return abs(rate - new) < abs(rate - best);
61 return new >= rate && new < best;
64 static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
65 unsigned long *best_parent_rate,
66 u8 width, unsigned long flags)
68 struct clk_multiplier *mult = to_clk_multiplier(hw);
69 unsigned long orig_parent_rate = *best_parent_rate;
70 unsigned long parent_rate, current_rate, best_rate = ~0;
71 unsigned int i, bestmult = 0;
72 unsigned int maxmult = (1 << width) - 1;
74 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
75 bestmult = rate / orig_parent_rate;
77 /* Make sure we don't end up with a 0 multiplier */
78 if ((bestmult == 0) &&
79 !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
82 /* Make sure we don't overflow the multiplier */
83 if (bestmult > maxmult)
89 for (i = 1; i < maxmult; i++) {
90 if (rate == orig_parent_rate * i) {
92 * This is the best case for us if we have a
93 * perfect match without changing the parent
96 *best_parent_rate = orig_parent_rate;
100 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
102 current_rate = parent_rate * i;
104 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
106 best_rate = current_rate;
107 *best_parent_rate = parent_rate;
114 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
115 unsigned long *parent_rate)
117 struct clk_multiplier *mult = to_clk_multiplier(hw);
118 unsigned long factor = __bestmult(hw, rate, parent_rate,
119 mult->width, mult->flags);
121 return *parent_rate * factor;
124 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
125 unsigned long parent_rate)
127 struct clk_multiplier *mult = to_clk_multiplier(hw);
128 unsigned long factor = __get_mult(mult, rate, parent_rate);
129 unsigned long flags = 0;
133 spin_lock_irqsave(mult->lock, flags);
135 __acquire(mult->lock);
137 val = clk_mult_readl(mult);
138 val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
139 val |= factor << mult->shift;
140 clk_mult_writel(mult, val);
143 spin_unlock_irqrestore(mult->lock, flags);
145 __release(mult->lock);
150 const struct clk_ops clk_multiplier_ops = {
151 .recalc_rate = clk_multiplier_recalc_rate,
152 .round_rate = clk_multiplier_round_rate,
153 .set_rate = clk_multiplier_set_rate,
155 EXPORT_SYMBOL_GPL(clk_multiplier_ops);