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