drivers/clk/tegra/clk-bpmp.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2016-2022 NVIDIA Corporation
   4  */
   5
   6 #include <linux/clk-provider.h>
   7 #include <linux/device.h>
   8 #include <linux/seq_buf.h>
   9 #include <linux/slab.h>
  10
  11 #include <soc/tegra/bpmp.h>
  12 #include <soc/tegra/bpmp-abi.h>
  13
  14 #define TEGRA_BPMP_DUMP_CLOCK_INFO      0
  15
  16 #define TEGRA_BPMP_CLK_HAS_MUX          BIT(0)
  17 #define TEGRA_BPMP_CLK_HAS_SET_RATE     BIT(1)
  18 #define TEGRA_BPMP_CLK_IS_ROOT          BIT(2)
  19
  20 struct tegra_bpmp_clk_info {
  21         unsigned int id;
  22         char name[MRQ_CLK_NAME_MAXLEN];
  23         unsigned int parents[MRQ_CLK_MAX_PARENTS];
  24         unsigned int num_parents;
  25         unsigned long flags;
  26 };
  27
  28 struct tegra_bpmp_clk {
  29         struct clk_hw hw;
  30
  31         struct tegra_bpmp *bpmp;
  32         unsigned int id;
  33
  34         unsigned int num_parents;
  35         unsigned int *parents;
  36 };
  37
  38 static inline struct tegra_bpmp_clk *to_tegra_bpmp_clk(struct clk_hw *hw)
  39 {
  40         return container_of(hw, struct tegra_bpmp_clk, hw);
  41 }
  42
  43 struct tegra_bpmp_clk_message {
  44         unsigned int cmd;
  45         unsigned int id;
  46
  47         struct {
  48                 const void *data;
  49                 size_t size;
  50         } tx;
  51
  52         struct {
  53                 void *data;
  54                 size_t size;
  55                 int ret;
  56         } rx;
  57 };
  58
  59 static int tegra_bpmp_clk_transfer(struct tegra_bpmp *bpmp,
  60                                    const struct tegra_bpmp_clk_message *clk)
  61 {
  62         struct mrq_clk_request request;
  63         struct tegra_bpmp_message msg;
  64         void *req = &request;
  65         int err;
  66
  67         memset(&request, 0, sizeof(request));
  68         request.cmd_and_id = (clk->cmd << 24) | clk->id;
  69
  70         /*
  71          * The mrq_clk_request structure has an anonymous union at offset 4
  72          * that contains all possible sub-command structures. Copy the data
  73          * to that union. Ideally we'd be able to refer to it by name, but
  74          * doing so would require changing the ABI header and increase the
  75          * maintenance burden.
  76          */
  77         memcpy(req + 4, clk->tx.data, clk->tx.size);
  78
  79         memset(&msg, 0, sizeof(msg));
  80         msg.mrq = MRQ_CLK;
  81         msg.tx.data = &request;
  82         msg.tx.size = sizeof(request);
  83         msg.rx.data = clk->rx.data;
  84         msg.rx.size = clk->rx.size;
  85
  86         err = tegra_bpmp_transfer(bpmp, &msg);
  87         if (err < 0)
  88                 return err;
  89         else if (msg.rx.ret < 0)
  90                 return -EINVAL;
  91
  92         return 0;
  93 }
  94
  95 static int tegra_bpmp_clk_prepare(struct clk_hw *hw)
  96 {
  97         struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
  98         struct tegra_bpmp_clk_message msg;
  99
 100         memset(&msg, 0, sizeof(msg));
 101         msg.cmd = CMD_CLK_ENABLE;
 102         msg.id = clk->id;
 103
 104         return tegra_bpmp_clk_transfer(clk->bpmp, &msg);
 105 }
 106
 107 static void tegra_bpmp_clk_unprepare(struct clk_hw *hw)
 108 {
 109         struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
 110         struct tegra_bpmp_clk_message msg;
 111         int err;
 112
 113         memset(&msg, 0, sizeof(msg));
 114         msg.cmd = CMD_CLK_DISABLE;
 115         msg.id = clk->id;
 116
 117         err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
 118         if (err < 0)
 119                 dev_err(clk->bpmp->dev, "failed to disable clock %s: %d\n",
 120                         clk_hw_get_name(hw), err);
 121 }
 122
 123 static int tegra_bpmp_clk_is_prepared(struct clk_hw *hw)
 124 {
 125         struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
 126         struct cmd_clk_is_enabled_response response;
 127         struct tegra_bpmp_clk_message msg;
 128         int err;
 129
 130         memset(&msg, 0, sizeof(msg));
 131         msg.cmd = CMD_CLK_IS_ENABLED;
 132         msg.id = clk->id;
 133         msg.rx.data = &response;
 134         msg.rx.size = sizeof(response);
 135
 136         err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
 137         if (err < 0)
 138                 return err;
 139
 140         return response.state;
 141 }
 142
 143 static unsigned long tegra_bpmp_clk_recalc_rate(struct clk_hw *hw,
 144                                                 unsigned long parent_rate)
 145 {
 146         struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
 147         struct cmd_clk_get_rate_response response;
 148         struct cmd_clk_get_rate_request request;
 149         struct tegra_bpmp_clk_message msg;
 150         int err;
 151
 152         memset(&msg, 0, sizeof(msg));
 153         msg.cmd = CMD_CLK_GET_RATE;
 154         msg.id = clk->id;
 155         msg.tx.data = &request;
 156         msg.tx.size = sizeof(request);
 157         msg.rx.data = &response;
 158         msg.rx.size = sizeof(response);
 159
 160         err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
 161         if (err < 0)
 162                 return err;
 163
 164         return response.rate;
 165 }
 166
 167 static int tegra_bpmp_clk_determine_rate(struct clk_hw *hw,
 168                                          struct clk_rate_request *rate_req)
 169 {
 170         struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
 171         struct cmd_clk_round_rate_response response;
 172         struct cmd_clk_round_rate_request request;
 173         struct tegra_bpmp_clk_message msg;
 174         unsigned long rate;
 175         int err;
 176
 177         rate = min(max(rate_req->rate, rate_req->min_rate), rate_req->max_rate);
 178
 179         memset(&request, 0, sizeof(request));
 180         request.rate = min_t(u64, rate, S64_MAX);
 181
 182         memset(&msg, 0, sizeof(msg));
 183         msg.cmd = CMD_CLK_ROUND_RATE;
 184         msg.id = clk->id;
 185         msg.tx.data = &request;
 186         msg.tx.size = sizeof(request);
 187         msg.rx.data = &response;
 188         msg.rx.size = sizeof(response);
 189
 190         err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
 191         if (err < 0)
 192                 return err;
 193
 194         rate_req->rate = (unsigned long)response.rate;
 195
 196         return 0;
 197 }
 198
 199 static int tegra_bpmp_clk_set_parent(struct clk_hw *hw, u8 index)
 200 {
 201         struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
 202         struct cmd_clk_set_parent_response response;
 203         struct cmd_clk_set_parent_request request;
 204         struct tegra_bpmp_clk_message msg;
 205         int err;
 206
 207         memset(&request, 0, sizeof(request));
 208         request.parent_id = clk->parents[index];
 209
 210         memset(&msg, 0, sizeof(msg));
 211         msg.cmd = CMD_CLK_SET_PARENT;
 212         msg.id = clk->id;
 213         msg.tx.data = &request;
 214         msg.tx.size = sizeof(request);
 215         msg.rx.data = &response;
 216         msg.rx.size = sizeof(response);
 217
 218         err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
 219         if (err < 0)
 220                 return err;
 221
 222         /* XXX check parent ID in response */
 223
 224         return 0;
 225 }
 226
 227 static u8 tegra_bpmp_clk_get_parent(struct clk_hw *hw)
 228 {
 229         struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
 230         struct cmd_clk_get_parent_response response;
 231         struct tegra_bpmp_clk_message msg;
 232         unsigned int i;
 233         int err;
 234
 235         memset(&msg, 0, sizeof(msg));
 236         msg.cmd = CMD_CLK_GET_PARENT;
 237         msg.id = clk->id;
 238         msg.rx.data = &response;
 239         msg.rx.size = sizeof(response);
 240
 241         err = tegra_bpmp_clk_transfer(clk->bpmp, &msg);
 242         if (err < 0) {
 243                 dev_err(clk->bpmp->dev, "failed to get parent for %s: %d\n",
 244                         clk_hw_get_name(hw), err);
 245                 return U8_MAX;
 246         }
 247
 248         for (i = 0; i < clk->num_parents; i++)
 249                 if (clk->parents[i] == response.parent_id)
 250                         return i;
 251
 252         return U8_MAX;
 253 }
 254
 255 static int tegra_bpmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 256                                    unsigned long parent_rate)
 257 {
 258         struct tegra_bpmp_clk *clk = to_tegra_bpmp_clk(hw);
 259         struct cmd_clk_set_rate_response response;
 260         struct cmd_clk_set_rate_request request;
 261         struct tegra_bpmp_clk_message msg;
 262
 263         memset(&request, 0, sizeof(request));
 264         request.rate = min_t(u64, rate, S64_MAX);
 265
 266         memset(&msg, 0, sizeof(msg));
 267         msg.cmd = CMD_CLK_SET_RATE;
 268         msg.id = clk->id;
 269         msg.tx.data = &request;
 270         msg.tx.size = sizeof(request);
 271         msg.rx.data = &response;
 272         msg.rx.size = sizeof(response);
 273
 274         return tegra_bpmp_clk_transfer(clk->bpmp, &msg);
 275 }
 276
 277 static const struct clk_ops tegra_bpmp_clk_gate_ops = {
 278         .prepare = tegra_bpmp_clk_prepare,
 279         .unprepare = tegra_bpmp_clk_unprepare,
 280         .is_prepared = tegra_bpmp_clk_is_prepared,
 281         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 282 };
 283
 284 static const struct clk_ops tegra_bpmp_clk_mux_ops = {
 285         .prepare = tegra_bpmp_clk_prepare,
 286         .unprepare = tegra_bpmp_clk_unprepare,
 287         .is_prepared = tegra_bpmp_clk_is_prepared,
 288         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 289         .set_parent = tegra_bpmp_clk_set_parent,
 290         .get_parent = tegra_bpmp_clk_get_parent,
 291 };
 292
 293 static const struct clk_ops tegra_bpmp_clk_rate_ops = {
 294         .prepare = tegra_bpmp_clk_prepare,
 295         .unprepare = tegra_bpmp_clk_unprepare,
 296         .is_prepared = tegra_bpmp_clk_is_prepared,
 297         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 298         .determine_rate = tegra_bpmp_clk_determine_rate,
 299         .set_rate = tegra_bpmp_clk_set_rate,
 300 };
 301
 302 static const struct clk_ops tegra_bpmp_clk_mux_rate_ops = {
 303         .prepare = tegra_bpmp_clk_prepare,
 304         .unprepare = tegra_bpmp_clk_unprepare,
 305         .is_prepared = tegra_bpmp_clk_is_prepared,
 306         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 307         .determine_rate = tegra_bpmp_clk_determine_rate,
 308         .set_parent = tegra_bpmp_clk_set_parent,
 309         .get_parent = tegra_bpmp_clk_get_parent,
 310         .set_rate = tegra_bpmp_clk_set_rate,
 311 };
 312
 313 static const struct clk_ops tegra_bpmp_clk_mux_read_only_ops = {
 314         .get_parent = tegra_bpmp_clk_get_parent,
 315         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 316 };
 317
 318 static const struct clk_ops tegra_bpmp_clk_read_only_ops = {
 319         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 320 };
 321
 322 static const struct clk_ops tegra_bpmp_clk_gate_mux_read_only_ops = {
 323         .prepare = tegra_bpmp_clk_prepare,
 324         .unprepare = tegra_bpmp_clk_unprepare,
 325         .is_prepared = tegra_bpmp_clk_is_prepared,
 326         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 327         .get_parent = tegra_bpmp_clk_get_parent,
 328 };
 329
 330 static int tegra_bpmp_clk_get_max_id(struct tegra_bpmp *bpmp)
 331 {
 332         struct cmd_clk_get_max_clk_id_response response;
 333         struct tegra_bpmp_clk_message msg;
 334         int err;
 335
 336         memset(&msg, 0, sizeof(msg));
 337         msg.cmd = CMD_CLK_GET_MAX_CLK_ID;
 338         msg.rx.data = &response;
 339         msg.rx.size = sizeof(response);
 340
 341         err = tegra_bpmp_clk_transfer(bpmp, &msg);
 342         if (err < 0)
 343                 return err;
 344
 345         if (response.max_id > INT_MAX)
 346                 return -E2BIG;
 347
 348         return response.max_id;
 349 }
 350
 351 static int tegra_bpmp_clk_get_info(struct tegra_bpmp *bpmp, unsigned int id,
 352                                    struct tegra_bpmp_clk_info *info)
 353 {
 354         struct cmd_clk_get_all_info_response response;
 355         struct tegra_bpmp_clk_message msg;
 356         unsigned int i;
 357         int err;
 358
 359         memset(&msg, 0, sizeof(msg));
 360         msg.cmd = CMD_CLK_GET_ALL_INFO;
 361         msg.id = id;
 362         msg.rx.data = &response;
 363         msg.rx.size = sizeof(response);
 364
 365         err = tegra_bpmp_clk_transfer(bpmp, &msg);
 366         if (err < 0)
 367                 return err;
 368
 369         strscpy(info->name, response.name, MRQ_CLK_NAME_MAXLEN);
 370         info->num_parents = response.num_parents;
 371
 372         for (i = 0; i < info->num_parents; i++)
 373                 info->parents[i] = response.parents[i];
 374
 375         info->flags = response.flags;
 376
 377         return 0;
 378 }
 379
 380 static void tegra_bpmp_clk_info_dump(struct tegra_bpmp *bpmp,
 381                                      const char *level,
 382                                      const struct tegra_bpmp_clk_info *info)
 383 {
 384         const char *prefix = "";
 385         struct seq_buf buf;
 386         unsigned int i;
 387         char flags[64];
 388
 389         seq_buf_init(&buf, flags, sizeof(flags));
 390
 391         if (info->flags)
 392                 seq_buf_printf(&buf, "(");
 393
 394         if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
 395                 seq_buf_printf(&buf, "%smux", prefix);
 396                 prefix = ", ";
 397         }
 398
 399         if ((info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) == 0) {
 400                 seq_buf_printf(&buf, "%sfixed", prefix);
 401                 prefix = ", ";
 402         }
 403
 404         if (info->flags & TEGRA_BPMP_CLK_IS_ROOT) {
 405                 seq_buf_printf(&buf, "%sroot", prefix);
 406                 prefix = ", ";
 407         }
 408
 409         if (info->flags)
 410                 seq_buf_printf(&buf, ")");
 411
 412         dev_printk(level, bpmp->dev, "%03u: %s\n", info->id, info->name);
 413         dev_printk(level, bpmp->dev, "  flags: %lx %s\n", info->flags, flags);
 414         dev_printk(level, bpmp->dev, "  parents: %u\n", info->num_parents);
 415
 416         for (i = 0; i < info->num_parents; i++)
 417                 dev_printk(level, bpmp->dev, "    %03u\n", info->parents[i]);
 418 }
 419
 420 static int tegra_bpmp_probe_clocks(struct tegra_bpmp *bpmp,
 421                                    struct tegra_bpmp_clk_info **clocksp)
 422 {
 423         struct tegra_bpmp_clk_info *clocks;
 424         unsigned int max_id, id, count = 0;
 425         unsigned int holes = 0;
 426         int err;
 427
 428         err = tegra_bpmp_clk_get_max_id(bpmp);
 429         if (err < 0)
 430                 return err;
 431
 432         max_id = err;
 433
 434         dev_dbg(bpmp->dev, "maximum clock ID: %u\n", max_id);
 435
 436         clocks = kcalloc(max_id + 1, sizeof(*clocks), GFP_KERNEL);
 437         if (!clocks)
 438                 return -ENOMEM;
 439
 440         for (id = 0; id <= max_id; id++) {
 441                 struct tegra_bpmp_clk_info *info = &clocks[count];
 442
 443                 err = tegra_bpmp_clk_get_info(bpmp, id, info);
 444                 if (err < 0)
 445                         continue;
 446
 447                 if (info->num_parents >= U8_MAX) {
 448                         dev_err(bpmp->dev,
 449                                 "clock %u has too many parents (%u, max: %u)\n",
 450                                 id, info->num_parents, U8_MAX);
 451                         continue;
 452                 }
 453
 454                 /* clock not exposed by BPMP */
 455                 if (info->name[0] == '\0') {
 456                         holes++;
 457                         continue;
 458                 }
 459
 460                 info->id = id;
 461                 count++;
 462
 463                 if (TEGRA_BPMP_DUMP_CLOCK_INFO)
 464                         tegra_bpmp_clk_info_dump(bpmp, KERN_DEBUG, info);
 465         }
 466
 467         dev_dbg(bpmp->dev, "holes: %u\n", holes);
 468         *clocksp = clocks;
 469
 470         return count;
 471 }
 472
 473 static unsigned int
 474 tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info *clocks,
 475                            unsigned int num_clocks, unsigned int id)
 476 {
 477         unsigned int i;
 478
 479         for (i = 0; i < num_clocks; i++)
 480                 if (clocks[i].id == id)
 481                         return i;
 482
 483         return UINT_MAX;
 484 }
 485
 486 static const struct tegra_bpmp_clk_info *
 487 tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info *clocks,
 488                     unsigned int num_clocks, unsigned int id)
 489 {
 490         unsigned int i;
 491
 492         i = tegra_bpmp_clk_id_to_index(clocks, num_clocks, id);
 493
 494         if (i < num_clocks)
 495                 return &clocks[i];
 496
 497         return NULL;
 498 }
 499
 500 static struct tegra_bpmp_clk *
 501 tegra_bpmp_clk_register(struct tegra_bpmp *bpmp,
 502                         const struct tegra_bpmp_clk_info *info,
 503                         const struct tegra_bpmp_clk_info *clocks,
 504                         unsigned int num_clocks)
 505 {
 506         struct tegra_bpmp_clk *clk;
 507         struct clk_init_data init;
 508         const char **parents;
 509         unsigned int i;
 510         int err;
 511
 512         clk = devm_kzalloc(bpmp->dev, sizeof(*clk), GFP_KERNEL);
 513         if (!clk)
 514                 return ERR_PTR(-ENOMEM);
 515
 516         clk->id = info->id;
 517         clk->bpmp = bpmp;
 518
 519         clk->parents = devm_kcalloc(bpmp->dev, info->num_parents,
 520                                     sizeof(*clk->parents), GFP_KERNEL);
 521         if (!clk->parents)
 522                 return ERR_PTR(-ENOMEM);
 523
 524         clk->num_parents = info->num_parents;
 525
 526         /* hardware clock initialization */
 527         memset(&init, 0, sizeof(init));
 528         init.name = info->name;
 529         clk->hw.init = &init;
 530         if (info->flags & BPMP_CLK_STATE_CHANGE_DENIED) {
 531                 if ((info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) == 0) {
 532                         dev_WARN(bpmp->dev,
 533                                 "Firmware bug! Inconsistent permission bits for clock %s. State and parent/rate changes disabled.",
 534                                  init.name);
 535                 }
 536                 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX)
 537                         init.ops = &tegra_bpmp_clk_mux_read_only_ops;
 538                 else
 539                         init.ops = &tegra_bpmp_clk_read_only_ops;
 540         } else if (info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) {
 541                 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX)
 542                         init.ops = &tegra_bpmp_clk_gate_mux_read_only_ops;
 543                 else
 544                         init.ops = &tegra_bpmp_clk_gate_ops;
 545         } else if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
 546                 if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
 547                         init.ops = &tegra_bpmp_clk_mux_rate_ops;
 548                 else
 549                         init.ops = &tegra_bpmp_clk_mux_ops;
 550         } else {
 551                 if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
 552                         init.ops = &tegra_bpmp_clk_rate_ops;
 553                 else
 554                         init.ops = &tegra_bpmp_clk_gate_ops;
 555         }
 556
 557         init.num_parents = info->num_parents;
 558
 559         parents = kcalloc(info->num_parents, sizeof(*parents), GFP_KERNEL);
 560         if (!parents)
 561                 return ERR_PTR(-ENOMEM);
 562
 563         for (i = 0; i < info->num_parents; i++) {
 564                 const struct tegra_bpmp_clk_info *parent;
 565
 566                 /* keep a private copy of the ID to parent index map */
 567                 clk->parents[i] = info->parents[i];
 568
 569                 parent = tegra_bpmp_clk_find(clocks, num_clocks,
 570                                              info->parents[i]);
 571                 if (!parent) {
 572                         dev_err(bpmp->dev, "no parent %u found for %u\n",
 573                                 info->parents[i], info->id);
 574                         continue;
 575                 }
 576
 577                 parents[i] = parent->name;
 578         }
 579
 580         init.parent_names = parents;
 581
 582         err = devm_clk_hw_register(bpmp->dev, &clk->hw);
 583
 584         kfree(parents);
 585
 586         if (err < 0)
 587                 return ERR_PTR(err);
 588
 589         return clk;
 590 }
 591
 592 static void tegra_bpmp_register_clocks_one(struct tegra_bpmp *bpmp,
 593                                            struct tegra_bpmp_clk_info *infos,
 594                                            unsigned int i,
 595                                            unsigned int count)
 596 {
 597         unsigned int j;
 598         struct tegra_bpmp_clk_info *info;
 599         struct tegra_bpmp_clk *clk;
 600
 601         if (bpmp->clocks[i]) {
 602                 /* already registered */
 603                 return;
 604         }
 605
 606         info = &infos[i];
 607         for (j = 0; j < info->num_parents; ++j) {
 608                 unsigned int p_id = info->parents[j];
 609                 unsigned int p_i = tegra_bpmp_clk_id_to_index(infos, count,
 610                                                               p_id);
 611                 if (p_i < count)
 612                         tegra_bpmp_register_clocks_one(bpmp, infos, p_i, count);
 613         }
 614
 615         clk = tegra_bpmp_clk_register(bpmp, info, infos, count);
 616         if (IS_ERR(clk)) {
 617                 dev_err(bpmp->dev,
 618                         "failed to register clock %u (%s): %ld\n",
 619                         info->id, info->name, PTR_ERR(clk));
 620                 /* intentionally store the error pointer to
 621                  * bpmp->clocks[i] to avoid re-attempting the
 622                  * registration later
 623                  */
 624         }
 625
 626         bpmp->clocks[i] = clk;
 627 }
 628
 629 static int tegra_bpmp_register_clocks(struct tegra_bpmp *bpmp,
 630                                       struct tegra_bpmp_clk_info *infos,
 631                                       unsigned int count)
 632 {
 633         unsigned int i;
 634
 635         bpmp->num_clocks = count;
 636
 637         bpmp->clocks = devm_kcalloc(bpmp->dev, count, sizeof(struct tegra_bpmp_clk), GFP_KERNEL);
 638         if (!bpmp->clocks)
 639                 return -ENOMEM;
 640
 641         for (i = 0; i < count; i++) {
 642                 tegra_bpmp_register_clocks_one(bpmp, infos, i, count);
 643         }
 644
 645         return 0;
 646 }
 647
 648 static void tegra_bpmp_unregister_clocks(struct tegra_bpmp *bpmp)
 649 {
 650         unsigned int i;
 651
 652         for (i = 0; i < bpmp->num_clocks; i++)
 653                 clk_hw_unregister(&bpmp->clocks[i]->hw);
 654 }
 655
 656 static struct clk_hw *tegra_bpmp_clk_of_xlate(struct of_phandle_args *clkspec,
 657                                               void *data)
 658 {
 659         unsigned int id = clkspec->args[0], i;
 660         struct tegra_bpmp *bpmp = data;
 661
 662         for (i = 0; i < bpmp->num_clocks; i++) {
 663                 struct tegra_bpmp_clk *clk = bpmp->clocks[i];
 664
 665                 if (!clk)
 666                         continue;
 667
 668                 if (clk->id == id)
 669                         return &clk->hw;
 670         }
 671
 672         return NULL;
 673 }
 674
 675 int tegra_bpmp_init_clocks(struct tegra_bpmp *bpmp)
 676 {
 677         struct tegra_bpmp_clk_info *clocks;
 678         unsigned int count;
 679         int err;
 680
 681         err = tegra_bpmp_probe_clocks(bpmp, &clocks);
 682         if (err < 0)
 683                 return err;
 684
 685         count = err;
 686
 687         dev_dbg(bpmp->dev, "%u clocks probed\n", count);
 688
 689         err = tegra_bpmp_register_clocks(bpmp, clocks, count);
 690         if (err < 0)
 691                 goto free;
 692
 693         err = of_clk_add_hw_provider(bpmp->dev->of_node,
 694                                      tegra_bpmp_clk_of_xlate,
 695                                      bpmp);
 696         if (err < 0) {
 697                 tegra_bpmp_unregister_clocks(bpmp);
 698                 goto free;
 699         }
 700
 701 free:
 702         kfree(clocks);
 703         return err;
 704 }