target/linux/bcm27xx/patches-5.4/950-0365-clk-raspberrypi-Also-support-v3d-clock.patch

   1 From e2262c8ab4755ab574580611d7da22509f07871c Mon Sep 17 00:00:00 2001
   2 From: popcornmix <popcornmix@gmail.com>
   3 Date: Wed, 21 Aug 2019 14:55:56 +0100
   4 Subject: [PATCH] clk-raspberrypi: Also support v3d clock
   5
   6 Signed-off-by: popcornmix <popcornmix@gmail.com>
   7 ---
   8  drivers/clk/bcm/clk-raspberrypi.c | 501 ++++++++++++++++++++++++------
   9  1 file changed, 412 insertions(+), 89 deletions(-)
  10
  11 --- a/drivers/clk/bcm/clk-raspberrypi.c
  12 +++ b/drivers/clk/bcm/clk-raspberrypi.c
  13 @@ -15,33 +15,103 @@
  14  #include <linux/io.h>
  15  #include <linux/module.h>
  16  #include <linux/platform_device.h>
  17 -
  18 +#include <dt-bindings/clock/bcm2835.h>
  19  #include <soc/bcm2835/raspberrypi-firmware.h>
  20
  21  #define RPI_FIRMWARE_ARM_CLK_ID                0x00000003
  22 +#define RPI_FIRMWARE_V3D_CLK_ID                0x00000005
  23
  24  #define RPI_FIRMWARE_STATE_ENABLE_BIT  BIT(0)
  25  #define RPI_FIRMWARE_STATE_WAIT_BIT    BIT(1)
  26
  27 -/*
  28 - * Even though the firmware interface alters 'pllb' the frequencies are
  29 - * provided as per 'pllb_arm'. We need to scale before passing them trough.
  30 - */
  31 -#define RPI_FIRMWARE_PLLB_ARM_DIV_RATE 2
  32 -
  33  #define A2W_PLL_FRAC_BITS              20
  34
  35 +#define SOC_BCM2835            BIT(0)
  36 +#define SOC_BCM2711            BIT(1)
  37 +#define SOC_ALL                        (SOC_BCM2835 | SOC_BCM2711)
  38 +
  39  struct raspberrypi_clk {
  40         struct device *dev;
  41         struct rpi_firmware *firmware;
  42         struct platform_device *cpufreq;
  43 +};
  44 +
  45 +typedef int (*raspberrypi_clk_register)(struct raspberrypi_clk *rpi,
  46 +                                             const void *data);
  47 +
  48 +
  49 +/* assignment helper macros for different clock types */
  50 +#define _REGISTER(f, s, ...) { .clk_register = (raspberrypi_clk_register)f, \
  51 +                              .supported = s,                          \
  52 +                              .data = __VA_ARGS__ }
  53 +#define REGISTER_PLL(s, ...)   _REGISTER(&raspberrypi_register_pll,    \
  54 +                                         s,                            \
  55 +                                         &(struct raspberrypi_pll_data)        \
  56 +                                         {__VA_ARGS__})
  57 +#define REGISTER_PLL_DIV(s, ...) _REGISTER(&raspberrypi_register_pll_divider, \
  58 +                                          s,                             \
  59 +                                          &(struct raspberrypi_pll_divider_data) \
  60 +                                          {__VA_ARGS__})
  61 +#define REGISTER_CLK(s, ...)   _REGISTER(&raspberrypi_register_clock,  \
  62 +                                         s,                            \
  63 +                                         &(struct raspberrypi_clock_data)      \
  64 +                                         {__VA_ARGS__})
  65 +
  66 +
  67 +struct raspberrypi_pll_data {
  68 +       const char *name;
  69 +       const char *const *parents;
  70 +       int num_parents;
  71 +       u32 clock_id;
  72 +};
  73 +
  74 +struct raspberrypi_clock_data {
  75 +       const char *name;
  76 +       const char *const *parents;
  77 +       int num_parents;
  78 +       u32 flags;
  79 +       u32 clock_id;
  80 +};
  81 +
  82 +struct raspberrypi_pll_divider_data {
  83 +       const char *name;
  84 +       const char *divider_name;
  85 +       const char *lookup;
  86 +       const char *source_pll;
  87 +
  88 +       u32 fixed_divider;
  89 +       u32 flags;
  90 +       u32 clock_id;
  91 +};
  92
  93 -       unsigned long min_rate;
  94 -       unsigned long max_rate;
  95 +struct raspberrypi_clk_desc {
  96 +       raspberrypi_clk_register clk_register;
  97 +       unsigned int supported;
  98 +       const void *data;
  99 +};
 100
 101 -       struct clk_hw pllb;
 102 -       struct clk_hw *pllb_arm;
 103 -       struct clk_lookup *pllb_arm_lookup;
 104 +struct raspberrypi_clock {
 105 +       struct clk_hw hw;
 106 +       struct raspberrypi_clk *rpi;
 107 +       u32 min_rate;
 108 +       u32 max_rate;
 109 +       const struct raspberrypi_clock_data *data;
 110 +};
 111 +
 112 +struct raspberrypi_pll {
 113 +       struct clk_hw hw;
 114 +       struct raspberrypi_clk *rpi;
 115 +       u32 min_rate;
 116 +       u32 max_rate;
 117 +       const struct raspberrypi_pll_data *data;
 118 +};
 119 +
 120 +struct raspberrypi_pll_divider {
 121 +       struct clk_divider div;
 122 +       struct raspberrypi_clk *rpi;
 123 +       u32 min_rate;
 124 +       u32 max_rate;
 125 +       const struct raspberrypi_pll_divider_data *data;
 126  };
 127
 128  /*
 129 @@ -83,56 +153,49 @@ static int raspberrypi_clock_property(st
 130         return 0;
 131  }
 132
 133 -static int raspberrypi_fw_pll_is_on(struct clk_hw *hw)
 134 +static int raspberrypi_fw_is_on(struct raspberrypi_clk *rpi, u32 clock_id, const char *name)
 135  {
 136 -       struct raspberrypi_clk *rpi = container_of(hw, struct raspberrypi_clk,
 137 -                                                  pllb);
 138         u32 val = 0;
 139         int ret;
 140
 141         ret = raspberrypi_clock_property(rpi->firmware,
 142                                          RPI_FIRMWARE_GET_CLOCK_STATE,
 143 -                                        RPI_FIRMWARE_ARM_CLK_ID, &val);
 144 +                                        clock_id, &val);
 145         if (ret)
 146                 return 0;
 147
 148         return !!(val & RPI_FIRMWARE_STATE_ENABLE_BIT);
 149  }
 150
 151 -
 152 -static unsigned long raspberrypi_fw_pll_get_rate(struct clk_hw *hw,
 153 -                                                unsigned long parent_rate)
 154 +static unsigned long raspberrypi_fw_get_rate(struct raspberrypi_clk *rpi,
 155 +                                                u32 clock_id, const char *name, unsigned long parent_rate)
 156  {
 157 -       struct raspberrypi_clk *rpi = container_of(hw, struct raspberrypi_clk,
 158 -                                                  pllb);
 159         u32 val = 0;
 160         int ret;
 161
 162         ret = raspberrypi_clock_property(rpi->firmware,
 163                                          RPI_FIRMWARE_GET_CLOCK_RATE,
 164 -                                        RPI_FIRMWARE_ARM_CLK_ID,
 165 +                                        clock_id,
 166                                          &val);
 167         if (ret)
 168 -               return ret;
 169 -
 170 -       return val * RPI_FIRMWARE_PLLB_ARM_DIV_RATE;
 171 +               dev_err_ratelimited(rpi->dev, "Failed to get %s frequency: %d",
 172 +                                   name, ret);
 173 +       return val;
 174  }
 175
 176 -static int raspberrypi_fw_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 177 -                                      unsigned long parent_rate)
 178 +static int raspberrypi_fw_set_rate(struct raspberrypi_clk *rpi,
 179 +                                  u32 clock_id, const char *name, u32 rate,
 180 +                                  unsigned long parent_rate)
 181  {
 182 -       struct raspberrypi_clk *rpi = container_of(hw, struct raspberrypi_clk,
 183 -                                                  pllb);
 184 -       u32 new_rate = rate / RPI_FIRMWARE_PLLB_ARM_DIV_RATE;
 185         int ret;
 186
 187         ret = raspberrypi_clock_property(rpi->firmware,
 188                                          RPI_FIRMWARE_SET_CLOCK_RATE,
 189 -                                        RPI_FIRMWARE_ARM_CLK_ID,
 190 -                                        &new_rate);
 191 +                                        clock_id,
 192 +                                        &rate);
 193         if (ret)
 194                 dev_err_ratelimited(rpi->dev, "Failed to change %s frequency: %d",
 195 -                                   clk_hw_get_name(hw), ret);
 196 +                                   name, ret);
 197
 198         return ret;
 199  }
 200 @@ -141,16 +204,18 @@ static int raspberrypi_fw_pll_set_rate(s
 201   * Sadly there is no firmware rate rounding interface. We borrowed it from
 202   * clk-bcm2835.
 203   */
 204 -static int raspberrypi_pll_determine_rate(struct clk_hw *hw,
 205 +static int raspberrypi_determine_rate(struct raspberrypi_clk *rpi,
 206 +                                         u32 clock_id, const char *name, unsigned long min_rate, unsigned long max_rate,
 207                                           struct clk_rate_request *req)
 208  {
 209 -       struct raspberrypi_clk *rpi = container_of(hw, struct raspberrypi_clk,
 210 -                                                  pllb);
 211 +#if 1
 212 +       req->rate = clamp(req->rate, min_rate, max_rate);
 213 +#else
 214         u64 div, final_rate;
 215         u32 ndiv, fdiv;
 216
 217         /* We can't use req->rate directly as it would overflow */
 218 -       final_rate = clamp(req->rate, rpi->min_rate, rpi->max_rate);
 219 +       final_rate = clamp(req->rate, min_rate, max_rate);
 220
 221         div = (u64)final_rate << A2W_PLL_FRAC_BITS;
 222         do_div(div, req->best_parent_rate);
 223 @@ -163,9 +228,129 @@ static int raspberrypi_pll_determine_rat
 224
 225         req->rate = final_rate >> A2W_PLL_FRAC_BITS;
 226
 227 +#endif
 228         return 0;
 229  }
 230
 231 +static int raspberrypi_fw_clock_is_on(struct clk_hw *hw)
 232 +{
 233 +       struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw);
 234 +       struct raspberrypi_clk *rpi = pll->rpi;
 235 +       const struct raspberrypi_clock_data *data = pll->data;
 236 +
 237 +       return raspberrypi_fw_is_on(rpi, data->clock_id, data->name);
 238 +}
 239 +
 240 +static unsigned long raspberrypi_fw_clock_get_rate(struct clk_hw *hw,
 241 +                                                unsigned long parent_rate)
 242 +{
 243 +       struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw);
 244 +       struct raspberrypi_clk *rpi = pll->rpi;
 245 +       const struct raspberrypi_clock_data *data = pll->data;
 246 +
 247 +       return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate);
 248 +}
 249 +
 250 +static int raspberrypi_fw_clock_set_rate(struct clk_hw *hw, unsigned long rate,
 251 +                                      unsigned long parent_rate)
 252 +{
 253 +       struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw);
 254 +       struct raspberrypi_clk *rpi = pll->rpi;
 255 +       const struct raspberrypi_clock_data *data = pll->data;
 256 +
 257 +       return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate);
 258 +}
 259 +
 260 +static int raspberrypi_clock_determine_rate(struct clk_hw *hw,
 261 +                                         struct clk_rate_request *req)
 262 +{
 263 +       struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw);
 264 +       struct raspberrypi_clk *rpi = pll->rpi;
 265 +       const struct raspberrypi_clock_data *data = pll->data;
 266 +
 267 +       return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req);
 268 +}
 269 +
 270 +static int raspberrypi_fw_pll_is_on(struct clk_hw *hw)
 271 +{
 272 +       struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw);
 273 +       struct raspberrypi_clk *rpi = pll->rpi;
 274 +       const struct raspberrypi_pll_data *data = pll->data;
 275 +
 276 +       return raspberrypi_fw_is_on(rpi, data->clock_id, data->name);
 277 +}
 278 +
 279 +static unsigned long raspberrypi_fw_pll_get_rate(struct clk_hw *hw,
 280 +                                                unsigned long parent_rate)
 281 +{
 282 +       struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw);
 283 +       struct raspberrypi_clk *rpi = pll->rpi;
 284 +       const struct raspberrypi_pll_data *data = pll->data;
 285 +
 286 +       return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate);
 287 +}
 288 +
 289 +static int raspberrypi_fw_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 290 +                                      unsigned long parent_rate)
 291 +{
 292 +       struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw);
 293 +       struct raspberrypi_clk *rpi = pll->rpi;
 294 +       const struct raspberrypi_pll_data *data = pll->data;
 295 +
 296 +       return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate);
 297 +}
 298 +
 299 +static int raspberrypi_pll_determine_rate(struct clk_hw *hw,
 300 +                                         struct clk_rate_request *req)
 301 +{
 302 +       struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw);
 303 +       struct raspberrypi_clk *rpi = pll->rpi;
 304 +       const struct raspberrypi_pll_data *data = pll->data;
 305 +
 306 +       return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req);
 307 +}
 308 +
 309 +
 310 +static int raspberrypi_fw_pll_div_is_on(struct clk_hw *hw)
 311 +{
 312 +       struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw);
 313 +       struct raspberrypi_clk *rpi = pll->rpi;
 314 +       const struct raspberrypi_pll_divider_data *data = pll->data;
 315 +
 316 +       return raspberrypi_fw_is_on(rpi, data->clock_id, data->name);
 317 +}
 318 +
 319 +static unsigned long raspberrypi_fw_pll_div_get_rate(struct clk_hw *hw,
 320 +                                                unsigned long parent_rate)
 321 +{
 322 +       struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw);
 323 +       struct raspberrypi_clk *rpi = pll->rpi;
 324 +       const struct raspberrypi_pll_divider_data *data = pll->data;
 325 +
 326 +       return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate);
 327 +}
 328 +
 329 +static int raspberrypi_fw_pll_div_set_rate(struct clk_hw *hw, unsigned long rate,
 330 +                                      unsigned long parent_rate)
 331 +{
 332 +       struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw);
 333 +       struct raspberrypi_clk *rpi = pll->rpi;
 334 +       const struct raspberrypi_pll_divider_data *data = pll->data;
 335 +
 336 +       return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate);
 337 +}
 338 +
 339 +static int raspberrypi_pll_div_determine_rate(struct clk_hw *hw,
 340 +                                         struct clk_rate_request *req)
 341 +{
 342 +       struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw);
 343 +       struct raspberrypi_clk *rpi = pll->rpi;
 344 +       const struct raspberrypi_pll_divider_data *data = pll->data;
 345 +
 346 +       return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req);
 347 +}
 348 +
 349 +
 350  static const struct clk_ops raspberrypi_firmware_pll_clk_ops = {
 351         .is_prepared = raspberrypi_fw_pll_is_on,
 352         .recalc_rate = raspberrypi_fw_pll_get_rate,
 353 @@ -173,87 +358,225 @@ static const struct clk_ops raspberrypi_
 354         .determine_rate = raspberrypi_pll_determine_rate,
 355  };
 356
 357 -static int raspberrypi_register_pllb(struct raspberrypi_clk *rpi)
 358 +static const struct clk_ops raspberrypi_firmware_pll_divider_clk_ops = {
 359 +       .is_prepared = raspberrypi_fw_pll_div_is_on,
 360 +       .recalc_rate = raspberrypi_fw_pll_div_get_rate,
 361 +       .set_rate = raspberrypi_fw_pll_div_set_rate,
 362 +       .determine_rate = raspberrypi_pll_div_determine_rate,
 363 +};
 364 +
 365 +static const struct clk_ops raspberrypi_firmware_clk_ops = {
 366 +       .is_prepared = raspberrypi_fw_clock_is_on,
 367 +       .recalc_rate = raspberrypi_fw_clock_get_rate,
 368 +       .set_rate = raspberrypi_fw_clock_set_rate,
 369 +       .determine_rate = raspberrypi_clock_determine_rate,
 370 +};
 371 +
 372 +
 373 +static int raspberrypi_get_clock_range(struct raspberrypi_clk *rpi, u32 clock_id, u32 *min_rate, u32 *max_rate)
 374  {
 375 -       u32 min_rate = 0, max_rate = 0;
 376 +       int ret;
 377 +
 378 +       /* Get min & max rates set by the firmware */
 379 +       ret = raspberrypi_clock_property(rpi->firmware,
 380 +                                        RPI_FIRMWARE_GET_MIN_CLOCK_RATE,
 381 +                                        clock_id,
 382 +                                        min_rate);
 383 +       if (ret) {
 384 +               dev_err(rpi->dev, "Failed to get clock %d min freq: %d (%d)\n",
 385 +                       clock_id, *min_rate, ret);
 386 +               return ret;
 387 +       }
 388 +
 389 +       ret = raspberrypi_clock_property(rpi->firmware,
 390 +                                        RPI_FIRMWARE_GET_MAX_CLOCK_RATE,
 391 +                                        clock_id,
 392 +                                        max_rate);
 393 +       if (ret) {
 394 +               dev_err(rpi->dev, "Failed to get clock %d max freq: %d (%d)\n",
 395 +                       clock_id, *max_rate, ret);
 396 +               return ret;
 397 +       }
 398 +       return 0;
 399 +}
 400 +
 401 +
 402 +static int raspberrypi_register_pll(struct raspberrypi_clk *rpi,
 403 +                                          const struct raspberrypi_pll_data *data)
 404 +{
 405 +       struct raspberrypi_pll *pll;
 406         struct clk_init_data init;
 407         int ret;
 408
 409         memset(&init, 0, sizeof(init));
 410
 411         /* All of the PLLs derive from the external oscillator. */
 412 -       init.parent_names = (const char *[]){ "osc" };
 413 -       init.num_parents = 1;
 414 -       init.name = "pllb";
 415 +       init.parent_names = data->parents;
 416 +       init.num_parents = data->num_parents;
 417 +       init.name = data->name;
 418         init.ops = &raspberrypi_firmware_pll_clk_ops;
 419         init.flags = CLK_GET_RATE_NOCACHE | CLK_IGNORE_UNUSED;
 420
 421 -       /* Get min & max rates set by the firmware */
 422 -       ret = raspberrypi_clock_property(rpi->firmware,
 423 -                                        RPI_FIRMWARE_GET_MIN_CLOCK_RATE,
 424 -                                        RPI_FIRMWARE_ARM_CLK_ID,
 425 -                                        &min_rate);
 426 +       pll = kzalloc(sizeof(*pll), GFP_KERNEL);
 427 +       if (!pll)
 428 +               return -ENOMEM;
 429 +       pll->rpi = rpi;
 430 +       pll->data = data;
 431 +       pll->hw.init = &init;
 432 +
 433 +       ret = raspberrypi_get_clock_range(rpi, data->clock_id, &pll->min_rate, &pll->max_rate);
 434         if (ret) {
 435 -               dev_err(rpi->dev, "Failed to get %s min freq: %d\n",
 436 -                       init.name, ret);
 437 +               dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret);
 438                 return ret;
 439         }
 440
 441 -       ret = raspberrypi_clock_property(rpi->firmware,
 442 -                                        RPI_FIRMWARE_GET_MAX_CLOCK_RATE,
 443 -                                        RPI_FIRMWARE_ARM_CLK_ID,
 444 -                                        &max_rate);
 445 +       ret = devm_clk_hw_register(rpi->dev, &pll->hw);
 446         if (ret) {
 447 -               dev_err(rpi->dev, "Failed to get %s max freq: %d\n",
 448 -                       init.name, ret);
 449 +               dev_err(rpi->dev, "%s: devm_clk_hw_register(%s) failed: %d\n", __func__, init.name, ret);
 450                 return ret;
 451         }
 452 +       return 0;
 453 +}
 454
 455 -       if (!min_rate || !max_rate) {
 456 -               dev_err(rpi->dev, "Unexpected frequency range: min %u, max %u\n",
 457 -                       min_rate, max_rate);
 458 -               return -EINVAL;
 459 -       }
 460 +static int
 461 +raspberrypi_register_pll_divider(struct raspberrypi_clk *rpi,
 462 +                            const struct raspberrypi_pll_divider_data *data)
 463 +{
 464 +       struct raspberrypi_pll_divider *divider;
 465 +       struct clk_init_data init;
 466 +       int ret;
 467 +
 468 +       memset(&init, 0, sizeof(init));
 469 +
 470 +       init.parent_names = &data->source_pll;
 471 +       init.num_parents = 1;
 472 +       init.name = data->name;
 473 +       init.ops = &raspberrypi_firmware_pll_divider_clk_ops;
 474 +       init.flags = data->flags | CLK_IGNORE_UNUSED;
 475
 476 -       dev_info(rpi->dev, "CPU frequency range: min %u, max %u\n",
 477 -                min_rate, max_rate);
 478 +       divider = devm_kzalloc(rpi->dev, sizeof(*divider), GFP_KERNEL);
 479 +       if (!divider)
 480 +               return -ENOMEM;
 481 +
 482 +       divider->div.hw.init = &init;
 483 +       divider->rpi = rpi;
 484 +       divider->data = data;
 485 +
 486 +       ret = raspberrypi_get_clock_range(rpi, data->clock_id, &divider->min_rate, &divider->max_rate);
 487 +       if (ret) {
 488 +               dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret);
 489 +               return ret;
 490 +       }
 491
 492 -       rpi->min_rate = min_rate * RPI_FIRMWARE_PLLB_ARM_DIV_RATE;
 493 -       rpi->max_rate = max_rate * RPI_FIRMWARE_PLLB_ARM_DIV_RATE;
 494 +       ret = devm_clk_hw_register(rpi->dev, &divider->div.hw);
 495 +       if (ret) {
 496 +               dev_err(rpi->dev, "%s: devm_clk_hw_register(%s) failed: %d\n", __func__, init.name, ret);
 497 +               return ret;
 498 +       }
 499
 500 -       rpi->pllb.init = &init;
 501 +       /*
 502 +        * PLLH's channels have a fixed divide by 10 afterwards, which
 503 +        * is what our consumers are actually using.
 504 +        */
 505 +       if (data->fixed_divider != 0) {
 506 +               struct clk_lookup *lookup;
 507 +               struct clk_hw *clk = clk_hw_register_fixed_factor(rpi->dev,
 508 +                                                   data->divider_name,
 509 +                                                   data->name,
 510 +                                                   CLK_SET_RATE_PARENT,
 511 +                                                   1,
 512 +                                                   data->fixed_divider);
 513 +               if (IS_ERR(clk)) {
 514 +                       dev_err(rpi->dev, "%s: clk_hw_register_fixed_factor(%s) failed: %ld\n", __func__, init.name, PTR_ERR(clk));
 515 +                       return PTR_ERR(clk);
 516 +               }
 517 +               if (data->lookup) {
 518 +                       lookup = clkdev_hw_create(clk, NULL, data->lookup);
 519 +                       if (IS_ERR(lookup)) {
 520 +                               dev_err(rpi->dev, "%s: clk_hw_register_fixed_factor(%s) failed: %ld\n", __func__, init.name, PTR_ERR(lookup));
 521 +                               return PTR_ERR(lookup);
 522 +                       }
 523 +               }
 524 +       }
 525
 526 -       return devm_clk_hw_register(rpi->dev, &rpi->pllb);
 527 +       return 0;
 528  }
 529
 530 -static int raspberrypi_register_pllb_arm(struct raspberrypi_clk *rpi)
 531 +static int raspberrypi_register_clock(struct raspberrypi_clk *rpi,
 532 +                                         const struct raspberrypi_clock_data *data)
 533  {
 534 -       rpi->pllb_arm = clk_hw_register_fixed_factor(rpi->dev,
 535 -                               "pllb_arm", "pllb",
 536 -                               CLK_SET_RATE_PARENT | CLK_GET_RATE_NOCACHE,
 537 -                               1, 2);
 538 -       if (IS_ERR(rpi->pllb_arm)) {
 539 -               dev_err(rpi->dev, "Failed to initialize pllb_arm\n");
 540 -               return PTR_ERR(rpi->pllb_arm);
 541 -       }
 542 +       struct raspberrypi_clock *clock;
 543 +       struct clk_init_data init;
 544 +       struct clk *clk;
 545 +       int ret;
 546 +
 547 +       memset(&init, 0, sizeof(init));
 548 +       init.parent_names = data->parents;
 549 +       init.num_parents = data->num_parents;
 550 +       init.name = data->name;
 551 +       init.flags = data->flags | CLK_IGNORE_UNUSED;
 552
 553 -       rpi->pllb_arm_lookup = clkdev_hw_create(rpi->pllb_arm, NULL, "cpu0");
 554 -       if (!rpi->pllb_arm_lookup) {
 555 -               dev_err(rpi->dev, "Failed to initialize pllb_arm_lookup\n");
 556 -               clk_hw_unregister_fixed_factor(rpi->pllb_arm);
 557 +       init.ops = &raspberrypi_firmware_clk_ops;
 558 +
 559 +       clock = devm_kzalloc(rpi->dev, sizeof(*clock), GFP_KERNEL);
 560 +       if (!clock)
 561                 return -ENOMEM;
 562 -       }
 563
 564 +       clock->rpi = rpi;
 565 +       clock->data = data;
 566 +       clock->hw.init = &init;
 567 +
 568 +       ret = raspberrypi_get_clock_range(rpi, data->clock_id, &clock->min_rate, &clock->max_rate);
 569 +       if (ret) {
 570 +               dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret);
 571 +               return ret;
 572 +       }
 573 +       clk = devm_clk_register(rpi->dev, &clock->hw);
 574 +       if (IS_ERR(clk)) {
 575 +               dev_err(rpi->dev, "%s: devm_clk_register(%s) failed: %ld\n", __func__, init.name, PTR_ERR(clk));
 576 +               return PTR_ERR(clk);
 577 +       }
 578 +       ret = clk_register_clkdev(clk, init.name, NULL);
 579 +       if (ret) {
 580 +               dev_err(rpi->dev, "%s: clk_register_clkdev(%s) failed: %d\n", __func__, init.name, ret);
 581 +               return ret;
 582 +       }
 583         return 0;
 584  }
 585
 586 +
 587 +/*
 588 + * the real definition of all the pll, pll_dividers and clocks
 589 + * these make use of the above REGISTER_* macros
 590 + */
 591 +static const struct raspberrypi_clk_desc clk_desc_array[] = {
 592 +       /* the PLL + PLL dividers */
 593 +       [BCM2835_CLOCK_V3D]     = REGISTER_CLK(
 594 +               SOC_ALL,
 595 +               .name = "v3d",
 596 +               .parents = (const char *[]){ "osc" },
 597 +               .num_parents = 1,
 598 +               .clock_id = RPI_FIRMWARE_V3D_CLK_ID),
 599 +       [BCM2835_PLLB_ARM]      = REGISTER_PLL_DIV(
 600 +               SOC_ALL,
 601 +               .name = "pllb",
 602 +               .source_pll = "osc",
 603 +               .divider_name = "pllb_arm",
 604 +               .lookup = "cpu0",
 605 +               .fixed_divider = 1,
 606 +               .clock_id = RPI_FIRMWARE_ARM_CLK_ID,
 607 +               .flags = CLK_SET_RATE_PARENT),
 608 +};
 609 +
 610  static int raspberrypi_clk_probe(struct platform_device *pdev)
 611  {
 612         struct device_node *firmware_node;
 613         struct device *dev = &pdev->dev;
 614         struct rpi_firmware *firmware;
 615         struct raspberrypi_clk *rpi;
 616 -       int ret;
 617 +       const struct raspberrypi_clk_desc *desc;
 618 +       const size_t asize = ARRAY_SIZE(clk_desc_array);
 619 +       int i;
 620
 621         firmware_node = of_find_compatible_node(NULL, NULL,
 622                                         "raspberrypi,bcm2835-firmware");
 623 @@ -275,16 +598,16 @@ static int raspberrypi_clk_probe(struct
 624         rpi->firmware = firmware;
 625         platform_set_drvdata(pdev, rpi);
 626
 627 -       ret = raspberrypi_register_pllb(rpi);
 628 -       if (ret) {
 629 -               dev_err(dev, "Failed to initialize pllb, %d\n", ret);
 630 -               return ret;
 631 +       for (i = 0; i < asize; i++) {
 632 +               desc = &clk_desc_array[i];
 633 +               if (desc->clk_register && desc->data /*&&
 634 +                   (desc->supported & pdata->soc)*/) {
 635 +                       int ret = desc->clk_register(rpi, desc->data);
 636 +                       if (ret)
 637 +                               return ret;
 638 +               }
 639         }
 640
 641 -       ret = raspberrypi_register_pllb_arm(rpi);
 642 -       if (ret)
 643 -               return ret;
 644 -
 645         rpi->cpufreq = platform_device_register_data(dev, "raspberrypi-cpufreq",
 646                                                      -1, NULL, 0);
 647