drivers/clk/bcm/clk-iproc-pll.c

   1 /*
   2  * Copyright (C) 2014 Broadcom Corporation
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License as
   6  * published by the Free Software Foundation version 2.
   7  *
   8  * This program is distributed "as is" WITHOUT ANY WARRANTY of any
   9  * kind, whether express or implied; without even the implied warranty
  10  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  */
  13
  14 #include <linux/kernel.h>
  15 #include <linux/err.h>
  16 #include <linux/clk-provider.h>
  17 #include <linux/io.h>
  18 #include <linux/of.h>
  19 #include <linux/clkdev.h>
  20 #include <linux/of_address.h>
  21 #include <linux/delay.h>
  22
  23 #include "clk-iproc.h"
  24
  25 #define PLL_VCO_HIGH_SHIFT 19
  26 #define PLL_VCO_LOW_SHIFT  30
  27
  28 /*
  29  * PLL MACRO_SELECT modes 0 to 5 choose pre-calculated PLL output frequencies
  30  * from a look-up table. Mode 7 allows user to manipulate PLL clock dividers
  31  */
  32 #define PLL_USER_MODE 7
  33
  34 /* number of delay loops waiting for PLL to lock */
  35 #define LOCK_DELAY 100
  36
  37 /* number of VCO frequency bands */
  38 #define NUM_FREQ_BANDS 8
  39
  40 #define NUM_KP_BANDS 3
  41 enum kp_band {
  42         KP_BAND_MID = 0,
  43         KP_BAND_HIGH,
  44         KP_BAND_HIGH_HIGH
  45 };
  46
  47 static const unsigned int kp_table[NUM_KP_BANDS][NUM_FREQ_BANDS] = {
  48         { 5, 6, 6, 7, 7, 8, 9, 10 },
  49         { 4, 4, 5, 5, 6, 7, 8, 9  },
  50         { 4, 5, 5, 6, 7, 8, 9, 10 },
  51 };
  52
  53 static const unsigned long ref_freq_table[NUM_FREQ_BANDS][2] = {
  54         { 10000000,  12500000  },
  55         { 12500000,  15000000  },
  56         { 15000000,  20000000  },
  57         { 20000000,  25000000  },
  58         { 25000000,  50000000  },
  59         { 50000000,  75000000  },
  60         { 75000000,  100000000 },
  61         { 100000000, 125000000 },
  62 };
  63
  64 enum vco_freq_range {
  65         VCO_LOW       = 700000000U,
  66         VCO_MID       = 1200000000U,
  67         VCO_HIGH      = 2200000000U,
  68         VCO_HIGH_HIGH = 3100000000U,
  69         VCO_MAX       = 4000000000U,
  70 };
  71
  72 struct iproc_pll {
  73         void __iomem *status_base;
  74         void __iomem *control_base;
  75         void __iomem *pwr_base;
  76         void __iomem *asiu_base;
  77
  78         const struct iproc_pll_ctrl *ctrl;
  79         const struct iproc_pll_vco_param *vco_param;
  80         unsigned int num_vco_entries;
  81 };
  82
  83 struct iproc_clk {
  84         struct clk_hw hw;
  85         struct iproc_pll *pll;
  86         const struct iproc_clk_ctrl *ctrl;
  87 };
  88
  89 #define to_iproc_clk(hw) container_of(hw, struct iproc_clk, hw)
  90
  91 /*
  92  * Based on the target frequency, find a match from the VCO frequency parameter
  93  * table and return its index
  94  */
  95 static int pll_get_rate_index(struct iproc_pll *pll, unsigned int target_rate)
  96 {
  97         int i;
  98
  99         for (i = 0; i < pll->num_vco_entries; i++)
 100                 if (target_rate == pll->vco_param[i].rate)
 101                         break;
 102
 103         if (i >= pll->num_vco_entries)
 104                 return -EINVAL;
 105
 106         return i;
 107 }
 108
 109 static int get_kp(unsigned long ref_freq, enum kp_band kp_index)
 110 {
 111         int i;
 112
 113         if (ref_freq < ref_freq_table[0][0])
 114                 return -EINVAL;
 115
 116         for (i = 0; i < NUM_FREQ_BANDS; i++) {
 117                 if (ref_freq >= ref_freq_table[i][0] &&
 118                     ref_freq < ref_freq_table[i][1])
 119                         return kp_table[kp_index][i];
 120         }
 121         return -EINVAL;
 122 }
 123
 124 static int pll_wait_for_lock(struct iproc_pll *pll)
 125 {
 126         int i;
 127         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 128
 129         for (i = 0; i < LOCK_DELAY; i++) {
 130                 u32 val = readl(pll->status_base + ctrl->status.offset);
 131
 132                 if (val & (1 << ctrl->status.shift))
 133                         return 0;
 134                 udelay(10);
 135         }
 136
 137         return -EIO;
 138 }
 139
 140 static void iproc_pll_write(const struct iproc_pll *pll, void __iomem *base,
 141                             const u32 offset, u32 val)
 142 {
 143         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 144
 145         writel(val, base + offset);
 146
 147         if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK &&
 148                      (base == pll->status_base || base == pll->control_base)))
 149                 val = readl(base + offset);
 150 }
 151
 152 static void __pll_disable(struct iproc_pll *pll)
 153 {
 154         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 155         u32 val;
 156
 157         if (ctrl->flags & IPROC_CLK_PLL_ASIU) {
 158                 val = readl(pll->asiu_base + ctrl->asiu.offset);
 159                 val &= ~(1 << ctrl->asiu.en_shift);
 160                 iproc_pll_write(pll, pll->asiu_base, ctrl->asiu.offset, val);
 161         }
 162
 163         if (ctrl->flags & IPROC_CLK_EMBED_PWRCTRL) {
 164                 val = readl(pll->control_base + ctrl->aon.offset);
 165                 val |= bit_mask(ctrl->aon.pwr_width) << ctrl->aon.pwr_shift;
 166                 iproc_pll_write(pll, pll->control_base, ctrl->aon.offset, val);
 167         }
 168
 169         if (pll->pwr_base) {
 170                 /* latch input value so core power can be shut down */
 171                 val = readl(pll->pwr_base + ctrl->aon.offset);
 172                 val |= 1 << ctrl->aon.iso_shift;
 173                 iproc_pll_write(pll, pll->pwr_base, ctrl->aon.offset, val);
 174
 175                 /* power down the core */
 176                 val &= ~(bit_mask(ctrl->aon.pwr_width) << ctrl->aon.pwr_shift);
 177                 iproc_pll_write(pll, pll->pwr_base, ctrl->aon.offset, val);
 178         }
 179 }
 180
 181 static int __pll_enable(struct iproc_pll *pll)
 182 {
 183         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 184         u32 val;
 185
 186         if (ctrl->flags & IPROC_CLK_EMBED_PWRCTRL) {
 187                 val = readl(pll->control_base + ctrl->aon.offset);
 188                 val &= ~(bit_mask(ctrl->aon.pwr_width) << ctrl->aon.pwr_shift);
 189                 iproc_pll_write(pll, pll->control_base, ctrl->aon.offset, val);
 190         }
 191
 192         if (pll->pwr_base) {
 193                 /* power up the PLL and make sure it's not latched */
 194                 val = readl(pll->pwr_base + ctrl->aon.offset);
 195                 val |= bit_mask(ctrl->aon.pwr_width) << ctrl->aon.pwr_shift;
 196                 val &= ~(1 << ctrl->aon.iso_shift);
 197                 iproc_pll_write(pll, pll->pwr_base, ctrl->aon.offset, val);
 198         }
 199
 200         /* certain PLLs also need to be ungated from the ASIU top level */
 201         if (ctrl->flags & IPROC_CLK_PLL_ASIU) {
 202                 val = readl(pll->asiu_base + ctrl->asiu.offset);
 203                 val |= (1 << ctrl->asiu.en_shift);
 204                 iproc_pll_write(pll, pll->asiu_base, ctrl->asiu.offset, val);
 205         }
 206
 207         return 0;
 208 }
 209
 210 static void __pll_put_in_reset(struct iproc_pll *pll)
 211 {
 212         u32 val;
 213         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 214         const struct iproc_pll_reset_ctrl *reset = &ctrl->reset;
 215
 216         val = readl(pll->control_base + reset->offset);
 217         if (ctrl->flags & IPROC_CLK_PLL_RESET_ACTIVE_LOW)
 218                 val |= BIT(reset->reset_shift) | BIT(reset->p_reset_shift);
 219         else
 220                 val &= ~(BIT(reset->reset_shift) | BIT(reset->p_reset_shift));
 221         iproc_pll_write(pll, pll->control_base, reset->offset, val);
 222 }
 223
 224 static void __pll_bring_out_reset(struct iproc_pll *pll, unsigned int kp,
 225                                   unsigned int ka, unsigned int ki)
 226 {
 227         u32 val;
 228         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 229         const struct iproc_pll_reset_ctrl *reset = &ctrl->reset;
 230         const struct iproc_pll_dig_filter_ctrl *dig_filter = &ctrl->dig_filter;
 231
 232         val = readl(pll->control_base + dig_filter->offset);
 233         val &= ~(bit_mask(dig_filter->ki_width) << dig_filter->ki_shift |
 234                 bit_mask(dig_filter->kp_width) << dig_filter->kp_shift |
 235                 bit_mask(dig_filter->ka_width) << dig_filter->ka_shift);
 236         val |= ki << dig_filter->ki_shift | kp << dig_filter->kp_shift |
 237                ka << dig_filter->ka_shift;
 238         iproc_pll_write(pll, pll->control_base, dig_filter->offset, val);
 239
 240         val = readl(pll->control_base + reset->offset);
 241         if (ctrl->flags & IPROC_CLK_PLL_RESET_ACTIVE_LOW)
 242                 val &= ~(BIT(reset->reset_shift) | BIT(reset->p_reset_shift));
 243         else
 244                 val |= BIT(reset->reset_shift) | BIT(reset->p_reset_shift);
 245         iproc_pll_write(pll, pll->control_base, reset->offset, val);
 246 }
 247
 248 static int pll_set_rate(struct iproc_clk *clk, unsigned int rate_index,
 249                         unsigned long parent_rate)
 250 {
 251         struct iproc_pll *pll = clk->pll;
 252         const struct iproc_pll_vco_param *vco = &pll->vco_param[rate_index];
 253         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 254         int ka = 0, ki, kp, ret;
 255         unsigned long rate = vco->rate;
 256         u32 val;
 257         enum kp_band kp_index;
 258         unsigned long ref_freq;
 259         const char *clk_name = clk_hw_get_name(&clk->hw);
 260
 261         /*
 262          * reference frequency = parent frequency / PDIV
 263          * If PDIV = 0, then it becomes a multiplier (x2)
 264          */
 265         if (vco->pdiv == 0)
 266                 ref_freq = parent_rate * 2;
 267         else
 268                 ref_freq = parent_rate / vco->pdiv;
 269
 270         /* determine Ki and Kp index based on target VCO frequency */
 271         if (rate >= VCO_LOW && rate < VCO_HIGH) {
 272                 ki = 4;
 273                 kp_index = KP_BAND_MID;
 274         } else if (rate >= VCO_HIGH && rate && rate < VCO_HIGH_HIGH) {
 275                 ki = 3;
 276                 kp_index = KP_BAND_HIGH;
 277         } else if (rate >= VCO_HIGH_HIGH && rate < VCO_MAX) {
 278                 ki = 3;
 279                 kp_index = KP_BAND_HIGH_HIGH;
 280         } else {
 281                 pr_err("%s: pll: %s has invalid rate: %lu\n", __func__,
 282                                 clk_name, rate);
 283                 return -EINVAL;
 284         }
 285
 286         kp = get_kp(ref_freq, kp_index);
 287         if (kp < 0) {
 288                 pr_err("%s: pll: %s has invalid kp\n", __func__, clk_name);
 289                 return kp;
 290         }
 291
 292         ret = __pll_enable(pll);
 293         if (ret) {
 294                 pr_err("%s: pll: %s fails to enable\n", __func__, clk_name);
 295                 return ret;
 296         }
 297
 298         /* put PLL in reset */
 299         __pll_put_in_reset(pll);
 300
 301         /* set PLL in user mode before modifying PLL controls */
 302         if (ctrl->flags & IPROC_CLK_PLL_USER_MODE_ON) {
 303                 val = readl(pll->control_base + ctrl->macro_mode.offset);
 304                 val &= ~(bit_mask(ctrl->macro_mode.width) <<
 305                         ctrl->macro_mode.shift);
 306                 val |= PLL_USER_MODE << ctrl->macro_mode.shift;
 307                 iproc_pll_write(pll, pll->control_base,
 308                         ctrl->macro_mode.offset, val);
 309         }
 310
 311         iproc_pll_write(pll, pll->control_base, ctrl->vco_ctrl.u_offset, 0);
 312
 313         val = readl(pll->control_base + ctrl->vco_ctrl.l_offset);
 314
 315         if (rate >= VCO_LOW && rate < VCO_MID)
 316                 val |= (1 << PLL_VCO_LOW_SHIFT);
 317
 318         if (rate < VCO_HIGH)
 319                 val &= ~(1 << PLL_VCO_HIGH_SHIFT);
 320         else
 321                 val |= (1 << PLL_VCO_HIGH_SHIFT);
 322
 323         iproc_pll_write(pll, pll->control_base, ctrl->vco_ctrl.l_offset, val);
 324
 325         /* program integer part of NDIV */
 326         val = readl(pll->control_base + ctrl->ndiv_int.offset);
 327         val &= ~(bit_mask(ctrl->ndiv_int.width) << ctrl->ndiv_int.shift);
 328         val |= vco->ndiv_int << ctrl->ndiv_int.shift;
 329         iproc_pll_write(pll, pll->control_base, ctrl->ndiv_int.offset, val);
 330
 331         /* program fractional part of NDIV */
 332         if (ctrl->flags & IPROC_CLK_PLL_HAS_NDIV_FRAC) {
 333                 val = readl(pll->control_base + ctrl->ndiv_frac.offset);
 334                 val &= ~(bit_mask(ctrl->ndiv_frac.width) <<
 335                          ctrl->ndiv_frac.shift);
 336                 val |= vco->ndiv_frac << ctrl->ndiv_frac.shift;
 337                 iproc_pll_write(pll, pll->control_base, ctrl->ndiv_frac.offset,
 338                                 val);
 339         }
 340
 341         /* program PDIV */
 342         val = readl(pll->control_base + ctrl->pdiv.offset);
 343         val &= ~(bit_mask(ctrl->pdiv.width) << ctrl->pdiv.shift);
 344         val |= vco->pdiv << ctrl->pdiv.shift;
 345         iproc_pll_write(pll, pll->control_base, ctrl->pdiv.offset, val);
 346
 347         __pll_bring_out_reset(pll, kp, ka, ki);
 348
 349         ret = pll_wait_for_lock(pll);
 350         if (ret < 0) {
 351                 pr_err("%s: pll: %s failed to lock\n", __func__, clk_name);
 352                 return ret;
 353         }
 354
 355         return 0;
 356 }
 357
 358 static int iproc_pll_enable(struct clk_hw *hw)
 359 {
 360         struct iproc_clk *clk = to_iproc_clk(hw);
 361         struct iproc_pll *pll = clk->pll;
 362
 363         return __pll_enable(pll);
 364 }
 365
 366 static void iproc_pll_disable(struct clk_hw *hw)
 367 {
 368         struct iproc_clk *clk = to_iproc_clk(hw);
 369         struct iproc_pll *pll = clk->pll;
 370         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 371
 372         if (ctrl->flags & IPROC_CLK_AON)
 373                 return;
 374
 375         __pll_disable(pll);
 376 }
 377
 378 static unsigned long iproc_pll_recalc_rate(struct clk_hw *hw,
 379                                            unsigned long parent_rate)
 380 {
 381         struct iproc_clk *clk = to_iproc_clk(hw);
 382         struct iproc_pll *pll = clk->pll;
 383         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 384         u32 val;
 385         u64 ndiv, ndiv_int, ndiv_frac;
 386         unsigned int pdiv;
 387         unsigned long rate;
 388
 389         if (parent_rate == 0)
 390                 return 0;
 391
 392         /* PLL needs to be locked */
 393         val = readl(pll->status_base + ctrl->status.offset);
 394         if ((val & (1 << ctrl->status.shift)) == 0)
 395                 return 0;
 396
 397         /*
 398          * PLL output frequency =
 399          *
 400          * ((ndiv_int + ndiv_frac / 2^20) * (parent clock rate / pdiv)
 401          */
 402         val = readl(pll->control_base + ctrl->ndiv_int.offset);
 403         ndiv_int = (val >> ctrl->ndiv_int.shift) &
 404                 bit_mask(ctrl->ndiv_int.width);
 405         ndiv = ndiv_int << 20;
 406
 407         if (ctrl->flags & IPROC_CLK_PLL_HAS_NDIV_FRAC) {
 408                 val = readl(pll->control_base + ctrl->ndiv_frac.offset);
 409                 ndiv_frac = (val >> ctrl->ndiv_frac.shift) &
 410                         bit_mask(ctrl->ndiv_frac.width);
 411                 ndiv += ndiv_frac;
 412         }
 413
 414         val = readl(pll->control_base + ctrl->pdiv.offset);
 415         pdiv = (val >> ctrl->pdiv.shift) & bit_mask(ctrl->pdiv.width);
 416
 417         rate = (ndiv * parent_rate) >> 20;
 418
 419         if (pdiv == 0)
 420                 rate *= 2;
 421         else
 422                 rate /= pdiv;
 423
 424         return rate;
 425 }
 426
 427 static long iproc_pll_round_rate(struct clk_hw *hw, unsigned long rate,
 428                                  unsigned long *parent_rate)
 429 {
 430         unsigned i;
 431         struct iproc_clk *clk = to_iproc_clk(hw);
 432         struct iproc_pll *pll = clk->pll;
 433
 434         if (rate == 0 || *parent_rate == 0 || !pll->vco_param)
 435                 return -EINVAL;
 436
 437         for (i = 0; i < pll->num_vco_entries; i++) {
 438                 if (rate <= pll->vco_param[i].rate)
 439                         break;
 440         }
 441
 442         if (i == pll->num_vco_entries)
 443                 i--;
 444
 445         return pll->vco_param[i].rate;
 446 }
 447
 448 static int iproc_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 449                 unsigned long parent_rate)
 450 {
 451         struct iproc_clk *clk = to_iproc_clk(hw);
 452         struct iproc_pll *pll = clk->pll;
 453         int rate_index, ret;
 454
 455         rate_index = pll_get_rate_index(pll, rate);
 456         if (rate_index < 0)
 457                 return rate_index;
 458
 459         ret = pll_set_rate(clk, rate_index, parent_rate);
 460         return ret;
 461 }
 462
 463 static const struct clk_ops iproc_pll_ops = {
 464         .enable = iproc_pll_enable,
 465         .disable = iproc_pll_disable,
 466         .recalc_rate = iproc_pll_recalc_rate,
 467         .round_rate = iproc_pll_round_rate,
 468         .set_rate = iproc_pll_set_rate,
 469 };
 470
 471 static int iproc_clk_enable(struct clk_hw *hw)
 472 {
 473         struct iproc_clk *clk = to_iproc_clk(hw);
 474         const struct iproc_clk_ctrl *ctrl = clk->ctrl;
 475         struct iproc_pll *pll = clk->pll;
 476         u32 val;
 477
 478         /* channel enable is active low */
 479         val = readl(pll->control_base + ctrl->enable.offset);
 480         val &= ~(1 << ctrl->enable.enable_shift);
 481         iproc_pll_write(pll, pll->control_base, ctrl->enable.offset, val);
 482
 483         /* also make sure channel is not held */
 484         val = readl(pll->control_base + ctrl->enable.offset);
 485         val &= ~(1 << ctrl->enable.hold_shift);
 486         iproc_pll_write(pll, pll->control_base, ctrl->enable.offset, val);
 487
 488         return 0;
 489 }
 490
 491 static void iproc_clk_disable(struct clk_hw *hw)
 492 {
 493         struct iproc_clk *clk = to_iproc_clk(hw);
 494         const struct iproc_clk_ctrl *ctrl = clk->ctrl;
 495         struct iproc_pll *pll = clk->pll;
 496         u32 val;
 497
 498         if (ctrl->flags & IPROC_CLK_AON)
 499                 return;
 500
 501         val = readl(pll->control_base + ctrl->enable.offset);
 502         val |= 1 << ctrl->enable.enable_shift;
 503         iproc_pll_write(pll, pll->control_base, ctrl->enable.offset, val);
 504 }
 505
 506 static unsigned long iproc_clk_recalc_rate(struct clk_hw *hw,
 507                 unsigned long parent_rate)
 508 {
 509         struct iproc_clk *clk = to_iproc_clk(hw);
 510         const struct iproc_clk_ctrl *ctrl = clk->ctrl;
 511         struct iproc_pll *pll = clk->pll;
 512         u32 val;
 513         unsigned int mdiv;
 514         unsigned long rate;
 515
 516         if (parent_rate == 0)
 517                 return 0;
 518
 519         val = readl(pll->control_base + ctrl->mdiv.offset);
 520         mdiv = (val >> ctrl->mdiv.shift) & bit_mask(ctrl->mdiv.width);
 521         if (mdiv == 0)
 522                 mdiv = 256;
 523
 524         if (ctrl->flags & IPROC_CLK_MCLK_DIV_BY_2)
 525                 rate = parent_rate / (mdiv * 2);
 526         else
 527                 rate = parent_rate / mdiv;
 528
 529         return rate;
 530 }
 531
 532 static long iproc_clk_round_rate(struct clk_hw *hw, unsigned long rate,
 533                 unsigned long *parent_rate)
 534 {
 535         unsigned int div;
 536
 537         if (rate == 0 || *parent_rate == 0)
 538                 return -EINVAL;
 539
 540         if (rate == *parent_rate)
 541                 return *parent_rate;
 542
 543         div = DIV_ROUND_UP(*parent_rate, rate);
 544         if (div < 2)
 545                 return *parent_rate;
 546
 547         if (div > 256)
 548                 div = 256;
 549
 550         return *parent_rate / div;
 551 }
 552
 553 static int iproc_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 554                 unsigned long parent_rate)
 555 {
 556         struct iproc_clk *clk = to_iproc_clk(hw);
 557         const struct iproc_clk_ctrl *ctrl = clk->ctrl;
 558         struct iproc_pll *pll = clk->pll;
 559         u32 val;
 560         unsigned int div;
 561
 562         if (rate == 0 || parent_rate == 0)
 563                 return -EINVAL;
 564
 565         if (ctrl->flags & IPROC_CLK_MCLK_DIV_BY_2)
 566                 div = DIV_ROUND_UP(parent_rate, rate * 2);
 567         else
 568                 div = DIV_ROUND_UP(parent_rate, rate);
 569         if (div > 256)
 570                 return -EINVAL;
 571
 572         val = readl(pll->control_base + ctrl->mdiv.offset);
 573         if (div == 256) {
 574                 val &= ~(bit_mask(ctrl->mdiv.width) << ctrl->mdiv.shift);
 575         } else {
 576                 val &= ~(bit_mask(ctrl->mdiv.width) << ctrl->mdiv.shift);
 577                 val |= div << ctrl->mdiv.shift;
 578         }
 579         iproc_pll_write(pll, pll->control_base, ctrl->mdiv.offset, val);
 580
 581         return 0;
 582 }
 583
 584 static const struct clk_ops iproc_clk_ops = {
 585         .enable = iproc_clk_enable,
 586         .disable = iproc_clk_disable,
 587         .recalc_rate = iproc_clk_recalc_rate,
 588         .round_rate = iproc_clk_round_rate,
 589         .set_rate = iproc_clk_set_rate,
 590 };
 591
 592 /**
 593  * Some PLLs require the PLL SW override bit to be set before changes can be
 594  * applied to the PLL
 595  */
 596 static void iproc_pll_sw_cfg(struct iproc_pll *pll)
 597 {
 598         const struct iproc_pll_ctrl *ctrl = pll->ctrl;
 599
 600         if (ctrl->flags & IPROC_CLK_PLL_NEEDS_SW_CFG) {
 601                 u32 val;
 602
 603                 val = readl(pll->control_base + ctrl->sw_ctrl.offset);
 604                 val |= BIT(ctrl->sw_ctrl.shift);
 605                 iproc_pll_write(pll, pll->control_base, ctrl->sw_ctrl.offset,
 606                                 val);
 607         }
 608 }
 609
 610 void __init iproc_pll_clk_setup(struct device_node *node,
 611                                 const struct iproc_pll_ctrl *pll_ctrl,
 612                                 const struct iproc_pll_vco_param *vco,
 613                                 unsigned int num_vco_entries,
 614                                 const struct iproc_clk_ctrl *clk_ctrl,
 615                                 unsigned int num_clks)
 616 {
 617         int i, ret;
 618         struct iproc_pll *pll;
 619         struct iproc_clk *iclk;
 620         struct clk_init_data init;
 621         const char *parent_name;
 622         struct iproc_clk *iclk_array;
 623         struct clk_hw_onecell_data *clk_data;
 624         const char *clk_name;
 625
 626         if (WARN_ON(!pll_ctrl) || WARN_ON(!clk_ctrl))
 627                 return;
 628
 629         pll = kzalloc(sizeof(*pll), GFP_KERNEL);
 630         if (WARN_ON(!pll))
 631                 return;
 632
 633         clk_data = kzalloc(sizeof(*clk_data->hws) * num_clks +
 634                                 sizeof(*clk_data), GFP_KERNEL);
 635         if (WARN_ON(!clk_data))
 636                 goto err_clk_data;
 637         clk_data->num = num_clks;
 638
 639         iclk_array = kcalloc(num_clks, sizeof(struct iproc_clk), GFP_KERNEL);
 640         if (WARN_ON(!iclk_array))
 641                 goto err_clks;
 642
 643         pll->control_base = of_iomap(node, 0);
 644         if (WARN_ON(!pll->control_base))
 645                 goto err_pll_iomap;
 646
 647         /* Some SoCs do not require the pwr_base, thus failing is not fatal */
 648         pll->pwr_base = of_iomap(node, 1);
 649
 650         /* some PLLs require gating control at the top ASIU level */
 651         if (pll_ctrl->flags & IPROC_CLK_PLL_ASIU) {
 652                 pll->asiu_base = of_iomap(node, 2);
 653                 if (WARN_ON(!pll->asiu_base))
 654                         goto err_asiu_iomap;
 655         }
 656
 657         if (pll_ctrl->flags & IPROC_CLK_PLL_SPLIT_STAT_CTRL) {
 658                 /* Some SoCs have a split status/control.  If this does not
 659                  * exist, assume they are unified.
 660                  */
 661                 pll->status_base = of_iomap(node, 2);
 662                 if (!pll->status_base)
 663                         goto err_status_iomap;
 664         } else
 665                 pll->status_base = pll->control_base;
 666
 667         /* initialize and register the PLL itself */
 668         pll->ctrl = pll_ctrl;
 669
 670         iclk = &iclk_array[0];
 671         iclk->pll = pll;
 672
 673         ret = of_property_read_string_index(node, "clock-output-names",
 674                                             0, &clk_name);
 675         if (WARN_ON(ret))
 676                 goto err_pll_register;
 677
 678         init.name = clk_name;
 679         init.ops = &iproc_pll_ops;
 680         init.flags = 0;
 681         parent_name = of_clk_get_parent_name(node, 0);
 682         init.parent_names = (parent_name ? &parent_name : NULL);
 683         init.num_parents = (parent_name ? 1 : 0);
 684         iclk->hw.init = &init;
 685
 686         if (vco) {
 687                 pll->num_vco_entries = num_vco_entries;
 688                 pll->vco_param = vco;
 689         }
 690
 691         iproc_pll_sw_cfg(pll);
 692
 693         ret = clk_hw_register(NULL, &iclk->hw);
 694         if (WARN_ON(ret))
 695                 goto err_pll_register;
 696
 697         clk_data->hws[0] = &iclk->hw;
 698         parent_name = clk_name;
 699
 700         /* now initialize and register all leaf clocks */
 701         for (i = 1; i < num_clks; i++) {
 702                 memset(&init, 0, sizeof(init));
 703
 704                 ret = of_property_read_string_index(node, "clock-output-names",
 705                                                     i, &clk_name);
 706                 if (WARN_ON(ret))
 707                         goto err_clk_register;
 708
 709                 iclk = &iclk_array[i];
 710                 iclk->pll = pll;
 711                 iclk->ctrl = &clk_ctrl[i];
 712
 713                 init.name = clk_name;
 714                 init.ops = &iproc_clk_ops;
 715                 init.flags = 0;
 716                 init.parent_names = (parent_name ? &parent_name : NULL);
 717                 init.num_parents = (parent_name ? 1 : 0);
 718                 iclk->hw.init = &init;
 719
 720                 ret = clk_hw_register(NULL, &iclk->hw);
 721                 if (WARN_ON(ret))
 722                         goto err_clk_register;
 723
 724                 clk_data->hws[i] = &iclk->hw;
 725         }
 726
 727         ret = of_clk_add_hw_provider(node, of_clk_hw_onecell_get, clk_data);
 728         if (WARN_ON(ret))
 729                 goto err_clk_register;
 730
 731         return;
 732
 733 err_clk_register:
 734         while (--i >= 0)
 735                 clk_hw_unregister(clk_data->hws[i]);
 736
 737 err_pll_register:
 738         if (pll->status_base != pll->control_base)
 739                 iounmap(pll->status_base);
 740
 741 err_status_iomap:
 742         if (pll->asiu_base)
 743                 iounmap(pll->asiu_base);
 744
 745 err_asiu_iomap:
 746         if (pll->pwr_base)
 747                 iounmap(pll->pwr_base);
 748
 749         iounmap(pll->control_base);
 750
 751 err_pll_iomap:
 752         kfree(iclk_array);
 753
 754 err_clks:
 755         kfree(clk_data);
 756
 757 err_clk_data:
 758         kfree(pll);
 759 }