1
2
3
4
5
6
7
8
9
10#include <linux/bitfield.h>
11#include <linux/clk.h>
12#include <linux/clk-provider.h>
13#include <linux/module.h>
14#include <linux/of_platform.h>
15#include <linux/slab.h>
16#include <linux/string.h>
17
18#include "clk-zynqmp.h"
19
20#define MAX_PARENT 100
21#define MAX_NODES 6
22#define MAX_NAME_LEN 50
23
24#define CLK_TYPE_SHIFT 2
25
26#define PM_API_PAYLOAD_LEN 3
27
28#define NA_PARENT 0xFFFFFFFF
29#define DUMMY_PARENT 0xFFFFFFFE
30
31#define CLK_TYPE_FIELD_LEN 4
32#define CLK_TOPOLOGY_NODE_OFFSET 16
33#define NODES_PER_RESP 3
34
35#define CLK_TYPE_FIELD_MASK 0xF
36#define CLK_FLAG_FIELD_MASK GENMASK(21, 8)
37#define CLK_TYPE_FLAG_FIELD_MASK GENMASK(31, 24)
38
39#define CLK_PARENTS_ID_LEN 16
40#define CLK_PARENTS_ID_MASK 0xFFFF
41
42
43#define PARENT_CLK_SELF 0
44#define PARENT_CLK_NODE1 1
45#define PARENT_CLK_NODE2 2
46#define PARENT_CLK_NODE3 3
47#define PARENT_CLK_NODE4 4
48#define PARENT_CLK_EXTERNAL 5
49
50#define END_OF_CLK_NAME "END_OF_CLK"
51#define END_OF_TOPOLOGY_NODE 1
52#define END_OF_PARENTS 1
53#define RESERVED_CLK_NAME ""
54
55#define CLK_VALID_MASK 0x1
56
57enum clk_type {
58 CLK_TYPE_OUTPUT,
59 CLK_TYPE_EXTERNAL,
60};
61
62
63
64
65
66
67
68struct clock_parent {
69 char name[MAX_NAME_LEN];
70 int id;
71 u32 flag;
72};
73
74
75
76
77
78
79
80
81
82
83
84struct zynqmp_clock {
85 char clk_name[MAX_NAME_LEN];
86 u32 valid;
87 enum clk_type type;
88 struct clock_topology node[MAX_NODES];
89 u32 num_nodes;
90 struct clock_parent parent[MAX_PARENT];
91 u32 num_parents;
92};
93
94static const char clk_type_postfix[][10] = {
95 [TYPE_INVALID] = "",
96 [TYPE_MUX] = "_mux",
97 [TYPE_GATE] = "",
98 [TYPE_DIV1] = "_div1",
99 [TYPE_DIV2] = "_div2",
100 [TYPE_FIXEDFACTOR] = "_ff",
101 [TYPE_PLL] = ""
102};
103
104static struct clk_hw *(* const clk_topology[]) (const char *name, u32 clk_id,
105 const char * const *parents,
106 u8 num_parents,
107 const struct clock_topology *nodes)
108 = {
109 [TYPE_INVALID] = NULL,
110 [TYPE_MUX] = zynqmp_clk_register_mux,
111 [TYPE_PLL] = zynqmp_clk_register_pll,
112 [TYPE_FIXEDFACTOR] = zynqmp_clk_register_fixed_factor,
113 [TYPE_DIV1] = zynqmp_clk_register_divider,
114 [TYPE_DIV2] = zynqmp_clk_register_divider,
115 [TYPE_GATE] = zynqmp_clk_register_gate
116};
117
118static struct zynqmp_clock *clock;
119static struct clk_hw_onecell_data *zynqmp_data;
120static unsigned int clock_max_idx;
121static const struct zynqmp_eemi_ops *eemi_ops;
122
123
124
125
126
127
128
129static inline int zynqmp_is_valid_clock(u32 clk_id)
130{
131 if (clk_id >= clock_max_idx)
132 return -ENODEV;
133
134 return clock[clk_id].valid;
135}
136
137
138
139
140
141
142
143
144static int zynqmp_get_clock_name(u32 clk_id, char *clk_name)
145{
146 int ret;
147
148 ret = zynqmp_is_valid_clock(clk_id);
149 if (ret == 1) {
150 strncpy(clk_name, clock[clk_id].clk_name, MAX_NAME_LEN);
151 return 0;
152 }
153
154 return ret == 0 ? -EINVAL : ret;
155}
156
157
158
159
160
161
162
163
164static int zynqmp_get_clock_type(u32 clk_id, u32 *type)
165{
166 int ret;
167
168 ret = zynqmp_is_valid_clock(clk_id);
169 if (ret == 1) {
170 *type = clock[clk_id].type;
171 return 0;
172 }
173
174 return ret == 0 ? -EINVAL : ret;
175}
176
177
178
179
180
181
182
183
184
185static int zynqmp_pm_clock_get_num_clocks(u32 *nclocks)
186{
187 struct zynqmp_pm_query_data qdata = {0};
188 u32 ret_payload[PAYLOAD_ARG_CNT];
189 int ret;
190
191 qdata.qid = PM_QID_CLOCK_GET_NUM_CLOCKS;
192
193 ret = eemi_ops->query_data(qdata, ret_payload);
194 *nclocks = ret_payload[1];
195
196 return ret;
197}
198
199
200
201
202
203
204
205
206
207
208
209static int zynqmp_pm_clock_get_name(u32 clock_id, char *name)
210{
211 struct zynqmp_pm_query_data qdata = {0};
212 u32 ret_payload[PAYLOAD_ARG_CNT];
213
214 qdata.qid = PM_QID_CLOCK_GET_NAME;
215 qdata.arg1 = clock_id;
216
217 eemi_ops->query_data(qdata, ret_payload);
218 memcpy(name, ret_payload, CLK_GET_NAME_RESP_LEN);
219
220 return 0;
221}
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240static int zynqmp_pm_clock_get_topology(u32 clock_id, u32 index, u32 *topology)
241{
242 struct zynqmp_pm_query_data qdata = {0};
243 u32 ret_payload[PAYLOAD_ARG_CNT];
244 int ret;
245
246 qdata.qid = PM_QID_CLOCK_GET_TOPOLOGY;
247 qdata.arg1 = clock_id;
248 qdata.arg2 = index;
249
250 ret = eemi_ops->query_data(qdata, ret_payload);
251 memcpy(topology, &ret_payload[1], CLK_GET_TOPOLOGY_RESP_WORDS * 4);
252
253 return ret;
254}
255
256
257
258
259
260
261
262
263
264
265
266
267struct clk_hw *zynqmp_clk_register_fixed_factor(const char *name, u32 clk_id,
268 const char * const *parents,
269 u8 num_parents,
270 const struct clock_topology *nodes)
271{
272 u32 mult, div;
273 struct clk_hw *hw;
274 struct zynqmp_pm_query_data qdata = {0};
275 u32 ret_payload[PAYLOAD_ARG_CNT];
276 int ret;
277
278 qdata.qid = PM_QID_CLOCK_GET_FIXEDFACTOR_PARAMS;
279 qdata.arg1 = clk_id;
280
281 ret = eemi_ops->query_data(qdata, ret_payload);
282 if (ret)
283 return ERR_PTR(ret);
284
285 mult = ret_payload[1];
286 div = ret_payload[2];
287
288 hw = clk_hw_register_fixed_factor(NULL, name,
289 parents[0],
290 nodes->flag, mult,
291 div);
292
293 return hw;
294}
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313static int zynqmp_pm_clock_get_parents(u32 clock_id, u32 index, u32 *parents)
314{
315 struct zynqmp_pm_query_data qdata = {0};
316 u32 ret_payload[PAYLOAD_ARG_CNT];
317 int ret;
318
319 qdata.qid = PM_QID_CLOCK_GET_PARENTS;
320 qdata.arg1 = clock_id;
321 qdata.arg2 = index;
322
323 ret = eemi_ops->query_data(qdata, ret_payload);
324 memcpy(parents, &ret_payload[1], CLK_GET_PARENTS_RESP_WORDS * 4);
325
326 return ret;
327}
328
329
330
331
332
333
334
335
336
337
338static int zynqmp_pm_clock_get_attributes(u32 clock_id, u32 *attr)
339{
340 struct zynqmp_pm_query_data qdata = {0};
341 u32 ret_payload[PAYLOAD_ARG_CNT];
342 int ret;
343
344 qdata.qid = PM_QID_CLOCK_GET_ATTRIBUTES;
345 qdata.arg1 = clock_id;
346
347 ret = eemi_ops->query_data(qdata, ret_payload);
348 memcpy(attr, &ret_payload[1], CLK_GET_ATTR_RESP_WORDS * 4);
349
350 return ret;
351}
352
353
354
355
356
357
358
359
360
361
362static int __zynqmp_clock_get_topology(struct clock_topology *topology,
363 u32 *data, u32 *nnodes)
364{
365 int i;
366
367 for (i = 0; i < PM_API_PAYLOAD_LEN; i++) {
368 if (!(data[i] & CLK_TYPE_FIELD_MASK))
369 return END_OF_TOPOLOGY_NODE;
370 topology[*nnodes].type = data[i] & CLK_TYPE_FIELD_MASK;
371 topology[*nnodes].flag = FIELD_GET(CLK_FLAG_FIELD_MASK,
372 data[i]);
373 topology[*nnodes].type_flag =
374 FIELD_GET(CLK_TYPE_FLAG_FIELD_MASK, data[i]);
375 (*nnodes)++;
376 }
377
378 return 0;
379}
380
381
382
383
384
385
386
387
388
389
390static int zynqmp_clock_get_topology(u32 clk_id,
391 struct clock_topology *topology,
392 u32 *num_nodes)
393{
394 int j, ret;
395 u32 pm_resp[PM_API_PAYLOAD_LEN] = {0};
396
397 *num_nodes = 0;
398 for (j = 0; j <= MAX_NODES; j += 3) {
399 ret = zynqmp_pm_clock_get_topology(clk_id, j, pm_resp);
400 if (ret)
401 return ret;
402 ret = __zynqmp_clock_get_topology(topology, pm_resp, num_nodes);
403 if (ret == END_OF_TOPOLOGY_NODE)
404 return 0;
405 }
406
407 return 0;
408}
409
410
411
412
413
414
415
416
417
418
419static int __zynqmp_clock_get_parents(struct clock_parent *parents, u32 *data,
420 u32 *nparent)
421{
422 int i;
423 struct clock_parent *parent;
424
425 for (i = 0; i < PM_API_PAYLOAD_LEN; i++) {
426 if (data[i] == NA_PARENT)
427 return END_OF_PARENTS;
428
429 parent = &parents[i];
430 parent->id = data[i] & CLK_PARENTS_ID_MASK;
431 if (data[i] == DUMMY_PARENT) {
432 strcpy(parent->name, "dummy_name");
433 parent->flag = 0;
434 } else {
435 parent->flag = data[i] >> CLK_PARENTS_ID_LEN;
436 if (zynqmp_get_clock_name(parent->id, parent->name))
437 continue;
438 }
439 *nparent += 1;
440 }
441
442 return 0;
443}
444
445
446
447
448
449
450
451
452
453static int zynqmp_clock_get_parents(u32 clk_id, struct clock_parent *parents,
454 u32 *num_parents)
455{
456 int j = 0, ret;
457 u32 pm_resp[PM_API_PAYLOAD_LEN] = {0};
458
459 *num_parents = 0;
460 do {
461
462 ret = zynqmp_pm_clock_get_parents(clk_id, j, pm_resp);
463 if (ret)
464 return ret;
465
466 ret = __zynqmp_clock_get_parents(&parents[j], pm_resp,
467 num_parents);
468 if (ret == END_OF_PARENTS)
469 return 0;
470 j += PM_API_PAYLOAD_LEN;
471 } while (*num_parents <= MAX_PARENT);
472
473 return 0;
474}
475
476
477
478
479
480
481
482
483
484
485static int zynqmp_get_parent_list(struct device_node *np, u32 clk_id,
486 const char **parent_list, u32 *num_parents)
487{
488 int i = 0, ret;
489 u32 total_parents = clock[clk_id].num_parents;
490 struct clock_topology *clk_nodes;
491 struct clock_parent *parents;
492
493 clk_nodes = clock[clk_id].node;
494 parents = clock[clk_id].parent;
495
496 for (i = 0; i < total_parents; i++) {
497 if (!parents[i].flag) {
498 parent_list[i] = parents[i].name;
499 } else if (parents[i].flag == PARENT_CLK_EXTERNAL) {
500 ret = of_property_match_string(np, "clock-names",
501 parents[i].name);
502 if (ret < 0)
503 strcpy(parents[i].name, "dummy_name");
504 parent_list[i] = parents[i].name;
505 } else {
506 strcat(parents[i].name,
507 clk_type_postfix[clk_nodes[parents[i].flag - 1].
508 type]);
509 parent_list[i] = parents[i].name;
510 }
511 }
512
513 *num_parents = total_parents;
514 return 0;
515}
516
517
518
519
520
521
522
523
524
525
526static struct clk_hw *zynqmp_register_clk_topology(int clk_id, char *clk_name,
527 int num_parents,
528 const char **parent_names)
529{
530 int j;
531 u32 num_nodes;
532 char *clk_out = NULL;
533 struct clock_topology *nodes;
534 struct clk_hw *hw = NULL;
535
536 nodes = clock[clk_id].node;
537 num_nodes = clock[clk_id].num_nodes;
538
539 for (j = 0; j < num_nodes; j++) {
540
541
542
543
544 if (j != (num_nodes - 1)) {
545 clk_out = kasprintf(GFP_KERNEL, "%s%s", clk_name,
546 clk_type_postfix[nodes[j].type]);
547 } else {
548 clk_out = kasprintf(GFP_KERNEL, "%s", clk_name);
549 }
550
551 if (!clk_topology[nodes[j].type])
552 continue;
553
554 hw = (*clk_topology[nodes[j].type])(clk_out, clk_id,
555 parent_names,
556 num_parents,
557 &nodes[j]);
558 if (IS_ERR(hw))
559 pr_warn_once("%s() %s register fail with %ld\n",
560 __func__, clk_name, PTR_ERR(hw));
561
562 parent_names[0] = clk_out;
563 }
564 kfree(clk_out);
565 return hw;
566}
567
568
569
570
571
572
573
574static int zynqmp_register_clocks(struct device_node *np)
575{
576 int ret;
577 u32 i, total_parents = 0, type = 0;
578 const char *parent_names[MAX_PARENT];
579
580 for (i = 0; i < clock_max_idx; i++) {
581 char clk_name[MAX_NAME_LEN];
582
583
584 if (zynqmp_get_clock_name(i, clk_name))
585 continue;
586
587
588
589
590 ret = zynqmp_get_clock_type(i, &type);
591 if (ret || type != CLK_TYPE_OUTPUT)
592 continue;
593
594
595 if (zynqmp_get_parent_list(np, i, parent_names,
596 &total_parents)) {
597 WARN_ONCE(1, "No parents found for %s\n",
598 clock[i].clk_name);
599 continue;
600 }
601
602 zynqmp_data->hws[i] =
603 zynqmp_register_clk_topology(i, clk_name,
604 total_parents,
605 parent_names);
606 }
607
608 for (i = 0; i < clock_max_idx; i++) {
609 if (IS_ERR(zynqmp_data->hws[i])) {
610 pr_err("Zynq Ultrascale+ MPSoC clk %s: register failed with %ld\n",
611 clock[i].clk_name, PTR_ERR(zynqmp_data->hws[i]));
612 WARN_ON(1);
613 }
614 }
615 return 0;
616}
617
618
619
620
621static void zynqmp_get_clock_info(void)
622{
623 int i, ret;
624 u32 attr, type = 0;
625
626 for (i = 0; i < clock_max_idx; i++) {
627 zynqmp_pm_clock_get_name(i, clock[i].clk_name);
628 if (!strcmp(clock[i].clk_name, RESERVED_CLK_NAME))
629 continue;
630
631 ret = zynqmp_pm_clock_get_attributes(i, &attr);
632 if (ret)
633 continue;
634
635 clock[i].valid = attr & CLK_VALID_MASK;
636 clock[i].type = attr >> CLK_TYPE_SHIFT ? CLK_TYPE_EXTERNAL :
637 CLK_TYPE_OUTPUT;
638 }
639
640
641 for (i = 0; i < clock_max_idx; i++) {
642 ret = zynqmp_get_clock_type(i, &type);
643 if (ret || type != CLK_TYPE_OUTPUT)
644 continue;
645
646 ret = zynqmp_clock_get_topology(i, clock[i].node,
647 &clock[i].num_nodes);
648 if (ret)
649 continue;
650
651 ret = zynqmp_clock_get_parents(i, clock[i].parent,
652 &clock[i].num_parents);
653 if (ret)
654 continue;
655 }
656}
657
658
659
660
661
662
663
664static int zynqmp_clk_setup(struct device_node *np)
665{
666 int ret;
667
668 ret = zynqmp_pm_clock_get_num_clocks(&clock_max_idx);
669 if (ret)
670 return ret;
671
672 zynqmp_data = kzalloc(struct_size(zynqmp_data, hws, clock_max_idx),
673 GFP_KERNEL);
674 if (!zynqmp_data)
675 return -ENOMEM;
676
677 clock = kcalloc(clock_max_idx, sizeof(*clock), GFP_KERNEL);
678 if (!clock) {
679 kfree(zynqmp_data);
680 return -ENOMEM;
681 }
682
683 zynqmp_get_clock_info();
684 zynqmp_register_clocks(np);
685
686 zynqmp_data->num = clock_max_idx;
687 of_clk_add_hw_provider(np, of_clk_hw_onecell_get, zynqmp_data);
688
689 return 0;
690}
691
692static int zynqmp_clock_probe(struct platform_device *pdev)
693{
694 int ret;
695 struct device *dev = &pdev->dev;
696
697 eemi_ops = zynqmp_pm_get_eemi_ops();
698 if (!eemi_ops)
699 return -ENXIO;
700
701 ret = zynqmp_clk_setup(dev->of_node);
702
703 return ret;
704}
705
706static const struct of_device_id zynqmp_clock_of_match[] = {
707 {.compatible = "xlnx,zynqmp-clk"},
708 {},
709};
710MODULE_DEVICE_TABLE(of, zynqmp_clock_of_match);
711
712static struct platform_driver zynqmp_clock_driver = {
713 .driver = {
714 .name = "zynqmp_clock",
715 .of_match_table = zynqmp_clock_of_match,
716 },
717 .probe = zynqmp_clock_probe,
718};
719module_platform_driver(zynqmp_clock_driver);
720