1
2
3
4
5
6
7
8
9
10
11
12
13#include <linux/clk-provider.h>
14#include <linux/module.h>
15#include <linux/slab.h>
16#include <linux/io.h>
17#include <linux/err.h>
18#include <linux/string.h>
19#include <linux/log2.h>
20
21
22
23
24
25
26
27
28
29
30
31static unsigned int _get_table_maxdiv(const struct clk_div_table *table,
32 u8 width)
33{
34 unsigned int maxdiv = 0, mask = clk_div_mask(width);
35 const struct clk_div_table *clkt;
36
37 for (clkt = table; clkt->div; clkt++)
38 if (clkt->div > maxdiv && clkt->val <= mask)
39 maxdiv = clkt->div;
40 return maxdiv;
41}
42
43static unsigned int _get_table_mindiv(const struct clk_div_table *table)
44{
45 unsigned int mindiv = UINT_MAX;
46 const struct clk_div_table *clkt;
47
48 for (clkt = table; clkt->div; clkt++)
49 if (clkt->div < mindiv)
50 mindiv = clkt->div;
51 return mindiv;
52}
53
54static unsigned int _get_maxdiv(const struct clk_div_table *table, u8 width,
55 unsigned long flags)
56{
57 if (flags & CLK_DIVIDER_ONE_BASED)
58 return clk_div_mask(width);
59 if (flags & CLK_DIVIDER_POWER_OF_TWO)
60 return 1 << clk_div_mask(width);
61 if (table)
62 return _get_table_maxdiv(table, width);
63 return clk_div_mask(width) + 1;
64}
65
66static unsigned int _get_table_div(const struct clk_div_table *table,
67 unsigned int val)
68{
69 const struct clk_div_table *clkt;
70
71 for (clkt = table; clkt->div; clkt++)
72 if (clkt->val == val)
73 return clkt->div;
74 return 0;
75}
76
77static unsigned int _get_div(const struct clk_div_table *table,
78 unsigned int val, unsigned long flags, u8 width)
79{
80 if (flags & CLK_DIVIDER_ONE_BASED)
81 return val;
82 if (flags & CLK_DIVIDER_POWER_OF_TWO)
83 return 1 << val;
84 if (flags & CLK_DIVIDER_MAX_AT_ZERO)
85 return val ? val : clk_div_mask(width) + 1;
86 if (table)
87 return _get_table_div(table, val);
88 return val + 1;
89}
90
91static unsigned int _get_table_val(const struct clk_div_table *table,
92 unsigned int div)
93{
94 const struct clk_div_table *clkt;
95
96 for (clkt = table; clkt->div; clkt++)
97 if (clkt->div == div)
98 return clkt->val;
99 return 0;
100}
101
102static unsigned int _get_val(const struct clk_div_table *table,
103 unsigned int div, unsigned long flags, u8 width)
104{
105 if (flags & CLK_DIVIDER_ONE_BASED)
106 return div;
107 if (flags & CLK_DIVIDER_POWER_OF_TWO)
108 return __ffs(div);
109 if (flags & CLK_DIVIDER_MAX_AT_ZERO)
110 return (div == clk_div_mask(width) + 1) ? 0 : div;
111 if (table)
112 return _get_table_val(table, div);
113 return div - 1;
114}
115
116unsigned long divider_recalc_rate(struct clk_hw *hw, unsigned long parent_rate,
117 unsigned int val,
118 const struct clk_div_table *table,
119 unsigned long flags, unsigned long width)
120{
121 unsigned int div;
122
123 div = _get_div(table, val, flags, width);
124 if (!div) {
125 WARN(!(flags & CLK_DIVIDER_ALLOW_ZERO),
126 "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
127 clk_hw_get_name(hw));
128 return parent_rate;
129 }
130
131 return DIV_ROUND_UP_ULL((u64)parent_rate, div);
132}
133EXPORT_SYMBOL_GPL(divider_recalc_rate);
134
135static unsigned long clk_divider_recalc_rate(struct clk_hw *hw,
136 unsigned long parent_rate)
137{
138 struct clk_divider *divider = to_clk_divider(hw);
139 unsigned int val;
140
141 val = clk_readl(divider->reg) >> divider->shift;
142 val &= clk_div_mask(divider->width);
143
144 return divider_recalc_rate(hw, parent_rate, val, divider->table,
145 divider->flags, divider->width);
146}
147
148static bool _is_valid_table_div(const struct clk_div_table *table,
149 unsigned int div)
150{
151 const struct clk_div_table *clkt;
152
153 for (clkt = table; clkt->div; clkt++)
154 if (clkt->div == div)
155 return true;
156 return false;
157}
158
159static bool _is_valid_div(const struct clk_div_table *table, unsigned int div,
160 unsigned long flags)
161{
162 if (flags & CLK_DIVIDER_POWER_OF_TWO)
163 return is_power_of_2(div);
164 if (table)
165 return _is_valid_table_div(table, div);
166 return true;
167}
168
169static int _round_up_table(const struct clk_div_table *table, int div)
170{
171 const struct clk_div_table *clkt;
172 int up = INT_MAX;
173
174 for (clkt = table; clkt->div; clkt++) {
175 if (clkt->div == div)
176 return clkt->div;
177 else if (clkt->div < div)
178 continue;
179
180 if ((clkt->div - div) < (up - div))
181 up = clkt->div;
182 }
183
184 return up;
185}
186
187static int _round_down_table(const struct clk_div_table *table, int div)
188{
189 const struct clk_div_table *clkt;
190 int down = _get_table_mindiv(table);
191
192 for (clkt = table; clkt->div; clkt++) {
193 if (clkt->div == div)
194 return clkt->div;
195 else if (clkt->div > div)
196 continue;
197
198 if ((div - clkt->div) < (div - down))
199 down = clkt->div;
200 }
201
202 return down;
203}
204
205static int _div_round_up(const struct clk_div_table *table,
206 unsigned long parent_rate, unsigned long rate,
207 unsigned long flags)
208{
209 int div = DIV_ROUND_UP_ULL((u64)parent_rate, rate);
210
211 if (flags & CLK_DIVIDER_POWER_OF_TWO)
212 div = __roundup_pow_of_two(div);
213 if (table)
214 div = _round_up_table(table, div);
215
216 return div;
217}
218
219static int _div_round_closest(const struct clk_div_table *table,
220 unsigned long parent_rate, unsigned long rate,
221 unsigned long flags)
222{
223 int up, down;
224 unsigned long up_rate, down_rate;
225
226 up = DIV_ROUND_UP_ULL((u64)parent_rate, rate);
227 down = parent_rate / rate;
228
229 if (flags & CLK_DIVIDER_POWER_OF_TWO) {
230 up = __roundup_pow_of_two(up);
231 down = __rounddown_pow_of_two(down);
232 } else if (table) {
233 up = _round_up_table(table, up);
234 down = _round_down_table(table, down);
235 }
236
237 up_rate = DIV_ROUND_UP_ULL((u64)parent_rate, up);
238 down_rate = DIV_ROUND_UP_ULL((u64)parent_rate, down);
239
240 return (rate - up_rate) <= (down_rate - rate) ? up : down;
241}
242
243static int _div_round(const struct clk_div_table *table,
244 unsigned long parent_rate, unsigned long rate,
245 unsigned long flags)
246{
247 if (flags & CLK_DIVIDER_ROUND_CLOSEST)
248 return _div_round_closest(table, parent_rate, rate, flags);
249
250 return _div_round_up(table, parent_rate, rate, flags);
251}
252
253static bool _is_best_div(unsigned long rate, unsigned long now,
254 unsigned long best, unsigned long flags)
255{
256 if (flags & CLK_DIVIDER_ROUND_CLOSEST)
257 return abs(rate - now) < abs(rate - best);
258
259 return now <= rate && now > best;
260}
261
262static int _next_div(const struct clk_div_table *table, int div,
263 unsigned long flags)
264{
265 div++;
266
267 if (flags & CLK_DIVIDER_POWER_OF_TWO)
268 return __roundup_pow_of_two(div);
269 if (table)
270 return _round_up_table(table, div);
271
272 return div;
273}
274
275static int clk_divider_bestdiv(struct clk_hw *hw, struct clk_hw *parent,
276 unsigned long rate,
277 unsigned long *best_parent_rate,
278 const struct clk_div_table *table, u8 width,
279 unsigned long flags)
280{
281 int i, bestdiv = 0;
282 unsigned long parent_rate, best = 0, now, maxdiv;
283 unsigned long parent_rate_saved = *best_parent_rate;
284
285 if (!rate)
286 rate = 1;
287
288 maxdiv = _get_maxdiv(table, width, flags);
289
290 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
291 parent_rate = *best_parent_rate;
292 bestdiv = _div_round(table, parent_rate, rate, flags);
293 bestdiv = bestdiv == 0 ? 1 : bestdiv;
294 bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv;
295 return bestdiv;
296 }
297
298
299
300
301
302 maxdiv = min(ULONG_MAX / rate, maxdiv);
303
304 for (i = _next_div(table, 0, flags); i <= maxdiv;
305 i = _next_div(table, i, flags)) {
306 if (rate * i == parent_rate_saved) {
307
308
309
310
311
312 *best_parent_rate = parent_rate_saved;
313 return i;
314 }
315 parent_rate = clk_hw_round_rate(parent, rate * i);
316 now = DIV_ROUND_UP_ULL((u64)parent_rate, i);
317 if (_is_best_div(rate, now, best, flags)) {
318 bestdiv = i;
319 best = now;
320 *best_parent_rate = parent_rate;
321 }
322 }
323
324 if (!bestdiv) {
325 bestdiv = _get_maxdiv(table, width, flags);
326 *best_parent_rate = clk_hw_round_rate(parent, 1);
327 }
328
329 return bestdiv;
330}
331
332long divider_round_rate_parent(struct clk_hw *hw, struct clk_hw *parent,
333 unsigned long rate, unsigned long *prate,
334 const struct clk_div_table *table,
335 u8 width, unsigned long flags)
336{
337 int div;
338
339 div = clk_divider_bestdiv(hw, parent, rate, prate, table, width, flags);
340
341 return DIV_ROUND_UP_ULL((u64)*prate, div);
342}
343EXPORT_SYMBOL_GPL(divider_round_rate_parent);
344
345long divider_ro_round_rate_parent(struct clk_hw *hw, struct clk_hw *parent,
346 unsigned long rate, unsigned long *prate,
347 const struct clk_div_table *table, u8 width,
348 unsigned long flags, unsigned int val)
349{
350 int div;
351
352 div = _get_div(table, val, flags, width);
353
354
355 if (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) {
356 if (!parent)
357 return -EINVAL;
358
359 *prate = clk_hw_round_rate(parent, rate * div);
360 }
361
362 return DIV_ROUND_UP_ULL((u64)*prate, div);
363}
364EXPORT_SYMBOL_GPL(divider_ro_round_rate_parent);
365
366
367static long clk_divider_round_rate(struct clk_hw *hw, unsigned long rate,
368 unsigned long *prate)
369{
370 struct clk_divider *divider = to_clk_divider(hw);
371
372
373 if (divider->flags & CLK_DIVIDER_READ_ONLY) {
374 u32 val;
375
376 val = clk_readl(divider->reg) >> divider->shift;
377 val &= clk_div_mask(divider->width);
378
379 return divider_ro_round_rate(hw, rate, prate, divider->table,
380 divider->width, divider->flags,
381 val);
382 }
383
384 return divider_round_rate(hw, rate, prate, divider->table,
385 divider->width, divider->flags);
386}
387
388int divider_get_val(unsigned long rate, unsigned long parent_rate,
389 const struct clk_div_table *table, u8 width,
390 unsigned long flags)
391{
392 unsigned int div, value;
393
394 div = DIV_ROUND_UP_ULL((u64)parent_rate, rate);
395
396 if (!_is_valid_div(table, div, flags))
397 return -EINVAL;
398
399 value = _get_val(table, div, flags, width);
400
401 return min_t(unsigned int, value, clk_div_mask(width));
402}
403EXPORT_SYMBOL_GPL(divider_get_val);
404
405static int clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
406 unsigned long parent_rate)
407{
408 struct clk_divider *divider = to_clk_divider(hw);
409 int value;
410 unsigned long flags = 0;
411 u32 val;
412
413 value = divider_get_val(rate, parent_rate, divider->table,
414 divider->width, divider->flags);
415 if (value < 0)
416 return value;
417
418 if (divider->lock)
419 spin_lock_irqsave(divider->lock, flags);
420 else
421 __acquire(divider->lock);
422
423 if (divider->flags & CLK_DIVIDER_HIWORD_MASK) {
424 val = clk_div_mask(divider->width) << (divider->shift + 16);
425 } else {
426 val = clk_readl(divider->reg);
427 val &= ~(clk_div_mask(divider->width) << divider->shift);
428 }
429 val |= (u32)value << divider->shift;
430 clk_writel(val, divider->reg);
431
432 if (divider->lock)
433 spin_unlock_irqrestore(divider->lock, flags);
434 else
435 __release(divider->lock);
436
437 return 0;
438}
439
440const struct clk_ops clk_divider_ops = {
441 .recalc_rate = clk_divider_recalc_rate,
442 .round_rate = clk_divider_round_rate,
443 .set_rate = clk_divider_set_rate,
444};
445EXPORT_SYMBOL_GPL(clk_divider_ops);
446
447const struct clk_ops clk_divider_ro_ops = {
448 .recalc_rate = clk_divider_recalc_rate,
449 .round_rate = clk_divider_round_rate,
450};
451EXPORT_SYMBOL_GPL(clk_divider_ro_ops);
452
453static struct clk_hw *_register_divider(struct device *dev, const char *name,
454 const char *parent_name, unsigned long flags,
455 void __iomem *reg, u8 shift, u8 width,
456 u8 clk_divider_flags, const struct clk_div_table *table,
457 spinlock_t *lock)
458{
459 struct clk_divider *div;
460 struct clk_hw *hw;
461 struct clk_init_data init;
462 int ret;
463
464 if (clk_divider_flags & CLK_DIVIDER_HIWORD_MASK) {
465 if (width + shift > 16) {
466 pr_warn("divider value exceeds LOWORD field\n");
467 return ERR_PTR(-EINVAL);
468 }
469 }
470
471
472 div = kzalloc(sizeof(*div), GFP_KERNEL);
473 if (!div)
474 return ERR_PTR(-ENOMEM);
475
476 init.name = name;
477 if (clk_divider_flags & CLK_DIVIDER_READ_ONLY)
478 init.ops = &clk_divider_ro_ops;
479 else
480 init.ops = &clk_divider_ops;
481 init.flags = flags | CLK_IS_BASIC;
482 init.parent_names = (parent_name ? &parent_name: NULL);
483 init.num_parents = (parent_name ? 1 : 0);
484
485
486 div->reg = reg;
487 div->shift = shift;
488 div->width = width;
489 div->flags = clk_divider_flags;
490 div->lock = lock;
491 div->hw.init = &init;
492 div->table = table;
493
494
495 hw = &div->hw;
496 ret = clk_hw_register(dev, hw);
497 if (ret) {
498 kfree(div);
499 hw = ERR_PTR(ret);
500 }
501
502 return hw;
503}
504
505
506
507
508
509
510
511
512
513
514
515
516
517struct clk *clk_register_divider(struct device *dev, const char *name,
518 const char *parent_name, unsigned long flags,
519 void __iomem *reg, u8 shift, u8 width,
520 u8 clk_divider_flags, spinlock_t *lock)
521{
522 struct clk_hw *hw;
523
524 hw = _register_divider(dev, name, parent_name, flags, reg, shift,
525 width, clk_divider_flags, NULL, lock);
526 if (IS_ERR(hw))
527 return ERR_CAST(hw);
528 return hw->clk;
529}
530EXPORT_SYMBOL_GPL(clk_register_divider);
531
532
533
534
535
536
537
538
539
540
541
542
543
544struct clk_hw *clk_hw_register_divider(struct device *dev, const char *name,
545 const char *parent_name, unsigned long flags,
546 void __iomem *reg, u8 shift, u8 width,
547 u8 clk_divider_flags, spinlock_t *lock)
548{
549 return _register_divider(dev, name, parent_name, flags, reg, shift,
550 width, clk_divider_flags, NULL, lock);
551}
552EXPORT_SYMBOL_GPL(clk_hw_register_divider);
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568struct clk *clk_register_divider_table(struct device *dev, const char *name,
569 const char *parent_name, unsigned long flags,
570 void __iomem *reg, u8 shift, u8 width,
571 u8 clk_divider_flags, const struct clk_div_table *table,
572 spinlock_t *lock)
573{
574 struct clk_hw *hw;
575
576 hw = _register_divider(dev, name, parent_name, flags, reg, shift,
577 width, clk_divider_flags, table, lock);
578 if (IS_ERR(hw))
579 return ERR_CAST(hw);
580 return hw->clk;
581}
582EXPORT_SYMBOL_GPL(clk_register_divider_table);
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598struct clk_hw *clk_hw_register_divider_table(struct device *dev,
599 const char *name, const char *parent_name, unsigned long flags,
600 void __iomem *reg, u8 shift, u8 width,
601 u8 clk_divider_flags, const struct clk_div_table *table,
602 spinlock_t *lock)
603{
604 return _register_divider(dev, name, parent_name, flags, reg, shift,
605 width, clk_divider_flags, table, lock);
606}
607EXPORT_SYMBOL_GPL(clk_hw_register_divider_table);
608
609void clk_unregister_divider(struct clk *clk)
610{
611 struct clk_divider *div;
612 struct clk_hw *hw;
613
614 hw = __clk_get_hw(clk);
615 if (!hw)
616 return;
617
618 div = to_clk_divider(hw);
619
620 clk_unregister(clk);
621 kfree(div);
622}
623EXPORT_SYMBOL_GPL(clk_unregister_divider);
624
625
626
627
628
629void clk_hw_unregister_divider(struct clk_hw *hw)
630{
631 struct clk_divider *div;
632
633 div = to_clk_divider(hw);
634
635 clk_hw_unregister(hw);
636 kfree(div);
637}
638EXPORT_SYMBOL_GPL(clk_hw_unregister_divider);
639