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