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