2 * Copyright (C) 2014 STMicroelectronics R&D Ltd
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.
12 * Stephen Gallimore <stephen.gallimore@st.com>,
13 * Pankaj Dev <pankaj.dev@st.com>.
16 #include <linux/slab.h>
17 #include <linux/of_address.h>
18 #include <linux/clk.h>
19 #include <linux/clk-provider.h>
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
28 #define QUADFS_NDIV_THRESHOLD 30000000
30 #define PLL_BW_GOODREF (0L)
31 #define PLL_BW_VBADREF (1L)
32 #define PLL_BW_BADREF (2L)
33 #define PLL_BW_VGOODREF (3L)
35 #define QUADFS_MAX_CHAN 4
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 */
79 struct clkgen_quadfs_data {
81 bool bwfilter_present;
82 bool lockstatus_present;
83 bool powerup_polarity;
84 bool standby_polarity;
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;
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];
101 const struct clk_ops *pll_ops;
102 const struct stm_fs *rtbl;
104 int (*get_rate)(unsigned long , const struct stm_fs *,
108 static const struct clk_ops st_quadfs_pll_c32_ops;
109 static const struct clk_ops st_quadfs_fs660c32_ops;
111 static int clk_fs660c32_dig_get_rate(unsigned long, const struct stm_fs *,
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,
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,};
200 * DOC: A Frequency Synthesizer that multiples its input clock by a fixed factor
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
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
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.
220 struct st_clk_quadfs_pll {
222 void __iomem *regs_base;
224 struct clkgen_quadfs_data *data;
228 #define to_quadfs_pll(_hw) container_of(_hw, struct st_clk_quadfs_pll, hw)
230 static int quadfs_pll_enable(struct clk_hw *hw)
232 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
233 unsigned long flags = 0, timeout = jiffies + msecs_to_jiffies(10);
236 spin_lock_irqsave(pll->lock, flags);
239 * Bring block out of reset if we have reset control.
241 if (pll->data->reset_present)
242 CLKGEN_WRITE(pll, nreset, 1);
245 * Use a fixed input clock noise bandwidth filter for the moment
247 if (pll->data->bwfilter_present)
248 CLKGEN_WRITE(pll, ref_bw, PLL_BW_GOODREF);
251 CLKGEN_WRITE(pll, ndiv, pll->ndiv);
256 CLKGEN_WRITE(pll, npda, !pll->data->powerup_polarity);
259 spin_unlock_irqrestore(pll->lock, flags);
261 if (pll->data->lockstatus_present)
262 while (!CLKGEN_READ(pll, lock_status)) {
263 if (time_after(jiffies, timeout))
271 static void quadfs_pll_disable(struct clk_hw *hw)
273 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
274 unsigned long flags = 0;
277 spin_lock_irqsave(pll->lock, flags);
280 * Powerdown the PLL and then put block into soft reset if we have
283 CLKGEN_WRITE(pll, npda, pll->data->powerup_polarity);
285 if (pll->data->reset_present)
286 CLKGEN_WRITE(pll, nreset, 0);
289 spin_unlock_irqrestore(pll->lock, flags);
292 static int quadfs_pll_is_enabled(struct clk_hw *hw)
294 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
295 u32 npda = CLKGEN_READ(pll, npda);
297 return pll->data->powerup_polarity ? !npda : !!npda;
300 static int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs,
303 unsigned long nd = fs->ndiv + 16; /* ndiv value */
310 static unsigned long quadfs_pll_fs660c32_recalc_rate(struct clk_hw *hw,
311 unsigned long parent_rate)
313 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
314 unsigned long rate = 0;
315 struct stm_fs params;
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__);
322 pll->ndiv = params.ndiv;
327 static int clk_fs660c32_vco_get_params(unsigned long input,
328 unsigned long output, struct stm_fs *fs)
331 VCO frequency = (fin x ndiv) / pdiv
332 ndiv = VCOfreq * pdiv / fin
334 unsigned long pdiv = 1, n;
336 /* Output clock range: 384Mhz to 660Mhz */
337 if (output < 384000000 || output > 660000000)
340 if (input > 40000000)
341 /* This means that PDIV would be 2 instead of 1.
342 Not supported today. */
348 n = output * pdiv / input;
351 fs->ndiv = n - 16; /* Converting formula value to reg value */
356 static long quadfs_pll_fs660c32_round_rate(struct clk_hw *hw,
358 unsigned long *prate)
360 struct stm_fs params;
362 if (clk_fs660c32_vco_get_params(*prate, rate, ¶ms))
365 clk_fs660c32_vco_get_rate(*prate, ¶ms, &rate);
367 pr_debug("%s: %s new rate %ld [ndiv=%u]\n",
368 __func__, clk_hw_get_name(hw),
369 rate, (unsigned int)params.ndiv);
374 static int quadfs_pll_fs660c32_set_rate(struct clk_hw *hw, unsigned long rate,
375 unsigned long parent_rate)
377 struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
378 struct stm_fs params;
380 unsigned long flags = 0;
383 if (!rate || !parent_rate)
386 ret = clk_fs660c32_vco_get_params(parent_rate, rate, ¶ms);
390 clk_fs660c32_vco_get_rate(parent_rate, ¶ms, &hwrate);
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);
399 pll->ndiv = params.ndiv;
402 spin_lock_irqsave(pll->lock, flags);
404 CLKGEN_WRITE(pll, ndiv, pll->ndiv);
407 spin_unlock_irqrestore(pll->lock, flags);
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,
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,
426 struct st_clk_quadfs_pll *pll;
428 struct clk_init_data init;
431 * Sanity check required pointers.
433 if (WARN_ON(!name || !parent_name))
434 return ERR_PTR(-EINVAL);
436 pll = kzalloc(sizeof(*pll), GFP_KERNEL);
438 return ERR_PTR(-ENOMEM);
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;
447 pll->regs_base = reg;
449 pll->hw.init = &init;
451 clk = clk_register(NULL, &pll->hw);
460 * DOC: A digital frequency synthesizer
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
470 * struct st_clk_quadfs_fsynth - One clock output from a four channel digital
471 * frequency synthesizer (fsynth) block.
473 * @hw: handle between common and hardware-specific interfaces
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.
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.
484 struct st_clk_quadfs_fsynth {
486 void __iomem *regs_base;
488 struct clkgen_quadfs_data *data;
492 * Cached hardware values from set_rate so we can program the
493 * hardware in enable. There are two reasons for this:
495 * 1. The registers may not be writable until the parent has been
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
508 #define to_quadfs_fsynth(_hw) \
509 container_of(_hw, struct st_clk_quadfs_fsynth, hw)
511 static void quadfs_fsynth_program_enable(struct st_clk_quadfs_fsynth *fs)
514 * Pulse the program enable register lsb to make the hardware take
515 * notice of the new md/pe values with a glitchless transition.
517 CLKGEN_WRITE(fs, en[fs->chan], 1);
518 CLKGEN_WRITE(fs, en[fs->chan], 0);
521 static void quadfs_fsynth_program_rate(struct st_clk_quadfs_fsynth *fs)
523 unsigned long flags = 0;
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.
530 CLKGEN_WRITE(fs, en[fs->chan], 0);
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);
537 spin_lock_irqsave(fs->lock, flags);
539 if (fs->data->nsdiv_present)
540 CLKGEN_WRITE(fs, nsdiv[fs->chan], fs->nsdiv);
543 spin_unlock_irqrestore(fs->lock, flags);
546 static int quadfs_fsynth_enable(struct clk_hw *hw)
548 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
549 unsigned long flags = 0;
551 pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
553 quadfs_fsynth_program_rate(fs);
556 spin_lock_irqsave(fs->lock, flags);
558 CLKGEN_WRITE(fs, nsb[fs->chan], !fs->data->standby_polarity);
560 if (fs->data->nrst_present)
561 CLKGEN_WRITE(fs, nrst[fs->chan], 0);
564 spin_unlock_irqrestore(fs->lock, flags);
566 quadfs_fsynth_program_enable(fs);
571 static void quadfs_fsynth_disable(struct clk_hw *hw)
573 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
574 unsigned long flags = 0;
576 pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
579 spin_lock_irqsave(fs->lock, flags);
581 CLKGEN_WRITE(fs, nsb[fs->chan], fs->data->standby_polarity);
584 spin_unlock_irqrestore(fs->lock, flags);
587 static int quadfs_fsynth_is_enabled(struct clk_hw *hw)
589 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
590 u32 nsb = CLKGEN_READ(fs, nsb[fs->chan]);
592 pr_debug("%s: %s enable bit = 0x%x\n",
593 __func__, clk_hw_get_name(hw), nsb);
595 return fs->data->standby_polarity ? !nsb : !!nsb;
598 #define P20 (uint64_t)(1 << 20)
600 static int clk_fs660c32_dig_get_rate(unsigned long input,
601 const struct stm_fs *fs, unsigned long *rate)
603 unsigned long s = (1 << fs->sdiv);
608 * 'nsdiv' is a register value ('BIN') which is translated
609 * to a decimal value according to following rules.
615 ns = (fs->nsdiv == 1) ? 1 : 3;
617 res = (P20 * (32 + fs->mdiv) + 32 * fs->pe) * s * ns;
618 *rate = (unsigned long)div64_u64(input * P20 * 32, res);
623 static int quadfs_fsynt_get_hw_value_for_recalc(struct st_clk_quadfs_fsynth *fs,
624 struct stm_fs *params)
627 * Get the initial hardware values for recalc_rate
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]);
633 if (fs->data->nsdiv_present)
634 params->nsdiv = CLKGEN_READ(fs, nsdiv[fs->chan]);
639 * If All are NULL then assume no clock rate is programmed.
641 if (!params->mdiv && !params->pe && !params->sdiv)
644 fs->md = params->mdiv;
646 fs->sdiv = params->sdiv;
647 fs->nsdiv = params->nsdiv;
652 static long quadfs_find_best_rate(struct clk_hw *hw, unsigned long drate,
653 unsigned long prate, struct stm_fs *params)
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;
663 clk_fs_get_rate = fs->data->get_rate;
665 for (index = 0; index < fs->data->rtbl_cnt; index++) {
668 *params = fs->data->rtbl[index];
669 prev_params = *params;
671 clk_fs_get_rate(prate, &fs->data->rtbl[index], &rate);
673 diff_rate = abs(drate - rate);
675 if (diff_rate > prev_diff_rate) {
677 *params = prev_params;
681 prev_diff_rate = diff_rate;
688 if (index == fs->data->rtbl_cnt)
689 *params = prev_params;
694 static unsigned long quadfs_recalc_rate(struct clk_hw *hw,
695 unsigned long parent_rate)
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 *);
703 clk_fs_get_rate = fs->data->get_rate;
705 if (quadfs_fsynt_get_hw_value_for_recalc(fs, ¶ms))
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__);
713 pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
718 static long quadfs_round_rate(struct clk_hw *hw, unsigned long rate,
719 unsigned long *prate)
721 struct stm_fs params;
723 rate = quadfs_find_best_rate(hw, rate, *prate, ¶ms);
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);
734 static void quadfs_program_and_enable(struct st_clk_quadfs_fsynth *fs,
735 struct stm_fs *params)
737 fs->md = params->mdiv;
739 fs->sdiv = params->sdiv;
740 fs->nsdiv = params->nsdiv;
743 * In some integrations you can only change the fsynth programming when
744 * the parent entity containing it is enabled.
746 quadfs_fsynth_program_rate(fs);
747 quadfs_fsynth_program_enable(fs);
750 static int quadfs_set_rate(struct clk_hw *hw, unsigned long rate,
751 unsigned long parent_rate)
753 struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
754 struct stm_fs params;
756 int uninitialized_var(i);
758 if (!rate || !parent_rate)
761 memset(¶ms, 0, sizeof(struct stm_fs));
763 hwrate = quadfs_find_best_rate(hw, rate, parent_rate, ¶ms);
767 quadfs_program_and_enable(fs, ¶ms);
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,
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)
788 struct st_clk_quadfs_fsynth *fs;
790 struct clk_init_data init;
793 * Sanity check required pointers, note that nsdiv3 is optional.
795 if (WARN_ON(!name || !parent_name))
796 return ERR_PTR(-EINVAL);
798 fs = kzalloc(sizeof(*fs), GFP_KERNEL);
800 return ERR_PTR(-ENOMEM);
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;
814 clk = clk_register(NULL, &fs->hw);
822 static void __init st_of_create_quadfs_fsynths(
823 struct device_node *np, const char *pll_name,
824 struct clkgen_quadfs_data *quadfs, void __iomem *reg,
827 struct clk_onecell_data *clk_data;
830 clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
834 clk_data->clk_num = QUADFS_MAX_CHAN;
835 clk_data->clks = kzalloc(QUADFS_MAX_CHAN * sizeof(struct clk *),
838 if (!clk_data->clks) {
843 for (fschan = 0; fschan < QUADFS_MAX_CHAN; fschan++) {
845 const char *clk_name;
846 unsigned long flags = 0;
848 if (of_property_read_string_index(np, "clock-output-names",
849 fschan, &clk_name)) {
854 * If we read an empty clock name then the channel is unused
856 if (*clk_name == '\0')
859 of_clk_detect_critical(np, fschan, &flags);
861 clk = st_clk_register_quadfs_fsynth(clk_name, pll_name,
866 * If there was an error registering this clock output, clean
867 * up and move on to the next one.
870 clk_data->clks[fschan] = clk;
871 pr_debug("%s: parent %s rate %u\n",
873 __clk_get_name(clk_get_parent(clk)),
874 (unsigned int)clk_get_rate(clk));
878 of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
881 static void __init st_of_quadfs_setup(struct device_node *np,
882 struct clkgen_quadfs_data *data)
885 const char *pll_name, *clk_parent_name;
889 reg = of_iomap(np, 0);
893 clk_parent_name = of_clk_get_parent_name(np, 0);
894 if (!clk_parent_name)
897 pll_name = kasprintf(GFP_KERNEL, "%s.pll", np->name);
901 lock = kzalloc(sizeof(*lock), GFP_KERNEL);
905 spin_lock_init(lock);
907 clk = st_clk_register_quadfs_pll(pll_name, clk_parent_name, data,
912 pr_debug("%s: parent %s rate %u\n",
914 __clk_get_name(clk_get_parent(clk)),
915 (unsigned int)clk_get_rate(clk));
917 st_of_create_quadfs_fsynths(np, pll_name, data, reg, lock);
920 kfree(pll_name); /* No longer need local copy of the PLL name */
923 static void __init st_of_quadfs660C_setup(struct device_node *np)
925 st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_C);
927 CLK_OF_DECLARE(quadfs660C, "st,quadfs-pll", st_of_quadfs660C_setup);
929 static void __init st_of_quadfs660D_setup(struct device_node *np)
931 st_of_quadfs_setup(np, (struct clkgen_quadfs_data *) &st_fs660c32_D);
933 CLK_OF_DECLARE(quadfs660D, "st,quadfs", st_of_quadfs660D_setup);