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