blob: 66cbef15ab8a68a26df2d5aeb325568ba1da0ed3 [file] [log] [blame]
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +05301// SPDX-License-Identifier: GPL-2.0+
2/*
3 * (C) Copyright 2019 Xilinx, Inc.
4 * Siva Durga Prasad Paladugu <siva.durga.paladugu@xilinx.com>
5 */
6
7#include <common.h>
8#include <linux/bitops.h>
9#include <linux/bitfield.h>
10#include <malloc.h>
11#include <clk-uclass.h>
12#include <clk.h>
13#include <dm.h>
14#include <asm/arch/sys_proto.h>
Michal Simek0f3604a2019-10-04 15:25:18 +020015#include <zynqmp_firmware.h>
Simon Glass61b29b82020-02-03 07:36:15 -070016#include <linux/err.h>
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +053017
18#define MAX_PARENT 100
19#define MAX_NODES 6
20#define MAX_NAME_LEN 50
21
22#define CLK_TYPE_SHIFT 2
23
24#define PM_API_PAYLOAD_LEN 3
25
26#define NA_PARENT 0xFFFFFFFF
27#define DUMMY_PARENT 0xFFFFFFFE
28
29#define CLK_TYPE_FIELD_LEN 4
30#define CLK_TOPOLOGY_NODE_OFFSET 16
31#define NODES_PER_RESP 3
32
33#define CLK_TYPE_FIELD_MASK 0xF
34#define CLK_FLAG_FIELD_MASK GENMASK(21, 8)
35#define CLK_TYPE_FLAG_FIELD_MASK GENMASK(31, 24)
36#define CLK_TYPE_FLAG2_FIELD_MASK GENMASK(7, 4)
37#define CLK_TYPE_FLAG_BITS 8
38
39#define CLK_PARENTS_ID_LEN 16
40#define CLK_PARENTS_ID_MASK 0xFFFF
41
42#define END_OF_TOPOLOGY_NODE 1
43#define END_OF_PARENTS 1
44
45#define CLK_VALID_MASK 0x1
46#define NODE_CLASS_SHIFT 26U
47#define NODE_SUBCLASS_SHIFT 20U
48#define NODE_TYPE_SHIFT 14U
49#define NODE_INDEX_SHIFT 0U
50
51#define CLK_GET_NAME_RESP_LEN 16
52#define CLK_GET_TOPOLOGY_RESP_WORDS 3
53#define CLK_GET_PARENTS_RESP_WORDS 3
54#define CLK_GET_ATTR_RESP_WORDS 1
55
56#define NODE_SUBCLASS_CLOCK_PLL 1
57#define NODE_SUBCLASS_CLOCK_OUT 2
58#define NODE_SUBCLASS_CLOCK_REF 3
59
60#define NODE_CLASS_CLOCK 2
61#define NODE_CLASS_MASK 0x3F
62
63#define CLOCK_NODE_TYPE_MUX 1
64#define CLOCK_NODE_TYPE_DIV 4
65#define CLOCK_NODE_TYPE_GATE 6
66
67enum pm_query_id {
68 PM_QID_INVALID,
69 PM_QID_CLOCK_GET_NAME,
70 PM_QID_CLOCK_GET_TOPOLOGY,
71 PM_QID_CLOCK_GET_FIXEDFACTOR_PARAMS,
72 PM_QID_CLOCK_GET_PARENTS,
73 PM_QID_CLOCK_GET_ATTRIBUTES,
74 PM_QID_PINCTRL_GET_NUM_PINS,
75 PM_QID_PINCTRL_GET_NUM_FUNCTIONS,
76 PM_QID_PINCTRL_GET_NUM_FUNCTION_GROUPS,
77 PM_QID_PINCTRL_GET_FUNCTION_NAME,
78 PM_QID_PINCTRL_GET_FUNCTION_GROUPS,
79 PM_QID_PINCTRL_GET_PIN_GROUPS,
80 PM_QID_CLOCK_GET_NUM_CLOCKS,
81 PM_QID_CLOCK_GET_MAX_DIVISOR,
82};
83
84enum clk_type {
85 CLK_TYPE_OUTPUT,
86 CLK_TYPE_EXTERNAL,
87};
88
89struct clock_parent {
90 char name[MAX_NAME_LEN];
91 int id;
92 u32 flag;
93};
94
95struct clock_topology {
96 u32 type;
97 u32 flag;
98 u32 type_flag;
99};
100
101struct versal_clock {
102 char clk_name[MAX_NAME_LEN];
103 u32 valid;
104 enum clk_type type;
105 struct clock_topology node[MAX_NODES];
106 u32 num_nodes;
107 struct clock_parent parent[MAX_PARENT];
108 u32 num_parents;
109 u32 clk_id;
110};
111
112struct versal_clk_priv {
113 struct versal_clock *clk;
114};
115
116static ulong alt_ref_clk;
117static ulong pl_alt_ref_clk;
118static ulong ref_clk;
119
120struct versal_pm_query_data {
121 u32 qid;
122 u32 arg1;
123 u32 arg2;
124 u32 arg3;
125};
126
127static struct versal_clock *clock;
128static unsigned int clock_max_idx;
129
130#define PM_QUERY_DATA 35
131
132static int versal_pm_query(struct versal_pm_query_data qdata, u32 *ret_payload)
133{
134 struct pt_regs regs;
135
136 regs.regs[0] = PM_SIP_SVC | PM_QUERY_DATA;
137 regs.regs[1] = ((u64)qdata.arg1 << 32) | qdata.qid;
138 regs.regs[2] = ((u64)qdata.arg3 << 32) | qdata.arg2;
139
140 smc_call(&regs);
141
142 if (ret_payload) {
143 ret_payload[0] = (u32)regs.regs[0];
144 ret_payload[1] = upper_32_bits(regs.regs[0]);
145 ret_payload[2] = (u32)regs.regs[1];
146 ret_payload[3] = upper_32_bits(regs.regs[1]);
147 ret_payload[4] = (u32)regs.regs[2];
148 }
149
150 return qdata.qid == PM_QID_CLOCK_GET_NAME ? 0 : regs.regs[0];
151}
152
153static inline int versal_is_valid_clock(u32 clk_id)
154{
155 if (clk_id >= clock_max_idx)
156 return -ENODEV;
157
158 return clock[clk_id].valid;
159}
160
161static int versal_get_clock_name(u32 clk_id, char *clk_name)
162{
163 int ret;
164
165 ret = versal_is_valid_clock(clk_id);
166 if (ret == 1) {
167 strncpy(clk_name, clock[clk_id].clk_name, MAX_NAME_LEN);
168 return 0;
169 }
170
171 return ret == 0 ? -EINVAL : ret;
172}
173
174static int versal_get_clock_type(u32 clk_id, u32 *type)
175{
176 int ret;
177
178 ret = versal_is_valid_clock(clk_id);
179 if (ret == 1) {
180 *type = clock[clk_id].type;
181 return 0;
182 }
183
184 return ret == 0 ? -EINVAL : ret;
185}
186
187static int versal_pm_clock_get_num_clocks(u32 *nclocks)
188{
189 struct versal_pm_query_data qdata = {0};
190 u32 ret_payload[PAYLOAD_ARG_CNT];
191 int ret;
192
193 qdata.qid = PM_QID_CLOCK_GET_NUM_CLOCKS;
194
195 ret = versal_pm_query(qdata, ret_payload);
196 *nclocks = ret_payload[1];
197
198 return ret;
199}
200
201static int versal_pm_clock_get_name(u32 clock_id, char *name)
202{
203 struct versal_pm_query_data qdata = {0};
204 u32 ret_payload[PAYLOAD_ARG_CNT];
205 int ret;
206
207 qdata.qid = PM_QID_CLOCK_GET_NAME;
208 qdata.arg1 = clock_id;
209
210 ret = versal_pm_query(qdata, ret_payload);
211 if (ret)
212 return ret;
213 memcpy(name, ret_payload, CLK_GET_NAME_RESP_LEN);
214
215 return 0;
216}
217
218static int versal_pm_clock_get_topology(u32 clock_id, u32 index, u32 *topology)
219{
220 struct versal_pm_query_data qdata = {0};
221 u32 ret_payload[PAYLOAD_ARG_CNT];
222 int ret;
223
224 qdata.qid = PM_QID_CLOCK_GET_TOPOLOGY;
225 qdata.arg1 = clock_id;
226 qdata.arg2 = index;
227
228 ret = versal_pm_query(qdata, ret_payload);
229 memcpy(topology, &ret_payload[1], CLK_GET_TOPOLOGY_RESP_WORDS * 4);
230
231 return ret;
232}
233
234static int versal_pm_clock_get_parents(u32 clock_id, u32 index, u32 *parents)
235{
236 struct versal_pm_query_data qdata = {0};
237 u32 ret_payload[PAYLOAD_ARG_CNT];
238 int ret;
239
240 qdata.qid = PM_QID_CLOCK_GET_PARENTS;
241 qdata.arg1 = clock_id;
242 qdata.arg2 = index;
243
244 ret = versal_pm_query(qdata, ret_payload);
245 memcpy(parents, &ret_payload[1], CLK_GET_PARENTS_RESP_WORDS * 4);
246
247 return ret;
248}
249
250static int versal_pm_clock_get_attributes(u32 clock_id, u32 *attr)
251{
252 struct versal_pm_query_data qdata = {0};
253 u32 ret_payload[PAYLOAD_ARG_CNT];
254 int ret;
255
256 qdata.qid = PM_QID_CLOCK_GET_ATTRIBUTES;
257 qdata.arg1 = clock_id;
258
259 ret = versal_pm_query(qdata, ret_payload);
260 memcpy(attr, &ret_payload[1], CLK_GET_ATTR_RESP_WORDS * 4);
261
262 return ret;
263}
264
265static int __versal_clock_get_topology(struct clock_topology *topology,
266 u32 *data, u32 *nnodes)
267{
268 int i;
269
270 for (i = 0; i < PM_API_PAYLOAD_LEN; i++) {
271 if (!(data[i] & CLK_TYPE_FIELD_MASK))
272 return END_OF_TOPOLOGY_NODE;
273 topology[*nnodes].type = data[i] & CLK_TYPE_FIELD_MASK;
274 topology[*nnodes].flag = FIELD_GET(CLK_FLAG_FIELD_MASK,
275 data[i]);
276 topology[*nnodes].type_flag =
277 FIELD_GET(CLK_TYPE_FLAG_FIELD_MASK, data[i]);
278 topology[*nnodes].type_flag |=
279 FIELD_GET(CLK_TYPE_FLAG2_FIELD_MASK, data[i]) <<
280 CLK_TYPE_FLAG_BITS;
281 debug("topology type:0x%x, flag:0x%x, type_flag:0x%x\n",
282 topology[*nnodes].type, topology[*nnodes].flag,
283 topology[*nnodes].type_flag);
284 (*nnodes)++;
285 }
286
287 return 0;
288}
289
290static int versal_clock_get_topology(u32 clk_id,
291 struct clock_topology *topology,
292 u32 *num_nodes)
293{
294 int j, ret;
295 u32 pm_resp[PM_API_PAYLOAD_LEN] = {0};
296
297 *num_nodes = 0;
298 for (j = 0; j <= MAX_NODES; j += 3) {
299 ret = versal_pm_clock_get_topology(clock[clk_id].clk_id, j,
300 pm_resp);
301 if (ret)
302 return ret;
303 ret = __versal_clock_get_topology(topology, pm_resp, num_nodes);
304 if (ret == END_OF_TOPOLOGY_NODE)
305 return 0;
306 }
307
308 return 0;
309}
310
311static int __versal_clock_get_parents(struct clock_parent *parents, u32 *data,
312 u32 *nparent)
313{
314 int i;
315 struct clock_parent *parent;
316
317 for (i = 0; i < PM_API_PAYLOAD_LEN; i++) {
318 if (data[i] == NA_PARENT)
319 return END_OF_PARENTS;
320
321 parent = &parents[i];
322 parent->id = data[i] & CLK_PARENTS_ID_MASK;
323 if (data[i] == DUMMY_PARENT) {
324 strcpy(parent->name, "dummy_name");
325 parent->flag = 0;
326 } else {
327 parent->flag = data[i] >> CLK_PARENTS_ID_LEN;
328 if (versal_get_clock_name(parent->id, parent->name))
329 continue;
330 }
331 debug("parent name:%s\n", parent->name);
332 *nparent += 1;
333 }
334
335 return 0;
336}
337
338static int versal_clock_get_parents(u32 clk_id, struct clock_parent *parents,
339 u32 *num_parents)
340{
341 int j = 0, ret;
342 u32 pm_resp[PM_API_PAYLOAD_LEN] = {0};
343
344 *num_parents = 0;
345 do {
346 /* Get parents from firmware */
347 ret = versal_pm_clock_get_parents(clock[clk_id].clk_id, j,
348 pm_resp);
349 if (ret)
350 return ret;
351
352 ret = __versal_clock_get_parents(&parents[j], pm_resp,
353 num_parents);
354 if (ret == END_OF_PARENTS)
355 return 0;
356 j += PM_API_PAYLOAD_LEN;
357 } while (*num_parents <= MAX_PARENT);
358
359 return 0;
360}
361
362static u32 versal_clock_get_div(u32 clk_id)
363{
364 u32 ret_payload[PAYLOAD_ARG_CNT];
365 u32 div;
366
Michal Simek65962702019-10-04 15:52:43 +0200367 xilinx_pm_request(PM_CLOCK_GETDIVIDER, clk_id, 0, 0, 0, ret_payload);
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +0530368 div = ret_payload[1];
369
370 return div;
371}
372
373static u32 versal_clock_set_div(u32 clk_id, u32 div)
374{
375 u32 ret_payload[PAYLOAD_ARG_CNT];
376
Michal Simek65962702019-10-04 15:52:43 +0200377 xilinx_pm_request(PM_CLOCK_SETDIVIDER, clk_id, div, 0, 0, ret_payload);
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +0530378
379 return div;
380}
381
382static u64 versal_clock_ref(u32 clk_id)
383{
384 u32 ret_payload[PAYLOAD_ARG_CNT];
385 int ref;
386
Michal Simek65962702019-10-04 15:52:43 +0200387 xilinx_pm_request(PM_CLOCK_GETPARENT, clk_id, 0, 0, 0, ret_payload);
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +0530388 ref = ret_payload[0];
389 if (!(ref & 1))
390 return ref_clk;
391 if (ref & 2)
392 return pl_alt_ref_clk;
393 return 0;
394}
395
396static u64 versal_clock_get_pll_rate(u32 clk_id)
397{
398 u32 ret_payload[PAYLOAD_ARG_CNT];
399 u32 fbdiv;
400 u32 res;
401 u32 frac;
402 u64 freq;
403 u32 parent_rate, parent_id;
404 u32 id = clk_id & 0xFFF;
405
Michal Simek65962702019-10-04 15:52:43 +0200406 xilinx_pm_request(PM_CLOCK_GETSTATE, clk_id, 0, 0, 0, ret_payload);
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +0530407 res = ret_payload[1];
408 if (!res) {
409 printf("0%x PLL not enabled\n", clk_id);
410 return 0;
411 }
412
413 parent_id = clock[clock[id].parent[0].id].clk_id;
414 parent_rate = versal_clock_ref(parent_id);
415
Michal Simek65962702019-10-04 15:52:43 +0200416 xilinx_pm_request(PM_CLOCK_GETDIVIDER, clk_id, 0, 0, 0, ret_payload);
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +0530417 fbdiv = ret_payload[1];
Michal Simek65962702019-10-04 15:52:43 +0200418 xilinx_pm_request(PM_CLOCK_PLL_GETPARAM, clk_id, 2, 0, 0, ret_payload);
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +0530419 frac = ret_payload[1];
420
421 freq = (fbdiv * parent_rate) >> (1 << frac);
422
423 return freq;
424}
425
426static u32 versal_clock_mux(u32 clk_id)
427{
428 int i;
429 u32 id = clk_id & 0xFFF;
430
431 for (i = 0; i < clock[id].num_nodes; i++)
432 if (clock[id].node[i].type == CLOCK_NODE_TYPE_MUX)
433 return 1;
434
435 return 0;
436}
437
438static u32 versal_clock_get_parentid(u32 clk_id)
439{
440 u32 parent_id = 0;
441 u32 ret_payload[PAYLOAD_ARG_CNT];
442 u32 id = clk_id & 0xFFF;
443
444 if (versal_clock_mux(clk_id)) {
Michal Simek65962702019-10-04 15:52:43 +0200445 xilinx_pm_request(PM_CLOCK_GETPARENT, clk_id, 0, 0, 0,
Siva Durga Prasad Paladugu95105082019-06-23 12:24:57 +0530446 ret_payload);
447 parent_id = ret_payload[1];
448 }
449
450 debug("parent_id:0x%x\n", clock[clock[id].parent[parent_id].id].clk_id);
451 return clock[clock[id].parent[parent_id].id].clk_id;
452}
453
454static u32 versal_clock_gate(u32 clk_id)
455{
456 u32 id = clk_id & 0xFFF;
457 int i;
458
459 for (i = 0; i < clock[id].num_nodes; i++)
460 if (clock[id].node[i].type == CLOCK_NODE_TYPE_GATE)
461 return 1;
462
463 return 0;
464}
465
466static u32 versal_clock_div(u32 clk_id)
467{
468 int i;
469 u32 id = clk_id & 0xFFF;
470
471 for (i = 0; i < clock[id].num_nodes; i++)
472 if (clock[id].node[i].type == CLOCK_NODE_TYPE_DIV)
473 return 1;
474
475 return 0;
476}
477
478static u32 versal_clock_pll(u32 clk_id, u64 *clk_rate)
479{
480 if (((clk_id >> NODE_SUBCLASS_SHIFT) & NODE_CLASS_MASK) ==
481 NODE_SUBCLASS_CLOCK_PLL &&
482 ((clk_id >> NODE_CLASS_SHIFT) & NODE_CLASS_MASK) ==
483 NODE_CLASS_CLOCK) {
484 *clk_rate = versal_clock_get_pll_rate(clk_id);
485 return 1;
486 }
487
488 return 0;
489}
490
491static u64 versal_clock_calc(u32 clk_id)
492{
493 u32 parent_id;
494 u64 clk_rate;
495 u32 div;
496
497 if (versal_clock_pll(clk_id, &clk_rate))
498 return clk_rate;
499
500 parent_id = versal_clock_get_parentid(clk_id);
501 if (((parent_id >> NODE_SUBCLASS_SHIFT) &
502 NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_REF)
503 return versal_clock_ref(clk_id);
504
505 clk_rate = versal_clock_calc(parent_id);
506
507 if (versal_clock_div(clk_id)) {
508 div = versal_clock_get_div(clk_id);
509 clk_rate = DIV_ROUND_CLOSEST(clk_rate, div);
510 }
511
512 return clk_rate;
513}
514
515static int versal_clock_get_rate(u32 clk_id, u64 *clk_rate)
516{
517 if (((clk_id >> NODE_SUBCLASS_SHIFT) &
518 NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_REF)
519 *clk_rate = versal_clock_ref(clk_id);
520
521 if (versal_clock_pll(clk_id, clk_rate))
522 return 0;
523
524 if (((clk_id >> NODE_SUBCLASS_SHIFT) &
525 NODE_CLASS_MASK) == NODE_SUBCLASS_CLOCK_OUT &&
526 ((clk_id >> NODE_CLASS_SHIFT) &
527 NODE_CLASS_MASK) == NODE_CLASS_CLOCK) {
528 if (!versal_clock_gate(clk_id))
529 return -EINVAL;
530 *clk_rate = versal_clock_calc(clk_id);
531 return 0;
532 }
533
534 return 0;
535}
536
537int soc_clk_dump(void)
538{
539 u64 clk_rate = 0;
540 u32 type, ret, i = 0;
541
542 printf("\n ****** VERSAL CLOCKS *****\n");
543
544 printf("alt_ref_clk:%ld pl_alt_ref_clk:%ld ref_clk:%ld\n",
545 alt_ref_clk, pl_alt_ref_clk, ref_clk);
546 for (i = 0; i < clock_max_idx; i++) {
547 debug("%s\n", clock[i].clk_name);
548 ret = versal_get_clock_type(i, &type);
549 if (ret || type != CLK_TYPE_OUTPUT)
550 continue;
551
552 ret = versal_clock_get_rate(clock[i].clk_id, &clk_rate);
553
554 if (ret != -EINVAL)
555 printf("clk: %s freq:%lld\n",
556 clock[i].clk_name, clk_rate);
557 }
558
559 return 0;
560}
561
562static void versal_get_clock_info(void)
563{
564 int i, ret;
565 u32 attr, type = 0, nodetype, subclass, class;
566
567 for (i = 0; i < clock_max_idx; i++) {
568 ret = versal_pm_clock_get_attributes(i, &attr);
569 if (ret)
570 continue;
571
572 clock[i].valid = attr & CLK_VALID_MASK;
573 clock[i].type = ((attr >> CLK_TYPE_SHIFT) & 0x1) ?
574 CLK_TYPE_EXTERNAL : CLK_TYPE_OUTPUT;
575 nodetype = (attr >> NODE_TYPE_SHIFT) & NODE_CLASS_MASK;
576 subclass = (attr >> NODE_SUBCLASS_SHIFT) & NODE_CLASS_MASK;
577 class = (attr >> NODE_CLASS_SHIFT) & NODE_CLASS_MASK;
578
579 clock[i].clk_id = (class << NODE_CLASS_SHIFT) |
580 (subclass << NODE_SUBCLASS_SHIFT) |
581 (nodetype << NODE_TYPE_SHIFT) |
582 (i << NODE_INDEX_SHIFT);
583
584 ret = versal_pm_clock_get_name(clock[i].clk_id,
585 clock[i].clk_name);
586 if (ret)
587 continue;
588 debug("clk name:%s, Valid:%d, type:%d, clk_id:0x%x\n",
589 clock[i].clk_name, clock[i].valid,
590 clock[i].type, clock[i].clk_id);
591 }
592
593 /* Get topology of all clock */
594 for (i = 0; i < clock_max_idx; i++) {
595 ret = versal_get_clock_type(i, &type);
596 if (ret || type != CLK_TYPE_OUTPUT)
597 continue;
598 debug("clk name:%s\n", clock[i].clk_name);
599 ret = versal_clock_get_topology(i, clock[i].node,
600 &clock[i].num_nodes);
601 if (ret)
602 continue;
603
604 ret = versal_clock_get_parents(i, clock[i].parent,
605 &clock[i].num_parents);
606 if (ret)
607 continue;
608 }
609}
610
611int versal_clock_setup(void)
612{
613 int ret;
614
615 ret = versal_pm_clock_get_num_clocks(&clock_max_idx);
616 if (ret)
617 return ret;
618
619 debug("%s, clock_max_idx:0x%x\n", __func__, clock_max_idx);
620 clock = calloc(clock_max_idx, sizeof(*clock));
621 if (!clock)
622 return -ENOMEM;
623
624 versal_get_clock_info();
625
626 return 0;
627}
628
629static int versal_clock_get_freq_by_name(char *name, struct udevice *dev,
630 ulong *freq)
631{
632 struct clk clk;
633 int ret;
634
635 ret = clk_get_by_name(dev, name, &clk);
636 if (ret < 0) {
637 dev_err(dev, "failed to get %s\n", name);
638 return ret;
639 }
640
641 *freq = clk_get_rate(&clk);
642 if (IS_ERR_VALUE(*freq)) {
643 dev_err(dev, "failed to get rate %s\n", name);
644 return -EINVAL;
645 }
646
647 return 0;
648}
649
650static int versal_clk_probe(struct udevice *dev)
651{
652 int ret;
653 struct versal_clk_priv *priv = dev_get_priv(dev);
654
655 debug("%s\n", __func__);
656
657 ret = versal_clock_get_freq_by_name("alt_ref_clk", dev, &alt_ref_clk);
658 if (ret < 0)
659 return -EINVAL;
660
661 ret = versal_clock_get_freq_by_name("pl_alt_ref_clk",
662 dev, &pl_alt_ref_clk);
663 if (ret < 0)
664 return -EINVAL;
665
666 ret = versal_clock_get_freq_by_name("ref_clk", dev, &ref_clk);
667 if (ret < 0)
668 return -EINVAL;
669
670 versal_clock_setup();
671
672 priv->clk = clock;
673
674 return ret;
675}
676
677static ulong versal_clk_get_rate(struct clk *clk)
678{
679 struct versal_clk_priv *priv = dev_get_priv(clk->dev);
680 u32 id = clk->id;
681 u32 clk_id;
682 u64 clk_rate = 0;
683
684 debug("%s\n", __func__);
685
686 clk_id = priv->clk[id].clk_id;
687
688 versal_clock_get_rate(clk_id, &clk_rate);
689
690 return clk_rate;
691}
692
693static ulong versal_clk_set_rate(struct clk *clk, ulong rate)
694{
695 struct versal_clk_priv *priv = dev_get_priv(clk->dev);
696 u32 id = clk->id;
697 u32 clk_id;
698 u64 clk_rate = 0;
699 u32 div;
700 int ret;
701
702 debug("%s\n", __func__);
703
704 clk_id = priv->clk[id].clk_id;
705
706 ret = versal_clock_get_rate(clk_id, &clk_rate);
707 if (ret) {
708 printf("Clock is not a Gate:0x%x\n", clk_id);
709 return 0;
710 }
711
712 do {
713 if (versal_clock_div(clk_id)) {
714 div = versal_clock_get_div(clk_id);
715 clk_rate *= div;
716 div = DIV_ROUND_CLOSEST(clk_rate, rate);
717 versal_clock_set_div(clk_id, div);
718 debug("%s, div:%d, newrate:%lld\n", __func__,
719 div, DIV_ROUND_CLOSEST(clk_rate, div));
720 return DIV_ROUND_CLOSEST(clk_rate, div);
721 }
722 clk_id = versal_clock_get_parentid(clk_id);
723 } while (((clk_id >> NODE_SUBCLASS_SHIFT) &
724 NODE_CLASS_MASK) != NODE_SUBCLASS_CLOCK_REF);
725
726 printf("Clock didn't has Divisors:0x%x\n", priv->clk[id].clk_id);
727
728 return clk_rate;
729}
730
731static struct clk_ops versal_clk_ops = {
732 .set_rate = versal_clk_set_rate,
733 .get_rate = versal_clk_get_rate,
734};
735
736static const struct udevice_id versal_clk_ids[] = {
737 { .compatible = "xlnx,versal-clk" },
738 { }
739};
740
741U_BOOT_DRIVER(versal_clk) = {
742 .name = "versal-clk",
743 .id = UCLASS_CLK,
744 .of_match = versal_clk_ids,
745 .probe = versal_clk_probe,
746 .ops = &versal_clk_ops,
747 .priv_auto_alloc_size = sizeof(struct versal_clk_priv),
748};