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