drivers/clk/clk_versal.c

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