1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com> |
4 | */ |
5 | |
6 | #include <linux/bitops.h> |
7 | #include <linux/clk-provider.h> |
8 | #include <linux/err.h> |
9 | #include <linux/export.h> |
10 | #include <linux/io.h> |
11 | #include <linux/kernel.h> |
12 | #include <linux/of.h> |
13 | #include <linux/slab.h> |
14 | |
15 | static inline u32 clk_mult_readl(struct clk_multiplier *mult) |
16 | { |
17 | if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN) |
18 | return ioread32be(mult->reg); |
19 | |
20 | return readl(addr: mult->reg); |
21 | } |
22 | |
23 | static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val) |
24 | { |
25 | if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN) |
26 | iowrite32be(val, mult->reg); |
27 | else |
28 | writel(val, addr: mult->reg); |
29 | } |
30 | |
31 | static unsigned long __get_mult(struct clk_multiplier *mult, |
32 | unsigned long rate, |
33 | unsigned long parent_rate) |
34 | { |
35 | if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST) |
36 | return DIV_ROUND_CLOSEST(rate, parent_rate); |
37 | |
38 | return rate / parent_rate; |
39 | } |
40 | |
41 | static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw, |
42 | unsigned long parent_rate) |
43 | { |
44 | struct clk_multiplier *mult = to_clk_multiplier(hw); |
45 | unsigned long val; |
46 | |
47 | val = clk_mult_readl(mult) >> mult->shift; |
48 | val &= GENMASK(mult->width - 1, 0); |
49 | |
50 | if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS) |
51 | val = 1; |
52 | |
53 | return parent_rate * val; |
54 | } |
55 | |
56 | static bool __is_best_rate(unsigned long rate, unsigned long new, |
57 | unsigned long best, unsigned long flags) |
58 | { |
59 | if (flags & CLK_MULTIPLIER_ROUND_CLOSEST) |
60 | return abs(rate - new) < abs(rate - best); |
61 | |
62 | return new >= rate && new < best; |
63 | } |
64 | |
65 | static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate, |
66 | unsigned long *best_parent_rate, |
67 | u8 width, unsigned long flags) |
68 | { |
69 | struct clk_multiplier *mult = to_clk_multiplier(hw); |
70 | unsigned long orig_parent_rate = *best_parent_rate; |
71 | unsigned long parent_rate, current_rate, best_rate = ~0; |
72 | unsigned int i, bestmult = 0; |
73 | unsigned int maxmult = (1 << width) - 1; |
74 | |
75 | if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { |
76 | bestmult = rate / orig_parent_rate; |
77 | |
78 | /* Make sure we don't end up with a 0 multiplier */ |
79 | if ((bestmult == 0) && |
80 | !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)) |
81 | bestmult = 1; |
82 | |
83 | /* Make sure we don't overflow the multiplier */ |
84 | if (bestmult > maxmult) |
85 | bestmult = maxmult; |
86 | |
87 | return bestmult; |
88 | } |
89 | |
90 | for (i = 1; i < maxmult; i++) { |
91 | if (rate == orig_parent_rate * i) { |
92 | /* |
93 | * This is the best case for us if we have a |
94 | * perfect match without changing the parent |
95 | * rate. |
96 | */ |
97 | *best_parent_rate = orig_parent_rate; |
98 | return i; |
99 | } |
100 | |
101 | parent_rate = clk_hw_round_rate(hw: clk_hw_get_parent(hw), |
102 | rate: rate / i); |
103 | current_rate = parent_rate * i; |
104 | |
105 | if (__is_best_rate(rate, new: current_rate, best: best_rate, flags)) { |
106 | bestmult = i; |
107 | best_rate = current_rate; |
108 | *best_parent_rate = parent_rate; |
109 | } |
110 | } |
111 | |
112 | return bestmult; |
113 | } |
114 | |
115 | static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate, |
116 | unsigned long *parent_rate) |
117 | { |
118 | struct clk_multiplier *mult = to_clk_multiplier(hw); |
119 | unsigned long factor = __bestmult(hw, rate, best_parent_rate: parent_rate, |
120 | width: mult->width, flags: mult->flags); |
121 | |
122 | return *parent_rate * factor; |
123 | } |
124 | |
125 | static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate, |
126 | unsigned long parent_rate) |
127 | { |
128 | struct clk_multiplier *mult = to_clk_multiplier(hw); |
129 | unsigned long factor = __get_mult(mult, rate, parent_rate); |
130 | unsigned long flags = 0; |
131 | unsigned long val; |
132 | |
133 | if (mult->lock) |
134 | spin_lock_irqsave(mult->lock, flags); |
135 | else |
136 | __acquire(mult->lock); |
137 | |
138 | val = clk_mult_readl(mult); |
139 | val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift); |
140 | val |= factor << mult->shift; |
141 | clk_mult_writel(mult, val); |
142 | |
143 | if (mult->lock) |
144 | spin_unlock_irqrestore(lock: mult->lock, flags); |
145 | else |
146 | __release(mult->lock); |
147 | |
148 | return 0; |
149 | } |
150 | |
151 | const struct clk_ops clk_multiplier_ops = { |
152 | .recalc_rate = clk_multiplier_recalc_rate, |
153 | .round_rate = clk_multiplier_round_rate, |
154 | .set_rate = clk_multiplier_set_rate, |
155 | }; |
156 | EXPORT_SYMBOL_GPL(clk_multiplier_ops); |
157 | |