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