1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * Copyright (C) 2016-2022 NVIDIA Corporation |
4 | */ |
5 | |
6 | #include <linux/clk-provider.h> |
7 | #include <linux/device.h> |
8 | #include <linux/seq_buf.h> |
9 | #include <linux/slab.h> |
10 | |
11 | #include <soc/tegra/bpmp.h> |
12 | #include <soc/tegra/bpmp-abi.h> |
13 | |
14 | #define TEGRA_BPMP_DUMP_CLOCK_INFO 0 |
15 | |
16 | #define TEGRA_BPMP_CLK_HAS_MUX BIT(0) |
17 | #define TEGRA_BPMP_CLK_HAS_SET_RATE BIT(1) |
18 | #define TEGRA_BPMP_CLK_IS_ROOT BIT(2) |
19 | |
20 | struct tegra_bpmp_clk_info { |
21 | unsigned int id; |
22 | char name[MRQ_CLK_NAME_MAXLEN]; |
23 | unsigned int parents[MRQ_CLK_MAX_PARENTS]; |
24 | unsigned int num_parents; |
25 | unsigned long flags; |
26 | }; |
27 | |
28 | struct tegra_bpmp_clk { |
29 | struct clk_hw hw; |
30 | |
31 | struct tegra_bpmp *bpmp; |
32 | unsigned int id; |
33 | |
34 | unsigned int num_parents; |
35 | unsigned int *parents; |
36 | }; |
37 | |
38 | static inline struct tegra_bpmp_clk *to_tegra_bpmp_clk(struct clk_hw *hw) |
39 | { |
40 | return container_of(hw, struct tegra_bpmp_clk, hw); |
41 | } |
42 | |
43 | struct tegra_bpmp_clk_message { |
44 | unsigned int cmd; |
45 | unsigned int id; |
46 | |
47 | struct { |
48 | const void *data; |
49 | size_t size; |
50 | } tx; |
51 | |
52 | struct { |
53 | void *data; |
54 | size_t size; |
55 | int ret; |
56 | } rx; |
57 | }; |
58 | |
59 | static int tegra_bpmp_clk_transfer(struct tegra_bpmp *bpmp, |
60 | const struct tegra_bpmp_clk_message *clk) |
61 | { |
62 | struct mrq_clk_request request; |
63 | struct tegra_bpmp_message msg; |
64 | void *req = &request; |
65 | int err; |
66 | |
67 | memset(&request, 0, sizeof(request)); |
68 | request.cmd_and_id = (clk->cmd << 24) | clk->id; |
69 | |
70 | /* |
71 | * The mrq_clk_request structure has an anonymous union at offset 4 |
72 | * that contains all possible sub-command structures. Copy the data |
73 | * to that union. Ideally we'd be able to refer to it by name, but |
74 | * doing so would require changing the ABI header and increase the |
75 | * maintenance burden. |
76 | */ |
77 | memcpy(req + 4, clk->tx.data, clk->tx.size); |
78 | |
79 | memset(&msg, 0, sizeof(msg)); |
80 | msg.mrq = MRQ_CLK; |
81 | msg.tx.data = &request; |
82 | msg.tx.size = sizeof(request); |
83 | msg.rx.data = clk->rx.data; |
84 | msg.rx.size = clk->rx.size; |
85 | |
86 | err = tegra_bpmp_transfer(bpmp, msg: &msg); |
87 | if (err < 0) |
88 | return err; |
89 | else if (msg.rx.ret < 0) |
90 | return -EINVAL; |
91 | |
92 | return 0; |
93 | } |
94 | |
95 | static int tegra_bpmp_clk_prepare(struct clk_hw *hw) |
96 | { |
97 | struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); |
98 | struct tegra_bpmp_clk_message msg; |
99 | |
100 | memset(&msg, 0, sizeof(msg)); |
101 | msg.cmd = CMD_CLK_ENABLE; |
102 | msg.id = clk->id; |
103 | |
104 | return tegra_bpmp_clk_transfer(bpmp: clk->bpmp, clk: &msg); |
105 | } |
106 | |
107 | static void tegra_bpmp_clk_unprepare(struct clk_hw *hw) |
108 | { |
109 | struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); |
110 | struct tegra_bpmp_clk_message msg; |
111 | int err; |
112 | |
113 | memset(&msg, 0, sizeof(msg)); |
114 | msg.cmd = CMD_CLK_DISABLE; |
115 | msg.id = clk->id; |
116 | |
117 | err = tegra_bpmp_clk_transfer(bpmp: clk->bpmp, clk: &msg); |
118 | if (err < 0) |
119 | dev_err(clk->bpmp->dev, "failed to disable clock %s: %d\n" , |
120 | clk_hw_get_name(hw), err); |
121 | } |
122 | |
123 | static int tegra_bpmp_clk_is_prepared(struct clk_hw *hw) |
124 | { |
125 | struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); |
126 | struct cmd_clk_is_enabled_response response; |
127 | struct tegra_bpmp_clk_message msg; |
128 | int err; |
129 | |
130 | memset(&msg, 0, sizeof(msg)); |
131 | msg.cmd = CMD_CLK_IS_ENABLED; |
132 | msg.id = clk->id; |
133 | msg.rx.data = &response; |
134 | msg.rx.size = sizeof(response); |
135 | |
136 | err = tegra_bpmp_clk_transfer(bpmp: clk->bpmp, clk: &msg); |
137 | if (err < 0) |
138 | return err; |
139 | |
140 | return response.state; |
141 | } |
142 | |
143 | static unsigned long tegra_bpmp_clk_recalc_rate(struct clk_hw *hw, |
144 | unsigned long parent_rate) |
145 | { |
146 | struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); |
147 | struct cmd_clk_get_rate_response response; |
148 | struct cmd_clk_get_rate_request request; |
149 | struct tegra_bpmp_clk_message msg; |
150 | int err; |
151 | |
152 | memset(&msg, 0, sizeof(msg)); |
153 | msg.cmd = CMD_CLK_GET_RATE; |
154 | msg.id = clk->id; |
155 | msg.tx.data = &request; |
156 | msg.tx.size = sizeof(request); |
157 | msg.rx.data = &response; |
158 | msg.rx.size = sizeof(response); |
159 | |
160 | err = tegra_bpmp_clk_transfer(bpmp: clk->bpmp, clk: &msg); |
161 | if (err < 0) |
162 | return 0; |
163 | |
164 | return response.rate; |
165 | } |
166 | |
167 | static int tegra_bpmp_clk_determine_rate(struct clk_hw *hw, |
168 | struct clk_rate_request *rate_req) |
169 | { |
170 | struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); |
171 | struct cmd_clk_round_rate_response response; |
172 | struct cmd_clk_round_rate_request request; |
173 | struct tegra_bpmp_clk_message msg; |
174 | unsigned long rate; |
175 | int err; |
176 | |
177 | rate = min(max(rate_req->rate, rate_req->min_rate), rate_req->max_rate); |
178 | |
179 | memset(&request, 0, sizeof(request)); |
180 | request.rate = min_t(u64, rate, S64_MAX); |
181 | |
182 | memset(&msg, 0, sizeof(msg)); |
183 | msg.cmd = CMD_CLK_ROUND_RATE; |
184 | msg.id = clk->id; |
185 | msg.tx.data = &request; |
186 | msg.tx.size = sizeof(request); |
187 | msg.rx.data = &response; |
188 | msg.rx.size = sizeof(response); |
189 | |
190 | err = tegra_bpmp_clk_transfer(bpmp: clk->bpmp, clk: &msg); |
191 | if (err < 0) |
192 | return err; |
193 | |
194 | rate_req->rate = (unsigned long)response.rate; |
195 | |
196 | return 0; |
197 | } |
198 | |
199 | static int tegra_bpmp_clk_set_parent(struct clk_hw *hw, u8 index) |
200 | { |
201 | struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); |
202 | struct cmd_clk_set_parent_response response; |
203 | struct cmd_clk_set_parent_request request; |
204 | struct tegra_bpmp_clk_message msg; |
205 | int err; |
206 | |
207 | memset(&request, 0, sizeof(request)); |
208 | request.parent_id = clk->parents[index]; |
209 | |
210 | memset(&msg, 0, sizeof(msg)); |
211 | msg.cmd = CMD_CLK_SET_PARENT; |
212 | msg.id = clk->id; |
213 | msg.tx.data = &request; |
214 | msg.tx.size = sizeof(request); |
215 | msg.rx.data = &response; |
216 | msg.rx.size = sizeof(response); |
217 | |
218 | err = tegra_bpmp_clk_transfer(bpmp: clk->bpmp, clk: &msg); |
219 | if (err < 0) |
220 | return err; |
221 | |
222 | /* XXX check parent ID in response */ |
223 | |
224 | return 0; |
225 | } |
226 | |
227 | static u8 tegra_bpmp_clk_get_parent(struct clk_hw *hw) |
228 | { |
229 | struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); |
230 | struct cmd_clk_get_parent_response response; |
231 | struct tegra_bpmp_clk_message msg; |
232 | unsigned int i; |
233 | int err; |
234 | |
235 | memset(&msg, 0, sizeof(msg)); |
236 | msg.cmd = CMD_CLK_GET_PARENT; |
237 | msg.id = clk->id; |
238 | msg.rx.data = &response; |
239 | msg.rx.size = sizeof(response); |
240 | |
241 | err = tegra_bpmp_clk_transfer(bpmp: clk->bpmp, clk: &msg); |
242 | if (err < 0) { |
243 | dev_err(clk->bpmp->dev, "failed to get parent for %s: %d\n" , |
244 | clk_hw_get_name(hw), err); |
245 | return U8_MAX; |
246 | } |
247 | |
248 | for (i = 0; i < clk->num_parents; i++) |
249 | if (clk->parents[i] == response.parent_id) |
250 | return i; |
251 | |
252 | return U8_MAX; |
253 | } |
254 | |
255 | static int tegra_bpmp_clk_set_rate(struct clk_hw *hw, unsigned long rate, |
256 | unsigned long parent_rate) |
257 | { |
258 | struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw); |
259 | struct cmd_clk_set_rate_response response; |
260 | struct cmd_clk_set_rate_request request; |
261 | struct tegra_bpmp_clk_message msg; |
262 | |
263 | memset(&request, 0, sizeof(request)); |
264 | request.rate = min_t(u64, rate, S64_MAX); |
265 | |
266 | memset(&msg, 0, sizeof(msg)); |
267 | msg.cmd = CMD_CLK_SET_RATE; |
268 | msg.id = clk->id; |
269 | msg.tx.data = &request; |
270 | msg.tx.size = sizeof(request); |
271 | msg.rx.data = &response; |
272 | msg.rx.size = sizeof(response); |
273 | |
274 | return tegra_bpmp_clk_transfer(bpmp: clk->bpmp, clk: &msg); |
275 | } |
276 | |
277 | static const struct clk_ops tegra_bpmp_clk_gate_ops = { |
278 | .prepare = tegra_bpmp_clk_prepare, |
279 | .unprepare = tegra_bpmp_clk_unprepare, |
280 | .is_prepared = tegra_bpmp_clk_is_prepared, |
281 | .recalc_rate = tegra_bpmp_clk_recalc_rate, |
282 | }; |
283 | |
284 | static const struct clk_ops tegra_bpmp_clk_mux_ops = { |
285 | .prepare = tegra_bpmp_clk_prepare, |
286 | .unprepare = tegra_bpmp_clk_unprepare, |
287 | .is_prepared = tegra_bpmp_clk_is_prepared, |
288 | .recalc_rate = tegra_bpmp_clk_recalc_rate, |
289 | .determine_rate = clk_hw_determine_rate_no_reparent, |
290 | .set_parent = tegra_bpmp_clk_set_parent, |
291 | .get_parent = tegra_bpmp_clk_get_parent, |
292 | }; |
293 | |
294 | static const struct clk_ops tegra_bpmp_clk_rate_ops = { |
295 | .prepare = tegra_bpmp_clk_prepare, |
296 | .unprepare = tegra_bpmp_clk_unprepare, |
297 | .is_prepared = tegra_bpmp_clk_is_prepared, |
298 | .recalc_rate = tegra_bpmp_clk_recalc_rate, |
299 | .determine_rate = tegra_bpmp_clk_determine_rate, |
300 | .set_rate = tegra_bpmp_clk_set_rate, |
301 | }; |
302 | |
303 | static const struct clk_ops tegra_bpmp_clk_mux_rate_ops = { |
304 | .prepare = tegra_bpmp_clk_prepare, |
305 | .unprepare = tegra_bpmp_clk_unprepare, |
306 | .is_prepared = tegra_bpmp_clk_is_prepared, |
307 | .recalc_rate = tegra_bpmp_clk_recalc_rate, |
308 | .determine_rate = tegra_bpmp_clk_determine_rate, |
309 | .set_parent = tegra_bpmp_clk_set_parent, |
310 | .get_parent = tegra_bpmp_clk_get_parent, |
311 | .set_rate = tegra_bpmp_clk_set_rate, |
312 | }; |
313 | |
314 | static const struct clk_ops tegra_bpmp_clk_mux_read_only_ops = { |
315 | .get_parent = tegra_bpmp_clk_get_parent, |
316 | .recalc_rate = tegra_bpmp_clk_recalc_rate, |
317 | }; |
318 | |
319 | static const struct clk_ops tegra_bpmp_clk_read_only_ops = { |
320 | .recalc_rate = tegra_bpmp_clk_recalc_rate, |
321 | }; |
322 | |
323 | static const struct clk_ops tegra_bpmp_clk_gate_mux_read_only_ops = { |
324 | .prepare = tegra_bpmp_clk_prepare, |
325 | .unprepare = tegra_bpmp_clk_unprepare, |
326 | .is_prepared = tegra_bpmp_clk_is_prepared, |
327 | .recalc_rate = tegra_bpmp_clk_recalc_rate, |
328 | .get_parent = tegra_bpmp_clk_get_parent, |
329 | }; |
330 | |
331 | static int tegra_bpmp_clk_get_max_id(struct tegra_bpmp *bpmp) |
332 | { |
333 | struct cmd_clk_get_max_clk_id_response response; |
334 | struct tegra_bpmp_clk_message msg; |
335 | int err; |
336 | |
337 | memset(&msg, 0, sizeof(msg)); |
338 | msg.cmd = CMD_CLK_GET_MAX_CLK_ID; |
339 | msg.rx.data = &response; |
340 | msg.rx.size = sizeof(response); |
341 | |
342 | err = tegra_bpmp_clk_transfer(bpmp, clk: &msg); |
343 | if (err < 0) |
344 | return err; |
345 | |
346 | if (response.max_id > INT_MAX) |
347 | return -E2BIG; |
348 | |
349 | return response.max_id; |
350 | } |
351 | |
352 | static int tegra_bpmp_clk_get_info(struct tegra_bpmp *bpmp, unsigned int id, |
353 | struct tegra_bpmp_clk_info *info) |
354 | { |
355 | struct cmd_clk_get_all_info_response response; |
356 | struct tegra_bpmp_clk_message msg; |
357 | unsigned int i; |
358 | int err; |
359 | |
360 | memset(&msg, 0, sizeof(msg)); |
361 | msg.cmd = CMD_CLK_GET_ALL_INFO; |
362 | msg.id = id; |
363 | msg.rx.data = &response; |
364 | msg.rx.size = sizeof(response); |
365 | |
366 | err = tegra_bpmp_clk_transfer(bpmp, clk: &msg); |
367 | if (err < 0) |
368 | return err; |
369 | |
370 | strscpy(p: info->name, q: response.name, MRQ_CLK_NAME_MAXLEN); |
371 | info->num_parents = response.num_parents; |
372 | |
373 | for (i = 0; i < info->num_parents; i++) |
374 | info->parents[i] = response.parents[i]; |
375 | |
376 | info->flags = response.flags; |
377 | |
378 | return 0; |
379 | } |
380 | |
381 | static void tegra_bpmp_clk_info_dump(struct tegra_bpmp *bpmp, |
382 | const char *level, |
383 | const struct tegra_bpmp_clk_info *info) |
384 | { |
385 | const char *prefix = "" ; |
386 | struct seq_buf buf; |
387 | unsigned int i; |
388 | char flags[64]; |
389 | |
390 | seq_buf_init(s: &buf, buf: flags, size: sizeof(flags)); |
391 | |
392 | if (info->flags) |
393 | seq_buf_printf(s: &buf, fmt: "(" ); |
394 | |
395 | if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) { |
396 | seq_buf_printf(s: &buf, fmt: "%smux" , prefix); |
397 | prefix = ", " ; |
398 | } |
399 | |
400 | if ((info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) == 0) { |
401 | seq_buf_printf(s: &buf, fmt: "%sfixed" , prefix); |
402 | prefix = ", " ; |
403 | } |
404 | |
405 | if (info->flags & TEGRA_BPMP_CLK_IS_ROOT) { |
406 | seq_buf_printf(s: &buf, fmt: "%sroot" , prefix); |
407 | prefix = ", " ; |
408 | } |
409 | |
410 | if (info->flags) |
411 | seq_buf_printf(s: &buf, fmt: ")" ); |
412 | |
413 | dev_printk(level, bpmp->dev, "%03u: %s\n" , info->id, info->name); |
414 | dev_printk(level, bpmp->dev, " flags: %lx %s\n" , info->flags, flags); |
415 | dev_printk(level, bpmp->dev, " parents: %u\n" , info->num_parents); |
416 | |
417 | for (i = 0; i < info->num_parents; i++) |
418 | dev_printk(level, bpmp->dev, " %03u\n" , info->parents[i]); |
419 | } |
420 | |
421 | static int tegra_bpmp_probe_clocks(struct tegra_bpmp *bpmp, |
422 | struct tegra_bpmp_clk_info **clocksp) |
423 | { |
424 | struct tegra_bpmp_clk_info *clocks; |
425 | unsigned int max_id, id, count = 0; |
426 | unsigned int holes = 0; |
427 | int err; |
428 | |
429 | err = tegra_bpmp_clk_get_max_id(bpmp); |
430 | if (err < 0) |
431 | return err; |
432 | |
433 | max_id = err; |
434 | |
435 | dev_dbg(bpmp->dev, "maximum clock ID: %u\n" , max_id); |
436 | |
437 | clocks = kcalloc(n: max_id + 1, size: sizeof(*clocks), GFP_KERNEL); |
438 | if (!clocks) |
439 | return -ENOMEM; |
440 | |
441 | for (id = 0; id <= max_id; id++) { |
442 | struct tegra_bpmp_clk_info *info = &clocks[count]; |
443 | |
444 | err = tegra_bpmp_clk_get_info(bpmp, id, info); |
445 | if (err < 0) |
446 | continue; |
447 | |
448 | if (info->num_parents >= U8_MAX) { |
449 | dev_err(bpmp->dev, |
450 | "clock %u has too many parents (%u, max: %u)\n" , |
451 | id, info->num_parents, U8_MAX); |
452 | continue; |
453 | } |
454 | |
455 | /* clock not exposed by BPMP */ |
456 | if (info->name[0] == '\0') { |
457 | holes++; |
458 | continue; |
459 | } |
460 | |
461 | info->id = id; |
462 | count++; |
463 | |
464 | if (TEGRA_BPMP_DUMP_CLOCK_INFO) |
465 | tegra_bpmp_clk_info_dump(bpmp, KERN_DEBUG, info); |
466 | } |
467 | |
468 | dev_dbg(bpmp->dev, "holes: %u\n" , holes); |
469 | *clocksp = clocks; |
470 | |
471 | return count; |
472 | } |
473 | |
474 | static unsigned int |
475 | tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info *clocks, |
476 | unsigned int num_clocks, unsigned int id) |
477 | { |
478 | unsigned int i; |
479 | |
480 | for (i = 0; i < num_clocks; i++) |
481 | if (clocks[i].id == id) |
482 | return i; |
483 | |
484 | return UINT_MAX; |
485 | } |
486 | |
487 | static const struct tegra_bpmp_clk_info * |
488 | tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info *clocks, |
489 | unsigned int num_clocks, unsigned int id) |
490 | { |
491 | unsigned int i; |
492 | |
493 | i = tegra_bpmp_clk_id_to_index(clocks, num_clocks, id); |
494 | |
495 | if (i < num_clocks) |
496 | return &clocks[i]; |
497 | |
498 | return NULL; |
499 | } |
500 | |
501 | static struct tegra_bpmp_clk * |
502 | tegra_bpmp_clk_register(struct tegra_bpmp *bpmp, |
503 | const struct tegra_bpmp_clk_info *info, |
504 | const struct tegra_bpmp_clk_info *clocks, |
505 | unsigned int num_clocks) |
506 | { |
507 | struct tegra_bpmp_clk *clk; |
508 | struct clk_init_data init; |
509 | const char **parents; |
510 | unsigned int i; |
511 | int err; |
512 | |
513 | clk = devm_kzalloc(dev: bpmp->dev, size: sizeof(*clk), GFP_KERNEL); |
514 | if (!clk) |
515 | return ERR_PTR(error: -ENOMEM); |
516 | |
517 | clk->id = info->id; |
518 | clk->bpmp = bpmp; |
519 | |
520 | clk->parents = devm_kcalloc(dev: bpmp->dev, n: info->num_parents, |
521 | size: sizeof(*clk->parents), GFP_KERNEL); |
522 | if (!clk->parents) |
523 | return ERR_PTR(error: -ENOMEM); |
524 | |
525 | clk->num_parents = info->num_parents; |
526 | |
527 | /* hardware clock initialization */ |
528 | memset(&init, 0, sizeof(init)); |
529 | init.name = info->name; |
530 | clk->hw.init = &init; |
531 | if (info->flags & BPMP_CLK_STATE_CHANGE_DENIED) { |
532 | if ((info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) == 0) { |
533 | dev_WARN(bpmp->dev, |
534 | "Firmware bug! Inconsistent permission bits for clock %s. State and parent/rate changes disabled." , |
535 | init.name); |
536 | } |
537 | if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) |
538 | init.ops = &tegra_bpmp_clk_mux_read_only_ops; |
539 | else |
540 | init.ops = &tegra_bpmp_clk_read_only_ops; |
541 | } else if (info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) { |
542 | if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) |
543 | init.ops = &tegra_bpmp_clk_gate_mux_read_only_ops; |
544 | else |
545 | init.ops = &tegra_bpmp_clk_gate_ops; |
546 | } else if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) { |
547 | if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) |
548 | init.ops = &tegra_bpmp_clk_mux_rate_ops; |
549 | else |
550 | init.ops = &tegra_bpmp_clk_mux_ops; |
551 | } else { |
552 | if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) |
553 | init.ops = &tegra_bpmp_clk_rate_ops; |
554 | else |
555 | init.ops = &tegra_bpmp_clk_gate_ops; |
556 | } |
557 | |
558 | init.num_parents = info->num_parents; |
559 | |
560 | parents = kcalloc(n: info->num_parents, size: sizeof(*parents), GFP_KERNEL); |
561 | if (!parents) |
562 | return ERR_PTR(error: -ENOMEM); |
563 | |
564 | for (i = 0; i < info->num_parents; i++) { |
565 | const struct tegra_bpmp_clk_info *parent; |
566 | |
567 | /* keep a private copy of the ID to parent index map */ |
568 | clk->parents[i] = info->parents[i]; |
569 | |
570 | parent = tegra_bpmp_clk_find(clocks, num_clocks, |
571 | id: info->parents[i]); |
572 | if (!parent) { |
573 | dev_err(bpmp->dev, "no parent %u found for %u\n" , |
574 | info->parents[i], info->id); |
575 | continue; |
576 | } |
577 | |
578 | parents[i] = parent->name; |
579 | } |
580 | |
581 | init.parent_names = parents; |
582 | |
583 | err = devm_clk_hw_register(dev: bpmp->dev, hw: &clk->hw); |
584 | |
585 | kfree(objp: parents); |
586 | |
587 | if (err < 0) |
588 | return ERR_PTR(error: err); |
589 | |
590 | return clk; |
591 | } |
592 | |
593 | static void tegra_bpmp_register_clocks_one(struct tegra_bpmp *bpmp, |
594 | struct tegra_bpmp_clk_info *infos, |
595 | unsigned int i, |
596 | unsigned int count) |
597 | { |
598 | unsigned int j; |
599 | struct tegra_bpmp_clk_info *info; |
600 | struct tegra_bpmp_clk *clk; |
601 | |
602 | if (bpmp->clocks[i]) { |
603 | /* already registered */ |
604 | return; |
605 | } |
606 | |
607 | info = &infos[i]; |
608 | for (j = 0; j < info->num_parents; ++j) { |
609 | unsigned int p_id = info->parents[j]; |
610 | unsigned int p_i = tegra_bpmp_clk_id_to_index(clocks: infos, num_clocks: count, |
611 | id: p_id); |
612 | if (p_i < count) |
613 | tegra_bpmp_register_clocks_one(bpmp, infos, i: p_i, count); |
614 | } |
615 | |
616 | clk = tegra_bpmp_clk_register(bpmp, info, clocks: infos, num_clocks: count); |
617 | if (IS_ERR(ptr: clk)) { |
618 | dev_err(bpmp->dev, |
619 | "failed to register clock %u (%s): %ld\n" , |
620 | info->id, info->name, PTR_ERR(clk)); |
621 | /* intentionally store the error pointer to |
622 | * bpmp->clocks[i] to avoid re-attempting the |
623 | * registration later |
624 | */ |
625 | } |
626 | |
627 | bpmp->clocks[i] = clk; |
628 | } |
629 | |
630 | static int tegra_bpmp_register_clocks(struct tegra_bpmp *bpmp, |
631 | struct tegra_bpmp_clk_info *infos, |
632 | unsigned int count) |
633 | { |
634 | unsigned int i; |
635 | |
636 | bpmp->num_clocks = count; |
637 | |
638 | bpmp->clocks = devm_kcalloc(dev: bpmp->dev, n: count, size: sizeof(struct tegra_bpmp_clk), GFP_KERNEL); |
639 | if (!bpmp->clocks) |
640 | return -ENOMEM; |
641 | |
642 | for (i = 0; i < count; i++) { |
643 | tegra_bpmp_register_clocks_one(bpmp, infos, i, count); |
644 | } |
645 | |
646 | return 0; |
647 | } |
648 | |
649 | static void tegra_bpmp_unregister_clocks(struct tegra_bpmp *bpmp) |
650 | { |
651 | unsigned int i; |
652 | |
653 | for (i = 0; i < bpmp->num_clocks; i++) |
654 | clk_hw_unregister(hw: &bpmp->clocks[i]->hw); |
655 | } |
656 | |
657 | static struct clk_hw *tegra_bpmp_clk_of_xlate(struct of_phandle_args *clkspec, |
658 | void *data) |
659 | { |
660 | unsigned int id = clkspec->args[0], i; |
661 | struct tegra_bpmp *bpmp = data; |
662 | |
663 | for (i = 0; i < bpmp->num_clocks; i++) { |
664 | struct tegra_bpmp_clk *clk = bpmp->clocks[i]; |
665 | |
666 | if (!clk) |
667 | continue; |
668 | |
669 | if (clk->id == id) |
670 | return &clk->hw; |
671 | } |
672 | |
673 | return NULL; |
674 | } |
675 | |
676 | int tegra_bpmp_init_clocks(struct tegra_bpmp *bpmp) |
677 | { |
678 | struct tegra_bpmp_clk_info *clocks; |
679 | unsigned int count; |
680 | int err; |
681 | |
682 | err = tegra_bpmp_probe_clocks(bpmp, clocksp: &clocks); |
683 | if (err < 0) |
684 | return err; |
685 | |
686 | count = err; |
687 | |
688 | dev_dbg(bpmp->dev, "%u clocks probed\n" , count); |
689 | |
690 | err = tegra_bpmp_register_clocks(bpmp, infos: clocks, count); |
691 | if (err < 0) |
692 | goto free; |
693 | |
694 | err = of_clk_add_hw_provider(np: bpmp->dev->of_node, |
695 | get: tegra_bpmp_clk_of_xlate, |
696 | data: bpmp); |
697 | if (err < 0) { |
698 | tegra_bpmp_unregister_clocks(bpmp); |
699 | goto free; |
700 | } |
701 | |
702 | free: |
703 | kfree(objp: clocks); |
704 | return err; |
705 | } |
706 | |