drivers/clk/st/clkgen-pll.c

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