1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Zynq UltraScale+ MPSoC Divider support |
4 | * |
5 | * Copyright (C) 2016-2019 Xilinx |
6 | * |
7 | * Adjustable divider clock implementation |
8 | */ |
9 | |
10 | #include <linux/clk.h> |
11 | #include <linux/clk-provider.h> |
12 | #include <linux/slab.h> |
13 | #include "clk-zynqmp.h" |
14 | |
15 | /* |
16 | * DOC: basic adjustable divider clock that cannot gate |
17 | * |
18 | * Traits of this clock: |
19 | * prepare - clk_prepare only ensures that parents are prepared |
20 | * enable - clk_enable only ensures that parents are enabled |
21 | * rate - rate is adjustable. clk->rate = ceiling(parent->rate / divisor) |
22 | * parent - fixed parent. No clk_set_parent support |
23 | */ |
24 | |
25 | #define to_zynqmp_clk_divider(_hw) \ |
26 | container_of(_hw, struct zynqmp_clk_divider, hw) |
27 | |
28 | #define CLK_FRAC BIT(13) /* has a fractional parent */ |
29 | #define CUSTOM_FLAG_CLK_FRAC BIT(0) /* has a fractional parent in custom type flag */ |
30 | |
31 | /** |
32 | * struct zynqmp_clk_divider - adjustable divider clock |
33 | * @hw: handle between common and hardware-specific interfaces |
34 | * @flags: Hardware specific flags |
35 | * @is_frac: The divider is a fractional divider |
36 | * @clk_id: Id of clock |
37 | * @div_type: divisor type (TYPE_DIV1 or TYPE_DIV2) |
38 | * @max_div: maximum supported divisor (fetched from firmware) |
39 | */ |
40 | struct zynqmp_clk_divider { |
41 | struct clk_hw hw; |
42 | u8 flags; |
43 | bool is_frac; |
44 | u32 clk_id; |
45 | u32 div_type; |
46 | u16 max_div; |
47 | }; |
48 | |
49 | static inline int zynqmp_divider_get_val(unsigned long parent_rate, |
50 | unsigned long rate, u16 flags) |
51 | { |
52 | int up, down; |
53 | unsigned long up_rate, down_rate; |
54 | |
55 | if (flags & CLK_DIVIDER_POWER_OF_TWO) { |
56 | up = DIV_ROUND_UP_ULL((u64)parent_rate, rate); |
57 | down = DIV_ROUND_DOWN_ULL((u64)parent_rate, rate); |
58 | |
59 | up = __roundup_pow_of_two(n: up); |
60 | down = __rounddown_pow_of_two(n: down); |
61 | |
62 | up_rate = DIV_ROUND_UP_ULL((u64)parent_rate, up); |
63 | down_rate = DIV_ROUND_UP_ULL((u64)parent_rate, down); |
64 | |
65 | return (rate - up_rate) <= (down_rate - rate) ? up : down; |
66 | |
67 | } else { |
68 | return DIV_ROUND_CLOSEST(parent_rate, rate); |
69 | } |
70 | } |
71 | |
72 | /** |
73 | * zynqmp_clk_divider_recalc_rate() - Recalc rate of divider clock |
74 | * @hw: handle between common and hardware-specific interfaces |
75 | * @parent_rate: rate of parent clock |
76 | * |
77 | * Return: 0 on success else error+reason |
78 | */ |
79 | static unsigned long zynqmp_clk_divider_recalc_rate(struct clk_hw *hw, |
80 | unsigned long parent_rate) |
81 | { |
82 | struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw); |
83 | const char *clk_name = clk_hw_get_name(hw); |
84 | u32 clk_id = divider->clk_id; |
85 | u32 div_type = divider->div_type; |
86 | u32 div, value; |
87 | int ret; |
88 | |
89 | ret = zynqmp_pm_clock_getdivider(clock_id: clk_id, divider: &div); |
90 | |
91 | if (ret) |
92 | pr_debug("%s() get divider failed for %s, ret = %d\n" , |
93 | __func__, clk_name, ret); |
94 | |
95 | if (div_type == TYPE_DIV1) |
96 | value = div & 0xFFFF; |
97 | else |
98 | value = div >> 16; |
99 | |
100 | if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) |
101 | value = 1 << value; |
102 | |
103 | if (!value) { |
104 | WARN(!(divider->flags & CLK_DIVIDER_ALLOW_ZERO), |
105 | "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n" , |
106 | clk_name); |
107 | return parent_rate; |
108 | } |
109 | |
110 | return DIV_ROUND_UP_ULL(parent_rate, value); |
111 | } |
112 | |
113 | static void zynqmp_get_divider2_val(struct clk_hw *hw, |
114 | unsigned long rate, |
115 | struct zynqmp_clk_divider *divider, |
116 | u32 *bestdiv) |
117 | { |
118 | int div1; |
119 | int div2; |
120 | long error = LONG_MAX; |
121 | unsigned long div1_prate; |
122 | struct clk_hw *div1_parent_hw; |
123 | struct zynqmp_clk_divider *pdivider; |
124 | struct clk_hw *div2_parent_hw = clk_hw_get_parent(hw); |
125 | |
126 | if (!div2_parent_hw) |
127 | return; |
128 | |
129 | pdivider = to_zynqmp_clk_divider(div2_parent_hw); |
130 | if (!pdivider) |
131 | return; |
132 | |
133 | div1_parent_hw = clk_hw_get_parent(hw: div2_parent_hw); |
134 | if (!div1_parent_hw) |
135 | return; |
136 | |
137 | div1_prate = clk_hw_get_rate(hw: div1_parent_hw); |
138 | *bestdiv = 1; |
139 | for (div1 = 1; div1 <= pdivider->max_div;) { |
140 | for (div2 = 1; div2 <= divider->max_div;) { |
141 | long new_error = ((div1_prate / div1) / div2) - rate; |
142 | |
143 | if (abs(new_error) < abs(error)) { |
144 | *bestdiv = div2; |
145 | error = new_error; |
146 | } |
147 | if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) |
148 | div2 = div2 << 1; |
149 | else |
150 | div2++; |
151 | } |
152 | if (pdivider->flags & CLK_DIVIDER_POWER_OF_TWO) |
153 | div1 = div1 << 1; |
154 | else |
155 | div1++; |
156 | } |
157 | } |
158 | |
159 | /** |
160 | * zynqmp_clk_divider_round_rate() - Round rate of divider clock |
161 | * @hw: handle between common and hardware-specific interfaces |
162 | * @rate: rate of clock to be set |
163 | * @prate: rate of parent clock |
164 | * |
165 | * Return: 0 on success else error+reason |
166 | */ |
167 | static long zynqmp_clk_divider_round_rate(struct clk_hw *hw, |
168 | unsigned long rate, |
169 | unsigned long *prate) |
170 | { |
171 | struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw); |
172 | const char *clk_name = clk_hw_get_name(hw); |
173 | u32 clk_id = divider->clk_id; |
174 | u32 div_type = divider->div_type; |
175 | u32 bestdiv; |
176 | int ret; |
177 | |
178 | /* if read only, just return current value */ |
179 | if (divider->flags & CLK_DIVIDER_READ_ONLY) { |
180 | ret = zynqmp_pm_clock_getdivider(clock_id: clk_id, divider: &bestdiv); |
181 | |
182 | if (ret) |
183 | pr_debug("%s() get divider failed for %s, ret = %d\n" , |
184 | __func__, clk_name, ret); |
185 | if (div_type == TYPE_DIV1) |
186 | bestdiv = bestdiv & 0xFFFF; |
187 | else |
188 | bestdiv = bestdiv >> 16; |
189 | |
190 | if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) |
191 | bestdiv = 1 << bestdiv; |
192 | |
193 | return DIV_ROUND_UP_ULL((u64)*prate, bestdiv); |
194 | } |
195 | |
196 | bestdiv = zynqmp_divider_get_val(parent_rate: *prate, rate, flags: divider->flags); |
197 | |
198 | /* |
199 | * In case of two divisors, compute best divider values and return |
200 | * divider2 value based on compute value. div1 will be automatically |
201 | * set to optimum based on required total divider value. |
202 | */ |
203 | if (div_type == TYPE_DIV2 && |
204 | (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { |
205 | zynqmp_get_divider2_val(hw, rate, divider, bestdiv: &bestdiv); |
206 | } |
207 | |
208 | if ((clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) && divider->is_frac) |
209 | bestdiv = rate % *prate ? 1 : bestdiv; |
210 | |
211 | bestdiv = min_t(u32, bestdiv, divider->max_div); |
212 | *prate = rate * bestdiv; |
213 | |
214 | return rate; |
215 | } |
216 | |
217 | /** |
218 | * zynqmp_clk_divider_set_rate() - Set rate of divider clock |
219 | * @hw: handle between common and hardware-specific interfaces |
220 | * @rate: rate of clock to be set |
221 | * @parent_rate: rate of parent clock |
222 | * |
223 | * Return: 0 on success else error+reason |
224 | */ |
225 | static int zynqmp_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate, |
226 | unsigned long parent_rate) |
227 | { |
228 | struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw); |
229 | const char *clk_name = clk_hw_get_name(hw); |
230 | u32 clk_id = divider->clk_id; |
231 | u32 div_type = divider->div_type; |
232 | u32 value, div; |
233 | int ret; |
234 | |
235 | value = zynqmp_divider_get_val(parent_rate, rate, flags: divider->flags); |
236 | if (div_type == TYPE_DIV1) { |
237 | div = value & 0xFFFF; |
238 | div |= 0xffff << 16; |
239 | } else { |
240 | div = 0xffff; |
241 | div |= value << 16; |
242 | } |
243 | |
244 | if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) |
245 | div = __ffs(div); |
246 | |
247 | ret = zynqmp_pm_clock_setdivider(clock_id: clk_id, divider: div); |
248 | |
249 | if (ret) |
250 | pr_debug("%s() set divider failed for %s, ret = %d\n" , |
251 | __func__, clk_name, ret); |
252 | |
253 | return ret; |
254 | } |
255 | |
256 | static const struct clk_ops zynqmp_clk_divider_ops = { |
257 | .recalc_rate = zynqmp_clk_divider_recalc_rate, |
258 | .round_rate = zynqmp_clk_divider_round_rate, |
259 | .set_rate = zynqmp_clk_divider_set_rate, |
260 | }; |
261 | |
262 | static const struct clk_ops zynqmp_clk_divider_ro_ops = { |
263 | .recalc_rate = zynqmp_clk_divider_recalc_rate, |
264 | .round_rate = zynqmp_clk_divider_round_rate, |
265 | }; |
266 | |
267 | /** |
268 | * zynqmp_clk_get_max_divisor() - Get maximum supported divisor from firmware. |
269 | * @clk_id: Id of clock |
270 | * @type: Divider type |
271 | * |
272 | * Return: Maximum divisor of a clock if query data is successful |
273 | * U16_MAX in case of query data is not success |
274 | */ |
275 | static u32 zynqmp_clk_get_max_divisor(u32 clk_id, u32 type) |
276 | { |
277 | struct zynqmp_pm_query_data qdata = {0}; |
278 | u32 ret_payload[PAYLOAD_ARG_CNT]; |
279 | int ret; |
280 | |
281 | qdata.qid = PM_QID_CLOCK_GET_MAX_DIVISOR; |
282 | qdata.arg1 = clk_id; |
283 | qdata.arg2 = type; |
284 | ret = zynqmp_pm_query_data(qdata, out: ret_payload); |
285 | /* |
286 | * To maintain backward compatibility return maximum possible value |
287 | * (0xFFFF) if query for max divisor is not successful. |
288 | */ |
289 | if (ret) |
290 | return U16_MAX; |
291 | |
292 | return ret_payload[1]; |
293 | } |
294 | |
295 | static inline unsigned long zynqmp_clk_map_divider_ccf_flags( |
296 | const u32 zynqmp_type_flag) |
297 | { |
298 | unsigned long ccf_flag = 0; |
299 | |
300 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ONE_BASED) |
301 | ccf_flag |= CLK_DIVIDER_ONE_BASED; |
302 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_POWER_OF_TWO) |
303 | ccf_flag |= CLK_DIVIDER_POWER_OF_TWO; |
304 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ALLOW_ZERO) |
305 | ccf_flag |= CLK_DIVIDER_ALLOW_ZERO; |
306 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_POWER_OF_TWO) |
307 | ccf_flag |= CLK_DIVIDER_HIWORD_MASK; |
308 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ROUND_CLOSEST) |
309 | ccf_flag |= CLK_DIVIDER_ROUND_CLOSEST; |
310 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_READ_ONLY) |
311 | ccf_flag |= CLK_DIVIDER_READ_ONLY; |
312 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_MAX_AT_ZERO) |
313 | ccf_flag |= CLK_DIVIDER_MAX_AT_ZERO; |
314 | |
315 | return ccf_flag; |
316 | } |
317 | |
318 | /** |
319 | * zynqmp_clk_register_divider() - Register a divider clock |
320 | * @name: Name of this clock |
321 | * @clk_id: Id of clock |
322 | * @parents: Name of this clock's parents |
323 | * @num_parents: Number of parents |
324 | * @nodes: Clock topology node |
325 | * |
326 | * Return: clock hardware to registered clock divider |
327 | */ |
328 | struct clk_hw *zynqmp_clk_register_divider(const char *name, |
329 | u32 clk_id, |
330 | const char * const *parents, |
331 | u8 num_parents, |
332 | const struct clock_topology *nodes) |
333 | { |
334 | struct zynqmp_clk_divider *div; |
335 | struct clk_hw *hw; |
336 | struct clk_init_data init; |
337 | int ret; |
338 | |
339 | /* allocate the divider */ |
340 | div = kzalloc(size: sizeof(*div), GFP_KERNEL); |
341 | if (!div) |
342 | return ERR_PTR(error: -ENOMEM); |
343 | |
344 | init.name = name; |
345 | if (nodes->type_flag & CLK_DIVIDER_READ_ONLY) |
346 | init.ops = &zynqmp_clk_divider_ro_ops; |
347 | else |
348 | init.ops = &zynqmp_clk_divider_ops; |
349 | |
350 | init.flags = zynqmp_clk_map_common_ccf_flags(zynqmp_flag: nodes->flag); |
351 | |
352 | init.parent_names = parents; |
353 | init.num_parents = 1; |
354 | |
355 | /* struct clk_divider assignments */ |
356 | div->is_frac = !!((nodes->flag & CLK_FRAC) | |
357 | (nodes->custom_type_flag & CUSTOM_FLAG_CLK_FRAC)); |
358 | div->flags = zynqmp_clk_map_divider_ccf_flags(zynqmp_type_flag: nodes->type_flag); |
359 | div->hw.init = &init; |
360 | div->clk_id = clk_id; |
361 | div->div_type = nodes->type; |
362 | |
363 | /* |
364 | * To achieve best possible rate, maximum limit of divider is required |
365 | * while computation. |
366 | */ |
367 | div->max_div = zynqmp_clk_get_max_divisor(clk_id, type: nodes->type); |
368 | |
369 | hw = &div->hw; |
370 | ret = clk_hw_register(NULL, hw); |
371 | if (ret) { |
372 | kfree(objp: div); |
373 | hw = ERR_PTR(error: ret); |
374 | } |
375 | |
376 | return hw; |
377 | } |
378 | |