1
2
3
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
20struct 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
28struct 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
38static 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
43struct 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
59static 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
72
73
74
75
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
95static 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
107static 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
123static 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
143static 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
167static long tegra_bpmp_clk_round_rate(struct clk_hw *hw, unsigned long rate,
168 unsigned long *parent_rate)
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 int err;
175
176 memset(&request, 0, sizeof(request));
177 request.rate = min_t(u64, rate, S64_MAX);
178
179 memset(&msg, 0, sizeof(msg));
180 msg.cmd = CMD_CLK_ROUND_RATE;
181 msg.id = clk->id;
182 msg.tx.data = &request;
183 msg.tx.size = sizeof(request);
184 msg.rx.data = &response;
185 msg.rx.size = sizeof(response);
186
187 err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
188 if (err < 0)
189 return err;
190
191 return response.rate;
192}
193
194static int tegra_bpmp_clk_set_parent(struct clk_hw *hw, u8 index)
195{
196 struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
197 struct cmd_clk_set_parent_response response;
198 struct cmd_clk_set_parent_request request;
199 struct tegra_bpmp_clk_message msg;
200 int err;
201
202 memset(&request, 0, sizeof(request));
203 request.parent_id = clk->parents[index];
204
205 memset(&msg, 0, sizeof(msg));
206 msg.cmd = CMD_CLK_SET_PARENT;
207 msg.id = clk->id;
208 msg.tx.data = &request;
209 msg.tx.size = sizeof(request);
210 msg.rx.data = &response;
211 msg.rx.size = sizeof(response);
212
213 err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
214 if (err < 0)
215 return err;
216
217
218
219 return 0;
220}
221
222static u8 tegra_bpmp_clk_get_parent(struct clk_hw *hw)
223{
224 struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
225 struct cmd_clk_get_parent_response response;
226 struct tegra_bpmp_clk_message msg;
227 unsigned int i;
228 int err;
229
230 memset(&msg, 0, sizeof(msg));
231 msg.cmd = CMD_CLK_GET_PARENT;
232 msg.id = clk->id;
233 msg.rx.data = &response;
234 msg.rx.size = sizeof(response);
235
236 err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
237 if (err < 0) {
238 dev_err(clk->bpmp->dev, "failed to get parent for %s: %d\n",
239 clk_hw_get_name(hw), err);
240 return U8_MAX;
241 }
242
243 for (i = 0; i < clk->num_parents; i++)
244 if (clk->parents[i] == response.parent_id)
245 return i;
246
247 return U8_MAX;
248}
249
250static int tegra_bpmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
251 unsigned long parent_rate)
252{
253 struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
254 struct cmd_clk_set_rate_response response;
255 struct cmd_clk_set_rate_request request;
256 struct tegra_bpmp_clk_message msg;
257
258 memset(&request, 0, sizeof(request));
259 request.rate = min_t(u64, rate, S64_MAX);
260
261 memset(&msg, 0, sizeof(msg));
262 msg.cmd = CMD_CLK_SET_RATE;
263 msg.id = clk->id;
264 msg.tx.data = &request;
265 msg.tx.size = sizeof(request);
266 msg.rx.data = &response;
267 msg.rx.size = sizeof(response);
268
269 return tegra_bpmp_clk_transfer(clk->bpmp, &msg);
270}
271
272static const struct clk_ops tegra_bpmp_clk_gate_ops = {
273 .prepare = tegra_bpmp_clk_prepare,
274 .unprepare = tegra_bpmp_clk_unprepare,
275 .is_prepared = tegra_bpmp_clk_is_prepared,
276 .recalc_rate = tegra_bpmp_clk_recalc_rate,
277};
278
279static const struct clk_ops tegra_bpmp_clk_mux_ops = {
280 .prepare = tegra_bpmp_clk_prepare,
281 .unprepare = tegra_bpmp_clk_unprepare,
282 .is_prepared = tegra_bpmp_clk_is_prepared,
283 .recalc_rate = tegra_bpmp_clk_recalc_rate,
284 .set_parent = tegra_bpmp_clk_set_parent,
285 .get_parent = tegra_bpmp_clk_get_parent,
286};
287
288static const struct clk_ops tegra_bpmp_clk_rate_ops = {
289 .prepare = tegra_bpmp_clk_prepare,
290 .unprepare = tegra_bpmp_clk_unprepare,
291 .is_prepared = tegra_bpmp_clk_is_prepared,
292 .recalc_rate = tegra_bpmp_clk_recalc_rate,
293 .round_rate = tegra_bpmp_clk_round_rate,
294 .set_rate = tegra_bpmp_clk_set_rate,
295};
296
297static const struct clk_ops tegra_bpmp_clk_mux_rate_ops = {
298 .prepare = tegra_bpmp_clk_prepare,
299 .unprepare = tegra_bpmp_clk_unprepare,
300 .is_prepared = tegra_bpmp_clk_is_prepared,
301 .recalc_rate = tegra_bpmp_clk_recalc_rate,
302 .round_rate = tegra_bpmp_clk_round_rate,
303 .set_parent = tegra_bpmp_clk_set_parent,
304 .get_parent = tegra_bpmp_clk_get_parent,
305 .set_rate = tegra_bpmp_clk_set_rate,
306};
307
308static int tegra_bpmp_clk_get_max_id(struct tegra_bpmp *bpmp)
309{
310 struct cmd_clk_get_max_clk_id_response response;
311 struct tegra_bpmp_clk_message msg;
312 int err;
313
314 memset(&msg, 0, sizeof(msg));
315 msg.cmd = CMD_CLK_GET_MAX_CLK_ID;
316 msg.rx.data = &response;
317 msg.rx.size = sizeof(response);
318
319 err = tegra_bpmp_clk_transfer(bpmp, &msg);
320 if (err < 0)
321 return err;
322
323 if (response.max_id > INT_MAX)
324 return -E2BIG;
325
326 return response.max_id;
327}
328
329static int tegra_bpmp_clk_get_info(struct tegra_bpmp *bpmp, unsigned int id,
330 struct tegra_bpmp_clk_info *info)
331{
332 struct cmd_clk_get_all_info_response response;
333 struct tegra_bpmp_clk_message msg;
334 unsigned int i;
335 int err;
336
337 memset(&msg, 0, sizeof(msg));
338 msg.cmd = CMD_CLK_GET_ALL_INFO;
339 msg.id = id;
340 msg.rx.data = &response;
341 msg.rx.size = sizeof(response);
342
343 err = tegra_bpmp_clk_transfer(bpmp, &msg);
344 if (err < 0)
345 return err;
346
347 strlcpy(info->name, response.name, MRQ_CLK_NAME_MAXLEN);
348 info->num_parents = response.num_parents;
349
350 for (i = 0; i < info->num_parents; i++)
351 info->parents[i] = response.parents[i];
352
353 info->flags = response.flags;
354
355 return 0;
356}
357
358static void tegra_bpmp_clk_info_dump(struct tegra_bpmp *bpmp,
359 const char *level,
360 const struct tegra_bpmp_clk_info *info)
361{
362 const char *prefix = "";
363 struct seq_buf buf;
364 unsigned int i;
365 char flags[64];
366
367 seq_buf_init(&buf, flags, sizeof(flags));
368
369 if (info->flags)
370 seq_buf_printf(&buf, "(");
371
372 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
373 seq_buf_printf(&buf, "%smux", prefix);
374 prefix = ", ";
375 }
376
377 if ((info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) == 0) {
378 seq_buf_printf(&buf, "%sfixed", prefix);
379 prefix = ", ";
380 }
381
382 if (info->flags & TEGRA_BPMP_CLK_IS_ROOT) {
383 seq_buf_printf(&buf, "%sroot", prefix);
384 prefix = ", ";
385 }
386
387 if (info->flags)
388 seq_buf_printf(&buf, ")");
389
390 dev_printk(level, bpmp->dev, "%03u: %s\n", info->id, info->name);
391 dev_printk(level, bpmp->dev, " flags: %lx %s\n", info->flags, flags);
392 dev_printk(level, bpmp->dev, " parents: %u\n", info->num_parents);
393
394 for (i = 0; i < info->num_parents; i++)
395 dev_printk(level, bpmp->dev, " %03u\n", info->parents[i]);
396}
397
398static int tegra_bpmp_probe_clocks(struct tegra_bpmp *bpmp,
399 struct tegra_bpmp_clk_info **clocksp)
400{
401 struct tegra_bpmp_clk_info *clocks;
402 unsigned int max_id, id, count = 0;
403 unsigned int holes = 0;
404 int err;
405
406 err = tegra_bpmp_clk_get_max_id(bpmp);
407 if (err < 0)
408 return err;
409
410 max_id = err;
411
412 dev_dbg(bpmp->dev, "maximum clock ID: %u\n", max_id);
413
414 clocks = kcalloc(max_id + 1, sizeof(*clocks), GFP_KERNEL);
415 if (!clocks)
416 return -ENOMEM;
417
418 for (id = 0; id <= max_id; id++) {
419 struct tegra_bpmp_clk_info *info = &clocks[count];
420
421 err = tegra_bpmp_clk_get_info(bpmp, id, info);
422 if (err < 0)
423 continue;
424
425 if (info->num_parents >= U8_MAX) {
426 dev_err(bpmp->dev,
427 "clock %u has too many parents (%u, max: %u)\n",
428 id, info->num_parents, U8_MAX);
429 continue;
430 }
431
432
433 if (info->name[0] == '\0') {
434 holes++;
435 continue;
436 }
437
438 info->id = id;
439 count++;
440
441 if (TEGRA_BPMP_DUMP_CLOCK_INFO)
442 tegra_bpmp_clk_info_dump(bpmp, KERN_DEBUG, info);
443 }
444
445 dev_dbg(bpmp->dev, "holes: %u\n", holes);
446 *clocksp = clocks;
447
448 return count;
449}
450
451static const struct tegra_bpmp_clk_info *
452tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info *clocks,
453 unsigned int num_clocks, unsigned int id)
454{
455 unsigned int i;
456
457 for (i = 0; i < num_clocks; i++)
458 if (clocks[i].id == id)
459 return &clocks[i];
460
461 return NULL;
462}
463
464static struct tegra_bpmp_clk *
465tegra_bpmp_clk_register(struct tegra_bpmp *bpmp,
466 const struct tegra_bpmp_clk_info *info,
467 const struct tegra_bpmp_clk_info *clocks,
468 unsigned int num_clocks)
469{
470 struct tegra_bpmp_clk *clk;
471 struct clk_init_data init;
472 const char **parents;
473 unsigned int i;
474 int err;
475
476 clk = devm_kzalloc(bpmp->dev, sizeof(*clk), GFP_KERNEL);
477 if (!clk)
478 return ERR_PTR(-ENOMEM);
479
480 clk->id = info->id;
481 clk->bpmp = bpmp;
482
483 clk->parents = devm_kcalloc(bpmp->dev, info->num_parents,
484 sizeof(*clk->parents), GFP_KERNEL);
485 if (!clk->parents)
486 return ERR_PTR(-ENOMEM);
487
488 clk->num_parents = info->num_parents;
489
490
491 memset(&init, 0, sizeof(init));
492 init.name = info->name;
493 clk->hw.init = &init;
494
495 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
496 if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
497 init.ops = &tegra_bpmp_clk_mux_rate_ops;
498 else
499 init.ops = &tegra_bpmp_clk_mux_ops;
500 } else {
501 if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
502 init.ops = &tegra_bpmp_clk_rate_ops;
503 else
504 init.ops = &tegra_bpmp_clk_gate_ops;
505 }
506
507 init.num_parents = info->num_parents;
508
509 parents = kcalloc(info->num_parents, sizeof(*parents), GFP_KERNEL);
510 if (!parents)
511 return ERR_PTR(-ENOMEM);
512
513 for (i = 0; i < info->num_parents; i++) {
514 const struct tegra_bpmp_clk_info *parent;
515
516
517 clk->parents[i] = info->parents[i];
518
519 parent = tegra_bpmp_clk_find(clocks, num_clocks,
520 info->parents[i]);
521 if (!parent) {
522 dev_err(bpmp->dev, "no parent %u found for %u\n",
523 info->parents[i], info->id);
524 continue;
525 }
526
527 parents[i] = parent->name;
528 }
529
530 init.parent_names = parents;
531
532 err = devm_clk_hw_register(bpmp->dev, &clk->hw);
533
534 kfree(parents);
535
536 if (err < 0)
537 return ERR_PTR(err);
538
539 return clk;
540}
541
542static int tegra_bpmp_register_clocks(struct tegra_bpmp *bpmp,
543 struct tegra_bpmp_clk_info *infos,
544 unsigned int count)
545{
546 struct tegra_bpmp_clk *clk;
547 unsigned int i;
548
549 bpmp->num_clocks = count;
550
551 bpmp->clocks = devm_kcalloc(bpmp->dev, count, sizeof(clk), GFP_KERNEL);
552 if (!bpmp->clocks)
553 return -ENOMEM;
554
555 for (i = 0; i < count; i++) {
556 struct tegra_bpmp_clk_info *info = &infos[i];
557
558 clk = tegra_bpmp_clk_register(bpmp, info, infos, count);
559 if (IS_ERR(clk)) {
560 dev_err(bpmp->dev,
561 "failed to register clock %u (%s): %ld\n",
562 info->id, info->name, PTR_ERR(clk));
563 continue;
564 }
565
566 bpmp->clocks[i] = clk;
567 }
568
569 return 0;
570}
571
572static void tegra_bpmp_unregister_clocks(struct tegra_bpmp *bpmp)
573{
574 unsigned int i;
575
576 for (i = 0; i < bpmp->num_clocks; i++)
577 clk_hw_unregister(&bpmp->clocks[i]->hw);
578}
579
580static struct clk_hw *tegra_bpmp_clk_of_xlate(struct of_phandle_args *clkspec,
581 void *data)
582{
583 unsigned int id = clkspec->args[0], i;
584 struct tegra_bpmp *bpmp = data;
585
586 for (i = 0; i < bpmp->num_clocks; i++) {
587 struct tegra_bpmp_clk *clk = bpmp->clocks[i];
588
589 if (!clk)
590 continue;
591
592 if (clk->id == id)
593 return &clk->hw;
594 }
595
596 return NULL;
597}
598
599int tegra_bpmp_init_clocks(struct tegra_bpmp *bpmp)
600{
601 struct tegra_bpmp_clk_info *clocks;
602 unsigned int count;
603 int err;
604
605 err = tegra_bpmp_probe_clocks(bpmp, &clocks);
606 if (err < 0)
607 return err;
608
609 count = err;
610
611 dev_dbg(bpmp->dev, "%u clocks probed\n", count);
612
613 err = tegra_bpmp_register_clocks(bpmp, clocks, count);
614 if (err < 0)
615 goto free;
616
617 err = of_clk_add_hw_provider(bpmp->dev->of_node,
618 tegra_bpmp_clk_of_xlate,
619 bpmp);
620 if (err < 0) {
621 tegra_bpmp_unregister_clocks(bpmp);
622 goto free;
623 }
624
625free:
626 kfree(clocks);
627 return err;
628}
629
