Commit | Line | Data |
---|---|---|
f2e0a532 MR |
1 | /* |
2 | * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com> | |
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 | #include <linux/bitops.h> | |
10 | #include <linux/clk-provider.h> | |
11 | #include <linux/err.h> | |
12 | #include <linux/export.h> | |
13 | #include <linux/kernel.h> | |
14 | #include <linux/of.h> | |
15 | #include <linux/slab.h> | |
16 | ||
f2e0a532 MR |
17 | static unsigned long __get_mult(struct clk_multiplier *mult, |
18 | unsigned long rate, | |
19 | unsigned long parent_rate) | |
20 | { | |
21 | if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST) | |
22 | return DIV_ROUND_CLOSEST(rate, parent_rate); | |
23 | ||
24 | return rate / parent_rate; | |
25 | } | |
26 | ||
27 | static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw, | |
28 | unsigned long parent_rate) | |
29 | { | |
30 | struct clk_multiplier *mult = to_clk_multiplier(hw); | |
31 | unsigned long val; | |
32 | ||
33 | val = clk_readl(mult->reg) >> mult->shift; | |
34 | val &= GENMASK(mult->width - 1, 0); | |
35 | ||
36 | if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS) | |
37 | val = 1; | |
38 | ||
39 | return parent_rate * val; | |
40 | } | |
41 | ||
42 | static bool __is_best_rate(unsigned long rate, unsigned long new, | |
43 | unsigned long best, unsigned long flags) | |
44 | { | |
45 | if (flags & CLK_MULTIPLIER_ROUND_CLOSEST) | |
46 | return abs(rate - new) < abs(rate - best); | |
47 | ||
48 | return new >= rate && new < best; | |
49 | } | |
50 | ||
51 | static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate, | |
52 | unsigned long *best_parent_rate, | |
53 | u8 width, unsigned long flags) | |
54 | { | |
55 | unsigned long orig_parent_rate = *best_parent_rate; | |
56 | unsigned long parent_rate, current_rate, best_rate = ~0; | |
57 | unsigned int i, bestmult = 0; | |
58 | ||
59 | if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) | |
60 | return rate / *best_parent_rate; | |
61 | ||
62 | for (i = 1; i < ((1 << width) - 1); i++) { | |
63 | if (rate == orig_parent_rate * i) { | |
64 | /* | |
65 | * This is the best case for us if we have a | |
66 | * perfect match without changing the parent | |
67 | * rate. | |
68 | */ | |
69 | *best_parent_rate = orig_parent_rate; | |
70 | return i; | |
71 | } | |
72 | ||
73 | parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), | |
74 | rate / i); | |
75 | current_rate = parent_rate * i; | |
76 | ||
77 | if (__is_best_rate(rate, current_rate, best_rate, flags)) { | |
78 | bestmult = i; | |
79 | best_rate = current_rate; | |
80 | *best_parent_rate = parent_rate; | |
81 | } | |
82 | } | |
83 | ||
84 | return bestmult; | |
85 | } | |
86 | ||
87 | static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate, | |
88 | unsigned long *parent_rate) | |
89 | { | |
90 | struct clk_multiplier *mult = to_clk_multiplier(hw); | |
91 | unsigned long factor = __bestmult(hw, rate, parent_rate, | |
92 | mult->width, mult->flags); | |
93 | ||
94 | return *parent_rate * factor; | |
95 | } | |
96 | ||
97 | static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate, | |
98 | unsigned long parent_rate) | |
99 | { | |
100 | struct clk_multiplier *mult = to_clk_multiplier(hw); | |
101 | unsigned long factor = __get_mult(mult, rate, parent_rate); | |
102 | unsigned long flags = 0; | |
103 | unsigned long val; | |
104 | ||
105 | if (mult->lock) | |
106 | spin_lock_irqsave(mult->lock, flags); | |
107 | else | |
108 | __acquire(mult->lock); | |
109 | ||
110 | val = clk_readl(mult->reg); | |
111 | val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift); | |
112 | val |= factor << mult->shift; | |
113 | clk_writel(val, mult->reg); | |
114 | ||
115 | if (mult->lock) | |
116 | spin_unlock_irqrestore(mult->lock, flags); | |
117 | else | |
118 | __release(mult->lock); | |
119 | ||
120 | return 0; | |
121 | } | |
122 | ||
123 | const struct clk_ops clk_multiplier_ops = { | |
124 | .recalc_rate = clk_multiplier_recalc_rate, | |
125 | .round_rate = clk_multiplier_round_rate, | |
126 | .set_rate = clk_multiplier_set_rate, | |
127 | }; | |
128 | EXPORT_SYMBOL_GPL(clk_multiplier_ops); |