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 /* number of delay loops waiting for PLL to lock */ 29 #define LOCK_DELAY 100 30 31 /* number of VCO frequency bands */ 32 #define NUM_FREQ_BANDS 8 33 34 #define NUM_KP_BANDS 3 35 enum kp_band { 36 KP_BAND_MID = 0, 37 KP_BAND_HIGH, 38 KP_BAND_HIGH_HIGH 39 }; 40 41 static const unsigned int kp_table[NUM_KP_BANDS][NUM_FREQ_BANDS] = { 42 { 5, 6, 6, 7, 7, 8, 9, 10 }, 43 { 4, 4, 5, 5, 6, 7, 8, 9 }, 44 { 4, 5, 5, 6, 7, 8, 9, 10 }, 45 }; 46 47 static const unsigned long ref_freq_table[NUM_FREQ_BANDS][2] = { 48 { 10000000, 12500000 }, 49 { 12500000, 15000000 }, 50 { 15000000, 20000000 }, 51 { 20000000, 25000000 }, 52 { 25000000, 50000000 }, 53 { 50000000, 75000000 }, 54 { 75000000, 100000000 }, 55 { 100000000, 125000000 }, 56 }; 57 58 enum vco_freq_range { 59 VCO_LOW = 700000000U, 60 VCO_MID = 1200000000U, 61 VCO_HIGH = 2200000000U, 62 VCO_HIGH_HIGH = 3100000000U, 63 VCO_MAX = 4000000000U, 64 }; 65 66 struct iproc_pll; 67 68 struct iproc_clk { 69 struct clk_hw hw; 70 const char *name; 71 struct iproc_pll *pll; 72 unsigned long rate; 73 const struct iproc_clk_ctrl *ctrl; 74 }; 75 76 struct iproc_pll { 77 void __iomem *pll_base; 78 void __iomem *pwr_base; 79 void __iomem *asiu_base; 80 81 const struct iproc_pll_ctrl *ctrl; 82 const struct iproc_pll_vco_param *vco_param; 83 unsigned int num_vco_entries; 84 85 struct clk_onecell_data clk_data; 86 struct iproc_clk *clks; 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->pll_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 __pll_disable(struct iproc_pll *pll) 141 { 142 const struct iproc_pll_ctrl *ctrl = pll->ctrl; 143 u32 val; 144 145 if (ctrl->flags & IPROC_CLK_PLL_ASIU) { 146 val = readl(pll->asiu_base + ctrl->asiu.offset); 147 val &= ~(1 << ctrl->asiu.en_shift); 148 writel(val, pll->asiu_base + ctrl->asiu.offset); 149 } 150 151 /* latch input value so core power can be shut down */ 152 val = readl(pll->pwr_base + ctrl->aon.offset); 153 val |= (1 << ctrl->aon.iso_shift); 154 writel(val, pll->pwr_base + ctrl->aon.offset); 155 156 /* power down the core */ 157 val &= ~(bit_mask(ctrl->aon.pwr_width) << ctrl->aon.pwr_shift); 158 writel(val, pll->pwr_base + ctrl->aon.offset); 159 } 160 161 static int __pll_enable(struct iproc_pll *pll) 162 { 163 const struct iproc_pll_ctrl *ctrl = pll->ctrl; 164 u32 val; 165 166 /* power up the PLL and make sure it's not latched */ 167 val = readl(pll->pwr_base + ctrl->aon.offset); 168 val |= bit_mask(ctrl->aon.pwr_width) << ctrl->aon.pwr_shift; 169 val &= ~(1 << ctrl->aon.iso_shift); 170 writel(val, pll->pwr_base + ctrl->aon.offset); 171 172 /* certain PLLs also need to be ungated from the ASIU top level */ 173 if (ctrl->flags & IPROC_CLK_PLL_ASIU) { 174 val = readl(pll->asiu_base + ctrl->asiu.offset); 175 val |= (1 << ctrl->asiu.en_shift); 176 writel(val, pll->asiu_base + ctrl->asiu.offset); 177 } 178 179 return 0; 180 } 181 182 static void __pll_put_in_reset(struct iproc_pll *pll) 183 { 184 u32 val; 185 const struct iproc_pll_ctrl *ctrl = pll->ctrl; 186 const struct iproc_pll_reset_ctrl *reset = &ctrl->reset; 187 188 val = readl(pll->pll_base + reset->offset); 189 val &= ~(1 << reset->reset_shift | 1 << reset->p_reset_shift); 190 writel(val, pll->pll_base + reset->offset); 191 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 192 readl(pll->pll_base + reset->offset); 193 } 194 195 static void __pll_bring_out_reset(struct iproc_pll *pll, unsigned int kp, 196 unsigned int ka, unsigned int ki) 197 { 198 u32 val; 199 const struct iproc_pll_ctrl *ctrl = pll->ctrl; 200 const struct iproc_pll_reset_ctrl *reset = &ctrl->reset; 201 202 val = readl(pll->pll_base + reset->offset); 203 val &= ~(bit_mask(reset->ki_width) << reset->ki_shift | 204 bit_mask(reset->kp_width) << reset->kp_shift | 205 bit_mask(reset->ka_width) << reset->ka_shift); 206 val |= ki << reset->ki_shift | kp << reset->kp_shift | 207 ka << reset->ka_shift; 208 val |= 1 << reset->reset_shift | 1 << reset->p_reset_shift; 209 writel(val, pll->pll_base + reset->offset); 210 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 211 readl(pll->pll_base + reset->offset); 212 } 213 214 static int pll_set_rate(struct iproc_clk *clk, unsigned int rate_index, 215 unsigned long parent_rate) 216 { 217 struct iproc_pll *pll = clk->pll; 218 const struct iproc_pll_vco_param *vco = &pll->vco_param[rate_index]; 219 const struct iproc_pll_ctrl *ctrl = pll->ctrl; 220 int ka = 0, ki, kp, ret; 221 unsigned long rate = vco->rate; 222 u32 val; 223 enum kp_band kp_index; 224 unsigned long ref_freq; 225 226 /* 227 * reference frequency = parent frequency / PDIV 228 * If PDIV = 0, then it becomes a multiplier (x2) 229 */ 230 if (vco->pdiv == 0) 231 ref_freq = parent_rate * 2; 232 else 233 ref_freq = parent_rate / vco->pdiv; 234 235 /* determine Ki and Kp index based on target VCO frequency */ 236 if (rate >= VCO_LOW && rate < VCO_HIGH) { 237 ki = 4; 238 kp_index = KP_BAND_MID; 239 } else if (rate >= VCO_HIGH && rate && rate < VCO_HIGH_HIGH) { 240 ki = 3; 241 kp_index = KP_BAND_HIGH; 242 } else if (rate >= VCO_HIGH_HIGH && rate < VCO_MAX) { 243 ki = 3; 244 kp_index = KP_BAND_HIGH_HIGH; 245 } else { 246 pr_err("%s: pll: %s has invalid rate: %lu\n", __func__, 247 clk->name, rate); 248 return -EINVAL; 249 } 250 251 kp = get_kp(ref_freq, kp_index); 252 if (kp < 0) { 253 pr_err("%s: pll: %s has invalid kp\n", __func__, clk->name); 254 return kp; 255 } 256 257 ret = __pll_enable(pll); 258 if (ret) { 259 pr_err("%s: pll: %s fails to enable\n", __func__, clk->name); 260 return ret; 261 } 262 263 /* put PLL in reset */ 264 __pll_put_in_reset(pll); 265 266 writel(0, pll->pll_base + ctrl->vco_ctrl.u_offset); 267 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 268 readl(pll->pll_base + ctrl->vco_ctrl.u_offset); 269 val = readl(pll->pll_base + ctrl->vco_ctrl.l_offset); 270 271 if (rate >= VCO_LOW && rate < VCO_MID) 272 val |= (1 << PLL_VCO_LOW_SHIFT); 273 274 if (rate < VCO_HIGH) 275 val &= ~(1 << PLL_VCO_HIGH_SHIFT); 276 else 277 val |= (1 << PLL_VCO_HIGH_SHIFT); 278 279 writel(val, pll->pll_base + ctrl->vco_ctrl.l_offset); 280 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 281 readl(pll->pll_base + ctrl->vco_ctrl.l_offset); 282 283 /* program integer part of NDIV */ 284 val = readl(pll->pll_base + ctrl->ndiv_int.offset); 285 val &= ~(bit_mask(ctrl->ndiv_int.width) << ctrl->ndiv_int.shift); 286 val |= vco->ndiv_int << ctrl->ndiv_int.shift; 287 writel(val, pll->pll_base + ctrl->ndiv_int.offset); 288 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 289 readl(pll->pll_base + ctrl->ndiv_int.offset); 290 291 /* program fractional part of NDIV */ 292 if (ctrl->flags & IPROC_CLK_PLL_HAS_NDIV_FRAC) { 293 val = readl(pll->pll_base + ctrl->ndiv_frac.offset); 294 val &= ~(bit_mask(ctrl->ndiv_frac.width) << 295 ctrl->ndiv_frac.shift); 296 val |= vco->ndiv_frac << ctrl->ndiv_frac.shift; 297 writel(val, pll->pll_base + ctrl->ndiv_frac.offset); 298 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 299 readl(pll->pll_base + ctrl->ndiv_frac.offset); 300 } 301 302 /* program PDIV */ 303 val = readl(pll->pll_base + ctrl->pdiv.offset); 304 val &= ~(bit_mask(ctrl->pdiv.width) << ctrl->pdiv.shift); 305 val |= vco->pdiv << ctrl->pdiv.shift; 306 writel(val, pll->pll_base + ctrl->pdiv.offset); 307 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 308 readl(pll->pll_base + ctrl->pdiv.offset); 309 310 __pll_bring_out_reset(pll, kp, ka, ki); 311 312 ret = pll_wait_for_lock(pll); 313 if (ret < 0) { 314 pr_err("%s: pll: %s failed to lock\n", __func__, clk->name); 315 return ret; 316 } 317 318 return 0; 319 } 320 321 static int iproc_pll_enable(struct clk_hw *hw) 322 { 323 struct iproc_clk *clk = to_iproc_clk(hw); 324 struct iproc_pll *pll = clk->pll; 325 326 return __pll_enable(pll); 327 } 328 329 static void iproc_pll_disable(struct clk_hw *hw) 330 { 331 struct iproc_clk *clk = to_iproc_clk(hw); 332 struct iproc_pll *pll = clk->pll; 333 const struct iproc_pll_ctrl *ctrl = pll->ctrl; 334 335 if (ctrl->flags & IPROC_CLK_AON) 336 return; 337 338 __pll_disable(pll); 339 } 340 341 static unsigned long iproc_pll_recalc_rate(struct clk_hw *hw, 342 unsigned long parent_rate) 343 { 344 struct iproc_clk *clk = to_iproc_clk(hw); 345 struct iproc_pll *pll = clk->pll; 346 const struct iproc_pll_ctrl *ctrl = pll->ctrl; 347 u32 val; 348 u64 ndiv; 349 unsigned int ndiv_int, ndiv_frac, pdiv; 350 351 if (parent_rate == 0) 352 return 0; 353 354 /* PLL needs to be locked */ 355 val = readl(pll->pll_base + ctrl->status.offset); 356 if ((val & (1 << ctrl->status.shift)) == 0) { 357 clk->rate = 0; 358 return 0; 359 } 360 361 /* 362 * PLL output frequency = 363 * 364 * ((ndiv_int + ndiv_frac / 2^20) * (parent clock rate / pdiv) 365 */ 366 val = readl(pll->pll_base + ctrl->ndiv_int.offset); 367 ndiv_int = (val >> ctrl->ndiv_int.shift) & 368 bit_mask(ctrl->ndiv_int.width); 369 ndiv = ndiv_int << ctrl->ndiv_int.shift; 370 371 if (ctrl->flags & IPROC_CLK_PLL_HAS_NDIV_FRAC) { 372 val = readl(pll->pll_base + ctrl->ndiv_frac.offset); 373 ndiv_frac = (val >> ctrl->ndiv_frac.shift) & 374 bit_mask(ctrl->ndiv_frac.width); 375 376 if (ndiv_frac != 0) 377 ndiv = (ndiv_int << ctrl->ndiv_int.shift) | ndiv_frac; 378 } 379 380 val = readl(pll->pll_base + ctrl->pdiv.offset); 381 pdiv = (val >> ctrl->pdiv.shift) & bit_mask(ctrl->pdiv.width); 382 383 clk->rate = (ndiv * parent_rate) >> ctrl->ndiv_int.shift; 384 385 if (pdiv == 0) 386 clk->rate *= 2; 387 else 388 clk->rate /= pdiv; 389 390 return clk->rate; 391 } 392 393 static long iproc_pll_round_rate(struct clk_hw *hw, unsigned long rate, 394 unsigned long *parent_rate) 395 { 396 unsigned i; 397 struct iproc_clk *clk = to_iproc_clk(hw); 398 struct iproc_pll *pll = clk->pll; 399 400 if (rate == 0 || *parent_rate == 0 || !pll->vco_param) 401 return -EINVAL; 402 403 for (i = 0; i < pll->num_vco_entries; i++) { 404 if (rate <= pll->vco_param[i].rate) 405 break; 406 } 407 408 if (i == pll->num_vco_entries) 409 i--; 410 411 return pll->vco_param[i].rate; 412 } 413 414 static int iproc_pll_set_rate(struct clk_hw *hw, unsigned long rate, 415 unsigned long parent_rate) 416 { 417 struct iproc_clk *clk = to_iproc_clk(hw); 418 struct iproc_pll *pll = clk->pll; 419 int rate_index, ret; 420 421 rate_index = pll_get_rate_index(pll, rate); 422 if (rate_index < 0) 423 return rate_index; 424 425 ret = pll_set_rate(clk, rate_index, parent_rate); 426 return ret; 427 } 428 429 static const struct clk_ops iproc_pll_ops = { 430 .enable = iproc_pll_enable, 431 .disable = iproc_pll_disable, 432 .recalc_rate = iproc_pll_recalc_rate, 433 .round_rate = iproc_pll_round_rate, 434 .set_rate = iproc_pll_set_rate, 435 }; 436 437 static int iproc_clk_enable(struct clk_hw *hw) 438 { 439 struct iproc_clk *clk = to_iproc_clk(hw); 440 const struct iproc_clk_ctrl *ctrl = clk->ctrl; 441 struct iproc_pll *pll = clk->pll; 442 u32 val; 443 444 /* channel enable is active low */ 445 val = readl(pll->pll_base + ctrl->enable.offset); 446 val &= ~(1 << ctrl->enable.enable_shift); 447 writel(val, pll->pll_base + ctrl->enable.offset); 448 449 /* also make sure channel is not held */ 450 val = readl(pll->pll_base + ctrl->enable.offset); 451 val &= ~(1 << ctrl->enable.hold_shift); 452 writel(val, pll->pll_base + ctrl->enable.offset); 453 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 454 readl(pll->pll_base + ctrl->enable.offset); 455 456 return 0; 457 } 458 459 static void iproc_clk_disable(struct clk_hw *hw) 460 { 461 struct iproc_clk *clk = to_iproc_clk(hw); 462 const struct iproc_clk_ctrl *ctrl = clk->ctrl; 463 struct iproc_pll *pll = clk->pll; 464 u32 val; 465 466 if (ctrl->flags & IPROC_CLK_AON) 467 return; 468 469 val = readl(pll->pll_base + ctrl->enable.offset); 470 val |= 1 << ctrl->enable.enable_shift; 471 writel(val, pll->pll_base + ctrl->enable.offset); 472 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 473 readl(pll->pll_base + ctrl->enable.offset); 474 } 475 476 static unsigned long iproc_clk_recalc_rate(struct clk_hw *hw, 477 unsigned long parent_rate) 478 { 479 struct iproc_clk *clk = to_iproc_clk(hw); 480 const struct iproc_clk_ctrl *ctrl = clk->ctrl; 481 struct iproc_pll *pll = clk->pll; 482 u32 val; 483 unsigned int mdiv; 484 485 if (parent_rate == 0) 486 return 0; 487 488 val = readl(pll->pll_base + ctrl->mdiv.offset); 489 mdiv = (val >> ctrl->mdiv.shift) & bit_mask(ctrl->mdiv.width); 490 if (mdiv == 0) 491 mdiv = 256; 492 493 clk->rate = parent_rate / mdiv; 494 495 return clk->rate; 496 } 497 498 static long iproc_clk_round_rate(struct clk_hw *hw, unsigned long rate, 499 unsigned long *parent_rate) 500 { 501 unsigned int div; 502 503 if (rate == 0 || *parent_rate == 0) 504 return -EINVAL; 505 506 if (rate == *parent_rate) 507 return *parent_rate; 508 509 div = DIV_ROUND_UP(*parent_rate, rate); 510 if (div < 2) 511 return *parent_rate; 512 513 if (div > 256) 514 div = 256; 515 516 return *parent_rate / div; 517 } 518 519 static int iproc_clk_set_rate(struct clk_hw *hw, unsigned long rate, 520 unsigned long parent_rate) 521 { 522 struct iproc_clk *clk = to_iproc_clk(hw); 523 const struct iproc_clk_ctrl *ctrl = clk->ctrl; 524 struct iproc_pll *pll = clk->pll; 525 u32 val; 526 unsigned int div; 527 528 if (rate == 0 || parent_rate == 0) 529 return -EINVAL; 530 531 div = DIV_ROUND_UP(parent_rate, rate); 532 if (div > 256) 533 return -EINVAL; 534 535 val = readl(pll->pll_base + ctrl->mdiv.offset); 536 if (div == 256) { 537 val &= ~(bit_mask(ctrl->mdiv.width) << ctrl->mdiv.shift); 538 } else { 539 val &= ~(bit_mask(ctrl->mdiv.width) << ctrl->mdiv.shift); 540 val |= div << ctrl->mdiv.shift; 541 } 542 writel(val, pll->pll_base + ctrl->mdiv.offset); 543 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 544 readl(pll->pll_base + ctrl->mdiv.offset); 545 clk->rate = parent_rate / div; 546 547 return 0; 548 } 549 550 static const struct clk_ops iproc_clk_ops = { 551 .enable = iproc_clk_enable, 552 .disable = iproc_clk_disable, 553 .recalc_rate = iproc_clk_recalc_rate, 554 .round_rate = iproc_clk_round_rate, 555 .set_rate = iproc_clk_set_rate, 556 }; 557 558 /** 559 * Some PLLs require the PLL SW override bit to be set before changes can be 560 * applied to the PLL 561 */ 562 static void iproc_pll_sw_cfg(struct iproc_pll *pll) 563 { 564 const struct iproc_pll_ctrl *ctrl = pll->ctrl; 565 566 if (ctrl->flags & IPROC_CLK_PLL_NEEDS_SW_CFG) { 567 u32 val; 568 569 val = readl(pll->pll_base + ctrl->sw_ctrl.offset); 570 val |= BIT(ctrl->sw_ctrl.shift); 571 writel(val, pll->pll_base + ctrl->sw_ctrl.offset); 572 if (unlikely(ctrl->flags & IPROC_CLK_NEEDS_READ_BACK)) 573 readl(pll->pll_base + ctrl->sw_ctrl.offset); 574 } 575 } 576 577 void __init iproc_pll_clk_setup(struct device_node *node, 578 const struct iproc_pll_ctrl *pll_ctrl, 579 const struct iproc_pll_vco_param *vco, 580 unsigned int num_vco_entries, 581 const struct iproc_clk_ctrl *clk_ctrl, 582 unsigned int num_clks) 583 { 584 int i, ret; 585 struct clk *clk; 586 struct iproc_pll *pll; 587 struct iproc_clk *iclk; 588 struct clk_init_data init; 589 const char *parent_name; 590 591 if (WARN_ON(!pll_ctrl) || WARN_ON(!clk_ctrl)) 592 return; 593 594 pll = kzalloc(sizeof(*pll), GFP_KERNEL); 595 if (WARN_ON(!pll)) 596 return; 597 598 pll->clk_data.clk_num = num_clks; 599 pll->clk_data.clks = kcalloc(num_clks, sizeof(*pll->clk_data.clks), 600 GFP_KERNEL); 601 if (WARN_ON(!pll->clk_data.clks)) 602 goto err_clk_data; 603 604 pll->clks = kcalloc(num_clks, sizeof(*pll->clks), GFP_KERNEL); 605 if (WARN_ON(!pll->clks)) 606 goto err_clks; 607 608 pll->pll_base = of_iomap(node, 0); 609 if (WARN_ON(!pll->pll_base)) 610 goto err_pll_iomap; 611 612 pll->pwr_base = of_iomap(node, 1); 613 if (WARN_ON(!pll->pwr_base)) 614 goto err_pwr_iomap; 615 616 /* some PLLs require gating control at the top ASIU level */ 617 if (pll_ctrl->flags & IPROC_CLK_PLL_ASIU) { 618 pll->asiu_base = of_iomap(node, 2); 619 if (WARN_ON(!pll->asiu_base)) 620 goto err_asiu_iomap; 621 } 622 623 /* initialize and register the PLL itself */ 624 pll->ctrl = pll_ctrl; 625 626 iclk = &pll->clks[0]; 627 iclk->pll = pll; 628 iclk->name = node->name; 629 630 init.name = node->name; 631 init.ops = &iproc_pll_ops; 632 init.flags = 0; 633 parent_name = of_clk_get_parent_name(node, 0); 634 init.parent_names = (parent_name ? &parent_name : NULL); 635 init.num_parents = (parent_name ? 1 : 0); 636 iclk->hw.init = &init; 637 638 if (vco) { 639 pll->num_vco_entries = num_vco_entries; 640 pll->vco_param = vco; 641 } 642 643 iproc_pll_sw_cfg(pll); 644 645 clk = clk_register(NULL, &iclk->hw); 646 if (WARN_ON(IS_ERR(clk))) 647 goto err_pll_register; 648 649 pll->clk_data.clks[0] = clk; 650 651 /* now initialize and register all leaf clocks */ 652 for (i = 1; i < num_clks; i++) { 653 const char *clk_name; 654 655 memset(&init, 0, sizeof(init)); 656 parent_name = node->name; 657 658 clk_name = kzalloc(IPROC_CLK_NAME_LEN, GFP_KERNEL); 659 if (WARN_ON(!clk_name)) 660 goto err_clk_register; 661 662 ret = of_property_read_string_index(node, "clock-output-names", 663 i, &clk_name); 664 if (WARN_ON(ret)) 665 goto err_clk_register; 666 667 iclk = &pll->clks[i]; 668 iclk->name = clk_name; 669 iclk->pll = pll; 670 iclk->ctrl = &clk_ctrl[i]; 671 672 init.name = clk_name; 673 init.ops = &iproc_clk_ops; 674 init.flags = 0; 675 init.parent_names = (parent_name ? &parent_name : NULL); 676 init.num_parents = (parent_name ? 1 : 0); 677 iclk->hw.init = &init; 678 679 clk = clk_register(NULL, &iclk->hw); 680 if (WARN_ON(IS_ERR(clk))) 681 goto err_clk_register; 682 683 pll->clk_data.clks[i] = clk; 684 } 685 686 ret = of_clk_add_provider(node, of_clk_src_onecell_get, &pll->clk_data); 687 if (WARN_ON(ret)) 688 goto err_clk_register; 689 690 return; 691 692 err_clk_register: 693 for (i = 0; i < num_clks; i++) { 694 kfree(pll->clks[i].name); 695 clk_unregister(pll->clk_data.clks[i]); 696 } 697 698 err_pll_register: 699 if (pll->asiu_base) 700 iounmap(pll->asiu_base); 701 702 err_asiu_iomap: 703 iounmap(pll->pwr_base); 704 705 err_pwr_iomap: 706 iounmap(pll->pll_base); 707 708 err_pll_iomap: 709 kfree(pll->clks); 710 711 err_clks: 712 kfree(pll->clk_data.clks); 713 714 err_clk_data: 715 kfree(pll); 716 } 717