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