target/linux/brcm2708/patches-4.19/950-0766-clk-raspberrypi-Also-support-v3d-clock.patch

   1 From 679f68410271a32f200e48b501027f68bd114d5c 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 | 497 ++++++++++++++++++++++++------
   9  1 file changed, 408 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,15 @@ 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         u64 div, final_rate;
 212         u32 ndiv, fdiv;
 213
 214         /* We can't use req->rate directly as it would overflow */
 215 -       final_rate = clamp(req->rate, rpi->min_rate, rpi->max_rate);
 216 +       final_rate = clamp(req->rate, min_rate, max_rate);
 217
 218         div = (u64)final_rate << A2W_PLL_FRAC_BITS;
 219         do_div(div, req->best_parent_rate);
 220 @@ -166,6 +228,125 @@ static int raspberrypi_pll_determine_rat
 221         return 0;
 222  }
 223
 224 +static int raspberrypi_fw_clock_is_on(struct clk_hw *hw)
 225 +{
 226 +       struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw);
 227 +       struct raspberrypi_clk *rpi = pll->rpi;
 228 +       const struct raspberrypi_clock_data *data = pll->data;
 229 +
 230 +       return raspberrypi_fw_is_on(rpi, data->clock_id, data->name);
 231 +}
 232 +
 233 +static unsigned long raspberrypi_fw_clock_get_rate(struct clk_hw *hw,
 234 +                                                unsigned long parent_rate)
 235 +{
 236 +       struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw);
 237 +       struct raspberrypi_clk *rpi = pll->rpi;
 238 +       const struct raspberrypi_clock_data *data = pll->data;
 239 +
 240 +       return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate);
 241 +}
 242 +
 243 +static int raspberrypi_fw_clock_set_rate(struct clk_hw *hw, unsigned long rate,
 244 +                                      unsigned long parent_rate)
 245 +{
 246 +       struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw);
 247 +       struct raspberrypi_clk *rpi = pll->rpi;
 248 +       const struct raspberrypi_clock_data *data = pll->data;
 249 +
 250 +       return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate);
 251 +}
 252 +
 253 +static int raspberrypi_clock_determine_rate(struct clk_hw *hw,
 254 +                                         struct clk_rate_request *req)
 255 +{
 256 +       struct raspberrypi_clock *pll = container_of(hw, struct raspberrypi_clock, hw);
 257 +       struct raspberrypi_clk *rpi = pll->rpi;
 258 +       const struct raspberrypi_clock_data *data = pll->data;
 259 +
 260 +       return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req);
 261 +}
 262 +
 263 +static int raspberrypi_fw_pll_is_on(struct clk_hw *hw)
 264 +{
 265 +       struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw);
 266 +       struct raspberrypi_clk *rpi = pll->rpi;
 267 +       const struct raspberrypi_pll_data *data = pll->data;
 268 +
 269 +       return raspberrypi_fw_is_on(rpi, data->clock_id, data->name);
 270 +}
 271 +
 272 +static unsigned long raspberrypi_fw_pll_get_rate(struct clk_hw *hw,
 273 +                                                unsigned long parent_rate)
 274 +{
 275 +       struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw);
 276 +       struct raspberrypi_clk *rpi = pll->rpi;
 277 +       const struct raspberrypi_pll_data *data = pll->data;
 278 +
 279 +       return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate);
 280 +}
 281 +
 282 +static int raspberrypi_fw_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 283 +                                      unsigned long parent_rate)
 284 +{
 285 +       struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw);
 286 +       struct raspberrypi_clk *rpi = pll->rpi;
 287 +       const struct raspberrypi_pll_data *data = pll->data;
 288 +
 289 +       return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate);
 290 +}
 291 +
 292 +static int raspberrypi_pll_determine_rate(struct clk_hw *hw,
 293 +                                         struct clk_rate_request *req)
 294 +{
 295 +       struct raspberrypi_pll *pll = container_of(hw, struct raspberrypi_pll, hw);
 296 +       struct raspberrypi_clk *rpi = pll->rpi;
 297 +       const struct raspberrypi_pll_data *data = pll->data;
 298 +
 299 +       return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req);
 300 +}
 301 +
 302 +
 303 +static int raspberrypi_fw_pll_div_is_on(struct clk_hw *hw)
 304 +{
 305 +       struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw);
 306 +       struct raspberrypi_clk *rpi = pll->rpi;
 307 +       const struct raspberrypi_pll_divider_data *data = pll->data;
 308 +
 309 +       return raspberrypi_fw_is_on(rpi, data->clock_id, data->name);
 310 +}
 311 +
 312 +static unsigned long raspberrypi_fw_pll_div_get_rate(struct clk_hw *hw,
 313 +                                                unsigned long parent_rate)
 314 +{
 315 +       struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw);
 316 +       struct raspberrypi_clk *rpi = pll->rpi;
 317 +       const struct raspberrypi_pll_divider_data *data = pll->data;
 318 +
 319 +       return raspberrypi_fw_get_rate(rpi, data->clock_id, data->name, parent_rate);
 320 +}
 321 +
 322 +static int raspberrypi_fw_pll_div_set_rate(struct clk_hw *hw, unsigned long rate,
 323 +                                      unsigned long parent_rate)
 324 +{
 325 +       struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw);
 326 +       struct raspberrypi_clk *rpi = pll->rpi;
 327 +       const struct raspberrypi_pll_divider_data *data = pll->data;
 328 +
 329 +       return raspberrypi_fw_set_rate(rpi, data->clock_id, data->name, rate, parent_rate);
 330 +}
 331 +
 332 +static int raspberrypi_pll_div_determine_rate(struct clk_hw *hw,
 333 +                                         struct clk_rate_request *req)
 334 +{
 335 +       struct raspberrypi_pll_divider *pll = container_of(hw, struct raspberrypi_pll_divider, div.hw);
 336 +       struct raspberrypi_clk *rpi = pll->rpi;
 337 +       const struct raspberrypi_pll_divider_data *data = pll->data;
 338 +
 339 +       return raspberrypi_determine_rate(rpi, data->clock_id, data->name, pll->min_rate, pll->max_rate, req);
 340 +}
 341 +
 342 +
 343  static const struct clk_ops raspberrypi_firmware_pll_clk_ops = {
 344         .is_prepared = raspberrypi_fw_pll_is_on,
 345         .recalc_rate = raspberrypi_fw_pll_get_rate,
 346 @@ -173,87 +354,225 @@ static const struct clk_ops raspberrypi_
 347         .determine_rate = raspberrypi_pll_determine_rate,
 348  };
 349
 350 -static int raspberrypi_register_pllb(struct raspberrypi_clk *rpi)
 351 +static const struct clk_ops raspberrypi_firmware_pll_divider_clk_ops = {
 352 +       .is_prepared = raspberrypi_fw_pll_div_is_on,
 353 +       .recalc_rate = raspberrypi_fw_pll_div_get_rate,
 354 +       .set_rate = raspberrypi_fw_pll_div_set_rate,
 355 +       .determine_rate = raspberrypi_pll_div_determine_rate,
 356 +};
 357 +
 358 +static const struct clk_ops raspberrypi_firmware_clk_ops = {
 359 +       .is_prepared = raspberrypi_fw_clock_is_on,
 360 +       .recalc_rate = raspberrypi_fw_clock_get_rate,
 361 +       .set_rate = raspberrypi_fw_clock_set_rate,
 362 +       .determine_rate = raspberrypi_clock_determine_rate,
 363 +};
 364 +
 365 +
 366 +static int raspberrypi_get_clock_range(struct raspberrypi_clk *rpi, u32 clock_id, u32 *min_rate, u32 *max_rate)
 367  {
 368 -       u32 min_rate = 0, max_rate = 0;
 369 +       int ret;
 370 +
 371 +       /* Get min & max rates set by the firmware */
 372 +       ret = raspberrypi_clock_property(rpi->firmware,
 373 +                                        RPI_FIRMWARE_GET_MIN_CLOCK_RATE,
 374 +                                        clock_id,
 375 +                                        min_rate);
 376 +       if (ret) {
 377 +               dev_err(rpi->dev, "Failed to get clock %d min freq: %d (%d)\n",
 378 +                       clock_id, *min_rate, ret);
 379 +               return ret;
 380 +       }
 381 +
 382 +       ret = raspberrypi_clock_property(rpi->firmware,
 383 +                                        RPI_FIRMWARE_GET_MAX_CLOCK_RATE,
 384 +                                        clock_id,
 385 +                                        max_rate);
 386 +       if (ret) {
 387 +               dev_err(rpi->dev, "Failed to get clock %d max freq: %d (%d)\n",
 388 +                       clock_id, *max_rate, ret);
 389 +               return ret;
 390 +       }
 391 +       return 0;
 392 +}
 393 +
 394 +
 395 +static int raspberrypi_register_pll(struct raspberrypi_clk *rpi,
 396 +                                          const struct raspberrypi_pll_data *data)
 397 +{
 398 +       struct raspberrypi_pll *pll;
 399         struct clk_init_data init;
 400         int ret;
 401
 402         memset(&init, 0, sizeof(init));
 403
 404         /* All of the PLLs derive from the external oscillator. */
 405 -       init.parent_names = (const char *[]){ "osc" };
 406 -       init.num_parents = 1;
 407 -       init.name = "pllb";
 408 +       init.parent_names = data->parents;
 409 +       init.num_parents = data->num_parents;
 410 +       init.name = data->name;
 411         init.ops = &raspberrypi_firmware_pll_clk_ops;
 412         init.flags = CLK_GET_RATE_NOCACHE | CLK_IGNORE_UNUSED;
 413
 414 -       /* Get min & max rates set by the firmware */
 415 -       ret = raspberrypi_clock_property(rpi->firmware,
 416 -                                        RPI_FIRMWARE_GET_MIN_CLOCK_RATE,
 417 -                                        RPI_FIRMWARE_ARM_CLK_ID,
 418 -                                        &min_rate);
 419 +       pll = kzalloc(sizeof(*pll), GFP_KERNEL);
 420 +       if (!pll)
 421 +               return -ENOMEM;
 422 +       pll->rpi = rpi;
 423 +       pll->data = data;
 424 +       pll->hw.init = &init;
 425 +
 426 +       ret = raspberrypi_get_clock_range(rpi, data->clock_id, &pll->min_rate, &pll->max_rate);
 427         if (ret) {
 428 -               dev_err(rpi->dev, "Failed to get %s min freq: %d\n",
 429 -                       init.name, ret);
 430 +               dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret);
 431                 return ret;
 432         }
 433
 434 -       ret = raspberrypi_clock_property(rpi->firmware,
 435 -                                        RPI_FIRMWARE_GET_MAX_CLOCK_RATE,
 436 -                                        RPI_FIRMWARE_ARM_CLK_ID,
 437 -                                        &max_rate);
 438 +       ret = devm_clk_hw_register(rpi->dev, &pll->hw);
 439         if (ret) {
 440 -               dev_err(rpi->dev, "Failed to get %s max freq: %d\n",
 441 -                       init.name, ret);
 442 +               dev_err(rpi->dev, "%s: devm_clk_hw_register(%s) failed: %d\n", __func__, init.name, ret);
 443                 return ret;
 444         }
 445 +       return 0;
 446 +}
 447
 448 -       if (!min_rate || !max_rate) {
 449 -               dev_err(rpi->dev, "Unexpected frequency range: min %u, max %u\n",
 450 -                       min_rate, max_rate);
 451 -               return -EINVAL;
 452 -       }
 453 +static int
 454 +raspberrypi_register_pll_divider(struct raspberrypi_clk *rpi,
 455 +                            const struct raspberrypi_pll_divider_data *data)
 456 +{
 457 +       struct raspberrypi_pll_divider *divider;
 458 +       struct clk_init_data init;
 459 +       int ret;
 460 +
 461 +       memset(&init, 0, sizeof(init));
 462 +
 463 +       init.parent_names = &data->source_pll;
 464 +       init.num_parents = 1;
 465 +       init.name = data->name;
 466 +       init.ops = &raspberrypi_firmware_pll_divider_clk_ops;
 467 +       init.flags = data->flags | CLK_IGNORE_UNUSED;
 468
 469 -       dev_info(rpi->dev, "CPU frequency range: min %u, max %u\n",
 470 -                min_rate, max_rate);
 471 +       divider = devm_kzalloc(rpi->dev, sizeof(*divider), GFP_KERNEL);
 472 +       if (!divider)
 473 +               return -ENOMEM;
 474 +
 475 +       divider->div.hw.init = &init;
 476 +       divider->rpi = rpi;
 477 +       divider->data = data;
 478 +
 479 +       ret = raspberrypi_get_clock_range(rpi, data->clock_id, &divider->min_rate, &divider->max_rate);
 480 +       if (ret) {
 481 +               dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret);
 482 +               return ret;
 483 +       }
 484
 485 -       rpi->min_rate = min_rate * RPI_FIRMWARE_PLLB_ARM_DIV_RATE;
 486 -       rpi->max_rate = max_rate * RPI_FIRMWARE_PLLB_ARM_DIV_RATE;
 487 +       ret = devm_clk_hw_register(rpi->dev, &divider->div.hw);
 488 +       if (ret) {
 489 +               dev_err(rpi->dev, "%s: devm_clk_hw_register(%s) failed: %d\n", __func__, init.name, ret);
 490 +               return ret;
 491 +       }
 492
 493 -       rpi->pllb.init = &init;
 494 +       /*
 495 +        * PLLH's channels have a fixed divide by 10 afterwards, which
 496 +        * is what our consumers are actually using.
 497 +        */
 498 +       if (data->fixed_divider != 1) {
 499 +               struct clk_lookup *lookup;
 500 +               struct clk_hw *clk = clk_hw_register_fixed_factor(rpi->dev,
 501 +                                                   data->divider_name,
 502 +                                                   data->name,
 503 +                                                   CLK_SET_RATE_PARENT,
 504 +                                                   1,
 505 +                                                   data->fixed_divider);
 506 +               if (IS_ERR(clk)) {
 507 +                       dev_err(rpi->dev, "%s: clk_hw_register_fixed_factor(%s) failed: %ld\n", __func__, init.name, PTR_ERR(clk));
 508 +                       return PTR_ERR(clk);
 509 +               }
 510 +               if (data->lookup) {
 511 +                       lookup = clkdev_hw_create(clk, NULL, data->lookup);
 512 +                       if (IS_ERR(lookup)) {
 513 +                               dev_err(rpi->dev, "%s: clk_hw_register_fixed_factor(%s) failed: %ld\n", __func__, init.name, PTR_ERR(lookup));
 514 +                               return PTR_ERR(lookup);
 515 +                       }
 516 +               }
 517 +       }
 518
 519 -       return devm_clk_hw_register(rpi->dev, &rpi->pllb);
 520 +       return 0;
 521  }
 522
 523 -static int raspberrypi_register_pllb_arm(struct raspberrypi_clk *rpi)
 524 +static int raspberrypi_register_clock(struct raspberrypi_clk *rpi,
 525 +                                         const struct raspberrypi_clock_data *data)
 526  {
 527 -       rpi->pllb_arm = clk_hw_register_fixed_factor(rpi->dev,
 528 -                               "pllb_arm", "pllb",
 529 -                               CLK_SET_RATE_PARENT | CLK_GET_RATE_NOCACHE,
 530 -                               1, 2);
 531 -       if (IS_ERR(rpi->pllb_arm)) {
 532 -               dev_err(rpi->dev, "Failed to initialize pllb_arm\n");
 533 -               return PTR_ERR(rpi->pllb_arm);
 534 -       }
 535 +       struct raspberrypi_clock *clock;
 536 +       struct clk_init_data init;
 537 +       struct clk *clk;
 538 +       int ret;
 539 +
 540 +       memset(&init, 0, sizeof(init));
 541 +       init.parent_names = data->parents;
 542 +       init.num_parents = data->num_parents;
 543 +       init.name = data->name;
 544 +       init.flags = data->flags | CLK_IGNORE_UNUSED;
 545
 546 -       rpi->pllb_arm_lookup = clkdev_hw_create(rpi->pllb_arm, NULL, "cpu0");
 547 -       if (!rpi->pllb_arm_lookup) {
 548 -               dev_err(rpi->dev, "Failed to initialize pllb_arm_lookup\n");
 549 -               clk_hw_unregister_fixed_factor(rpi->pllb_arm);
 550 +       init.ops = &raspberrypi_firmware_clk_ops;
 551 +
 552 +       clock = devm_kzalloc(rpi->dev, sizeof(*clock), GFP_KERNEL);
 553 +       if (!clock)
 554                 return -ENOMEM;
 555 -       }
 556
 557 +       clock->rpi = rpi;
 558 +       clock->data = data;
 559 +       clock->hw.init = &init;
 560 +
 561 +       ret = raspberrypi_get_clock_range(rpi, data->clock_id, &clock->min_rate, &clock->max_rate);
 562 +       if (ret) {
 563 +               dev_err(rpi->dev, "%s: raspberrypi_get_clock_range(%s) failed: %d\n", __func__, init.name, ret);
 564 +               return ret;
 565 +       }
 566 +       clk = devm_clk_register(rpi->dev, &clock->hw);
 567 +       if (IS_ERR(clk)) {
 568 +               dev_err(rpi->dev, "%s: devm_clk_register(%s) failed: %ld\n", __func__, init.name, PTR_ERR(clk));
 569 +               return PTR_ERR(clk);
 570 +       }
 571 +       ret = clk_register_clkdev(clk, init.name, NULL);
 572 +       if (ret) {
 573 +               dev_err(rpi->dev, "%s: clk_register_clkdev(%s) failed: %d\n", __func__, init.name, ret);
 574 +               return ret;
 575 +       }
 576         return 0;
 577  }
 578
 579 +
 580 +/*
 581 + * the real definition of all the pll, pll_dividers and clocks
 582 + * these make use of the above REGISTER_* macros
 583 + */
 584 +static const struct raspberrypi_clk_desc clk_desc_array[] = {
 585 +       /* the PLL + PLL dividers */
 586 +       [BCM2835_CLOCK_V3D]     = REGISTER_CLK(
 587 +               SOC_ALL,
 588 +               .name = "v3d",
 589 +               .parents = (const char *[]){ "osc" },
 590 +               .num_parents = 1,
 591 +               .clock_id = RPI_FIRMWARE_V3D_CLK_ID),
 592 +       [BCM2835_PLLB_ARM]      = REGISTER_PLL_DIV(
 593 +               SOC_ALL,
 594 +               .name = "pllb",
 595 +               .source_pll = "osc",
 596 +               .divider_name = "pllb_arm",
 597 +               .lookup = "cpu0",
 598 +               .fixed_divider = 2,
 599 +               .clock_id = RPI_FIRMWARE_ARM_CLK_ID,
 600 +               .flags = CLK_SET_RATE_PARENT),
 601 +};
 602 +
 603  static int raspberrypi_clk_probe(struct platform_device *pdev)
 604  {
 605         struct device_node *firmware_node;
 606         struct device *dev = &pdev->dev;
 607         struct rpi_firmware *firmware;
 608         struct raspberrypi_clk *rpi;
 609 -       int ret;
 610 +       const struct raspberrypi_clk_desc *desc;
 611 +       const size_t asize = ARRAY_SIZE(clk_desc_array);
 612 +       int i;
 613
 614         firmware_node = of_find_compatible_node(NULL, NULL,
 615                                         "raspberrypi,bcm2835-firmware");
 616 @@ -275,16 +594,16 @@ static int raspberrypi_clk_probe(struct
 617         rpi->firmware = firmware;
 618         platform_set_drvdata(pdev, rpi);
 619
 620 -       ret = raspberrypi_register_pllb(rpi);
 621 -       if (ret) {
 622 -               dev_err(dev, "Failed to initialize pllb, %d\n", ret);
 623 -               return ret;
 624 +       for (i = 0; i < asize; i++) {
 625 +               desc = &clk_desc_array[i];
 626 +               if (desc->clk_register && desc->data /*&&
 627 +                   (desc->supported & pdata->soc)*/) {
 628 +                       int ret = desc->clk_register(rpi, desc->data);
 629 +                       if (ret)
 630 +                               return ret;
 631 +               }
 632         }
 633
 634 -       ret = raspberrypi_register_pllb_arm(rpi);
 635 -       if (ret)
 636 -               return ret;
 637 -
 638         rpi->cpufreq = platform_device_register_data(dev, "raspberrypi-cpufreq",
 639                                                      -1, NULL, 0);
 640