]>
Commit | Line | Data |
---|---|---|
956060a5 EZ |
1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* | |
3 | * Copyright (c) 2018 Fuzhou Rockchip Electronics Co., Ltd | |
4 | */ | |
5 | ||
956060a5 | 6 | #include <linux/clk-provider.h> |
62e59c4e SB |
7 | #include <linux/io.h> |
8 | #include <linux/slab.h> | |
956060a5 EZ |
9 | #include "clk.h" |
10 | ||
11 | #define div_mask(width) ((1 << (width)) - 1) | |
12 | ||
13 | static bool _is_best_half_div(unsigned long rate, unsigned long now, | |
14 | unsigned long best, unsigned long flags) | |
15 | { | |
16 | if (flags & CLK_DIVIDER_ROUND_CLOSEST) | |
17 | return abs(rate - now) < abs(rate - best); | |
18 | ||
19 | return now <= rate && now > best; | |
20 | } | |
21 | ||
22 | static unsigned long clk_half_divider_recalc_rate(struct clk_hw *hw, | |
23 | unsigned long parent_rate) | |
24 | { | |
25 | struct clk_divider *divider = to_clk_divider(hw); | |
26 | unsigned int val; | |
27 | ||
5834fd75 | 28 | val = readl(divider->reg) >> divider->shift; |
956060a5 EZ |
29 | val &= div_mask(divider->width); |
30 | val = val * 2 + 3; | |
31 | ||
32 | return DIV_ROUND_UP_ULL(((u64)parent_rate * 2), val); | |
33 | } | |
34 | ||
35 | static int clk_half_divider_bestdiv(struct clk_hw *hw, unsigned long rate, | |
36 | unsigned long *best_parent_rate, u8 width, | |
37 | unsigned long flags) | |
38 | { | |
39 | unsigned int i, bestdiv = 0; | |
40 | unsigned long parent_rate, best = 0, now, maxdiv; | |
41 | unsigned long parent_rate_saved = *best_parent_rate; | |
42 | ||
43 | if (!rate) | |
44 | rate = 1; | |
45 | ||
46 | maxdiv = div_mask(width); | |
47 | ||
48 | if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { | |
49 | parent_rate = *best_parent_rate; | |
50 | bestdiv = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), rate); | |
51 | if (bestdiv < 3) | |
52 | bestdiv = 0; | |
53 | else | |
54 | bestdiv = (bestdiv - 3) / 2; | |
55 | bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv; | |
56 | return bestdiv; | |
57 | } | |
58 | ||
59 | /* | |
60 | * The maximum divider we can use without overflowing | |
61 | * unsigned long in rate * i below | |
62 | */ | |
63 | maxdiv = min(ULONG_MAX / rate, maxdiv); | |
64 | ||
65 | for (i = 0; i <= maxdiv; i++) { | |
66 | if (((u64)rate * (i * 2 + 3)) == ((u64)parent_rate_saved * 2)) { | |
67 | /* | |
68 | * It's the most ideal case if the requested rate can be | |
69 | * divided from parent clock without needing to change | |
70 | * parent rate, so return the divider immediately. | |
71 | */ | |
72 | *best_parent_rate = parent_rate_saved; | |
73 | return i; | |
74 | } | |
75 | parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), | |
76 | ((u64)rate * (i * 2 + 3)) / 2); | |
77 | now = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), | |
78 | (i * 2 + 3)); | |
79 | ||
80 | if (_is_best_half_div(rate, now, best, flags)) { | |
81 | bestdiv = i; | |
82 | best = now; | |
83 | *best_parent_rate = parent_rate; | |
84 | } | |
85 | } | |
86 | ||
87 | if (!bestdiv) { | |
88 | bestdiv = div_mask(width); | |
89 | *best_parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), 1); | |
90 | } | |
91 | ||
92 | return bestdiv; | |
93 | } | |
94 | ||
95 | static long clk_half_divider_round_rate(struct clk_hw *hw, unsigned long rate, | |
96 | unsigned long *prate) | |
97 | { | |
98 | struct clk_divider *divider = to_clk_divider(hw); | |
99 | int div; | |
100 | ||
101 | div = clk_half_divider_bestdiv(hw, rate, prate, | |
102 | divider->width, | |
103 | divider->flags); | |
104 | ||
105 | return DIV_ROUND_UP_ULL(((u64)*prate * 2), div * 2 + 3); | |
106 | } | |
107 | ||
108 | static int clk_half_divider_set_rate(struct clk_hw *hw, unsigned long rate, | |
109 | unsigned long parent_rate) | |
110 | { | |
111 | struct clk_divider *divider = to_clk_divider(hw); | |
112 | unsigned int value; | |
113 | unsigned long flags = 0; | |
114 | u32 val; | |
115 | ||
116 | value = DIV_ROUND_UP_ULL(((u64)parent_rate * 2), rate); | |
117 | value = (value - 3) / 2; | |
118 | value = min_t(unsigned int, value, div_mask(divider->width)); | |
119 | ||
120 | if (divider->lock) | |
121 | spin_lock_irqsave(divider->lock, flags); | |
122 | else | |
123 | __acquire(divider->lock); | |
124 | ||
125 | if (divider->flags & CLK_DIVIDER_HIWORD_MASK) { | |
126 | val = div_mask(divider->width) << (divider->shift + 16); | |
127 | } else { | |
5834fd75 | 128 | val = readl(divider->reg); |
956060a5 EZ |
129 | val &= ~(div_mask(divider->width) << divider->shift); |
130 | } | |
131 | val |= value << divider->shift; | |
5834fd75 | 132 | writel(val, divider->reg); |
956060a5 EZ |
133 | |
134 | if (divider->lock) | |
135 | spin_unlock_irqrestore(divider->lock, flags); | |
136 | else | |
137 | __release(divider->lock); | |
138 | ||
139 | return 0; | |
140 | } | |
141 | ||
bbbbd246 | 142 | static const struct clk_ops clk_half_divider_ops = { |
956060a5 EZ |
143 | .recalc_rate = clk_half_divider_recalc_rate, |
144 | .round_rate = clk_half_divider_round_rate, | |
145 | .set_rate = clk_half_divider_set_rate, | |
146 | }; | |
956060a5 EZ |
147 | |
148 | /** | |
149 | * Register a clock branch. | |
150 | * Most clock branches have a form like | |
151 | * | |
152 | * src1 --|--\ | |
153 | * |M |--[GATE]-[DIV]- | |
154 | * src2 --|--/ | |
155 | * | |
156 | * sometimes without one of those components. | |
157 | */ | |
158 | struct clk *rockchip_clk_register_halfdiv(const char *name, | |
159 | const char *const *parent_names, | |
160 | u8 num_parents, void __iomem *base, | |
161 | int muxdiv_offset, u8 mux_shift, | |
162 | u8 mux_width, u8 mux_flags, | |
163 | u8 div_shift, u8 div_width, | |
164 | u8 div_flags, int gate_offset, | |
165 | u8 gate_shift, u8 gate_flags, | |
166 | unsigned long flags, | |
167 | spinlock_t *lock) | |
168 | { | |
169 | struct clk *clk; | |
170 | struct clk_mux *mux = NULL; | |
171 | struct clk_gate *gate = NULL; | |
172 | struct clk_divider *div = NULL; | |
173 | const struct clk_ops *mux_ops = NULL, *div_ops = NULL, | |
174 | *gate_ops = NULL; | |
175 | ||
176 | if (num_parents > 1) { | |
177 | mux = kzalloc(sizeof(*mux), GFP_KERNEL); | |
178 | if (!mux) | |
179 | return ERR_PTR(-ENOMEM); | |
180 | ||
181 | mux->reg = base + muxdiv_offset; | |
182 | mux->shift = mux_shift; | |
183 | mux->mask = BIT(mux_width) - 1; | |
184 | mux->flags = mux_flags; | |
185 | mux->lock = lock; | |
186 | mux_ops = (mux_flags & CLK_MUX_READ_ONLY) ? &clk_mux_ro_ops | |
187 | : &clk_mux_ops; | |
188 | } | |
189 | ||
190 | if (gate_offset >= 0) { | |
191 | gate = kzalloc(sizeof(*gate), GFP_KERNEL); | |
192 | if (!gate) | |
193 | goto err_gate; | |
194 | ||
195 | gate->flags = gate_flags; | |
196 | gate->reg = base + gate_offset; | |
197 | gate->bit_idx = gate_shift; | |
198 | gate->lock = lock; | |
199 | gate_ops = &clk_gate_ops; | |
200 | } | |
201 | ||
202 | if (div_width > 0) { | |
203 | div = kzalloc(sizeof(*div), GFP_KERNEL); | |
204 | if (!div) | |
205 | goto err_div; | |
206 | ||
207 | div->flags = div_flags; | |
208 | div->reg = base + muxdiv_offset; | |
209 | div->shift = div_shift; | |
210 | div->width = div_width; | |
211 | div->lock = lock; | |
212 | div_ops = &clk_half_divider_ops; | |
213 | } | |
214 | ||
215 | clk = clk_register_composite(NULL, name, parent_names, num_parents, | |
216 | mux ? &mux->hw : NULL, mux_ops, | |
217 | div ? &div->hw : NULL, div_ops, | |
218 | gate ? &gate->hw : NULL, gate_ops, | |
219 | flags); | |
220 | ||
221 | return clk; | |
222 | err_div: | |
223 | kfree(gate); | |
224 | err_gate: | |
225 | kfree(mux); | |
226 | return ERR_PTR(-ENOMEM); | |
227 | } |