1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Copyright (c) 2013 NVIDIA CORPORATION. All rights reserved. |
4 | */ |
5 | |
6 | #include <linux/clk-provider.h> |
7 | #include <linux/device.h> |
8 | #include <linux/err.h> |
9 | #include <linux/slab.h> |
10 | |
11 | static u8 clk_composite_get_parent(struct clk_hw *hw) |
12 | { |
13 | struct clk_composite *composite = to_clk_composite(hw); |
14 | const struct clk_ops *mux_ops = composite->mux_ops; |
15 | struct clk_hw *mux_hw = composite->mux_hw; |
16 | |
17 | __clk_hw_set_clk(dst: mux_hw, src: hw); |
18 | |
19 | return mux_ops->get_parent(mux_hw); |
20 | } |
21 | |
22 | static int clk_composite_set_parent(struct clk_hw *hw, u8 index) |
23 | { |
24 | struct clk_composite *composite = to_clk_composite(hw); |
25 | const struct clk_ops *mux_ops = composite->mux_ops; |
26 | struct clk_hw *mux_hw = composite->mux_hw; |
27 | |
28 | __clk_hw_set_clk(dst: mux_hw, src: hw); |
29 | |
30 | return mux_ops->set_parent(mux_hw, index); |
31 | } |
32 | |
33 | static unsigned long clk_composite_recalc_rate(struct clk_hw *hw, |
34 | unsigned long parent_rate) |
35 | { |
36 | struct clk_composite *composite = to_clk_composite(hw); |
37 | const struct clk_ops *rate_ops = composite->rate_ops; |
38 | struct clk_hw *rate_hw = composite->rate_hw; |
39 | |
40 | __clk_hw_set_clk(dst: rate_hw, src: hw); |
41 | |
42 | return rate_ops->recalc_rate(rate_hw, parent_rate); |
43 | } |
44 | |
45 | static int clk_composite_determine_rate_for_parent(struct clk_hw *rate_hw, |
46 | struct clk_rate_request *req, |
47 | struct clk_hw *parent_hw, |
48 | const struct clk_ops *rate_ops) |
49 | { |
50 | long rate; |
51 | |
52 | req->best_parent_hw = parent_hw; |
53 | req->best_parent_rate = clk_hw_get_rate(hw: parent_hw); |
54 | |
55 | if (rate_ops->determine_rate) |
56 | return rate_ops->determine_rate(rate_hw, req); |
57 | |
58 | rate = rate_ops->round_rate(rate_hw, req->rate, |
59 | &req->best_parent_rate); |
60 | if (rate < 0) |
61 | return rate; |
62 | |
63 | req->rate = rate; |
64 | |
65 | return 0; |
66 | } |
67 | |
68 | static int clk_composite_determine_rate(struct clk_hw *hw, |
69 | struct clk_rate_request *req) |
70 | { |
71 | struct clk_composite *composite = to_clk_composite(hw); |
72 | const struct clk_ops *rate_ops = composite->rate_ops; |
73 | const struct clk_ops *mux_ops = composite->mux_ops; |
74 | struct clk_hw *rate_hw = composite->rate_hw; |
75 | struct clk_hw *mux_hw = composite->mux_hw; |
76 | struct clk_hw *parent; |
77 | unsigned long rate_diff; |
78 | unsigned long best_rate_diff = ULONG_MAX; |
79 | unsigned long best_rate = 0; |
80 | int i, ret; |
81 | |
82 | if (rate_hw && rate_ops && |
83 | (rate_ops->determine_rate || rate_ops->round_rate) && |
84 | mux_hw && mux_ops && mux_ops->set_parent) { |
85 | req->best_parent_hw = NULL; |
86 | |
87 | if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) { |
88 | struct clk_rate_request tmp_req; |
89 | |
90 | parent = clk_hw_get_parent(hw: mux_hw); |
91 | |
92 | clk_hw_forward_rate_request(core: hw, old_req: req, parent, req: &tmp_req, parent_rate: req->rate); |
93 | ret = clk_composite_determine_rate_for_parent(rate_hw, |
94 | req: &tmp_req, |
95 | parent_hw: parent, |
96 | rate_ops); |
97 | if (ret) |
98 | return ret; |
99 | |
100 | req->rate = tmp_req.rate; |
101 | req->best_parent_hw = tmp_req.best_parent_hw; |
102 | req->best_parent_rate = tmp_req.best_parent_rate; |
103 | |
104 | return 0; |
105 | } |
106 | |
107 | for (i = 0; i < clk_hw_get_num_parents(hw: mux_hw); i++) { |
108 | struct clk_rate_request tmp_req; |
109 | |
110 | parent = clk_hw_get_parent_by_index(hw: mux_hw, index: i); |
111 | if (!parent) |
112 | continue; |
113 | |
114 | clk_hw_forward_rate_request(core: hw, old_req: req, parent, req: &tmp_req, parent_rate: req->rate); |
115 | ret = clk_composite_determine_rate_for_parent(rate_hw, |
116 | req: &tmp_req, |
117 | parent_hw: parent, |
118 | rate_ops); |
119 | if (ret) |
120 | continue; |
121 | |
122 | if (req->rate >= tmp_req.rate) |
123 | rate_diff = req->rate - tmp_req.rate; |
124 | else |
125 | rate_diff = tmp_req.rate - req->rate; |
126 | |
127 | if (!rate_diff || !req->best_parent_hw |
128 | || best_rate_diff > rate_diff) { |
129 | req->best_parent_hw = parent; |
130 | req->best_parent_rate = tmp_req.best_parent_rate; |
131 | best_rate_diff = rate_diff; |
132 | best_rate = tmp_req.rate; |
133 | } |
134 | |
135 | if (!rate_diff) |
136 | return 0; |
137 | } |
138 | |
139 | req->rate = best_rate; |
140 | return 0; |
141 | } else if (rate_hw && rate_ops && rate_ops->determine_rate) { |
142 | __clk_hw_set_clk(dst: rate_hw, src: hw); |
143 | return rate_ops->determine_rate(rate_hw, req); |
144 | } else if (mux_hw && mux_ops && mux_ops->determine_rate) { |
145 | __clk_hw_set_clk(dst: mux_hw, src: hw); |
146 | return mux_ops->determine_rate(mux_hw, req); |
147 | } else { |
148 | pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n" ); |
149 | return -EINVAL; |
150 | } |
151 | } |
152 | |
153 | static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate, |
154 | unsigned long *prate) |
155 | { |
156 | struct clk_composite *composite = to_clk_composite(hw); |
157 | const struct clk_ops *rate_ops = composite->rate_ops; |
158 | struct clk_hw *rate_hw = composite->rate_hw; |
159 | |
160 | __clk_hw_set_clk(dst: rate_hw, src: hw); |
161 | |
162 | return rate_ops->round_rate(rate_hw, rate, prate); |
163 | } |
164 | |
165 | static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate, |
166 | unsigned long parent_rate) |
167 | { |
168 | struct clk_composite *composite = to_clk_composite(hw); |
169 | const struct clk_ops *rate_ops = composite->rate_ops; |
170 | struct clk_hw *rate_hw = composite->rate_hw; |
171 | |
172 | __clk_hw_set_clk(dst: rate_hw, src: hw); |
173 | |
174 | return rate_ops->set_rate(rate_hw, rate, parent_rate); |
175 | } |
176 | |
177 | static int clk_composite_set_rate_and_parent(struct clk_hw *hw, |
178 | unsigned long rate, |
179 | unsigned long parent_rate, |
180 | u8 index) |
181 | { |
182 | struct clk_composite *composite = to_clk_composite(hw); |
183 | const struct clk_ops *rate_ops = composite->rate_ops; |
184 | const struct clk_ops *mux_ops = composite->mux_ops; |
185 | struct clk_hw *rate_hw = composite->rate_hw; |
186 | struct clk_hw *mux_hw = composite->mux_hw; |
187 | unsigned long temp_rate; |
188 | |
189 | __clk_hw_set_clk(dst: rate_hw, src: hw); |
190 | __clk_hw_set_clk(dst: mux_hw, src: hw); |
191 | |
192 | temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate); |
193 | if (temp_rate > rate) { |
194 | rate_ops->set_rate(rate_hw, rate, parent_rate); |
195 | mux_ops->set_parent(mux_hw, index); |
196 | } else { |
197 | mux_ops->set_parent(mux_hw, index); |
198 | rate_ops->set_rate(rate_hw, rate, parent_rate); |
199 | } |
200 | |
201 | return 0; |
202 | } |
203 | |
204 | static int clk_composite_is_enabled(struct clk_hw *hw) |
205 | { |
206 | struct clk_composite *composite = to_clk_composite(hw); |
207 | const struct clk_ops *gate_ops = composite->gate_ops; |
208 | struct clk_hw *gate_hw = composite->gate_hw; |
209 | |
210 | __clk_hw_set_clk(dst: gate_hw, src: hw); |
211 | |
212 | return gate_ops->is_enabled(gate_hw); |
213 | } |
214 | |
215 | static int clk_composite_enable(struct clk_hw *hw) |
216 | { |
217 | struct clk_composite *composite = to_clk_composite(hw); |
218 | const struct clk_ops *gate_ops = composite->gate_ops; |
219 | struct clk_hw *gate_hw = composite->gate_hw; |
220 | |
221 | __clk_hw_set_clk(dst: gate_hw, src: hw); |
222 | |
223 | return gate_ops->enable(gate_hw); |
224 | } |
225 | |
226 | static void clk_composite_disable(struct clk_hw *hw) |
227 | { |
228 | struct clk_composite *composite = to_clk_composite(hw); |
229 | const struct clk_ops *gate_ops = composite->gate_ops; |
230 | struct clk_hw *gate_hw = composite->gate_hw; |
231 | |
232 | __clk_hw_set_clk(dst: gate_hw, src: hw); |
233 | |
234 | gate_ops->disable(gate_hw); |
235 | } |
236 | |
237 | static struct clk_hw *__clk_hw_register_composite(struct device *dev, |
238 | const char *name, const char * const *parent_names, |
239 | const struct clk_parent_data *pdata, int num_parents, |
240 | struct clk_hw *mux_hw, const struct clk_ops *mux_ops, |
241 | struct clk_hw *rate_hw, const struct clk_ops *rate_ops, |
242 | struct clk_hw *gate_hw, const struct clk_ops *gate_ops, |
243 | unsigned long flags) |
244 | { |
245 | struct clk_hw *hw; |
246 | struct clk_init_data init = {}; |
247 | struct clk_composite *composite; |
248 | struct clk_ops *clk_composite_ops; |
249 | int ret; |
250 | |
251 | composite = kzalloc(size: sizeof(*composite), GFP_KERNEL); |
252 | if (!composite) |
253 | return ERR_PTR(error: -ENOMEM); |
254 | |
255 | init.name = name; |
256 | init.flags = flags; |
257 | if (parent_names) |
258 | init.parent_names = parent_names; |
259 | else |
260 | init.parent_data = pdata; |
261 | init.num_parents = num_parents; |
262 | hw = &composite->hw; |
263 | |
264 | clk_composite_ops = &composite->ops; |
265 | |
266 | if (mux_hw && mux_ops) { |
267 | if (!mux_ops->get_parent) { |
268 | hw = ERR_PTR(error: -EINVAL); |
269 | goto err; |
270 | } |
271 | |
272 | composite->mux_hw = mux_hw; |
273 | composite->mux_ops = mux_ops; |
274 | clk_composite_ops->get_parent = clk_composite_get_parent; |
275 | if (mux_ops->set_parent) |
276 | clk_composite_ops->set_parent = clk_composite_set_parent; |
277 | if (mux_ops->determine_rate) |
278 | clk_composite_ops->determine_rate = clk_composite_determine_rate; |
279 | } |
280 | |
281 | if (rate_hw && rate_ops) { |
282 | if (!rate_ops->recalc_rate) { |
283 | hw = ERR_PTR(error: -EINVAL); |
284 | goto err; |
285 | } |
286 | clk_composite_ops->recalc_rate = clk_composite_recalc_rate; |
287 | |
288 | if (rate_ops->determine_rate) |
289 | clk_composite_ops->determine_rate = |
290 | clk_composite_determine_rate; |
291 | else if (rate_ops->round_rate) |
292 | clk_composite_ops->round_rate = |
293 | clk_composite_round_rate; |
294 | |
295 | /* .set_rate requires either .round_rate or .determine_rate */ |
296 | if (rate_ops->set_rate) { |
297 | if (rate_ops->determine_rate || rate_ops->round_rate) |
298 | clk_composite_ops->set_rate = |
299 | clk_composite_set_rate; |
300 | else |
301 | WARN(1, "%s: missing round_rate op is required\n" , |
302 | __func__); |
303 | } |
304 | |
305 | composite->rate_hw = rate_hw; |
306 | composite->rate_ops = rate_ops; |
307 | } |
308 | |
309 | if (mux_hw && mux_ops && rate_hw && rate_ops) { |
310 | if (mux_ops->set_parent && rate_ops->set_rate) |
311 | clk_composite_ops->set_rate_and_parent = |
312 | clk_composite_set_rate_and_parent; |
313 | } |
314 | |
315 | if (gate_hw && gate_ops) { |
316 | if (!gate_ops->is_enabled || !gate_ops->enable || |
317 | !gate_ops->disable) { |
318 | hw = ERR_PTR(error: -EINVAL); |
319 | goto err; |
320 | } |
321 | |
322 | composite->gate_hw = gate_hw; |
323 | composite->gate_ops = gate_ops; |
324 | clk_composite_ops->is_enabled = clk_composite_is_enabled; |
325 | clk_composite_ops->enable = clk_composite_enable; |
326 | clk_composite_ops->disable = clk_composite_disable; |
327 | } |
328 | |
329 | init.ops = clk_composite_ops; |
330 | composite->hw.init = &init; |
331 | |
332 | ret = clk_hw_register(dev, hw); |
333 | if (ret) { |
334 | hw = ERR_PTR(error: ret); |
335 | goto err; |
336 | } |
337 | |
338 | if (composite->mux_hw) |
339 | composite->mux_hw->clk = hw->clk; |
340 | |
341 | if (composite->rate_hw) |
342 | composite->rate_hw->clk = hw->clk; |
343 | |
344 | if (composite->gate_hw) |
345 | composite->gate_hw->clk = hw->clk; |
346 | |
347 | return hw; |
348 | |
349 | err: |
350 | kfree(objp: composite); |
351 | return hw; |
352 | } |
353 | |
354 | struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name, |
355 | const char * const *parent_names, int num_parents, |
356 | struct clk_hw *mux_hw, const struct clk_ops *mux_ops, |
357 | struct clk_hw *rate_hw, const struct clk_ops *rate_ops, |
358 | struct clk_hw *gate_hw, const struct clk_ops *gate_ops, |
359 | unsigned long flags) |
360 | { |
361 | return __clk_hw_register_composite(dev, name, parent_names, NULL, |
362 | num_parents, mux_hw, mux_ops, |
363 | rate_hw, rate_ops, gate_hw, |
364 | gate_ops, flags); |
365 | } |
366 | EXPORT_SYMBOL_GPL(clk_hw_register_composite); |
367 | |
368 | struct clk_hw *clk_hw_register_composite_pdata(struct device *dev, |
369 | const char *name, |
370 | const struct clk_parent_data *parent_data, |
371 | int num_parents, |
372 | struct clk_hw *mux_hw, const struct clk_ops *mux_ops, |
373 | struct clk_hw *rate_hw, const struct clk_ops *rate_ops, |
374 | struct clk_hw *gate_hw, const struct clk_ops *gate_ops, |
375 | unsigned long flags) |
376 | { |
377 | return __clk_hw_register_composite(dev, name, NULL, pdata: parent_data, |
378 | num_parents, mux_hw, mux_ops, |
379 | rate_hw, rate_ops, gate_hw, |
380 | gate_ops, flags); |
381 | } |
382 | |
383 | struct clk *clk_register_composite(struct device *dev, const char *name, |
384 | const char * const *parent_names, int num_parents, |
385 | struct clk_hw *mux_hw, const struct clk_ops *mux_ops, |
386 | struct clk_hw *rate_hw, const struct clk_ops *rate_ops, |
387 | struct clk_hw *gate_hw, const struct clk_ops *gate_ops, |
388 | unsigned long flags) |
389 | { |
390 | struct clk_hw *hw; |
391 | |
392 | hw = clk_hw_register_composite(dev, name, parent_names, num_parents, |
393 | mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops, |
394 | flags); |
395 | if (IS_ERR(ptr: hw)) |
396 | return ERR_CAST(ptr: hw); |
397 | return hw->clk; |
398 | } |
399 | EXPORT_SYMBOL_GPL(clk_register_composite); |
400 | |
401 | struct clk *clk_register_composite_pdata(struct device *dev, const char *name, |
402 | const struct clk_parent_data *parent_data, |
403 | int num_parents, |
404 | struct clk_hw *mux_hw, const struct clk_ops *mux_ops, |
405 | struct clk_hw *rate_hw, const struct clk_ops *rate_ops, |
406 | struct clk_hw *gate_hw, const struct clk_ops *gate_ops, |
407 | unsigned long flags) |
408 | { |
409 | struct clk_hw *hw; |
410 | |
411 | hw = clk_hw_register_composite_pdata(dev, name, parent_data, |
412 | num_parents, mux_hw, mux_ops, rate_hw, rate_ops, |
413 | gate_hw, gate_ops, flags); |
414 | if (IS_ERR(ptr: hw)) |
415 | return ERR_CAST(ptr: hw); |
416 | return hw->clk; |
417 | } |
418 | |
419 | void clk_unregister_composite(struct clk *clk) |
420 | { |
421 | struct clk_composite *composite; |
422 | struct clk_hw *hw; |
423 | |
424 | hw = __clk_get_hw(clk); |
425 | if (!hw) |
426 | return; |
427 | |
428 | composite = to_clk_composite(hw); |
429 | |
430 | clk_unregister(clk); |
431 | kfree(objp: composite); |
432 | } |
433 | |
434 | void clk_hw_unregister_composite(struct clk_hw *hw) |
435 | { |
436 | struct clk_composite *composite; |
437 | |
438 | composite = to_clk_composite(hw); |
439 | |
440 | clk_hw_unregister(hw); |
441 | kfree(objp: composite); |
442 | } |
443 | EXPORT_SYMBOL_GPL(clk_hw_unregister_composite); |
444 | |
445 | static void devm_clk_hw_release_composite(struct device *dev, void *res) |
446 | { |
447 | clk_hw_unregister_composite(*(struct clk_hw **)res); |
448 | } |
449 | |
450 | static struct clk_hw *__devm_clk_hw_register_composite(struct device *dev, |
451 | const char *name, const char * const *parent_names, |
452 | const struct clk_parent_data *pdata, int num_parents, |
453 | struct clk_hw *mux_hw, const struct clk_ops *mux_ops, |
454 | struct clk_hw *rate_hw, const struct clk_ops *rate_ops, |
455 | struct clk_hw *gate_hw, const struct clk_ops *gate_ops, |
456 | unsigned long flags) |
457 | { |
458 | struct clk_hw **ptr, *hw; |
459 | |
460 | ptr = devres_alloc(devm_clk_hw_release_composite, sizeof(*ptr), |
461 | GFP_KERNEL); |
462 | if (!ptr) |
463 | return ERR_PTR(error: -ENOMEM); |
464 | |
465 | hw = __clk_hw_register_composite(dev, name, parent_names, pdata, |
466 | num_parents, mux_hw, mux_ops, rate_hw, |
467 | rate_ops, gate_hw, gate_ops, flags); |
468 | |
469 | if (!IS_ERR(ptr: hw)) { |
470 | *ptr = hw; |
471 | devres_add(dev, res: ptr); |
472 | } else { |
473 | devres_free(res: ptr); |
474 | } |
475 | |
476 | return hw; |
477 | } |
478 | |
479 | struct clk_hw *devm_clk_hw_register_composite_pdata(struct device *dev, |
480 | const char *name, |
481 | const struct clk_parent_data *parent_data, |
482 | int num_parents, |
483 | struct clk_hw *mux_hw, const struct clk_ops *mux_ops, |
484 | struct clk_hw *rate_hw, const struct clk_ops *rate_ops, |
485 | struct clk_hw *gate_hw, const struct clk_ops *gate_ops, |
486 | unsigned long flags) |
487 | { |
488 | return __devm_clk_hw_register_composite(dev, name, NULL, pdata: parent_data, |
489 | num_parents, mux_hw, mux_ops, |
490 | rate_hw, rate_ops, gate_hw, |
491 | gate_ops, flags); |
492 | } |
493 | |