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