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         .determine_rate = clk_hw_determine_rate_no_reparent,
 290         .set_parent = tegra_bpmp_clk_set_parent,
 291         .get_parent = tegra_bpmp_clk_get_parent,
 292 };
 293
 294 static const struct clk_ops tegra_bpmp_clk_rate_ops = {
 295         .prepare = tegra_bpmp_clk_prepare,
 296         .unprepare = tegra_bpmp_clk_unprepare,
 297         .is_prepared = tegra_bpmp_clk_is_prepared,
 298         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 299         .determine_rate = tegra_bpmp_clk_determine_rate,
 300         .set_rate = tegra_bpmp_clk_set_rate,
 301 };
 302
 303 static const struct clk_ops tegra_bpmp_clk_mux_rate_ops = {
 304         .prepare = tegra_bpmp_clk_prepare,
 305         .unprepare = tegra_bpmp_clk_unprepare,
 306         .is_prepared = tegra_bpmp_clk_is_prepared,
 307         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 308         .determine_rate = tegra_bpmp_clk_determine_rate,
 309         .set_parent = tegra_bpmp_clk_set_parent,
 310         .get_parent = tegra_bpmp_clk_get_parent,
 311         .set_rate = tegra_bpmp_clk_set_rate,
 312 };
 313
 314 static const struct clk_ops tegra_bpmp_clk_mux_read_only_ops = {
 315         .get_parent = tegra_bpmp_clk_get_parent,
 316         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 317 };
 318
 319 static const struct clk_ops tegra_bpmp_clk_read_only_ops = {
 320         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 321 };
 322
 323 static const struct clk_ops tegra_bpmp_clk_gate_mux_read_only_ops = {
 324         .prepare = tegra_bpmp_clk_prepare,
 325         .unprepare = tegra_bpmp_clk_unprepare,
 326         .is_prepared = tegra_bpmp_clk_is_prepared,
 327         .recalc_rate = tegra_bpmp_clk_recalc_rate,
 328         .get_parent = tegra_bpmp_clk_get_parent,
 329 };
 330
 331 static int tegra_bpmp_clk_get_max_id(struct tegra_bpmp *bpmp)
 332 {
 333         struct cmd_clk_get_max_clk_id_response response;
 334         struct tegra_bpmp_clk_message msg;
 335         int err;
 336
 337         memset(&msg, 0, sizeof(msg));
 338         msg.cmd = CMD_CLK_GET_MAX_CLK_ID;
 339         msg.rx.data = &response;
 340         msg.rx.size = sizeof(response);
 341
 342         err = tegra_bpmp_clk_transfer(bpmp, &msg);
 343         if (err < 0)
 344                 return err;
 345
 346         if (response.max_id > INT_MAX)
 347                 return -E2BIG;
 348
 349         return response.max_id;
 350 }
 351
 352 static int tegra_bpmp_clk_get_info(struct tegra_bpmp *bpmp, unsigned int id,
 353                                    struct tegra_bpmp_clk_info *info)
 354 {
 355         struct cmd_clk_get_all_info_response response;
 356         struct tegra_bpmp_clk_message msg;
 357         unsigned int i;
 358         int err;
 359
 360         memset(&msg, 0, sizeof(msg));
 361         msg.cmd = CMD_CLK_GET_ALL_INFO;
 362         msg.id = id;
 363         msg.rx.data = &response;
 364         msg.rx.size = sizeof(response);
 365
 366         err = tegra_bpmp_clk_transfer(bpmp, &msg);
 367         if (err < 0)
 368                 return err;
 369
 370         strscpy(info->name, response.name, MRQ_CLK_NAME_MAXLEN);
 371         info->num_parents = response.num_parents;
 372
 373         for (i = 0; i < info->num_parents; i++)
 374                 info->parents[i] = response.parents[i];
 375
 376         info->flags = response.flags;
 377
 378         return 0;
 379 }
 380
 381 static void tegra_bpmp_clk_info_dump(struct tegra_bpmp *bpmp,
 382                                      const char *level,
 383                                      const struct tegra_bpmp_clk_info *info)
 384 {
 385         const char *prefix = "";
 386         struct seq_buf buf;
 387         unsigned int i;
 388         char flags[64];
 389
 390         seq_buf_init(&buf, flags, sizeof(flags));
 391
 392         if (info->flags)
 393                 seq_buf_printf(&buf, "(");
 394
 395         if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
 396                 seq_buf_printf(&buf, "%smux", prefix);
 397                 prefix = ", ";
 398         }
 399
 400         if ((info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE) == 0) {
 401                 seq_buf_printf(&buf, "%sfixed", prefix);
 402                 prefix = ", ";
 403         }
 404
 405         if (info->flags & TEGRA_BPMP_CLK_IS_ROOT) {
 406                 seq_buf_printf(&buf, "%sroot", prefix);
 407                 prefix = ", ";
 408         }
 409
 410         if (info->flags)
 411                 seq_buf_printf(&buf, ")");
 412
 413         dev_printk(level, bpmp->dev, "%03u: %s\n", info->id, info->name);
 414         dev_printk(level, bpmp->dev, "  flags: %lx %s\n", info->flags, flags);
 415         dev_printk(level, bpmp->dev, "  parents: %u\n", info->num_parents);
 416
 417         for (i = 0; i < info->num_parents; i++)
 418                 dev_printk(level, bpmp->dev, "    %03u\n", info->parents[i]);
 419 }
 420
 421 static int tegra_bpmp_probe_clocks(struct tegra_bpmp *bpmp,
 422                                    struct tegra_bpmp_clk_info **clocksp)
 423 {
 424         struct tegra_bpmp_clk_info *clocks;
 425         unsigned int max_id, id, count = 0;
 426         unsigned int holes = 0;
 427         int err;
 428
 429         err = tegra_bpmp_clk_get_max_id(bpmp);
 430         if (err < 0)
 431                 return err;
 432
 433         max_id = err;
 434
 435         dev_dbg(bpmp->dev, "maximum clock ID: %u\n", max_id);
 436
 437         clocks = kcalloc(max_id + 1, sizeof(*clocks), GFP_KERNEL);
 438         if (!clocks)
 439                 return -ENOMEM;
 440
 441         for (id = 0; id <= max_id; id++) {
 442                 struct tegra_bpmp_clk_info *info = &clocks[count];
 443
 444                 err = tegra_bpmp_clk_get_info(bpmp, id, info);
 445                 if (err < 0)
 446                         continue;
 447
 448                 if (info->num_parents >= U8_MAX) {
 449                         dev_err(bpmp->dev,
 450                                 "clock %u has too many parents (%u, max: %u)\n",
 451                                 id, info->num_parents, U8_MAX);
 452                         continue;
 453                 }
 454
 455                 /* clock not exposed by BPMP */
 456                 if (info->name[0] == '\0') {
 457                         holes++;
 458                         continue;
 459                 }
 460
 461                 info->id = id;
 462                 count++;
 463
 464                 if (TEGRA_BPMP_DUMP_CLOCK_INFO)
 465                         tegra_bpmp_clk_info_dump(bpmp, KERN_DEBUG, info);
 466         }
 467
 468         dev_dbg(bpmp->dev, "holes: %u\n", holes);
 469         *clocksp = clocks;
 470
 471         return count;
 472 }
 473
 474 static unsigned int
 475 tegra_bpmp_clk_id_to_index(const struct tegra_bpmp_clk_info *clocks,
 476                            unsigned int num_clocks, unsigned int id)
 477 {
 478         unsigned int i;
 479
 480         for (i = 0; i < num_clocks; i++)
 481                 if (clocks[i].id == id)
 482                         return i;
 483
 484         return UINT_MAX;
 485 }
 486
 487 static const struct tegra_bpmp_clk_info *
 488 tegra_bpmp_clk_find(const struct tegra_bpmp_clk_info *clocks,
 489                     unsigned int num_clocks, unsigned int id)
 490 {
 491         unsigned int i;
 492
 493         i = tegra_bpmp_clk_id_to_index(clocks, num_clocks, id);
 494
 495         if (i < num_clocks)
 496                 return &clocks[i];
 497
 498         return NULL;
 499 }
 500
 501 static struct tegra_bpmp_clk *
 502 tegra_bpmp_clk_register(struct tegra_bpmp *bpmp,
 503                         const struct tegra_bpmp_clk_info *info,
 504                         const struct tegra_bpmp_clk_info *clocks,
 505                         unsigned int num_clocks)
 506 {
 507         struct tegra_bpmp_clk *clk;
 508         struct clk_init_data init;
 509         const char **parents;
 510         unsigned int i;
 511         int err;
 512
 513         clk = devm_kzalloc(bpmp->dev, sizeof(*clk), GFP_KERNEL);
 514         if (!clk)
 515                 return ERR_PTR(-ENOMEM);
 516
 517         clk->id = info->id;
 518         clk->bpmp = bpmp;
 519
 520         clk->parents = devm_kcalloc(bpmp->dev, info->num_parents,
 521                                     sizeof(*clk->parents), GFP_KERNEL);
 522         if (!clk->parents)
 523                 return ERR_PTR(-ENOMEM);
 524
 525         clk->num_parents = info->num_parents;
 526
 527         /* hardware clock initialization */
 528         memset(&init, 0, sizeof(init));
 529         init.name = info->name;
 530         clk->hw.init = &init;
 531         if (info->flags & BPMP_CLK_STATE_CHANGE_DENIED) {
 532                 if ((info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) == 0) {
 533                         dev_WARN(bpmp->dev,
 534                                 "Firmware bug! Inconsistent permission bits for clock %s. State and parent/rate changes disabled.",
 535                                  init.name);
 536                 }
 537                 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX)
 538                         init.ops = &tegra_bpmp_clk_mux_read_only_ops;
 539                 else
 540                         init.ops = &tegra_bpmp_clk_read_only_ops;
 541         } else if (info->flags & BPMP_CLK_RATE_PARENT_CHANGE_DENIED) {
 542                 if (info->flags & TEGRA_BPMP_CLK_HAS_MUX)
 543                         init.ops = &tegra_bpmp_clk_gate_mux_read_only_ops;
 544                 else
 545                         init.ops = &tegra_bpmp_clk_gate_ops;
 546         } else if (info->flags & TEGRA_BPMP_CLK_HAS_MUX) {
 547                 if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
 548                         init.ops = &tegra_bpmp_clk_mux_rate_ops;
 549                 else
 550                         init.ops = &tegra_bpmp_clk_mux_ops;
 551         } else {
 552                 if (info->flags & TEGRA_BPMP_CLK_HAS_SET_RATE)
 553                         init.ops = &tegra_bpmp_clk_rate_ops;
 554                 else
 555                         init.ops = &tegra_bpmp_clk_gate_ops;
 556         }
 557
 558         init.num_parents = info->num_parents;
 559
 560         parents = kcalloc(info->num_parents, sizeof(*parents), GFP_KERNEL);
 561         if (!parents)
 562                 return ERR_PTR(-ENOMEM);
 563
 564         for (i = 0; i < info->num_parents; i++) {
 565                 const struct tegra_bpmp_clk_info *parent;
 566
 567                 /* keep a private copy of the ID to parent index map */
 568                 clk->parents[i] = info->parents[i];
 569
 570                 parent = tegra_bpmp_clk_find(clocks, num_clocks,
 571                                              info->parents[i]);
 572                 if (!parent) {
 573                         dev_err(bpmp->dev, "no parent %u found for %u\n",
 574                                 info->parents[i], info->id);
 575                         continue;
 576                 }
 577
 578                 parents[i] = parent->name;
 579         }
 580
 581         init.parent_names = parents;
 582
 583         err = devm_clk_hw_register(bpmp->dev, &clk->hw);
 584
 585         kfree(parents);
 586
 587         if (err < 0)
 588                 return ERR_PTR(err);
 589
 590         return clk;
 591 }
 592
 593 static void tegra_bpmp_register_clocks_one(struct tegra_bpmp *bpmp,
 594                                            struct tegra_bpmp_clk_info *infos,
 595                                            unsigned int i,
 596                                            unsigned int count)
 597 {
 598         unsigned int j;
 599         struct tegra_bpmp_clk_info *info;
 600         struct tegra_bpmp_clk *clk;
 601
 602         if (bpmp->clocks[i]) {
 603                 /* already registered */
 604                 return;
 605         }
 606
 607         info = &infos[i];
 608         for (j = 0; j < info->num_parents; ++j) {
 609                 unsigned int p_id = info->parents[j];
 610                 unsigned int p_i = tegra_bpmp_clk_id_to_index(infos, count,
 611                                                               p_id);
 612                 if (p_i < count)
 613                         tegra_bpmp_register_clocks_one(bpmp, infos, p_i, count);
 614         }
 615
 616         clk = tegra_bpmp_clk_register(bpmp, info, infos, count);
 617         if (IS_ERR(clk)) {
 618                 dev_err(bpmp->dev,
 619                         "failed to register clock %u (%s): %ld\n",
 620                         info->id, info->name, PTR_ERR(clk));
 621                 /* intentionally store the error pointer to
 622                  * bpmp->clocks[i] to avoid re-attempting the
 623                  * registration later
 624                  */
 625         }
 626
 627         bpmp->clocks[i] = clk;
 628 }
 629
 630 static int tegra_bpmp_register_clocks(struct tegra_bpmp *bpmp,
 631                                       struct tegra_bpmp_clk_info *infos,
 632                                       unsigned int count)
 633 {
 634         unsigned int i;
 635
 636         bpmp->num_clocks = count;
 637
 638         bpmp->clocks = devm_kcalloc(bpmp->dev, count, sizeof(struct tegra_bpmp_clk), GFP_KERNEL);
 639         if (!bpmp->clocks)
 640                 return -ENOMEM;
 641
 642         for (i = 0; i < count; i++) {
 643                 tegra_bpmp_register_clocks_one(bpmp, infos, i, count);
 644         }
 645
 646         return 0;
 647 }
 648
 649 static void tegra_bpmp_unregister_clocks(struct tegra_bpmp *bpmp)
 650 {
 651         unsigned int i;
 652
 653         for (i = 0; i < bpmp->num_clocks; i++)
 654                 clk_hw_unregister(&bpmp->clocks[i]->hw);
 655 }
 656
 657 static struct clk_hw *tegra_bpmp_clk_of_xlate(struct of_phandle_args *clkspec,
 658                                               void *data)
 659 {
 660         unsigned int id = clkspec->args[0], i;
 661         struct tegra_bpmp *bpmp = data;
 662
 663         for (i = 0; i < bpmp->num_clocks; i++) {
 664                 struct tegra_bpmp_clk *clk = bpmp->clocks[i];
 665
 666                 if (!clk)
 667                         continue;
 668
 669                 if (clk->id == id)
 670                         return &clk->hw;
 671         }
 672
 673         return NULL;
 674 }
 675
 676 int tegra_bpmp_init_clocks(struct tegra_bpmp *bpmp)
 677 {
 678         struct tegra_bpmp_clk_info *clocks;
 679         unsigned int count;
 680         int err;
 681
 682         err = tegra_bpmp_probe_clocks(bpmp, &clocks);
 683         if (err < 0)
 684                 return err;
 685
 686         count = err;
 687
 688         dev_dbg(bpmp->dev, "%u clocks probed\n", count);
 689
 690         err = tegra_bpmp_register_clocks(bpmp, clocks, count);
 691         if (err < 0)
 692                 goto free;
 693
 694         err = of_clk_add_hw_provider(bpmp->dev->of_node,
 695                                      tegra_bpmp_clk_of_xlate,
 696                                      bpmp);
 697         if (err < 0) {
 698                 tegra_bpmp_unregister_clocks(bpmp);
 699                 goto free;
 700         }
 701
 702 free:
 703         kfree(clocks);
 704         return err;
 705 }