1// SPDX-License-Identifier: GPL-2.0-only 2/* 3 * Copyright (C) 2014 STMicroelectronics R&D Ltd 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 17#include "clkgen.h" 18 19/* 20 * Maximum input clock to the PLL before we divide it down by 2 21 * although in reality in actual systems this has never been seen to 22 * be used. 23 */ 24#define QUADFS_NDIV_THRESHOLD 30000000 25 26#define PLL_BW_GOODREF (0L) 27#define PLL_BW_VBADREF (1L) 28#define PLL_BW_BADREF (2L) 29#define PLL_BW_VGOODREF (3L) 30 31#define QUADFS_MAX_CHAN 4 32 33struct stm_fs { 34 unsigned long ndiv; 35 unsigned long mdiv; 36 unsigned long pe; 37 unsigned long sdiv; 38 unsigned long nsdiv; 39}; 40 41struct clkgen_quadfs_data { 42 bool reset_present; 43 bool bwfilter_present; 44 bool lockstatus_present; 45 bool powerup_polarity; 46 bool standby_polarity; 47 bool nsdiv_present; 48 bool nrst_present; 49 struct clkgen_field ndiv; 50 struct clkgen_field ref_bw; 51 struct clkgen_field nreset; 52 struct clkgen_field npda; 53 struct clkgen_field lock_status; 54 55 struct clkgen_field nrst[QUADFS_MAX_CHAN]; 56 struct clkgen_field nsb[QUADFS_MAX_CHAN]; 57 struct clkgen_field en[QUADFS_MAX_CHAN]; 58 struct clkgen_field mdiv[QUADFS_MAX_CHAN]; 59 struct clkgen_field pe[QUADFS_MAX_CHAN]; 60 struct clkgen_field sdiv[QUADFS_MAX_CHAN]; 61 struct clkgen_field nsdiv[QUADFS_MAX_CHAN]; 62 63 const struct clk_ops *pll_ops; 64 int (*get_params)(unsigned long, unsigned long, struct stm_fs *); 65 int (*get_rate)(unsigned long , const struct stm_fs *, 66 unsigned long *); 67}; 68 69static const struct clk_ops st_quadfs_pll_c32_ops; 70 71static int clk_fs660c32_dig_get_params(unsigned long input, 72 unsigned long output, struct stm_fs *fs); 73static int clk_fs660c32_dig_get_rate(unsigned long, const struct stm_fs *, 74 unsigned long *); 75 76static const struct clkgen_quadfs_data st_fs660c32_C = { 77 .nrst_present = true, 78 .nrst = { CLKGEN_FIELD(0x2f0, 0x1, 0), 79 CLKGEN_FIELD(0x2f0, 0x1, 1), 80 CLKGEN_FIELD(0x2f0, 0x1, 2), 81 CLKGEN_FIELD(0x2f0, 0x1, 3) }, 82 .npda = CLKGEN_FIELD(0x2f0, 0x1, 12), 83 .nsb = { CLKGEN_FIELD(0x2f0, 0x1, 8), 84 CLKGEN_FIELD(0x2f0, 0x1, 9), 85 CLKGEN_FIELD(0x2f0, 0x1, 10), 86 CLKGEN_FIELD(0x2f0, 0x1, 11) }, 87 .nsdiv_present = true, 88 .nsdiv = { CLKGEN_FIELD(0x304, 0x1, 24), 89 CLKGEN_FIELD(0x308, 0x1, 24), 90 CLKGEN_FIELD(0x30c, 0x1, 24), 91 CLKGEN_FIELD(0x310, 0x1, 24) }, 92 .mdiv = { CLKGEN_FIELD(0x304, 0x1f, 15), 93 CLKGEN_FIELD(0x308, 0x1f, 15), 94 CLKGEN_FIELD(0x30c, 0x1f, 15), 95 CLKGEN_FIELD(0x310, 0x1f, 15) }, 96 .en = { CLKGEN_FIELD(0x2fc, 0x1, 0), 97 CLKGEN_FIELD(0x2fc, 0x1, 1), 98 CLKGEN_FIELD(0x2fc, 0x1, 2), 99 CLKGEN_FIELD(0x2fc, 0x1, 3) }, 100 .ndiv = CLKGEN_FIELD(0x2f4, 0x7, 16), 101 .pe = { CLKGEN_FIELD(0x304, 0x7fff, 0), 102 CLKGEN_FIELD(0x308, 0x7fff, 0), 103 CLKGEN_FIELD(0x30c, 0x7fff, 0), 104 CLKGEN_FIELD(0x310, 0x7fff, 0) }, 105 .sdiv = { CLKGEN_FIELD(0x304, 0xf, 20), 106 CLKGEN_FIELD(0x308, 0xf, 20), 107 CLKGEN_FIELD(0x30c, 0xf, 20), 108 CLKGEN_FIELD(0x310, 0xf, 20) }, 109 .lockstatus_present = true, 110 .lock_status = CLKGEN_FIELD(0x2f0, 0x1, 24), 111 .powerup_polarity = 1, 112 .standby_polarity = 1, 113 .pll_ops = &st_quadfs_pll_c32_ops, 114 .get_params = clk_fs660c32_dig_get_params, 115 .get_rate = clk_fs660c32_dig_get_rate, 116}; 117 118static const struct clkgen_quadfs_data st_fs660c32_D = { 119 .nrst_present = true, 120 .nrst = { CLKGEN_FIELD(0x2a0, 0x1, 0), 121 CLKGEN_FIELD(0x2a0, 0x1, 1), 122 CLKGEN_FIELD(0x2a0, 0x1, 2), 123 CLKGEN_FIELD(0x2a0, 0x1, 3) }, 124 .ndiv = CLKGEN_FIELD(0x2a4, 0x7, 16), 125 .pe = { CLKGEN_FIELD(0x2b4, 0x7fff, 0), 126 CLKGEN_FIELD(0x2b8, 0x7fff, 0), 127 CLKGEN_FIELD(0x2bc, 0x7fff, 0), 128 CLKGEN_FIELD(0x2c0, 0x7fff, 0) }, 129 .sdiv = { CLKGEN_FIELD(0x2b4, 0xf, 20), 130 CLKGEN_FIELD(0x2b8, 0xf, 20), 131 CLKGEN_FIELD(0x2bc, 0xf, 20), 132 CLKGEN_FIELD(0x2c0, 0xf, 20) }, 133 .npda = CLKGEN_FIELD(0x2a0, 0x1, 12), 134 .nsb = { CLKGEN_FIELD(0x2a0, 0x1, 8), 135 CLKGEN_FIELD(0x2a0, 0x1, 9), 136 CLKGEN_FIELD(0x2a0, 0x1, 10), 137 CLKGEN_FIELD(0x2a0, 0x1, 11) }, 138 .nsdiv_present = true, 139 .nsdiv = { CLKGEN_FIELD(0x2b4, 0x1, 24), 140 CLKGEN_FIELD(0x2b8, 0x1, 24), 141 CLKGEN_FIELD(0x2bc, 0x1, 24), 142 CLKGEN_FIELD(0x2c0, 0x1, 24) }, 143 .mdiv = { CLKGEN_FIELD(0x2b4, 0x1f, 15), 144 CLKGEN_FIELD(0x2b8, 0x1f, 15), 145 CLKGEN_FIELD(0x2bc, 0x1f, 15), 146 CLKGEN_FIELD(0x2c0, 0x1f, 15) }, 147 .en = { CLKGEN_FIELD(0x2ac, 0x1, 0), 148 CLKGEN_FIELD(0x2ac, 0x1, 1), 149 CLKGEN_FIELD(0x2ac, 0x1, 2), 150 CLKGEN_FIELD(0x2ac, 0x1, 3) }, 151 .lockstatus_present = true, 152 .lock_status = CLKGEN_FIELD(0x2A0, 0x1, 24), 153 .powerup_polarity = 1, 154 .standby_polarity = 1, 155 .pll_ops = &st_quadfs_pll_c32_ops, 156 .get_params = clk_fs660c32_dig_get_params, 157 .get_rate = clk_fs660c32_dig_get_rate,}; 158 159/** 160 * DOC: A Frequency Synthesizer that multiples its input clock by a fixed factor 161 * 162 * Traits of this clock: 163 * prepare - clk_(un)prepare only ensures parent is (un)prepared 164 * enable - clk_enable and clk_disable are functional & control the Fsyn 165 * rate - inherits rate from parent. set_rate/round_rate/recalc_rate 166 * parent - fixed parent. No clk_set_parent support 167 */ 168 169/** 170 * struct st_clk_quadfs_pll - A pll which outputs a fixed multiplier of 171 * its parent clock, found inside a type of 172 * ST quad channel frequency synthesizer block 173 * 174 * @hw: handle between common and hardware-specific interfaces. 175 * @ndiv: regmap field for the ndiv control. 176 * @regs_base: base address of the configuration registers. 177 * @lock: spinlock. 178 * 179 */ 180struct st_clk_quadfs_pll { 181 struct clk_hw hw; 182 void __iomem *regs_base; 183 spinlock_t *lock; 184 struct clkgen_quadfs_data *data; 185 u32 ndiv; 186}; 187 188#define to_quadfs_pll(_hw) container_of(_hw, struct st_clk_quadfs_pll, hw) 189 190static int quadfs_pll_enable(struct clk_hw *hw) 191{ 192 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw); 193 unsigned long flags = 0, timeout = jiffies + msecs_to_jiffies(10); 194 195 if (pll->lock) 196 spin_lock_irqsave(pll->lock, flags); 197 198 /* 199 * Bring block out of reset if we have reset control. 200 */ 201 if (pll->data->reset_present) 202 CLKGEN_WRITE(pll, nreset, 1); 203 204 /* 205 * Use a fixed input clock noise bandwidth filter for the moment 206 */ 207 if (pll->data->bwfilter_present) 208 CLKGEN_WRITE(pll, ref_bw, PLL_BW_GOODREF); 209 210 211 CLKGEN_WRITE(pll, ndiv, pll->ndiv); 212 213 /* 214 * Power up the PLL 215 */ 216 CLKGEN_WRITE(pll, npda, !pll->data->powerup_polarity); 217 218 if (pll->lock) 219 spin_unlock_irqrestore(pll->lock, flags); 220 221 if (pll->data->lockstatus_present) 222 while (!CLKGEN_READ(pll, lock_status)) { 223 if (time_after(jiffies, timeout)) 224 return -ETIMEDOUT; 225 cpu_relax(); 226 } 227 228 return 0; 229} 230 231static void quadfs_pll_disable(struct clk_hw *hw) 232{ 233 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw); 234 unsigned long flags = 0; 235 236 if (pll->lock) 237 spin_lock_irqsave(pll->lock, flags); 238 239 /* 240 * Powerdown the PLL and then put block into soft reset if we have 241 * reset control. 242 */ 243 CLKGEN_WRITE(pll, npda, pll->data->powerup_polarity); 244 245 if (pll->data->reset_present) 246 CLKGEN_WRITE(pll, nreset, 0); 247 248 if (pll->lock) 249 spin_unlock_irqrestore(pll->lock, flags); 250} 251 252static int quadfs_pll_is_enabled(struct clk_hw *hw) 253{ 254 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw); 255 u32 npda = CLKGEN_READ(pll, npda); 256 257 return pll->data->powerup_polarity ? !npda : !!npda; 258} 259 260static int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs, 261 unsigned long *rate) 262{ 263 unsigned long nd = fs->ndiv + 16; /* ndiv value */ 264 265 *rate = input * nd; 266 267 return 0; 268} 269 270static unsigned long quadfs_pll_fs660c32_recalc_rate(struct clk_hw *hw, 271 unsigned long parent_rate) 272{ 273 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw); 274 unsigned long rate = 0; 275 struct stm_fs params; 276 277 params.ndiv = CLKGEN_READ(pll, ndiv); 278 if (clk_fs660c32_vco_get_rate(parent_rate, ¶ms, &rate)) 279 pr_err("%s:%s error calculating rate\n", 280 clk_hw_get_name(hw), __func__); 281 282 pll->ndiv = params.ndiv; 283 284 return rate; 285} 286 287static int clk_fs660c32_vco_get_params(unsigned long input, 288 unsigned long output, struct stm_fs *fs) 289{ 290/* Formula 291 VCO frequency = (fin x ndiv) / pdiv 292 ndiv = VCOfreq * pdiv / fin 293 */ 294 unsigned long pdiv = 1, n; 295 296 /* Output clock range: 384Mhz to 660Mhz */ 297 if (output < 384000000 || output > 660000000) 298 return -EINVAL; 299 300 if (input > 40000000) 301 /* This means that PDIV would be 2 instead of 1. 302 Not supported today. */ 303 return -EINVAL; 304 305 input /= 1000; 306 output /= 1000; 307 308 n = output * pdiv / input; 309 if (n < 16) 310 n = 16; 311 fs->ndiv = n - 16; /* Converting formula value to reg value */ 312 313 return 0; 314} 315 316static long quadfs_pll_fs660c32_round_rate(struct clk_hw *hw, 317 unsigned long rate, 318 unsigned long *prate) 319{ 320 struct stm_fs params; 321 322 if (clk_fs660c32_vco_get_params(*prate, rate, ¶ms)) 323 return rate; 324 325 clk_fs660c32_vco_get_rate(*prate, ¶ms, &rate); 326 327 pr_debug("%s: %s new rate %ld [ndiv=%u]\n", 328 __func__, clk_hw_get_name(hw), 329 rate, (unsigned int)params.ndiv); 330 331 return rate; 332} 333 334static int quadfs_pll_fs660c32_set_rate(struct clk_hw *hw, unsigned long rate, 335 unsigned long parent_rate) 336{ 337 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw); 338 struct stm_fs params; 339 long hwrate = 0; 340 unsigned long flags = 0; 341 int ret; 342 343 if (!rate || !parent_rate) 344 return -EINVAL; 345 346 ret = clk_fs660c32_vco_get_params(parent_rate, rate, ¶ms); 347 if (ret) 348 return ret; 349 350 clk_fs660c32_vco_get_rate(parent_rate, ¶ms, &hwrate); 351 352 pr_debug("%s: %s new rate %ld [ndiv=0x%x]\n", 353 __func__, clk_hw_get_name(hw), 354 hwrate, (unsigned int)params.ndiv); 355 356 if (!hwrate) 357 return -EINVAL; 358 359 pll->ndiv = params.ndiv; 360 361 if (pll->lock) 362 spin_lock_irqsave(pll->lock, flags); 363 364 CLKGEN_WRITE(pll, ndiv, pll->ndiv); 365 366 if (pll->lock) 367 spin_unlock_irqrestore(pll->lock, flags); 368 369 return 0; 370} 371 372static const struct clk_ops st_quadfs_pll_c32_ops = { 373 .enable = quadfs_pll_enable, 374 .disable = quadfs_pll_disable, 375 .is_enabled = quadfs_pll_is_enabled, 376 .recalc_rate = quadfs_pll_fs660c32_recalc_rate, 377 .round_rate = quadfs_pll_fs660c32_round_rate, 378 .set_rate = quadfs_pll_fs660c32_set_rate, 379}; 380 381static struct clk * __init st_clk_register_quadfs_pll( 382 const char *name, const char *parent_name, 383 struct clkgen_quadfs_data *quadfs, void __iomem *reg, 384 spinlock_t *lock) 385{ 386 struct st_clk_quadfs_pll *pll; 387 struct clk *clk; 388 struct clk_init_data init; 389 390 /* 391 * Sanity check required pointers. 392 */ 393 if (WARN_ON(!name || !parent_name)) 394 return ERR_PTR(-EINVAL); 395 396 pll = kzalloc(sizeof(*pll), GFP_KERNEL); 397 if (!pll) 398 return ERR_PTR(-ENOMEM); 399 400 init.name = name; 401 init.ops = quadfs->pll_ops; 402 init.flags = CLK_GET_RATE_NOCACHE; 403 init.parent_names = &parent_name; 404 init.num_parents = 1; 405 406 pll->data = quadfs; 407 pll->regs_base = reg; 408 pll->lock = lock; 409 pll->hw.init = &init; 410 411 clk = clk_register(NULL, &pll->hw); 412 413 if (IS_ERR(clk)) 414 kfree(pll); 415 416 return clk; 417} 418 419/** 420 * DOC: A digital frequency synthesizer 421 * 422 * Traits of this clock: 423 * prepare - clk_(un)prepare only ensures parent is (un)prepared 424 * enable - clk_enable and clk_disable are functional 425 * rate - set rate is functional 426 * parent - fixed parent. No clk_set_parent support 427 */ 428 429/** 430 * struct st_clk_quadfs_fsynth - One clock output from a four channel digital 431 * frequency synthesizer (fsynth) block. 432 * 433 * @hw: handle between common and hardware-specific interfaces 434 * 435 * @nsb: regmap field in the output control register for the digital 436 * standby of this fsynth channel. This control is active low so 437 * the channel is in standby when the control bit is cleared. 438 * 439 * @nsdiv: regmap field in the output control register for 440 * for the optional divide by 3 of this fsynth channel. This control 441 * is active low so the divide by 3 is active when the control bit is 442 * cleared and the divide is bypassed when the bit is set. 443 */ 444struct st_clk_quadfs_fsynth { 445 struct clk_hw hw; 446 void __iomem *regs_base; 447 spinlock_t *lock; 448 struct clkgen_quadfs_data *data; 449 450 u32 chan; 451 /* 452 * Cached hardware values from set_rate so we can program the 453 * hardware in enable. There are two reasons for this: 454 * 455 * 1. The registers may not be writable until the parent has been 456 * enabled. 457 * 458 * 2. It restores the clock rate when a driver does an enable 459 * on PM restore, after a suspend to RAM has lost the hardware 460 * setup. 461 */ 462 u32 md; 463 u32 pe; 464 u32 sdiv; 465 u32 nsdiv; 466}; 467 468#define to_quadfs_fsynth(_hw) \ 469 container_of(_hw, struct st_clk_quadfs_fsynth, hw) 470 471static void quadfs_fsynth_program_enable(struct st_clk_quadfs_fsynth *fs) 472{ 473 /* 474 * Pulse the program enable register lsb to make the hardware take 475 * notice of the new md/pe values with a glitchless transition. 476 */ 477 CLKGEN_WRITE(fs, en[fs->chan], 1); 478 CLKGEN_WRITE(fs, en[fs->chan], 0); 479} 480 481static void quadfs_fsynth_program_rate(struct st_clk_quadfs_fsynth *fs) 482{ 483 unsigned long flags = 0; 484 485 /* 486 * Ensure the md/pe parameters are ignored while we are 487 * reprogramming them so we can get a glitchless change 488 * when fine tuning the speed of a running clock. 489 */ 490 CLKGEN_WRITE(fs, en[fs->chan], 0); 491 492 CLKGEN_WRITE(fs, mdiv[fs->chan], fs->md); 493 CLKGEN_WRITE(fs, pe[fs->chan], fs->pe); 494 CLKGEN_WRITE(fs, sdiv[fs->chan], fs->sdiv); 495 496 if (fs->lock) 497 spin_lock_irqsave(fs->lock, flags); 498 499 if (fs->data->nsdiv_present) 500 CLKGEN_WRITE(fs, nsdiv[fs->chan], fs->nsdiv); 501 502 if (fs->lock) 503 spin_unlock_irqrestore(fs->lock, flags); 504} 505 506static int quadfs_fsynth_enable(struct clk_hw *hw) 507{ 508 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw); 509 unsigned long flags = 0; 510 511 pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw)); 512 513 quadfs_fsynth_program_rate(fs); 514 515 if (fs->lock) 516 spin_lock_irqsave(fs->lock, flags); 517 518 CLKGEN_WRITE(fs, nsb[fs->chan], !fs->data->standby_polarity); 519 520 if (fs->data->nrst_present) 521 CLKGEN_WRITE(fs, nrst[fs->chan], 0); 522 523 if (fs->lock) 524 spin_unlock_irqrestore(fs->lock, flags); 525 526 quadfs_fsynth_program_enable(fs); 527 528 return 0; 529} 530 531static void quadfs_fsynth_disable(struct clk_hw *hw) 532{ 533 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw); 534 unsigned long flags = 0; 535 536 pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw)); 537 538 if (fs->lock) 539 spin_lock_irqsave(fs->lock, flags); 540 541 CLKGEN_WRITE(fs, nsb[fs->chan], fs->data->standby_polarity); 542 543 if (fs->lock) 544 spin_unlock_irqrestore(fs->lock, flags); 545} 546 547static int quadfs_fsynth_is_enabled(struct clk_hw *hw) 548{ 549 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw); 550 u32 nsb = CLKGEN_READ(fs, nsb[fs->chan]); 551 552 pr_debug("%s: %s enable bit = 0x%x\n", 553 __func__, clk_hw_get_name(hw), nsb); 554 555 return fs->data->standby_polarity ? !nsb : !!nsb; 556} 557 558#define P20 (uint64_t)(1 << 20) 559 560static int clk_fs660c32_dig_get_rate(unsigned long input, 561 const struct stm_fs *fs, unsigned long *rate) 562{ 563 unsigned long s = (1 << fs->sdiv); 564 unsigned long ns; 565 uint64_t res; 566 567 /* 568 * 'nsdiv' is a register value ('BIN') which is translated 569 * to a decimal value according to following rules. 570 * 571 * nsdiv ns.dec 572 * 0 3 573 * 1 1 574 */ 575 ns = (fs->nsdiv == 1) ? 1 : 3; 576 577 res = (P20 * (32 + fs->mdiv) + 32 * fs->pe) * s * ns; 578 *rate = (unsigned long)div64_u64(input * P20 * 32, res); 579 580 return 0; 581} 582 583 584static int clk_fs660c32_get_pe(int m, int si, unsigned long *deviation, 585 signed long input, unsigned long output, uint64_t *p, 586 struct stm_fs *fs) 587{ 588 unsigned long new_freq, new_deviation; 589 struct stm_fs fs_tmp; 590 uint64_t val; 591 592 val = (uint64_t)output << si; 593 594 *p = (uint64_t)input * P20 - (32LL + (uint64_t)m) * val * (P20 / 32LL); 595 596 *p = div64_u64(*p, val); 597 598 if (*p > 32767LL) 599 return 1; 600 601 fs_tmp.mdiv = (unsigned long) m; 602 fs_tmp.pe = (unsigned long)*p; 603 fs_tmp.sdiv = si; 604 fs_tmp.nsdiv = 1; 605 606 clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq); 607 608 new_deviation = abs(output - new_freq); 609 610 if (new_deviation < *deviation) { 611 fs->mdiv = m; 612 fs->pe = (unsigned long)*p; 613 fs->sdiv = si; 614 fs->nsdiv = 1; 615 *deviation = new_deviation; 616 } 617 return 0; 618} 619 620static int clk_fs660c32_dig_get_params(unsigned long input, 621 unsigned long output, struct stm_fs *fs) 622{ 623 int si; /* sdiv_reg (8 downto 0) */ 624 int m; /* md value */ 625 unsigned long new_freq, new_deviation; 626 /* initial condition to say: "infinite deviation" */ 627 unsigned long deviation = ~0; 628 uint64_t p, p1, p2; /* pe value */ 629 int r1, r2; 630 631 struct stm_fs fs_tmp; 632 633 for (si = 0; (si <= 8) && deviation; si++) { 634 635 /* Boundary test to avoid useless iteration */ 636 r1 = clk_fs660c32_get_pe(0, si, &deviation, 637 input, output, &p1, fs); 638 r2 = clk_fs660c32_get_pe(31, si, &deviation, 639 input, output, &p2, fs); 640 641 /* No solution */ 642 if (r1 && r2 && (p1 > p2)) 643 continue; 644 645 /* Try to find best deviation */ 646 for (m = 1; (m < 31) && deviation; m++) 647 clk_fs660c32_get_pe(m, si, &deviation, 648 input, output, &p, fs); 649 650 } 651 652 if (deviation == ~0) /* No solution found */ 653 return -1; 654 655 /* pe fine tuning if deviation not 0: +/- 2 around computed pe value */ 656 if (deviation) { 657 fs_tmp.mdiv = fs->mdiv; 658 fs_tmp.sdiv = fs->sdiv; 659 fs_tmp.nsdiv = fs->nsdiv; 660 661 if (fs->pe > 2) 662 p2 = fs->pe - 2; 663 else 664 p2 = 0; 665 666 for (; p2 < 32768ll && (p2 <= (fs->pe + 2)); p2++) { 667 fs_tmp.pe = (unsigned long)p2; 668 669 clk_fs660c32_dig_get_rate(input, &fs_tmp, &new_freq); 670 671 new_deviation = abs(output - new_freq); 672 673 /* Check if this is a better solution */ 674 if (new_deviation < deviation) { 675 fs->pe = (unsigned long)p2; 676 deviation = new_deviation; 677 678 } 679 } 680 } 681 return 0; 682} 683 684static int quadfs_fsynt_get_hw_value_for_recalc(struct st_clk_quadfs_fsynth *fs, 685 struct stm_fs *params) 686{ 687 /* 688 * Get the initial hardware values for recalc_rate 689 */ 690 params->mdiv = CLKGEN_READ(fs, mdiv[fs->chan]); 691 params->pe = CLKGEN_READ(fs, pe[fs->chan]); 692 params->sdiv = CLKGEN_READ(fs, sdiv[fs->chan]); 693 694 if (fs->data->nsdiv_present) 695 params->nsdiv = CLKGEN_READ(fs, nsdiv[fs->chan]); 696 else 697 params->nsdiv = 1; 698 699 /* 700 * If All are NULL then assume no clock rate is programmed. 701 */ 702 if (!params->mdiv && !params->pe && !params->sdiv) 703 return 1; 704 705 fs->md = params->mdiv; 706 fs->pe = params->pe; 707 fs->sdiv = params->sdiv; 708 fs->nsdiv = params->nsdiv; 709 710 return 0; 711} 712 713static long quadfs_find_best_rate(struct clk_hw *hw, unsigned long drate, 714 unsigned long prate, struct stm_fs *params) 715{ 716 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw); 717 int (*clk_fs_get_rate)(unsigned long , 718 const struct stm_fs *, unsigned long *); 719 int (*clk_fs_get_params)(unsigned long, unsigned long, struct stm_fs *); 720 unsigned long rate = 0; 721 722 clk_fs_get_rate = fs->data->get_rate; 723 clk_fs_get_params = fs->data->get_params; 724 725 if (!clk_fs_get_params(prate, drate, params)) 726 clk_fs_get_rate(prate, params, &rate); 727 728 return rate; 729} 730 731static unsigned long quadfs_recalc_rate(struct clk_hw *hw, 732 unsigned long parent_rate) 733{ 734 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw); 735 unsigned long rate = 0; 736 struct stm_fs params; 737 int (*clk_fs_get_rate)(unsigned long , 738 const struct stm_fs *, unsigned long *); 739 740 clk_fs_get_rate = fs->data->get_rate; 741 742 if (quadfs_fsynt_get_hw_value_for_recalc(fs, ¶ms)) 743 return 0; 744 745 if (clk_fs_get_rate(parent_rate, ¶ms, &rate)) { 746 pr_err("%s:%s error calculating rate\n", 747 clk_hw_get_name(hw), __func__); 748 } 749 750 pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate); 751 752 return rate; 753} 754 755static long quadfs_round_rate(struct clk_hw *hw, unsigned long rate, 756 unsigned long *prate) 757{ 758 struct stm_fs params; 759 760 rate = quadfs_find_best_rate(hw, rate, *prate, ¶ms); 761 762 pr_debug("%s: %s new rate %ld [sdiv=0x%x,md=0x%x,pe=0x%x,nsdiv3=%u]\n", 763 __func__, clk_hw_get_name(hw), 764 rate, (unsigned int)params.sdiv, (unsigned int)params.mdiv, 765 (unsigned int)params.pe, (unsigned int)params.nsdiv); 766 767 return rate; 768} 769 770 771static void quadfs_program_and_enable(struct st_clk_quadfs_fsynth *fs, 772 struct stm_fs *params) 773{ 774 fs->md = params->mdiv; 775 fs->pe = params->pe; 776 fs->sdiv = params->sdiv; 777 fs->nsdiv = params->nsdiv; 778 779 /* 780 * In some integrations you can only change the fsynth programming when 781 * the parent entity containing it is enabled. 782 */ 783 quadfs_fsynth_program_rate(fs); 784 quadfs_fsynth_program_enable(fs); 785} 786 787static int quadfs_set_rate(struct clk_hw *hw, unsigned long rate, 788 unsigned long parent_rate) 789{ 790 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw); 791 struct stm_fs params; 792 long hwrate; 793 794 if (!rate || !parent_rate) 795 return -EINVAL; 796 797 memset(¶ms, 0, sizeof(struct stm_fs)); 798 799 hwrate = quadfs_find_best_rate(hw, rate, parent_rate, ¶ms); 800 if (!hwrate) 801 return -EINVAL; 802 803 quadfs_program_and_enable(fs, ¶ms); 804 805 return 0; 806} 807 808 809 810static const struct clk_ops st_quadfs_ops = { 811 .enable = quadfs_fsynth_enable, 812 .disable = quadfs_fsynth_disable, 813 .is_enabled = quadfs_fsynth_is_enabled, 814 .round_rate = quadfs_round_rate, 815 .set_rate = quadfs_set_rate, 816 .recalc_rate = quadfs_recalc_rate, 817}; 818 819static struct clk * __init st_clk_register_quadfs_fsynth( 820 const char *name, const char *parent_name, 821 struct clkgen_quadfs_data *quadfs, void __iomem *reg, u32 chan, 822 unsigned long flags, spinlock_t *lock) 823{ 824 struct st_clk_quadfs_fsynth *fs; 825 struct clk *clk; 826 struct clk_init_data init; 827 828 /* 829 * Sanity check required pointers, note that nsdiv3 is optional. 830 */ 831 if (WARN_ON(!name || !parent_name)) 832 return ERR_PTR(-EINVAL); 833 834 fs = kzalloc(sizeof(*fs), GFP_KERNEL); 835 if (!fs) 836 return ERR_PTR(-ENOMEM); 837 838 init.name = name; 839 init.ops = &st_quadfs_ops; 840 init.flags = flags | CLK_GET_RATE_NOCACHE; 841 init.parent_names = &parent_name; 842 init.num_parents = 1; 843 844 fs->data = quadfs; 845 fs->regs_base = reg; 846 fs->chan = chan; 847 fs->lock = lock; 848 fs->hw.init = &init; 849 850 clk = clk_register(NULL, &fs->hw); 851 852 if (IS_ERR(clk)) 853 kfree(fs); 854 855 return clk; 856} 857 858static void __init st_of_create_quadfs_fsynths( 859 struct device_node *np, const char *pll_name, 860 struct clkgen_quadfs_data *quadfs, void __iomem *reg, 861 spinlock_t *lock) 862{ 863 struct clk_onecell_data *clk_data; 864 int fschan; 865 866 clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL); 867 if (!clk_data) 868 return; 869 870 clk_data->clk_num = QUADFS_MAX_CHAN; 871 clk_data->clks = kcalloc(QUADFS_MAX_CHAN, sizeof(struct clk *), 872 GFP_KERNEL); 873 874 if (!clk_data->clks) { 875 kfree(clk_data); 876 return; 877 } 878 879 for (fschan = 0; fschan < QUADFS_MAX_CHAN; fschan++) { 880 struct clk *clk; 881 const char *clk_name; 882 unsigned long flags = 0; 883 884 if (of_property_read_string_index(np, "clock-output-names", 885 fschan, &clk_name)) { 886 break; 887 } 888 889 /* 890 * If we read an empty clock name then the channel is unused 891 */ 892 if (*clk_name == '\0') 893 continue; 894 895 of_clk_detect_critical(np, fschan, &flags); 896 897 clk = st_clk_register_quadfs_fsynth(clk_name, pll_name, 898 quadfs, reg, fschan, 899 flags, lock); 900 901 /* 902 * If there was an error registering this clock output, clean 903 * up and move on to the next one. 904 */ 905 if (!IS_ERR(clk)) { 906 clk_data->clks[fschan] = clk; 907 pr_debug("%s: parent %s rate %u\n", 908 __clk_get_name(clk), 909 __clk_get_name(clk_get_parent(clk)), 910 (unsigned int)clk_get_rate(clk)); 911 } 912 } 913 914 of_clk_add_provider(np, of_clk_src_onecell_get, clk_data); 915} 916 917static void __init st_of_quadfs_setup(struct device_node *np, 918 struct clkgen_quadfs_data *data) 919{ 920 struct clk *clk; 921 const char *pll_name, *clk_parent_name; 922 void __iomem *reg; 923 spinlock_t *lock; 924 925 reg = of_iomap(np, 0); 926 if (!reg) 927 return; 928 929 clk_parent_name = of_clk_get_parent_name(np, 0); 930 if (!clk_parent_name) 931 return; 932 933 pll_name = kasprintf(GFP_KERNEL, "%pOFn.pll", np); 934 if (!pll_name) 935 return; 936 937 lock = kzalloc(sizeof(*lock), GFP_KERNEL); 938 if (!lock) 939 goto err_exit; 940 941 spin_lock_init(lock); 942 943 clk = st_clk_register_quadfs_pll(pll_name, clk_parent_name, data, 944 reg, lock); 945 if (IS_ERR(clk)) { 946 kfree(lock); 947 goto err_exit; 948 } else 949 pr_debug("%s: parent %s rate %u\n", 950 __clk_get_name(clk), 951 __clk_get_name(clk_get_parent(clk)), 952 (unsigned int)clk_get_rate(clk)); 953 954 st_of_create_quadfs_fsynths(np, pll_name, data, reg, lock); 955 956err_exit: 957 kfree(pll_name); /* No longer need local copy of the PLL name */ 958} 959 960static void __init st_of_quadfs660C_setup(struct device_node *np) 961{ 962 st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_C); 963} 964CLK_OF_DECLARE(quadfs660C, "st,quadfs-pll", st_of_quadfs660C_setup); 965 966static void __init st_of_quadfs660D_setup(struct device_node *np) 967{ 968 st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_D); 969} 970CLK_OF_DECLARE(quadfs660D, "st,quadfs", st_of_quadfs660D_setup); 971