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
to_tegra_bpmp_clk(struct clk_hw * hw)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
tegra_bpmp_clk_transfer(struct tegra_bpmp * bpmp,const struct tegra_bpmp_clk_message * clk)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);
87 if (err < 0)
88 return err;
89 else if (msg.rx.ret < 0)
90 return -EINVAL;
91
92 return 0;
93 }
94
tegra_bpmp_clk_prepare(struct clk_hw * hw)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(clk->bpmp, &msg);
105 }
106
tegra_bpmp_clk_unprepare(struct clk_hw * hw)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(clk->bpmp, &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
tegra_bpmp_clk_is_prepared(struct clk_hw * hw)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(clk->bpmp, &msg);
137 if (err < 0)
138 return err;
139
140 return response.state;
141 }
142
tegra_bpmp_clk_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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(clk->bpmp, &msg);
161 if (err < 0)
162 return err;
163
164 return response.rate;
165 }
166
tegra_bpmp_clk_determine_rate(struct clk_hw * hw,struct clk_rate_request * rate_req)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(clk->bpmp, &msg);
191 if (err < 0)
192 return err;
193
194 rate_req->rate = (unsigned long)response.rate;
195
196 return 0;
197 }
198
tegra_bpmp_clk_set_parent(struct clk_hw * hw,u8 index)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(clk->bpmp, &msg);
219 if (err < 0)
220 return err;
221
222 /* XXX check parent ID in response */
223
224 return 0;
225 }
226
tegra_bpmp_clk_get_parent(struct clk_hw * hw)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(clk->bpmp, &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
tegra_bpmp_clk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)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(clk->bpmp, &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 .set_parent = tegra_bpmp_clk_set_parent,
290 .get_parent = tegra_bpmp_clk_get_parent,
291 };
292
293 static const struct clk_ops tegra_bpmp_clk_rate_ops = {
294 .prepare = tegra_bpmp_clk_prepare,
295 .unprepare = tegra_bpmp_clk_unprepare,
296 .is_prepared = tegra_bpmp_clk_is_prepared,
297 .recalc_rate = tegra_bpmp_clk_recalc_rate,
298 .determine_rate = tegra_bpmp_clk_determine_rate,
299 .set_rate = tegra_bpmp_clk_set_rate,
300 };
301
302 static const struct clk_ops tegra_bpmp_clk_mux_rate_ops = {
303 .prepare = tegra_bpmp_clk_prepare,
304 .unprepare = tegra_bpmp_clk_unprepare,
305 .is_prepared = tegra_bpmp_clk_is_prepared,
306 .recalc_rate = tegra_bpmp_clk_recalc_rate,
307 .determine_rate = tegra_bpmp_clk_determine_rate,
308 .set_parent = tegra_bpmp_clk_set_parent,
309 .get_parent = tegra_bpmp_clk_get_parent,
310 .set_rate = tegra_bpmp_clk_set_rate,
311 };
312
313 static const struct clk_ops tegra_bpmp_clk_mux_read_only_ops = {
314 .get_parent = tegra_bpmp_clk_get_parent,
315 .recalc_rate = tegra_bpmp_clk_recalc_rate,
316 };
317
318 static const struct clk_ops tegra_bpmp_clk_read_only_ops = {
319 .recalc_rate = tegra_bpmp_clk_recalc_rate,
320 };
321
322 static const struct clk_ops tegra_bpmp_clk_gate_mux_read_only_ops = {
323 .prepare = tegra_bpmp_clk_prepare,
324 .unprepare = tegra_bpmp_clk_unprepare,
325 .is_prepared = tegra_bpmp_clk_is_prepared,
326 .recalc_rate = tegra_bpmp_clk_recalc_rate,
327 .get_parent = tegra_bpmp_clk_get_parent,
328 };
329
tegra_bpmp_clk_get_max_id(struct tegra_bpmp * bpmp)330 static int tegra_bpmp_clk_get_max_id(struct tegra_bpmp *bpmp)
331 {
332 struct cmd_clk_get_max_clk_id_response response;
333 struct tegra_bpmp_clk_message msg;
334 int err;
335
336 memset(&msg, 0, sizeof(msg));
337 msg.cmd = CMD_CLK_GET_MAX_CLK_ID;
338 msg.rx.data = &response;
339 msg.rx.size = sizeof(response);
340
341 err = tegra_bpmp_clk_transfer(bpmp, &msg);
342 if (err < 0)
343 return err;
344
345 if (response.max_id > INT_MAX)
346 return -E2BIG;
347
348 return response.max_id;
349 }
350
tegra_bpmp_clk_get_info(struct tegra_bpmp * bpmp,unsigned int id,struct tegra_bpmp_clk_info * info)351 static int tegra_bpmp_clk_get_info(struct tegra_bpmp *bpmp, unsigned int id,
352 struct tegra_bpmp_clk_info *info)
353 {
354 struct cmd_clk_get_all_info_response response;
355 struct tegra_bpmp_clk_message msg;
356 unsigned int i;
357 int err;
358
359 memset(&msg, 0, sizeof(msg));
360 msg.cmd = CMD_CLK_GET_ALL_INFO;
361 msg.id = id;
362 msg.rx.data = &response;
363 msg.rx.size = sizeof(response);
364
365 err = tegra_bpmp_clk_transfer(bpmp, &msg);
366 if (err < 0)
367 return err;
368
369 strscpy(info->name, response.name, MRQ_CLK_NAME_MAXLEN);
370 info->num_parents = response.num_parents;
371
372 for (i = 0; i < info->num_parents; i++)
373 info->parents[i] = response.parents[i];
374
375 info->flags = response.flags;
376
377 return 0;
378 }
379
tegra_bpmp_clk_info_dump(struct tegra_bpmp * bpmp,const char * level,const struct tegra_bpmp_clk_info * info)380 static void tegra_bpmp_clk_info_dump(struct tegra_bpmp *bpmp,
381 const char *level,
382 const struct tegra_bpmp_clk_info *info)
383 {
384 const char *prefix = "";
385 struct seq_buf buf;
386 unsigned int i;
387 char flags[64];
388
389 seq_buf_init(&buf, flags, sizeof(flags));
390
391 if (info->flags)
392 seq_buf_printf(&buf, "(");
393
394 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
395 seq_buf_printf(&buf, "%smux", prefix);
396 prefix = ", ";
397 }
398
399 if ((info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) == 0) {
400 seq_buf_printf(&buf, "%sfixed", prefix);
401 prefix = ", ";
402 }
403
404 if (info->flags & TEGRA_BPMP_CLK_IS_ROOT) {
405 seq_buf_printf(&buf, "%sroot", prefix);
406 prefix = ", ";
407 }
408
409 if (info->flags)
410 seq_buf_printf(&buf, ")");
411
412 dev_printk(level, bpmp->dev, "%03u: %s\n", info->id, info->name);
413 dev_printk(level, bpmp->dev, " flags: %lx %s\n", info->flags, flags);
414 dev_printk(level, bpmp->dev, " parents: %u\n", info->num_parents);
415
416 for (i = 0; i < info->num_parents; i++)
417 dev_printk(level, bpmp->dev, " %03u\n", info->parents[i]);
418 }
419
tegra_bpmp_probe_clocks(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info ** clocksp)420 static int tegra_bpmp_probe_clocks(struct tegra_bpmp *bpmp,
421 struct tegra_bpmp_clk_info **clocksp)
422 {
423 struct tegra_bpmp_clk_info *clocks;
424 unsigned int max_id, id, count = 0;
425 unsigned int holes = 0;
426 int err;
427
428 err = tegra_bpmp_clk_get_max_id(bpmp);
429 if (err < 0)
430 return err;
431
432 max_id = err;
433
434 dev_dbg(bpmp->dev, "maximum clock ID: %u\n", max_id);
435
436 clocks = kcalloc(max_id + 1, sizeof(*clocks), GFP_KERNEL);
437 if (!clocks)
438 return -ENOMEM;
439
440 for (id = 0; id <= max_id; id++) {
441 struct tegra_bpmp_clk_info *info = &clocks[count];
442
443 err = tegra_bpmp_clk_get_info(bpmp, id, info);
444 if (err < 0)
445 continue;
446
447 if (info->num_parents >= U8_MAX) {
448 dev_err(bpmp->dev,
449 "clock %u has too many parents (%u, max: %u)\n",
450 id, info->num_parents, U8_MAX);
451 continue;
452 }
453
454 /* clock not exposed by BPMP */
455 if (info->name[0] == '\0') {
456 holes++;
457 continue;
458 }
459
460 info->id = id;
461 count++;
462
463 if (TEGRA_BPMP_DUMP_CLOCK_INFO)
464 tegra_bpmp_clk_info_dump(bpmp, KERN_DEBUG, info);
465 }
466
467 dev_dbg(bpmp->dev, "holes: %u\n", holes);
468 *clocksp = clocks;
469
470 return count;
471 }
472
473 static unsigned int
tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info * clocks,unsigned int num_clocks,unsigned int id)474 tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info *clocks,
475 unsigned int num_clocks, unsigned int id)
476 {
477 unsigned int i;
478
479 for (i = 0; i < num_clocks; i++)
480 if (clocks[i].id == id)
481 return i;
482
483 return UINT_MAX;
484 }
485
486 static const struct tegra_bpmp_clk_info *
tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info * clocks,unsigned int num_clocks,unsigned int id)487 tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info *clocks,
488 unsigned int num_clocks, unsigned int id)
489 {
490 unsigned int i;
491
492 i = tegra_bpmp_clk_id_to_index(clocks, num_clocks, id);
493
494 if (i < num_clocks)
495 return &clocks[i];
496
497 return NULL;
498 }
499
500 static struct tegra_bpmp_clk *
tegra_bpmp_clk_register(struct tegra_bpmp * bpmp,const struct tegra_bpmp_clk_info * info,const struct tegra_bpmp_clk_info * clocks,unsigned int num_clocks)501 tegra_bpmp_clk_register(struct tegra_bpmp *bpmp,
502 const struct tegra_bpmp_clk_info *info,
503 const struct tegra_bpmp_clk_info *clocks,
504 unsigned int num_clocks)
505 {
506 struct tegra_bpmp_clk *clk;
507 struct clk_init_data init;
508 const char **parents;
509 unsigned int i;
510 int err;
511
512 clk = devm_kzalloc(bpmp->dev, sizeof(*clk), GFP_KERNEL);
513 if (!clk)
514 return ERR_PTR(-ENOMEM);
515
516 clk->id = info->id;
517 clk->bpmp = bpmp;
518
519 clk->parents = devm_kcalloc(bpmp->dev, info->num_parents,
520 sizeof(*clk->parents), GFP_KERNEL);
521 if (!clk->parents)
522 return ERR_PTR(-ENOMEM);
523
524 clk->num_parents = info->num_parents;
525
526 /* hardware clock initialization */
527 memset(&init, 0, sizeof(init));
528 init.name = info->name;
529 clk->hw.init = &init;
530 if (info->flags & BPMP_CLK_STATE_CHANGE_DENIED) {
531 if ((info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) == 0) {
532 dev_WARN(bpmp->dev,
533 "Firmware bug! Inconsistent permission bits for clock %s. State and parent/rate changes disabled.",
534 init.name);
535 }
536 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX)
537 init.ops = &tegra_bpmp_clk_mux_read_only_ops;
538 else
539 init.ops = &tegra_bpmp_clk_read_only_ops;
540 } else if (info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) {
541 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX)
542 init.ops = &tegra_bpmp_clk_gate_mux_read_only_ops;
543 else
544 init.ops = &tegra_bpmp_clk_gate_ops;
545 } else if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
546 if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
547 init.ops = &tegra_bpmp_clk_mux_rate_ops;
548 else
549 init.ops = &tegra_bpmp_clk_mux_ops;
550 } else {
551 if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
552 init.ops = &tegra_bpmp_clk_rate_ops;
553 else
554 init.ops = &tegra_bpmp_clk_gate_ops;
555 }
556
557 init.num_parents = info->num_parents;
558
559 parents = kcalloc(info->num_parents, sizeof(*parents), GFP_KERNEL);
560 if (!parents)
561 return ERR_PTR(-ENOMEM);
562
563 for (i = 0; i < info->num_parents; i++) {
564 const struct tegra_bpmp_clk_info *parent;
565
566 /* keep a private copy of the ID to parent index map */
567 clk->parents[i] = info->parents[i];
568
569 parent = tegra_bpmp_clk_find(clocks, num_clocks,
570 info->parents[i]);
571 if (!parent) {
572 dev_err(bpmp->dev, "no parent %u found for %u\n",
573 info->parents[i], info->id);
574 continue;
575 }
576
577 parents[i] = parent->name;
578 }
579
580 init.parent_names = parents;
581
582 err = devm_clk_hw_register(bpmp->dev, &clk->hw);
583
584 kfree(parents);
585
586 if (err < 0)
587 return ERR_PTR(err);
588
589 return clk;
590 }
591
tegra_bpmp_register_clocks_one(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info * infos,unsigned int i,unsigned int count)592 static void tegra_bpmp_register_clocks_one(struct tegra_bpmp *bpmp,
593 struct tegra_bpmp_clk_info *infos,
594 unsigned int i,
595 unsigned int count)
596 {
597 unsigned int j;
598 struct tegra_bpmp_clk_info *info;
599 struct tegra_bpmp_clk *clk;
600
601 if (bpmp->clocks[i]) {
602 /* already registered */
603 return;
604 }
605
606 info = &infos[i];
607 for (j = 0; j < info->num_parents; ++j) {
608 unsigned int p_id = info->parents[j];
609 unsigned int p_i = tegra_bpmp_clk_id_to_index(infos, count,
610 p_id);
611 if (p_i < count)
612 tegra_bpmp_register_clocks_one(bpmp, infos, p_i, count);
613 }
614
615 clk = tegra_bpmp_clk_register(bpmp, info, infos, count);
616 if (IS_ERR(clk)) {
617 dev_err(bpmp->dev,
618 "failed to register clock %u (%s): %ld\n",
619 info->id, info->name, PTR_ERR(clk));
620 /* intentionally store the error pointer to
621 * bpmp->clocks[i] to avoid re-attempting the
622 * registration later
623 */
624 }
625
626 bpmp->clocks[i] = clk;
627 }
628
tegra_bpmp_register_clocks(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info * infos,unsigned int count)629 static int tegra_bpmp_register_clocks(struct tegra_bpmp *bpmp,
630 struct tegra_bpmp_clk_info *infos,
631 unsigned int count)
632 {
633 unsigned int i;
634
635 bpmp->num_clocks = count;
636
637 bpmp->clocks = devm_kcalloc(bpmp->dev, count, sizeof(struct tegra_bpmp_clk), GFP_KERNEL);
638 if (!bpmp->clocks)
639 return -ENOMEM;
640
641 for (i = 0; i < count; i++) {
642 tegra_bpmp_register_clocks_one(bpmp, infos, i, count);
643 }
644
645 return 0;
646 }
647
tegra_bpmp_unregister_clocks(struct tegra_bpmp * bpmp)648 static void tegra_bpmp_unregister_clocks(struct tegra_bpmp *bpmp)
649 {
650 unsigned int i;
651
652 for (i = 0; i < bpmp->num_clocks; i++)
653 clk_hw_unregister(&bpmp->clocks[i]->hw);
654 }
655
tegra_bpmp_clk_of_xlate(struct of_phandle_args * clkspec,void * data)656 static struct clk_hw *tegra_bpmp_clk_of_xlate(struct of_phandle_args *clkspec,
657 void *data)
658 {
659 unsigned int id = clkspec->args[0], i;
660 struct tegra_bpmp *bpmp = data;
661
662 for (i = 0; i < bpmp->num_clocks; i++) {
663 struct tegra_bpmp_clk *clk = bpmp->clocks[i];
664
665 if (!clk)
666 continue;
667
668 if (clk->id == id)
669 return &clk->hw;
670 }
671
672 return NULL;
673 }
674
tegra_bpmp_init_clocks(struct tegra_bpmp * bpmp)675 int tegra_bpmp_init_clocks(struct tegra_bpmp *bpmp)
676 {
677 struct tegra_bpmp_clk_info *clocks;
678 unsigned int count;
679 int err;
680
681 err = tegra_bpmp_probe_clocks(bpmp, &clocks);
682 if (err < 0)
683 return err;
684
685 count = err;
686
687 dev_dbg(bpmp->dev, "%u clocks probed\n", count);
688
689 err = tegra_bpmp_register_clocks(bpmp, clocks, count);
690 if (err < 0)
691 goto free;
692
693 err = of_clk_add_hw_provider(bpmp->dev->of_node,
694 tegra_bpmp_clk_of_xlate,
695 bpmp);
696 if (err < 0) {
697 tegra_bpmp_unregister_clocks(bpmp);
698 goto free;
699 }
700
701 free:
702 kfree(clocks);
703 return err;
704 }
705