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 0;
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 .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
tegra_bpmp_clk_get_max_id(struct tegra_bpmp * bpmp)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, &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
tegra_bpmp_clk_get_info(struct tegra_bpmp * bpmp,unsigned int id,struct tegra_bpmp_clk_info * info)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, &msg);
367 if (err < 0)
368 return err;
369
370 strscpy(info->name, 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
tegra_bpmp_clk_info_dump(struct tegra_bpmp * bpmp,const char * level,const struct tegra_bpmp_clk_info * info)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(&buf, flags, sizeof(flags));
391
392 if (info->flags)
393 seq_buf_printf(&buf, "(");
394
395 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
396 seq_buf_printf(&buf, "%smux", prefix);
397 prefix = ", ";
398 }
399
400 if ((info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) == 0) {
401 seq_buf_printf(&buf, "%sfixed", prefix);
402 prefix = ", ";
403 }
404
405 if (info->flags & TEGRA_BPMP_CLK_IS_ROOT) {
406 seq_buf_printf(&buf, "%sroot", prefix);
407 prefix = ", ";
408 }
409
410 if (info->flags)
411 seq_buf_printf(&buf, ")");
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
tegra_bpmp_probe_clocks(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info ** clocksp)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(max_id + 1, 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
tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info * clocks,unsigned int num_clocks,unsigned int id)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 *
tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info * clocks,unsigned int num_clocks,unsigned int id)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 *
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)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(bpmp->dev, sizeof(*clk), GFP_KERNEL);
514 if (!clk)
515 return ERR_PTR(-ENOMEM);
516
517 clk->id = info->id;
518 clk->bpmp = bpmp;
519
520 clk->parents = devm_kcalloc(bpmp->dev, info->num_parents,
521 sizeof(*clk->parents), GFP_KERNEL);
522 if (!clk->parents)
523 return ERR_PTR(-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(info->num_parents, sizeof(*parents), GFP_KERNEL);
561 if (!parents)
562 return ERR_PTR(-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 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(bpmp->dev, &clk->hw);
584
585 kfree(parents);
586
587 if (err < 0)
588 return ERR_PTR(err);
589
590 return clk;
591 }
592
tegra_bpmp_register_clocks_one(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info * infos,unsigned int i,unsigned int count)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(infos, count,
611 p_id);
612 if (p_i < count)
613 tegra_bpmp_register_clocks_one(bpmp, infos, p_i, count);
614 }
615
616 clk = tegra_bpmp_clk_register(bpmp, info, infos, count);
617 if (IS_ERR(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
tegra_bpmp_register_clocks(struct tegra_bpmp * bpmp,struct tegra_bpmp_clk_info * infos,unsigned int count)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(bpmp->dev, count, 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
tegra_bpmp_unregister_clocks(struct tegra_bpmp * bpmp)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(&bpmp->clocks[i]->hw);
655 }
656
tegra_bpmp_clk_of_xlate(struct of_phandle_args * clkspec,void * data)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
tegra_bpmp_init_clocks(struct tegra_bpmp * bpmp)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, &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, clocks, count);
691 if (err < 0)
692 goto free;
693
694 err = of_clk_add_hw_provider(bpmp->dev->of_node,
695 tegra_bpmp_clk_of_xlate,
696 bpmp);
697 if (err < 0) {
698 tegra_bpmp_unregister_clocks(bpmp);
699 goto free;
700 }
701
702 free:
703 kfree(clocks);
704 return err;
705 }
706