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