1
2
3
4
5
6
7
8
9
10#include <linux/clk-provider.h>
11#include <linux/err.h>
12#include <linux/gcd.h>
13#include <linux/io.h>
14#include <linux/iopoll.h>
15#include <linux/slab.h>
16#include <linux/spinlock.h>
17#include <asm/div64.h>
18
19#include "clk.h"
20
21#define to_clk_zx_pll(_hw) container_of(_hw, struct clk_zx_pll, hw)
22#define to_clk_zx_audio(_hw) container_of(_hw, struct clk_zx_audio, hw)
23
24#define CFG0_CFG1_OFFSET 4
25#define LOCK_FLAG 30
26#define POWER_DOWN 31
27
28static int rate_to_idx(struct clk_zx_pll *zx_pll, unsigned long rate)
29{
30 const struct zx_pll_config *config = zx_pll->lookup_table;
31 int i;
32
33 for (i = 0; i < zx_pll->count; i++) {
34 if (config[i].rate > rate)
35 return i > 0 ? i - 1 : 0;
36
37 if (config[i].rate == rate)
38 return i;
39 }
40
41 return i - 1;
42}
43
44static int hw_to_idx(struct clk_zx_pll *zx_pll)
45{
46 const struct zx_pll_config *config = zx_pll->lookup_table;
47 u32 hw_cfg0, hw_cfg1;
48 int i;
49
50 hw_cfg0 = readl_relaxed(zx_pll->reg_base);
51 hw_cfg1 = readl_relaxed(zx_pll->reg_base + CFG0_CFG1_OFFSET);
52
53
54 hw_cfg0 &= ~BIT(zx_pll->lock_bit);
55
56
57 if (zx_pll->pd_bit < 32)
58 hw_cfg0 |= BIT(zx_pll->pd_bit);
59
60 for (i = 0; i < zx_pll->count; i++) {
61 if (hw_cfg0 == config[i].cfg0 && hw_cfg1 == config[i].cfg1)
62 return i;
63 }
64
65 return -EINVAL;
66}
67
68static unsigned long zx_pll_recalc_rate(struct clk_hw *hw,
69 unsigned long parent_rate)
70{
71 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
72 int idx;
73
74 idx = hw_to_idx(zx_pll);
75 if (unlikely(idx == -EINVAL))
76 return 0;
77
78 return zx_pll->lookup_table[idx].rate;
79}
80
81static long zx_pll_round_rate(struct clk_hw *hw, unsigned long rate,
82 unsigned long *prate)
83{
84 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
85 int idx;
86
87 idx = rate_to_idx(zx_pll, rate);
88
89 return zx_pll->lookup_table[idx].rate;
90}
91
92static int zx_pll_set_rate(struct clk_hw *hw, unsigned long rate,
93 unsigned long parent_rate)
94{
95
96 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
97 const struct zx_pll_config *config;
98 int idx;
99
100 idx = rate_to_idx(zx_pll, rate);
101 config = &zx_pll->lookup_table[idx];
102
103 writel_relaxed(config->cfg0, zx_pll->reg_base);
104 writel_relaxed(config->cfg1, zx_pll->reg_base + CFG0_CFG1_OFFSET);
105
106 return 0;
107}
108
109static int zx_pll_enable(struct clk_hw *hw)
110{
111 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
112 u32 reg;
113
114
115 if (zx_pll->pd_bit > 31)
116 return 0;
117
118 reg = readl_relaxed(zx_pll->reg_base);
119 writel_relaxed(reg & ~BIT(zx_pll->pd_bit), zx_pll->reg_base);
120
121 return readl_relaxed_poll_timeout(zx_pll->reg_base, reg,
122 reg & BIT(zx_pll->lock_bit), 0, 100);
123}
124
125static void zx_pll_disable(struct clk_hw *hw)
126{
127 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
128 u32 reg;
129
130 if (zx_pll->pd_bit > 31)
131 return;
132
133 reg = readl_relaxed(zx_pll->reg_base);
134 writel_relaxed(reg | BIT(zx_pll->pd_bit), zx_pll->reg_base);
135}
136
137static int zx_pll_is_enabled(struct clk_hw *hw)
138{
139 struct clk_zx_pll *zx_pll = to_clk_zx_pll(hw);
140 u32 reg;
141
142 reg = readl_relaxed(zx_pll->reg_base);
143
144 return !(reg & BIT(zx_pll->pd_bit));
145}
146
147const struct clk_ops zx_pll_ops = {
148 .recalc_rate = zx_pll_recalc_rate,
149 .round_rate = zx_pll_round_rate,
150 .set_rate = zx_pll_set_rate,
151 .enable = zx_pll_enable,
152 .disable = zx_pll_disable,
153 .is_enabled = zx_pll_is_enabled,
154};
155EXPORT_SYMBOL(zx_pll_ops);
156
157struct clk *clk_register_zx_pll(const char *name, const char *parent_name,
158 unsigned long flags, void __iomem *reg_base,
159 const struct zx_pll_config *lookup_table,
160 int count, spinlock_t *lock)
161{
162 struct clk_zx_pll *zx_pll;
163 struct clk *clk;
164 struct clk_init_data init;
165
166 zx_pll = kzalloc(sizeof(*zx_pll), GFP_KERNEL);
167 if (!zx_pll)
168 return ERR_PTR(-ENOMEM);
169
170 init.name = name;
171 init.ops = &zx_pll_ops;
172 init.flags = flags;
173 init.parent_names = parent_name ? &parent_name : NULL;
174 init.num_parents = parent_name ? 1 : 0;
175
176 zx_pll->reg_base = reg_base;
177 zx_pll->lookup_table = lookup_table;
178 zx_pll->count = count;
179 zx_pll->lock_bit = LOCK_FLAG;
180 zx_pll->pd_bit = POWER_DOWN;
181 zx_pll->lock = lock;
182 zx_pll->hw.init = &init;
183
184 clk = clk_register(NULL, &zx_pll->hw);
185 if (IS_ERR(clk))
186 kfree(zx_pll);
187
188 return clk;
189}
190
191#define BPAR 1000000
192static u32 calc_reg(u32 parent_rate, u32 rate)
193{
194 u32 sel, integ, fra_div, tmp;
195 u64 tmp64 = (u64)parent_rate * BPAR;
196
197 do_div(tmp64, rate);
198 integ = (u32)tmp64 / BPAR;
199 integ = integ >> 1;
200
201 tmp = (u32)tmp64 % BPAR;
202 sel = tmp / BPAR;
203
204 tmp = tmp % BPAR;
205 fra_div = tmp * 0xff / BPAR;
206 tmp = (sel << 24) | (integ << 16) | (0xff << 8) | fra_div;
207
208
209
210
211 tmp |= BIT(28);
212 return tmp;
213}
214
215static u32 calc_rate(u32 reg, u32 parent_rate)
216{
217 u32 sel, integ, fra_div, tmp;
218 u64 tmp64 = (u64)parent_rate * BPAR;
219
220 tmp = reg;
221 sel = (tmp >> 24) & BIT(0);
222 integ = (tmp >> 16) & 0xff;
223 fra_div = tmp & 0xff;
224
225 tmp = fra_div * BPAR;
226 tmp = tmp / 0xff;
227 tmp += sel * BPAR;
228 tmp += 2 * integ * BPAR;
229 do_div(tmp64, tmp);
230
231 return (u32)tmp64;
232}
233
234static unsigned long zx_audio_recalc_rate(struct clk_hw *hw,
235 unsigned long parent_rate)
236{
237 struct clk_zx_audio *zx_audio = to_clk_zx_audio(hw);
238 u32 reg;
239
240 reg = readl_relaxed(zx_audio->reg_base);
241 return calc_rate(reg, parent_rate);
242}
243
244static long zx_audio_round_rate(struct clk_hw *hw, unsigned long rate,
245 unsigned long *prate)
246{
247 u32 reg;
248
249 if (rate * 2 > *prate)
250 return -EINVAL;
251
252 reg = calc_reg(*prate, rate);
253 return calc_rate(reg, *prate);
254}
255
256static int zx_audio_set_rate(struct clk_hw *hw, unsigned long rate,
257 unsigned long parent_rate)
258{
259 struct clk_zx_audio *zx_audio = to_clk_zx_audio(hw);
260 u32 reg;
261
262 reg = calc_reg(parent_rate, rate);
263 writel_relaxed(reg, zx_audio->reg_base);
264
265 return 0;
266}
267
268#define ZX_AUDIO_EN BIT(25)
269static int zx_audio_enable(struct clk_hw *hw)
270{
271 struct clk_zx_audio *zx_audio = to_clk_zx_audio(hw);
272 u32 reg;
273
274 reg = readl_relaxed(zx_audio->reg_base);
275 writel_relaxed(reg & ~ZX_AUDIO_EN, zx_audio->reg_base);
276 return 0;
277}
278
279static void zx_audio_disable(struct clk_hw *hw)
280{
281 struct clk_zx_audio *zx_audio = to_clk_zx_audio(hw);
282 u32 reg;
283
284 reg = readl_relaxed(zx_audio->reg_base);
285 writel_relaxed(reg | ZX_AUDIO_EN, zx_audio->reg_base);
286}
287
288static const struct clk_ops zx_audio_ops = {
289 .recalc_rate = zx_audio_recalc_rate,
290 .round_rate = zx_audio_round_rate,
291 .set_rate = zx_audio_set_rate,
292 .enable = zx_audio_enable,
293 .disable = zx_audio_disable,
294};
295
296struct clk *clk_register_zx_audio(const char *name,
297 const char * const parent_name,
298 unsigned long flags,
299 void __iomem *reg_base)
300{
301 struct clk_zx_audio *zx_audio;
302 struct clk *clk;
303 struct clk_init_data init;
304
305 zx_audio = kzalloc(sizeof(*zx_audio), GFP_KERNEL);
306 if (!zx_audio)
307 return ERR_PTR(-ENOMEM);
308
309 init.name = name;
310 init.ops = &zx_audio_ops;
311 init.flags = flags;
312 init.parent_names = parent_name ? &parent_name : NULL;
313 init.num_parents = parent_name ? 1 : 0;
314
315 zx_audio->reg_base = reg_base;
316 zx_audio->hw.init = &init;
317
318 clk = clk_register(NULL, &zx_audio->hw);
319 if (IS_ERR(clk))
320 kfree(zx_audio);
321
322 return clk;
323}
324
325#define CLK_AUDIO_DIV_FRAC BIT(0)
326#define CLK_AUDIO_DIV_INT BIT(1)
327#define CLK_AUDIO_DIV_UNCOMMON BIT(1)
328
329#define CLK_AUDIO_DIV_FRAC_NSHIFT 16
330#define CLK_AUDIO_DIV_INT_FRAC_RE BIT(16)
331#define CLK_AUDIO_DIV_INT_FRAC_MAX (0xffff)
332#define CLK_AUDIO_DIV_INT_FRAC_MIN (0x2)
333#define CLK_AUDIO_DIV_INT_INT_SHIFT 24
334#define CLK_AUDIO_DIV_INT_INT_WIDTH 4
335
336struct zx_clk_audio_div_table {
337 unsigned long rate;
338 unsigned int int_reg;
339 unsigned int frac_reg;
340};
341
342#define to_clk_zx_audio_div(_hw) container_of(_hw, struct clk_zx_audio_divider, hw)
343
344static unsigned long audio_calc_rate(struct clk_zx_audio_divider *audio_div,
345 u32 reg_frac, u32 reg_int,
346 unsigned long parent_rate)
347{
348 unsigned long rate, m, n;
349
350 m = reg_frac & 0xffff;
351 n = (reg_frac >> 16) & 0xffff;
352
353 m = (reg_int & 0xffff) * n + m;
354 rate = (parent_rate * n) / m;
355
356 return rate;
357}
358
359static void audio_calc_reg(struct clk_zx_audio_divider *audio_div,
360 struct zx_clk_audio_div_table *div_table,
361 unsigned long rate, unsigned long parent_rate)
362{
363 unsigned int reg_int, reg_frac;
364 unsigned long m, n, div;
365
366 reg_int = parent_rate / rate;
367
368 if (reg_int > CLK_AUDIO_DIV_INT_FRAC_MAX)
369 reg_int = CLK_AUDIO_DIV_INT_FRAC_MAX;
370 else if (reg_int < CLK_AUDIO_DIV_INT_FRAC_MIN)
371 reg_int = 0;
372 m = parent_rate - rate * reg_int;
373 n = rate;
374
375 div = gcd(m, n);
376 m = m / div;
377 n = n / div;
378
379 if ((m >> 16) || (n >> 16)) {
380 if (m > n) {
381 n = n * 0xffff / m;
382 m = 0xffff;
383 } else {
384 m = m * 0xffff / n;
385 n = 0xffff;
386 }
387 }
388 reg_frac = m | (n << 16);
389
390 div_table->rate = parent_rate * n / (reg_int * n + m);
391 div_table->int_reg = reg_int;
392 div_table->frac_reg = reg_frac;
393}
394
395static unsigned long zx_audio_div_recalc_rate(struct clk_hw *hw,
396 unsigned long parent_rate)
397{
398 struct clk_zx_audio_divider *zx_audio_div = to_clk_zx_audio_div(hw);
399 u32 reg_frac, reg_int;
400
401 reg_frac = readl_relaxed(zx_audio_div->reg_base);
402 reg_int = readl_relaxed(zx_audio_div->reg_base + 0x4);
403
404 return audio_calc_rate(zx_audio_div, reg_frac, reg_int, parent_rate);
405}
406
407static long zx_audio_div_round_rate(struct clk_hw *hw, unsigned long rate,
408 unsigned long *prate)
409{
410 struct clk_zx_audio_divider *zx_audio_div = to_clk_zx_audio_div(hw);
411 struct zx_clk_audio_div_table divt;
412
413 audio_calc_reg(zx_audio_div, &divt, rate, *prate);
414
415 return audio_calc_rate(zx_audio_div, divt.frac_reg, divt.int_reg, *prate);
416}
417
418static int zx_audio_div_set_rate(struct clk_hw *hw, unsigned long rate,
419 unsigned long parent_rate)
420{
421 struct clk_zx_audio_divider *zx_audio_div = to_clk_zx_audio_div(hw);
422 struct zx_clk_audio_div_table divt;
423 unsigned int val;
424
425 audio_calc_reg(zx_audio_div, &divt, rate, parent_rate);
426 if (divt.rate != rate)
427 pr_debug("the real rate is:%ld", divt.rate);
428
429 writel_relaxed(divt.frac_reg, zx_audio_div->reg_base);
430
431 val = readl_relaxed(zx_audio_div->reg_base + 0x4);
432 val &= ~0xffff;
433 val |= divt.int_reg | CLK_AUDIO_DIV_INT_FRAC_RE;
434 writel_relaxed(val, zx_audio_div->reg_base + 0x4);
435
436 mdelay(1);
437
438 val = readl_relaxed(zx_audio_div->reg_base + 0x4);
439 val &= ~CLK_AUDIO_DIV_INT_FRAC_RE;
440 writel_relaxed(val, zx_audio_div->reg_base + 0x4);
441
442 return 0;
443}
444
445const struct clk_ops zx_audio_div_ops = {
446 .recalc_rate = zx_audio_div_recalc_rate,
447 .round_rate = zx_audio_div_round_rate,
448 .set_rate = zx_audio_div_set_rate,
449};
450
