1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | // |
3 | // OWL factor clock driver |
4 | // |
5 | // Copyright (c) 2014 Actions Semi Inc. |
6 | // Author: David Liu <liuwei@actions-semi.com> |
7 | // |
8 | // Copyright (c) 2018 Linaro Ltd. |
9 | // Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org> |
10 | |
11 | #include <linux/clk-provider.h> |
12 | #include <linux/regmap.h> |
13 | |
14 | #include "owl-factor.h" |
15 | |
16 | static unsigned int _get_table_maxval(const struct clk_factor_table *table) |
17 | { |
18 | unsigned int maxval = 0; |
19 | const struct clk_factor_table *clkt; |
20 | |
21 | for (clkt = table; clkt->div; clkt++) |
22 | if (clkt->val > maxval) |
23 | maxval = clkt->val; |
24 | return maxval; |
25 | } |
26 | |
27 | static int _get_table_div_mul(const struct clk_factor_table *table, |
28 | unsigned int val, unsigned int *mul, unsigned int *div) |
29 | { |
30 | const struct clk_factor_table *clkt; |
31 | |
32 | for (clkt = table; clkt->div; clkt++) { |
33 | if (clkt->val == val) { |
34 | *mul = clkt->mul; |
35 | *div = clkt->div; |
36 | return 1; |
37 | } |
38 | } |
39 | |
40 | return 0; |
41 | } |
42 | |
43 | static unsigned int _get_table_val(const struct clk_factor_table *table, |
44 | unsigned long rate, unsigned long parent_rate) |
45 | { |
46 | const struct clk_factor_table *clkt; |
47 | int val = -1; |
48 | u64 calc_rate; |
49 | |
50 | for (clkt = table; clkt->div; clkt++) { |
51 | calc_rate = parent_rate * clkt->mul; |
52 | do_div(calc_rate, clkt->div); |
53 | |
54 | if ((unsigned long)calc_rate <= rate) { |
55 | val = clkt->val; |
56 | break; |
57 | } |
58 | } |
59 | |
60 | if (val == -1) |
61 | val = _get_table_maxval(table); |
62 | |
63 | return val; |
64 | } |
65 | |
66 | static int owl_clk_val_best(const struct owl_factor_hw *factor_hw, |
67 | struct clk_hw *hw, unsigned long rate, |
68 | unsigned long *best_parent_rate) |
69 | { |
70 | const struct clk_factor_table *clkt = factor_hw->table; |
71 | unsigned long parent_rate, try_parent_rate, best = 0, cur_rate; |
72 | unsigned long parent_rate_saved = *best_parent_rate; |
73 | int bestval = 0; |
74 | |
75 | if (!rate) |
76 | rate = 1; |
77 | |
78 | if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { |
79 | parent_rate = *best_parent_rate; |
80 | bestval = _get_table_val(table: clkt, rate, parent_rate); |
81 | return bestval; |
82 | } |
83 | |
84 | for (clkt = factor_hw->table; clkt->div; clkt++) { |
85 | try_parent_rate = rate * clkt->div / clkt->mul; |
86 | |
87 | if (try_parent_rate == parent_rate_saved) { |
88 | pr_debug("%s: [%d %d %d] found try_parent_rate %ld\n" , |
89 | __func__, clkt->val, clkt->mul, clkt->div, |
90 | try_parent_rate); |
91 | /* |
92 | * It's the most ideal case if the requested rate can be |
93 | * divided from parent clock without any need to change |
94 | * parent rate, so return the divider immediately. |
95 | */ |
96 | *best_parent_rate = parent_rate_saved; |
97 | return clkt->val; |
98 | } |
99 | |
100 | parent_rate = clk_hw_round_rate(hw: clk_hw_get_parent(hw), |
101 | rate: try_parent_rate); |
102 | cur_rate = DIV_ROUND_UP(parent_rate, clkt->div) * clkt->mul; |
103 | if (cur_rate <= rate && cur_rate > best) { |
104 | bestval = clkt->val; |
105 | best = cur_rate; |
106 | *best_parent_rate = parent_rate; |
107 | } |
108 | } |
109 | |
110 | if (!bestval) { |
111 | bestval = _get_table_maxval(table: clkt); |
112 | *best_parent_rate = clk_hw_round_rate( |
113 | hw: clk_hw_get_parent(hw), rate: 1); |
114 | } |
115 | |
116 | return bestval; |
117 | } |
118 | |
119 | long owl_factor_helper_round_rate(struct owl_clk_common *common, |
120 | const struct owl_factor_hw *factor_hw, |
121 | unsigned long rate, |
122 | unsigned long *parent_rate) |
123 | { |
124 | const struct clk_factor_table *clkt = factor_hw->table; |
125 | unsigned int val, mul = 0, div = 1; |
126 | |
127 | val = owl_clk_val_best(factor_hw, hw: &common->hw, rate, best_parent_rate: parent_rate); |
128 | _get_table_div_mul(table: clkt, val, mul: &mul, div: &div); |
129 | |
130 | return *parent_rate * mul / div; |
131 | } |
132 | |
133 | static long owl_factor_round_rate(struct clk_hw *hw, unsigned long rate, |
134 | unsigned long *parent_rate) |
135 | { |
136 | struct owl_factor *factor = hw_to_owl_factor(hw); |
137 | struct owl_factor_hw *factor_hw = &factor->factor_hw; |
138 | |
139 | return owl_factor_helper_round_rate(common: &factor->common, factor_hw, |
140 | rate, parent_rate); |
141 | } |
142 | |
143 | unsigned long owl_factor_helper_recalc_rate(struct owl_clk_common *common, |
144 | const struct owl_factor_hw *factor_hw, |
145 | unsigned long parent_rate) |
146 | { |
147 | const struct clk_factor_table *clkt = factor_hw->table; |
148 | unsigned long long int rate; |
149 | u32 reg, val, mul, div; |
150 | |
151 | div = 0; |
152 | mul = 0; |
153 | |
154 | regmap_read(map: common->regmap, reg: factor_hw->reg, val: ®); |
155 | |
156 | val = reg >> factor_hw->shift; |
157 | val &= div_mask(factor_hw); |
158 | |
159 | _get_table_div_mul(table: clkt, val, mul: &mul, div: &div); |
160 | if (!div) { |
161 | WARN(!(factor_hw->fct_flags & CLK_DIVIDER_ALLOW_ZERO), |
162 | "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n" , |
163 | __clk_get_name(common->hw.clk)); |
164 | return parent_rate; |
165 | } |
166 | |
167 | rate = (unsigned long long int)parent_rate * mul; |
168 | do_div(rate, div); |
169 | |
170 | return rate; |
171 | } |
172 | |
173 | static unsigned long owl_factor_recalc_rate(struct clk_hw *hw, |
174 | unsigned long parent_rate) |
175 | { |
176 | struct owl_factor *factor = hw_to_owl_factor(hw); |
177 | struct owl_factor_hw *factor_hw = &factor->factor_hw; |
178 | struct owl_clk_common *common = &factor->common; |
179 | |
180 | return owl_factor_helper_recalc_rate(common, factor_hw, parent_rate); |
181 | } |
182 | |
183 | int owl_factor_helper_set_rate(const struct owl_clk_common *common, |
184 | const struct owl_factor_hw *factor_hw, |
185 | unsigned long rate, |
186 | unsigned long parent_rate) |
187 | { |
188 | u32 val, reg; |
189 | |
190 | val = _get_table_val(table: factor_hw->table, rate, parent_rate); |
191 | |
192 | if (val > div_mask(factor_hw)) |
193 | val = div_mask(factor_hw); |
194 | |
195 | regmap_read(map: common->regmap, reg: factor_hw->reg, val: ®); |
196 | |
197 | reg &= ~(div_mask(factor_hw) << factor_hw->shift); |
198 | reg |= val << factor_hw->shift; |
199 | |
200 | regmap_write(map: common->regmap, reg: factor_hw->reg, val: reg); |
201 | |
202 | return 0; |
203 | } |
204 | |
205 | static int owl_factor_set_rate(struct clk_hw *hw, unsigned long rate, |
206 | unsigned long parent_rate) |
207 | { |
208 | struct owl_factor *factor = hw_to_owl_factor(hw); |
209 | struct owl_factor_hw *factor_hw = &factor->factor_hw; |
210 | struct owl_clk_common *common = &factor->common; |
211 | |
212 | return owl_factor_helper_set_rate(common, factor_hw, |
213 | rate, parent_rate); |
214 | } |
215 | |
216 | const struct clk_ops owl_factor_ops = { |
217 | .round_rate = owl_factor_round_rate, |
218 | .recalc_rate = owl_factor_recalc_rate, |
219 | .set_rate = owl_factor_set_rate, |
220 | }; |
221 | |