abfc2b675aea72cdd2271885c4c514b288f20276
[sfrench/cifs-2.6.git] / drivers / clk / qcom / clk-rcg.c
1 /*
2  * Copyright (c) 2013, The Linux Foundation. All rights reserved.
3  *
4  * This software is licensed under the terms of the GNU General Public
5  * License version 2, as published by the Free Software Foundation, and
6  * may be copied, distributed, and modified under those terms.
7  *
8  * This program is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11  * GNU General Public License for more details.
12  */
13
14 #include <linux/kernel.h>
15 #include <linux/bitops.h>
16 #include <linux/err.h>
17 #include <linux/export.h>
18 #include <linux/clk-provider.h>
19 #include <linux/regmap.h>
20
21 #include <asm/div64.h>
22
23 #include "clk-rcg.h"
24
25 static u32 ns_to_src(struct src_sel *s, u32 ns)
26 {
27         ns >>= s->src_sel_shift;
28         ns &= SRC_SEL_MASK;
29         return ns;
30 }
31
32 static u32 src_to_ns(struct src_sel *s, u8 src, u32 ns)
33 {
34         u32 mask;
35
36         mask = SRC_SEL_MASK;
37         mask <<= s->src_sel_shift;
38         ns &= ~mask;
39
40         ns |= src << s->src_sel_shift;
41         return ns;
42 }
43
44 static u8 clk_rcg_get_parent(struct clk_hw *hw)
45 {
46         struct clk_rcg *rcg = to_clk_rcg(hw);
47         int num_parents = __clk_get_num_parents(hw->clk);
48         u32 ns;
49         int i;
50
51         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
52         ns = ns_to_src(&rcg->s, ns);
53         for (i = 0; i < num_parents; i++)
54                 if (ns == rcg->s.parent_map[i])
55                         return i;
56
57         return -EINVAL;
58 }
59
60 static int reg_to_bank(struct clk_dyn_rcg *rcg, u32 bank)
61 {
62         bank &= BIT(rcg->mux_sel_bit);
63         return !!bank;
64 }
65
66 static u8 clk_dyn_rcg_get_parent(struct clk_hw *hw)
67 {
68         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
69         int num_parents = __clk_get_num_parents(hw->clk);
70         u32 ns, ctl;
71         int bank;
72         int i;
73         struct src_sel *s;
74
75         regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
76         bank = reg_to_bank(rcg, ctl);
77         s = &rcg->s[bank];
78
79         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
80         ns = ns_to_src(s, ns);
81
82         for (i = 0; i < num_parents; i++)
83                 if (ns == s->parent_map[i])
84                         return i;
85
86         return -EINVAL;
87 }
88
89 static int clk_rcg_set_parent(struct clk_hw *hw, u8 index)
90 {
91         struct clk_rcg *rcg = to_clk_rcg(hw);
92         u32 ns;
93
94         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
95         ns = src_to_ns(&rcg->s, rcg->s.parent_map[index], ns);
96         regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
97
98         return 0;
99 }
100
101 static u32 md_to_m(struct mn *mn, u32 md)
102 {
103         md >>= mn->m_val_shift;
104         md &= BIT(mn->width) - 1;
105         return md;
106 }
107
108 static u32 ns_to_pre_div(struct pre_div *p, u32 ns)
109 {
110         ns >>= p->pre_div_shift;
111         ns &= BIT(p->pre_div_width) - 1;
112         return ns;
113 }
114
115 static u32 pre_div_to_ns(struct pre_div *p, u8 pre_div, u32 ns)
116 {
117         u32 mask;
118
119         mask = BIT(p->pre_div_width) - 1;
120         mask <<= p->pre_div_shift;
121         ns &= ~mask;
122
123         ns |= pre_div << p->pre_div_shift;
124         return ns;
125 }
126
127 static u32 mn_to_md(struct mn *mn, u32 m, u32 n, u32 md)
128 {
129         u32 mask, mask_w;
130
131         mask_w = BIT(mn->width) - 1;
132         mask = (mask_w << mn->m_val_shift) | mask_w;
133         md &= ~mask;
134
135         if (n) {
136                 m <<= mn->m_val_shift;
137                 md |= m;
138                 md |= ~n & mask_w;
139         }
140
141         return md;
142 }
143
144 static u32 ns_m_to_n(struct mn *mn, u32 ns, u32 m)
145 {
146         ns = ~ns >> mn->n_val_shift;
147         ns &= BIT(mn->width) - 1;
148         return ns + m;
149 }
150
151 static u32 reg_to_mnctr_mode(struct mn *mn, u32 val)
152 {
153         val >>= mn->mnctr_mode_shift;
154         val &= MNCTR_MODE_MASK;
155         return val;
156 }
157
158 static u32 mn_to_ns(struct mn *mn, u32 m, u32 n, u32 ns)
159 {
160         u32 mask;
161
162         mask = BIT(mn->width) - 1;
163         mask <<= mn->n_val_shift;
164         ns &= ~mask;
165
166         if (n) {
167                 n = n - m;
168                 n = ~n;
169                 n &= BIT(mn->width) - 1;
170                 n <<= mn->n_val_shift;
171                 ns |= n;
172         }
173
174         return ns;
175 }
176
177 static u32 mn_to_reg(struct mn *mn, u32 m, u32 n, u32 val)
178 {
179         u32 mask;
180
181         mask = MNCTR_MODE_MASK << mn->mnctr_mode_shift;
182         mask |= BIT(mn->mnctr_en_bit);
183         val &= ~mask;
184
185         if (n) {
186                 val |= BIT(mn->mnctr_en_bit);
187                 val |= MNCTR_MODE_DUAL << mn->mnctr_mode_shift;
188         }
189
190         return val;
191 }
192
193 static void configure_bank(struct clk_dyn_rcg *rcg, const struct freq_tbl *f)
194 {
195         u32 ns, md, ctl, *regp;
196         int bank, new_bank;
197         struct mn *mn;
198         struct pre_div *p;
199         struct src_sel *s;
200         bool enabled;
201         u32 md_reg;
202         u32 bank_reg;
203         bool banked_mn = !!rcg->mn[1].width;
204         struct clk_hw *hw = &rcg->clkr.hw;
205
206         enabled = __clk_is_enabled(hw->clk);
207
208         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
209         regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
210
211         if (banked_mn) {
212                 regp = &ctl;
213                 bank_reg = rcg->clkr.enable_reg;
214         } else {
215                 regp = &ns;
216                 bank_reg = rcg->ns_reg;
217         }
218
219         bank = reg_to_bank(rcg, *regp);
220         new_bank = enabled ? !bank : bank;
221
222         if (banked_mn) {
223                 mn = &rcg->mn[new_bank];
224                 md_reg = rcg->md_reg[new_bank];
225
226                 ns |= BIT(mn->mnctr_reset_bit);
227                 regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
228
229                 regmap_read(rcg->clkr.regmap, md_reg, &md);
230                 md = mn_to_md(mn, f->m, f->n, md);
231                 regmap_write(rcg->clkr.regmap, md_reg, md);
232
233                 ns = mn_to_ns(mn, f->m, f->n, ns);
234                 regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
235
236                 ctl = mn_to_reg(mn, f->m, f->n, ctl);
237                 regmap_write(rcg->clkr.regmap, rcg->clkr.enable_reg, ctl);
238
239                 ns &= ~BIT(mn->mnctr_reset_bit);
240                 regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
241         } else {
242                 p = &rcg->p[new_bank];
243                 ns = pre_div_to_ns(p, f->pre_div - 1, ns);
244         }
245
246         s = &rcg->s[new_bank];
247         ns = src_to_ns(s, s->parent_map[f->src], ns);
248         regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
249
250         if (enabled) {
251                 *regp ^= BIT(rcg->mux_sel_bit);
252                 regmap_write(rcg->clkr.regmap, bank_reg, *regp);
253         }
254 }
255
256 static int clk_dyn_rcg_set_parent(struct clk_hw *hw, u8 index)
257 {
258         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
259         u32 ns, ctl, md, reg;
260         int bank;
261         struct freq_tbl f = { 0 };
262         bool banked_mn = !!rcg->mn[1].width;
263
264         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
265         regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
266         reg = banked_mn ? ctl : ns;
267
268         bank = reg_to_bank(rcg, reg);
269
270         if (banked_mn) {
271                 regmap_read(rcg->clkr.regmap, rcg->md_reg[bank], &md);
272                 f.m = md_to_m(&rcg->mn[bank], md);
273                 f.n = ns_m_to_n(&rcg->mn[bank], ns, f.m);
274         } else {
275                 f.pre_div = ns_to_pre_div(&rcg->p[bank], ns) + 1;
276         }
277         f.src = index;
278
279         configure_bank(rcg, &f);
280
281         return 0;
282 }
283
284 /*
285  * Calculate m/n:d rate
286  *
287  *          parent_rate     m
288  *   rate = ----------- x  ---
289  *            pre_div       n
290  */
291 static unsigned long
292 calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 pre_div)
293 {
294         if (pre_div)
295                 rate /= pre_div + 1;
296
297         if (mode) {
298                 u64 tmp = rate;
299                 tmp *= m;
300                 do_div(tmp, n);
301                 rate = tmp;
302         }
303
304         return rate;
305 }
306
307 static unsigned long
308 clk_rcg_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
309 {
310         struct clk_rcg *rcg = to_clk_rcg(hw);
311         u32 pre_div, m = 0, n = 0, ns, md, mode = 0;
312         struct mn *mn = &rcg->mn;
313
314         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
315         pre_div = ns_to_pre_div(&rcg->p, ns);
316
317         if (rcg->mn.width) {
318                 regmap_read(rcg->clkr.regmap, rcg->md_reg, &md);
319                 m = md_to_m(mn, md);
320                 n = ns_m_to_n(mn, ns, m);
321                 /* MN counter mode is in hw.enable_reg sometimes */
322                 if (rcg->clkr.enable_reg != rcg->ns_reg)
323                         regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &mode);
324                 else
325                         mode = ns;
326                 mode = reg_to_mnctr_mode(mn, mode);
327         }
328
329         return calc_rate(parent_rate, m, n, mode, pre_div);
330 }
331
332 static unsigned long
333 clk_dyn_rcg_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
334 {
335         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
336         u32 m, n, pre_div, ns, md, mode, reg;
337         int bank;
338         struct mn *mn;
339         bool banked_mn = !!rcg->mn[1].width;
340
341         regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
342
343         if (banked_mn)
344                 regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &reg);
345         else
346                 reg = ns;
347
348         bank = reg_to_bank(rcg, reg);
349
350         if (banked_mn) {
351                 mn = &rcg->mn[bank];
352                 regmap_read(rcg->clkr.regmap, rcg->md_reg[bank], &md);
353                 m = md_to_m(mn, md);
354                 n = ns_m_to_n(mn, ns, m);
355                 mode = reg_to_mnctr_mode(mn, reg);
356                 return calc_rate(parent_rate, m, n, mode, 0);
357         } else {
358                 pre_div = ns_to_pre_div(&rcg->p[bank], ns);
359                 return calc_rate(parent_rate, 0, 0, 0, pre_div);
360         }
361 }
362
363 static const
364 struct freq_tbl *find_freq(const struct freq_tbl *f, unsigned long rate)
365 {
366         if (!f)
367                 return NULL;
368
369         for (; f->freq; f++)
370                 if (rate <= f->freq)
371                         return f;
372
373         return NULL;
374 }
375
376 static long _freq_tbl_determine_rate(struct clk_hw *hw,
377                 const struct freq_tbl *f, unsigned long rate,
378                 unsigned long *p_rate, struct clk **p)
379 {
380         unsigned long clk_flags;
381
382         f = find_freq(f, rate);
383         if (!f)
384                 return -EINVAL;
385
386         clk_flags = __clk_get_flags(hw->clk);
387         *p = clk_get_parent_by_index(hw->clk, f->src);
388         if (clk_flags & CLK_SET_RATE_PARENT) {
389                 rate = rate * f->pre_div;
390                 if (f->n) {
391                         u64 tmp = rate;
392                         tmp = tmp * f->n;
393                         do_div(tmp, f->m);
394                         rate = tmp;
395                 }
396         } else {
397                 rate =  __clk_get_rate(*p);
398         }
399         *p_rate = rate;
400
401         return f->freq;
402 }
403
404 static long clk_rcg_determine_rate(struct clk_hw *hw, unsigned long rate,
405                 unsigned long *p_rate, struct clk **p)
406 {
407         struct clk_rcg *rcg = to_clk_rcg(hw);
408
409         return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, p_rate, p);
410 }
411
412 static long clk_dyn_rcg_determine_rate(struct clk_hw *hw, unsigned long rate,
413                 unsigned long *p_rate, struct clk **p)
414 {
415         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
416
417         return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, p_rate, p);
418 }
419
420 static int clk_rcg_set_rate(struct clk_hw *hw, unsigned long rate,
421                             unsigned long parent_rate)
422 {
423         struct clk_rcg *rcg = to_clk_rcg(hw);
424         const struct freq_tbl *f;
425         u32 ns, md, ctl;
426         struct mn *mn = &rcg->mn;
427         u32 mask = 0;
428         unsigned int reset_reg;
429
430         f = find_freq(rcg->freq_tbl, rate);
431         if (!f)
432                 return -EINVAL;
433
434         if (rcg->mn.reset_in_cc)
435                 reset_reg = rcg->clkr.enable_reg;
436         else
437                 reset_reg = rcg->ns_reg;
438
439         if (rcg->mn.width) {
440                 mask = BIT(mn->mnctr_reset_bit);
441                 regmap_update_bits(rcg->clkr.regmap, reset_reg, mask, mask);
442
443                 regmap_read(rcg->clkr.regmap, rcg->md_reg, &md);
444                 md = mn_to_md(mn, f->m, f->n, md);
445                 regmap_write(rcg->clkr.regmap, rcg->md_reg, md);
446
447                 regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
448                 /* MN counter mode is in hw.enable_reg sometimes */
449                 if (rcg->clkr.enable_reg != rcg->ns_reg) {
450                         regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
451                         ctl = mn_to_reg(mn, f->m, f->n, ctl);
452                         regmap_write(rcg->clkr.regmap, rcg->clkr.enable_reg, ctl);
453                 } else {
454                         ns = mn_to_reg(mn, f->m, f->n, ns);
455                 }
456                 ns = mn_to_ns(mn, f->m, f->n, ns);
457         } else {
458                 regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
459         }
460
461         ns = pre_div_to_ns(&rcg->p, f->pre_div - 1, ns);
462         regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
463
464         regmap_update_bits(rcg->clkr.regmap, reset_reg, mask, 0);
465
466         return 0;
467 }
468
469 static int __clk_dyn_rcg_set_rate(struct clk_hw *hw, unsigned long rate)
470 {
471         struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
472         const struct freq_tbl *f;
473
474         f = find_freq(rcg->freq_tbl, rate);
475         if (!f)
476                 return -EINVAL;
477
478         configure_bank(rcg, f);
479
480         return 0;
481 }
482
483 static int clk_dyn_rcg_set_rate(struct clk_hw *hw, unsigned long rate,
484                             unsigned long parent_rate)
485 {
486         return __clk_dyn_rcg_set_rate(hw, rate);
487 }
488
489 static int clk_dyn_rcg_set_rate_and_parent(struct clk_hw *hw,
490                 unsigned long rate, unsigned long parent_rate, u8 index)
491 {
492         return __clk_dyn_rcg_set_rate(hw, rate);
493 }
494
495 const struct clk_ops clk_rcg_ops = {
496         .enable = clk_enable_regmap,
497         .disable = clk_disable_regmap,
498         .get_parent = clk_rcg_get_parent,
499         .set_parent = clk_rcg_set_parent,
500         .recalc_rate = clk_rcg_recalc_rate,
501         .determine_rate = clk_rcg_determine_rate,
502         .set_rate = clk_rcg_set_rate,
503 };
504 EXPORT_SYMBOL_GPL(clk_rcg_ops);
505
506 const struct clk_ops clk_dyn_rcg_ops = {
507         .enable = clk_enable_regmap,
508         .is_enabled = clk_is_enabled_regmap,
509         .disable = clk_disable_regmap,
510         .get_parent = clk_dyn_rcg_get_parent,
511         .set_parent = clk_dyn_rcg_set_parent,
512         .recalc_rate = clk_dyn_rcg_recalc_rate,
513         .determine_rate = clk_dyn_rcg_determine_rate,
514         .set_rate = clk_dyn_rcg_set_rate,
515         .set_rate_and_parent = clk_dyn_rcg_set_rate_and_parent,
516 };
517 EXPORT_SYMBOL_GPL(clk_dyn_rcg_ops);