drivers/clk/st/clkgen-pll.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (C) 2014 STMicroelectronics (R&D) Limited
   4  */
   5
   6 /*
   7  * Authors:
   8  * Stephen Gallimore <stephen.gallimore@st.com>,
   9  * Pankaj Dev <pankaj.dev@st.com>.
  10  */
  11
  12 #include <linux/slab.h>
  13 #include <linux/of_address.h>
  14 #include <linux/clk.h>
  15 #include <linux/clk-provider.h>
  16 #include <linux/iopoll.h>
  17
  18 #include "clkgen.h"
  19
  20 static DEFINE_SPINLOCK(clkgena_c32_odf_lock);
  21 DEFINE_SPINLOCK(clkgen_a9_lock);
  22
  23 /*
  24  * PLL configuration register bits for PLL3200 C32
  25  */
  26 #define C32_NDIV_MASK (0xff)
  27 #define C32_IDF_MASK (0x7)
  28 #define C32_ODF_MASK (0x3f)
  29 #define C32_LDF_MASK (0x7f)
  30 #define C32_CP_MASK (0x1f)
  31
  32 #define C32_MAX_ODFS (4)
  33
  34 /*
  35  * PLL configuration register bits for PLL4600 C28
  36  */
  37 #define C28_NDIV_MASK (0xff)
  38 #define C28_IDF_MASK (0x7)
  39 #define C28_ODF_MASK (0x3f)
  40
  41 struct clkgen_pll_data {
  42         struct clkgen_field pdn_status;
  43         struct clkgen_field pdn_ctrl;
  44         struct clkgen_field locked_status;
  45         struct clkgen_field mdiv;
  46         struct clkgen_field ndiv;
  47         struct clkgen_field pdiv;
  48         struct clkgen_field idf;
  49         struct clkgen_field ldf;
  50         struct clkgen_field cp;
  51         unsigned int num_odfs;
  52         struct clkgen_field odf[C32_MAX_ODFS];
  53         struct clkgen_field odf_gate[C32_MAX_ODFS];
  54         bool switch2pll_en;
  55         struct clkgen_field switch2pll;
  56         spinlock_t *lock;
  57         const struct clk_ops *ops;
  58 };
  59
  60 static const struct clk_ops stm_pll3200c32_ops;
  61 static const struct clk_ops stm_pll3200c32_a9_ops;
  62 static const struct clk_ops stm_pll4600c28_ops;
  63
  64 static const struct clkgen_pll_data st_pll3200c32_cx_0 = {
  65         /* 407 C0 PLL0 */
  66         .pdn_status     = CLKGEN_FIELD(0x2a0,   0x1,                    8),
  67         .pdn_ctrl       = CLKGEN_FIELD(0x2a0,   0x1,                    8),
  68         .locked_status  = CLKGEN_FIELD(0x2a0,   0x1,                    24),
  69         .ndiv           = CLKGEN_FIELD(0x2a4,   C32_NDIV_MASK,          16),
  70         .idf            = CLKGEN_FIELD(0x2a4,   C32_IDF_MASK,           0x0),
  71         .num_odfs = 1,
  72         .odf            = { CLKGEN_FIELD(0x2b4, C32_ODF_MASK,           0) },
  73         .odf_gate       = { CLKGEN_FIELD(0x2b4, 0x1,                    6) },
  74         .ops            = &stm_pll3200c32_ops,
  75 };
  76
  77 static const struct clkgen_pll_data st_pll3200c32_cx_1 = {
  78         /* 407 C0 PLL1 */
  79         .pdn_status     = CLKGEN_FIELD(0x2c8,   0x1,                    8),
  80         .pdn_ctrl       = CLKGEN_FIELD(0x2c8,   0x1,                    8),
  81         .locked_status  = CLKGEN_FIELD(0x2c8,   0x1,                    24),
  82         .ndiv           = CLKGEN_FIELD(0x2cc,   C32_NDIV_MASK,          16),
  83         .idf            = CLKGEN_FIELD(0x2cc,   C32_IDF_MASK,           0x0),
  84         .num_odfs = 1,
  85         .odf            = { CLKGEN_FIELD(0x2dc, C32_ODF_MASK,           0) },
  86         .odf_gate       = { CLKGEN_FIELD(0x2dc, 0x1,                    6) },
  87         .ops            = &stm_pll3200c32_ops,
  88 };
  89
  90 static const struct clkgen_pll_data st_pll3200c32_407_a9 = {
  91         /* 407 A9 */
  92         .pdn_status     = CLKGEN_FIELD(0x1a8,   0x1,                    0),
  93         .pdn_ctrl       = CLKGEN_FIELD(0x1a8,   0x1,                    0),
  94         .locked_status  = CLKGEN_FIELD(0x87c,   0x1,                    0),
  95         .ndiv           = CLKGEN_FIELD(0x1b0,   C32_NDIV_MASK,          0),
  96         .idf            = CLKGEN_FIELD(0x1a8,   C32_IDF_MASK,           25),
  97         .num_odfs = 1,
  98         .odf            = { CLKGEN_FIELD(0x1b0, C32_ODF_MASK,           8) },
  99         .odf_gate       = { CLKGEN_FIELD(0x1ac, 0x1,                    28) },
 100         .switch2pll_en  = true,
 101         .cp             = CLKGEN_FIELD(0x1a8,   C32_CP_MASK,            1),
 102         .switch2pll     = CLKGEN_FIELD(0x1a4,   0x1,                    1),
 103         .lock = &clkgen_a9_lock,
 104         .ops            = &stm_pll3200c32_a9_ops,
 105 };
 106
 107 static struct clkgen_pll_data st_pll4600c28_418_a9 = {
 108         /* 418 A9 */
 109         .pdn_status     = CLKGEN_FIELD(0x1a8,   0x1,                    0),
 110         .pdn_ctrl       = CLKGEN_FIELD(0x1a8,   0x1,                    0),
 111         .locked_status  = CLKGEN_FIELD(0x87c,   0x1,                    0),
 112         .ndiv           = CLKGEN_FIELD(0x1b0,   C28_NDIV_MASK,          0),
 113         .idf            = CLKGEN_FIELD(0x1a8,   C28_IDF_MASK,           25),
 114         .num_odfs = 1,
 115         .odf            = { CLKGEN_FIELD(0x1b0, C28_ODF_MASK,           8) },
 116         .odf_gate       = { CLKGEN_FIELD(0x1ac, 0x1,                    28) },
 117         .switch2pll_en  = true,
 118         .switch2pll     = CLKGEN_FIELD(0x1a4,   0x1,                    1),
 119         .lock           = &clkgen_a9_lock,
 120         .ops            = &stm_pll4600c28_ops,
 121 };
 122
 123 /**
 124  * DOC: Clock Generated by PLL, rate set and enabled by bootloader
 125  *
 126  * Traits of this clock:
 127  * prepare - clk_(un)prepare only ensures parent is (un)prepared
 128  * enable - clk_enable/disable only ensures parent is enabled
 129  * rate - rate is fixed. No clk_set_rate support
 130  * parent - fixed parent.  No clk_set_parent support
 131  */
 132
 133 /*
 134  * PLL clock that is integrated in the ClockGenA instances on the STiH415
 135  * and STiH416.
 136  *
 137  * @hw: handle between common and hardware-specific interfaces.
 138  * @regs_base: base of the PLL configuration register(s).
 139  *
 140  */
 141 struct clkgen_pll {
 142         struct clk_hw           hw;
 143         struct clkgen_pll_data  *data;
 144         void __iomem            *regs_base;
 145         spinlock_t      *lock;
 146
 147         u32 ndiv;
 148         u32 idf;
 149         u32 odf;
 150         u32 cp;
 151 };
 152
 153 #define to_clkgen_pll(_hw) container_of(_hw, struct clkgen_pll, hw)
 154
 155 struct stm_pll {
 156         unsigned long mdiv;
 157         unsigned long ndiv;
 158         unsigned long pdiv;
 159         unsigned long odf;
 160         unsigned long idf;
 161         unsigned long ldf;
 162         unsigned long cp;
 163 };
 164
 165 static int clkgen_pll_is_locked(struct clk_hw *hw)
 166 {
 167         struct clkgen_pll *pll = to_clkgen_pll(hw);
 168         u32 locked = CLKGEN_READ(pll, locked_status);
 169
 170         return !!locked;
 171 }
 172
 173 static int clkgen_pll_is_enabled(struct clk_hw *hw)
 174 {
 175         struct clkgen_pll *pll = to_clkgen_pll(hw);
 176         u32 poweroff = CLKGEN_READ(pll, pdn_status);
 177         return !poweroff;
 178 }
 179
 180 static int __clkgen_pll_enable(struct clk_hw *hw)
 181 {
 182         struct clkgen_pll *pll = to_clkgen_pll(hw);
 183         void __iomem *base =  pll->regs_base;
 184         struct clkgen_field *field = &pll->data->locked_status;
 185         int ret = 0;
 186         u32 reg;
 187
 188         if (clkgen_pll_is_enabled(hw))
 189                 return 0;
 190
 191         CLKGEN_WRITE(pll, pdn_ctrl, 0);
 192
 193         ret = readl_relaxed_poll_timeout(base + field->offset, reg,
 194                         !!((reg >> field->shift) & field->mask),  0, 10000);
 195
 196         if (!ret) {
 197                 if (pll->data->switch2pll_en)
 198                         CLKGEN_WRITE(pll, switch2pll, 0);
 199
 200                 pr_debug("%s:%s enabled\n", __clk_get_name(hw->clk), __func__);
 201         }
 202
 203         return ret;
 204 }
 205
 206 static int clkgen_pll_enable(struct clk_hw *hw)
 207 {
 208         struct clkgen_pll *pll = to_clkgen_pll(hw);
 209         unsigned long flags = 0;
 210         int ret = 0;
 211
 212         if (pll->lock)
 213                 spin_lock_irqsave(pll->lock, flags);
 214
 215         ret = __clkgen_pll_enable(hw);
 216
 217         if (pll->lock)
 218                 spin_unlock_irqrestore(pll->lock, flags);
 219
 220         return ret;
 221 }
 222
 223 static void __clkgen_pll_disable(struct clk_hw *hw)
 224 {
 225         struct clkgen_pll *pll = to_clkgen_pll(hw);
 226
 227         if (!clkgen_pll_is_enabled(hw))
 228                 return;
 229
 230         if (pll->data->switch2pll_en)
 231                 CLKGEN_WRITE(pll, switch2pll, 1);
 232
 233         CLKGEN_WRITE(pll, pdn_ctrl, 1);
 234
 235         pr_debug("%s:%s disabled\n", __clk_get_name(hw->clk), __func__);
 236 }
 237
 238 static void clkgen_pll_disable(struct clk_hw *hw)
 239 {
 240         struct clkgen_pll *pll = to_clkgen_pll(hw);
 241         unsigned long flags = 0;
 242
 243         if (pll->lock)
 244                 spin_lock_irqsave(pll->lock, flags);
 245
 246         __clkgen_pll_disable(hw);
 247
 248         if (pll->lock)
 249                 spin_unlock_irqrestore(pll->lock, flags);
 250 }
 251
 252 static int clk_pll3200c32_get_params(unsigned long input, unsigned long output,
 253                           struct stm_pll *pll)
 254 {
 255         unsigned long i, n;
 256         unsigned long deviation = ~0;
 257         unsigned long new_freq;
 258         long new_deviation;
 259         /* Charge pump table: highest ndiv value for cp=6 to 25 */
 260         static const unsigned char cp_table[] = {
 261                 48, 56, 64, 72, 80, 88, 96, 104, 112, 120,
 262                 128, 136, 144, 152, 160, 168, 176, 184, 192
 263         };
 264
 265         /* Output clock range: 800Mhz to 1600Mhz */
 266         if (output < 800000000 || output > 1600000000)
 267                 return -EINVAL;
 268
 269         input /= 1000;
 270         output /= 1000;
 271
 272         for (i = 1; i <= 7 && deviation; i++) {
 273                 n = i * output / (2 * input);
 274
 275                 /* Checks */
 276                 if (n < 8)
 277                         continue;
 278                 if (n > 200)
 279                         break;
 280
 281                 new_freq = (input * 2 * n) / i;
 282
 283                 new_deviation = abs(new_freq - output);
 284
 285                 if (!new_deviation || new_deviation < deviation) {
 286                         pll->idf  = i;
 287                         pll->ndiv = n;
 288                         deviation = new_deviation;
 289                 }
 290         }
 291
 292         if (deviation == ~0) /* No solution found */
 293                 return -EINVAL;
 294
 295         /* Computing recommended charge pump value */
 296         for (pll->cp = 6; pll->ndiv > cp_table[pll->cp-6]; (pll->cp)++)
 297                 ;
 298
 299         return 0;
 300 }
 301
 302 static int clk_pll3200c32_get_rate(unsigned long input, struct stm_pll *pll,
 303                         unsigned long *rate)
 304 {
 305         if (!pll->idf)
 306                 pll->idf = 1;
 307
 308         *rate = ((2 * (input / 1000) * pll->ndiv) / pll->idf) * 1000;
 309
 310         return 0;
 311 }
 312
 313 static unsigned long recalc_stm_pll3200c32(struct clk_hw *hw,
 314                 unsigned long parent_rate)
 315 {
 316         struct clkgen_pll *pll = to_clkgen_pll(hw);
 317         unsigned long ndiv, idf;
 318         unsigned long rate = 0;
 319
 320         if (!clkgen_pll_is_enabled(hw) || !clkgen_pll_is_locked(hw))
 321                 return 0;
 322
 323         ndiv = CLKGEN_READ(pll, ndiv);
 324         idf = CLKGEN_READ(pll, idf);
 325
 326         if (idf)
 327                 /* Note: input is divided to avoid overflow */
 328                 rate = ((2 * (parent_rate/1000) * ndiv) / idf) * 1000;
 329
 330         pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
 331
 332         return rate;
 333 }
 334
 335 static long round_rate_stm_pll3200c32(struct clk_hw *hw, unsigned long rate,
 336                 unsigned long *prate)
 337 {
 338         struct stm_pll params;
 339
 340         if (!clk_pll3200c32_get_params(*prate, rate, &params))
 341                 clk_pll3200c32_get_rate(*prate, &params, &rate);
 342         else {
 343                 pr_debug("%s: %s rate %ld Invalid\n", __func__,
 344                          __clk_get_name(hw->clk), rate);
 345                 return 0;
 346         }
 347
 348         pr_debug("%s: %s new rate %ld [ndiv=%u] [idf=%u]\n",
 349                  __func__, __clk_get_name(hw->clk),
 350                  rate, (unsigned int)params.ndiv,
 351                  (unsigned int)params.idf);
 352
 353         return rate;
 354 }
 355
 356 static int set_rate_stm_pll3200c32(struct clk_hw *hw, unsigned long rate,
 357                                 unsigned long parent_rate)
 358 {
 359         struct clkgen_pll *pll = to_clkgen_pll(hw);
 360         struct stm_pll params;
 361         long hwrate = 0;
 362         unsigned long flags = 0;
 363
 364         if (!rate || !parent_rate)
 365                 return -EINVAL;
 366
 367         if (!clk_pll3200c32_get_params(parent_rate, rate, &params))
 368                 clk_pll3200c32_get_rate(parent_rate, &params, &hwrate);
 369
 370         pr_debug("%s: %s new rate %ld [ndiv=0x%x] [idf=0x%x]\n",
 371                  __func__, __clk_get_name(hw->clk),
 372                  hwrate, (unsigned int)params.ndiv,
 373                  (unsigned int)params.idf);
 374
 375         if (!hwrate)
 376                 return -EINVAL;
 377
 378         pll->ndiv = params.ndiv;
 379         pll->idf = params.idf;
 380         pll->cp = params.cp;
 381
 382         __clkgen_pll_disable(hw);
 383
 384         if (pll->lock)
 385                 spin_lock_irqsave(pll->lock, flags);
 386
 387         CLKGEN_WRITE(pll, ndiv, pll->ndiv);
 388         CLKGEN_WRITE(pll, idf, pll->idf);
 389         CLKGEN_WRITE(pll, cp, pll->cp);
 390
 391         if (pll->lock)
 392                 spin_unlock_irqrestore(pll->lock, flags);
 393
 394         __clkgen_pll_enable(hw);
 395
 396         return 0;
 397 }
 398
 399 /* PLL output structure
 400  * FVCO >> /2 >> FVCOBY2 (no output)
 401  *                 |> Divider (ODF) >> PHI
 402  *
 403  * FVCOby2 output = (input * 2 * NDIV) / IDF (assuming FRAC_CONTROL==L)
 404  *
 405  * Rules:
 406  *   4Mhz <= INFF input <= 350Mhz
 407  *   4Mhz <= INFIN (INFF / IDF) <= 50Mhz
 408  *   19.05Mhz <= FVCOby2 output (PHI w ODF=1) <= 3000Mhz
 409  *   1 <= i (register/dec value for IDF) <= 7
 410  *   8 <= n (register/dec value for NDIV) <= 246
 411  */
 412
 413 static int clk_pll4600c28_get_params(unsigned long input, unsigned long output,
 414                           struct stm_pll *pll)
 415 {
 416
 417         unsigned long i, infin, n;
 418         unsigned long deviation = ~0;
 419         unsigned long new_freq, new_deviation;
 420
 421         /* Output clock range: 19Mhz to 3000Mhz */
 422         if (output < 19000000 || output > 3000000000u)
 423                 return -EINVAL;
 424
 425         /* For better jitter, IDF should be smallest and NDIV must be maximum */
 426         for (i = 1; i <= 7 && deviation; i++) {
 427                 /* INFIN checks */
 428                 infin = input / i;
 429                 if (infin < 4000000 || infin > 50000000)
 430                         continue;       /* Invalid case */
 431
 432                 n = output / (infin * 2);
 433                 if (n < 8 || n > 246)
 434                         continue;       /* Invalid case */
 435                 if (n < 246)
 436                         n++;    /* To work around 'y' when n=x.y */
 437
 438                 for (; n >= 8 && deviation; n--) {
 439                         new_freq = infin * 2 * n;
 440                         if (new_freq < output)
 441                                 break;  /* Optimization: shorting loop */
 442
 443                         new_deviation = new_freq - output;
 444                         if (!new_deviation || new_deviation < deviation) {
 445                                 pll->idf  = i;
 446                                 pll->ndiv = n;
 447                                 deviation = new_deviation;
 448                         }
 449                 }
 450         }
 451
 452         if (deviation == ~0) /* No solution found */
 453                 return -EINVAL;
 454
 455         return 0;
 456 }
 457
 458 static int clk_pll4600c28_get_rate(unsigned long input, struct stm_pll *pll,
 459                         unsigned long *rate)
 460 {
 461         if (!pll->idf)
 462                 pll->idf = 1;
 463
 464         *rate = (input / pll->idf) * 2 * pll->ndiv;
 465
 466         return 0;
 467 }
 468
 469 static unsigned long recalc_stm_pll4600c28(struct clk_hw *hw,
 470                                     unsigned long parent_rate)
 471 {
 472         struct clkgen_pll *pll = to_clkgen_pll(hw);
 473         struct stm_pll params;
 474         unsigned long rate;
 475
 476         if (!clkgen_pll_is_enabled(hw) || !clkgen_pll_is_locked(hw))
 477                 return 0;
 478
 479         params.ndiv = CLKGEN_READ(pll, ndiv);
 480         params.idf = CLKGEN_READ(pll, idf);
 481
 482         clk_pll4600c28_get_rate(parent_rate, &params, &rate);
 483
 484         pr_debug("%s:%s rate %lu\n", __clk_get_name(hw->clk), __func__, rate);
 485
 486         return rate;
 487 }
 488
 489 static long round_rate_stm_pll4600c28(struct clk_hw *hw, unsigned long rate,
 490                                       unsigned long *prate)
 491 {
 492         struct stm_pll params;
 493
 494         if (!clk_pll4600c28_get_params(*prate, rate, &params)) {
 495                 clk_pll4600c28_get_rate(*prate, &params, &rate);
 496         } else {
 497                 pr_debug("%s: %s rate %ld Invalid\n", __func__,
 498                          __clk_get_name(hw->clk), rate);
 499                 return 0;
 500         }
 501
 502         pr_debug("%s: %s new rate %ld [ndiv=%u] [idf=%u]\n",
 503                  __func__, __clk_get_name(hw->clk),
 504                  rate, (unsigned int)params.ndiv,
 505                  (unsigned int)params.idf);
 506
 507         return rate;
 508 }
 509
 510 static int set_rate_stm_pll4600c28(struct clk_hw *hw, unsigned long rate,
 511                                    unsigned long parent_rate)
 512 {
 513         struct clkgen_pll *pll = to_clkgen_pll(hw);
 514         struct stm_pll params;
 515         long hwrate;
 516         unsigned long flags = 0;
 517
 518         if (!rate || !parent_rate)
 519                 return -EINVAL;
 520
 521         if (!clk_pll4600c28_get_params(parent_rate, rate, &params)) {
 522                 clk_pll4600c28_get_rate(parent_rate, &params, &hwrate);
 523         } else {
 524                 pr_debug("%s: %s rate %ld Invalid\n", __func__,
 525                          __clk_get_name(hw->clk), rate);
 526                 return -EINVAL;
 527         }
 528
 529         pr_debug("%s: %s new rate %ld [ndiv=0x%x] [idf=0x%x]\n",
 530                  __func__, __clk_get_name(hw->clk),
 531                  hwrate, (unsigned int)params.ndiv,
 532                  (unsigned int)params.idf);
 533
 534         if (!hwrate)
 535                 return -EINVAL;
 536
 537         pll->ndiv = params.ndiv;
 538         pll->idf = params.idf;
 539
 540         __clkgen_pll_disable(hw);
 541
 542         if (pll->lock)
 543                 spin_lock_irqsave(pll->lock, flags);
 544
 545         CLKGEN_WRITE(pll, ndiv, pll->ndiv);
 546         CLKGEN_WRITE(pll, idf, pll->idf);
 547
 548         if (pll->lock)
 549                 spin_unlock_irqrestore(pll->lock, flags);
 550
 551         __clkgen_pll_enable(hw);
 552
 553         return 0;
 554 }
 555
 556 static const struct clk_ops stm_pll3200c32_ops = {
 557         .enable         = clkgen_pll_enable,
 558         .disable        = clkgen_pll_disable,
 559         .is_enabled     = clkgen_pll_is_enabled,
 560         .recalc_rate    = recalc_stm_pll3200c32,
 561 };
 562
 563 static const struct clk_ops stm_pll3200c32_a9_ops = {
 564         .enable         = clkgen_pll_enable,
 565         .disable        = clkgen_pll_disable,
 566         .is_enabled     = clkgen_pll_is_enabled,
 567         .recalc_rate    = recalc_stm_pll3200c32,
 568         .round_rate     = round_rate_stm_pll3200c32,
 569         .set_rate       = set_rate_stm_pll3200c32,
 570 };
 571
 572 static const struct clk_ops stm_pll4600c28_ops = {
 573         .enable         = clkgen_pll_enable,
 574         .disable        = clkgen_pll_disable,
 575         .is_enabled     = clkgen_pll_is_enabled,
 576         .recalc_rate    = recalc_stm_pll4600c28,
 577         .round_rate     = round_rate_stm_pll4600c28,
 578         .set_rate       = set_rate_stm_pll4600c28,
 579 };
 580
 581 static struct clk * __init clkgen_pll_register(const char *parent_name,
 582                                 struct clkgen_pll_data  *pll_data,
 583                                 void __iomem *reg, unsigned long pll_flags,
 584                                 const char *clk_name, spinlock_t *lock)
 585 {
 586         struct clkgen_pll *pll;
 587         struct clk *clk;
 588         struct clk_init_data init;
 589
 590         pll = kzalloc(sizeof(*pll), GFP_KERNEL);
 591         if (!pll)
 592                 return ERR_PTR(-ENOMEM);
 593
 594         init.name = clk_name;
 595         init.ops = pll_data->ops;
 596
 597         init.flags = pll_flags | CLK_GET_RATE_NOCACHE;
 598         init.parent_names = &parent_name;
 599         init.num_parents  = 1;
 600
 601         pll->data = pll_data;
 602         pll->regs_base = reg;
 603         pll->hw.init = &init;
 604         pll->lock = lock;
 605
 606         clk = clk_register(NULL, &pll->hw);
 607         if (IS_ERR(clk)) {
 608                 kfree(pll);
 609                 return clk;
 610         }
 611
 612         pr_debug("%s: parent %s rate %lu\n",
 613                         __clk_get_name(clk),
 614                         __clk_get_name(clk_get_parent(clk)),
 615                         clk_get_rate(clk));
 616
 617         return clk;
 618 }
 619
 620 static void __iomem * __init clkgen_get_register_base(
 621                                 struct device_node *np)
 622 {
 623         struct device_node *pnode;
 624         void __iomem *reg = NULL;
 625
 626         pnode = of_get_parent(np);
 627         if (!pnode)
 628                 return NULL;
 629
 630         reg = of_iomap(pnode, 0);
 631
 632         of_node_put(pnode);
 633         return reg;
 634 }
 635
 636 static struct clk * __init clkgen_odf_register(const char *parent_name,
 637                                                void __iomem *reg,
 638                                                struct clkgen_pll_data *pll_data,
 639                                                unsigned long pll_flags, int odf,
 640                                                spinlock_t *odf_lock,
 641                                                const char *odf_name)
 642 {
 643         struct clk *clk;
 644         unsigned long flags;
 645         struct clk_gate *gate;
 646         struct clk_divider *div;
 647
 648         flags = pll_flags | CLK_GET_RATE_NOCACHE | CLK_SET_RATE_PARENT;
 649
 650         gate = kzalloc(sizeof(*gate), GFP_KERNEL);
 651         if (!gate)
 652                 return ERR_PTR(-ENOMEM);
 653
 654         gate->flags = CLK_GATE_SET_TO_DISABLE;
 655         gate->reg = reg + pll_data->odf_gate[odf].offset;
 656         gate->bit_idx = pll_data->odf_gate[odf].shift;
 657         gate->lock = odf_lock;
 658
 659         div = kzalloc(sizeof(*div), GFP_KERNEL);
 660         if (!div) {
 661                 kfree(gate);
 662                 return ERR_PTR(-ENOMEM);
 663         }
 664
 665         div->flags = CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO;
 666         div->reg = reg + pll_data->odf[odf].offset;
 667         div->shift = pll_data->odf[odf].shift;
 668         div->width = fls(pll_data->odf[odf].mask);
 669         div->lock = odf_lock;
 670
 671         clk = clk_register_composite(NULL, odf_name, &parent_name, 1,
 672                                      NULL, NULL,
 673                                      &div->hw, &clk_divider_ops,
 674                                      &gate->hw, &clk_gate_ops,
 675                                      flags);
 676         if (IS_ERR(clk))
 677                 return clk;
 678
 679         pr_debug("%s: parent %s rate %lu\n",
 680                         __clk_get_name(clk),
 681                         __clk_get_name(clk_get_parent(clk)),
 682                         clk_get_rate(clk));
 683         return clk;
 684 }
 685
 686
 687 static void __init clkgen_c32_pll_setup(struct device_node *np,
 688                 struct clkgen_pll_data *data)
 689 {
 690         struct clk *clk;
 691         const char *parent_name, *pll_name;
 692         void __iomem *pll_base;
 693         int num_odfs, odf;
 694         struct clk_onecell_data *clk_data;
 695         unsigned long pll_flags = 0;
 696
 697
 698         parent_name = of_clk_get_parent_name(np, 0);
 699         if (!parent_name)
 700                 return;
 701
 702         pll_base = clkgen_get_register_base(np);
 703         if (!pll_base)
 704                 return;
 705
 706         of_clk_detect_critical(np, 0, &pll_flags);
 707
 708         clk = clkgen_pll_register(parent_name, data, pll_base, pll_flags,
 709                                   np->name, data->lock);
 710         if (IS_ERR(clk))
 711                 return;
 712
 713         pll_name = __clk_get_name(clk);
 714
 715         num_odfs = data->num_odfs;
 716
 717         clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
 718         if (!clk_data)
 719                 return;
 720
 721         clk_data->clk_num = num_odfs;
 722         clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
 723                                  GFP_KERNEL);
 724
 725         if (!clk_data->clks)
 726                 goto err;
 727
 728         for (odf = 0; odf < num_odfs; odf++) {
 729                 struct clk *clk;
 730                 const char *clk_name;
 731                 unsigned long odf_flags = 0;
 732
 733                 if (of_property_read_string_index(np, "clock-output-names",
 734                                                   odf, &clk_name))
 735                         return;
 736
 737                 of_clk_detect_critical(np, odf, &odf_flags);
 738
 739                 clk = clkgen_odf_register(pll_name, pll_base, data, odf_flags,
 740                                 odf, &clkgena_c32_odf_lock, clk_name);
 741                 if (IS_ERR(clk))
 742                         goto err;
 743
 744                 clk_data->clks[odf] = clk;
 745         }
 746
 747         of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
 748         return;
 749
 750 err:
 751         kfree(pll_name);
 752         kfree(clk_data->clks);
 753         kfree(clk_data);
 754 }
 755 static void __init clkgen_c32_pll0_setup(struct device_node *np)
 756 {
 757         clkgen_c32_pll_setup(np,
 758                         (struct clkgen_pll_data *) &st_pll3200c32_cx_0);
 759 }
 760 CLK_OF_DECLARE(c32_pll0, "st,clkgen-pll0", clkgen_c32_pll0_setup);
 761
 762 static void __init clkgen_c32_pll1_setup(struct device_node *np)
 763 {
 764         clkgen_c32_pll_setup(np,
 765                         (struct clkgen_pll_data *) &st_pll3200c32_cx_1);
 766 }
 767 CLK_OF_DECLARE(c32_pll1, "st,clkgen-pll1", clkgen_c32_pll1_setup);
 768
 769 static void __init clkgen_c32_plla9_setup(struct device_node *np)
 770 {
 771         clkgen_c32_pll_setup(np,
 772                         (struct clkgen_pll_data *) &st_pll3200c32_407_a9);
 773 }
 774 CLK_OF_DECLARE(c32_plla9, "st,stih407-clkgen-plla9", clkgen_c32_plla9_setup);
 775
 776 static void __init clkgen_c28_plla9_setup(struct device_node *np)
 777 {
 778         clkgen_c32_pll_setup(np,
 779                         (struct clkgen_pll_data *) &st_pll4600c28_418_a9);
 780 }
 781 CLK_OF_DECLARE(c28_plla9, "st,stih418-clkgen-plla9", clkgen_c28_plla9_setup);