1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Zynq UltraScale+ MPSoC Divider support |
4 | * |
5 | * Copyright (C) 2016-2019 Xilinx |
6 | * |
7 | * Adjustable divider clock implementation |
8 | */ |
9 | |
10 | #include <linux/clk.h> |
11 | #include <linux/clk-provider.h> |
12 | #include <linux/slab.h> |
13 | #include "clk-zynqmp.h" |
14 | |
15 | /* |
16 | * DOC: basic adjustable divider clock that cannot gate |
17 | * |
18 | * Traits of this clock: |
19 | * prepare - clk_prepare only ensures that parents are prepared |
20 | * enable - clk_enable only ensures that parents are enabled |
21 | * rate - rate is adjustable. clk->rate = ceiling(parent->rate / divisor) |
22 | * parent - fixed parent. No clk_set_parent support |
23 | */ |
24 | |
25 | #define to_zynqmp_clk_divider(_hw) \ |
26 | container_of(_hw, struct zynqmp_clk_divider, hw) |
27 | |
28 | #define CLK_FRAC BIT(13) /* has a fractional parent */ |
29 | #define CUSTOM_FLAG_CLK_FRAC BIT(0) /* has a fractional parent in custom type flag */ |
30 | |
31 | /** |
32 | * struct zynqmp_clk_divider - adjustable divider clock |
33 | * @hw: handle between common and hardware-specific interfaces |
34 | * @flags: Hardware specific flags |
35 | * @is_frac: The divider is a fractional divider |
36 | * @clk_id: Id of clock |
37 | * @div_type: divisor type (TYPE_DIV1 or TYPE_DIV2) |
38 | * @max_div: maximum supported divisor (fetched from firmware) |
39 | */ |
40 | struct zynqmp_clk_divider { |
41 | struct clk_hw hw; |
42 | u8 flags; |
43 | bool is_frac; |
44 | u32 clk_id; |
45 | u32 div_type; |
46 | u16 max_div; |
47 | }; |
48 | |
49 | static inline int zynqmp_divider_get_val(unsigned long parent_rate, |
50 | unsigned long rate, u16 flags) |
51 | { |
52 | int up, down; |
53 | unsigned long up_rate, down_rate; |
54 | |
55 | if (flags & CLK_DIVIDER_POWER_OF_TWO) { |
56 | up = DIV_ROUND_UP_ULL((u64)parent_rate, rate); |
57 | down = DIV_ROUND_DOWN_ULL((u64)parent_rate, rate); |
58 | |
59 | up = __roundup_pow_of_two(n: up); |
60 | down = __rounddown_pow_of_two(n: down); |
61 | |
62 | up_rate = DIV_ROUND_UP_ULL((u64)parent_rate, up); |
63 | down_rate = DIV_ROUND_UP_ULL((u64)parent_rate, down); |
64 | |
65 | return (rate - up_rate) <= (down_rate - rate) ? up : down; |
66 | |
67 | } else { |
68 | return DIV_ROUND_CLOSEST(parent_rate, rate); |
69 | } |
70 | } |
71 | |
72 | /** |
73 | * zynqmp_clk_divider_recalc_rate() - Recalc rate of divider clock |
74 | * @hw: handle between common and hardware-specific interfaces |
75 | * @parent_rate: rate of parent clock |
76 | * |
77 | * Return: 0 on success else error+reason |
78 | */ |
79 | static unsigned long zynqmp_clk_divider_recalc_rate(struct clk_hw *hw, |
80 | unsigned long parent_rate) |
81 | { |
82 | struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw); |
83 | const char *clk_name = clk_hw_get_name(hw); |
84 | u32 clk_id = divider->clk_id; |
85 | u32 div_type = divider->div_type; |
86 | u32 div, value; |
87 | int ret; |
88 | |
89 | ret = zynqmp_pm_clock_getdivider(clock_id: clk_id, divider: &div); |
90 | |
91 | if (ret) |
92 | pr_debug("%s() get divider failed for %s, ret = %d\n" , |
93 | __func__, clk_name, ret); |
94 | |
95 | if (div_type == TYPE_DIV1) |
96 | value = div & 0xFFFF; |
97 | else |
98 | value = div >> 16; |
99 | |
100 | if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) |
101 | value = 1 << value; |
102 | |
103 | if (!value) { |
104 | WARN(!(divider->flags & CLK_DIVIDER_ALLOW_ZERO), |
105 | "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n" , |
106 | clk_name); |
107 | return parent_rate; |
108 | } |
109 | |
110 | return DIV_ROUND_UP_ULL(parent_rate, value); |
111 | } |
112 | |
113 | /** |
114 | * zynqmp_clk_divider_round_rate() - Round rate of divider clock |
115 | * @hw: handle between common and hardware-specific interfaces |
116 | * @rate: rate of clock to be set |
117 | * @prate: rate of parent clock |
118 | * |
119 | * Return: 0 on success else error+reason |
120 | */ |
121 | static long zynqmp_clk_divider_round_rate(struct clk_hw *hw, |
122 | unsigned long rate, |
123 | unsigned long *prate) |
124 | { |
125 | struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw); |
126 | const char *clk_name = clk_hw_get_name(hw); |
127 | u32 clk_id = divider->clk_id; |
128 | u32 div_type = divider->div_type; |
129 | u32 bestdiv; |
130 | int ret; |
131 | u8 width; |
132 | |
133 | /* if read only, just return current value */ |
134 | if (divider->flags & CLK_DIVIDER_READ_ONLY) { |
135 | ret = zynqmp_pm_clock_getdivider(clock_id: clk_id, divider: &bestdiv); |
136 | |
137 | if (ret) |
138 | pr_debug("%s() get divider failed for %s, ret = %d\n" , |
139 | __func__, clk_name, ret); |
140 | if (div_type == TYPE_DIV1) |
141 | bestdiv = bestdiv & 0xFFFF; |
142 | else |
143 | bestdiv = bestdiv >> 16; |
144 | |
145 | if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) |
146 | bestdiv = 1 << bestdiv; |
147 | |
148 | return DIV_ROUND_UP_ULL((u64)*prate, bestdiv); |
149 | } |
150 | |
151 | width = fls(x: divider->max_div); |
152 | |
153 | rate = divider_round_rate(hw, rate, prate, NULL, width, flags: divider->flags); |
154 | |
155 | if (divider->is_frac && (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) && (rate % *prate)) |
156 | *prate = rate; |
157 | |
158 | return rate; |
159 | } |
160 | |
161 | /** |
162 | * zynqmp_clk_divider_set_rate() - Set rate of divider clock |
163 | * @hw: handle between common and hardware-specific interfaces |
164 | * @rate: rate of clock to be set |
165 | * @parent_rate: rate of parent clock |
166 | * |
167 | * Return: 0 on success else error+reason |
168 | */ |
169 | static int zynqmp_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate, |
170 | unsigned long parent_rate) |
171 | { |
172 | struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw); |
173 | const char *clk_name = clk_hw_get_name(hw); |
174 | u32 clk_id = divider->clk_id; |
175 | u32 div_type = divider->div_type; |
176 | u32 value, div; |
177 | int ret; |
178 | |
179 | value = zynqmp_divider_get_val(parent_rate, rate, flags: divider->flags); |
180 | if (div_type == TYPE_DIV1) { |
181 | div = value & 0xFFFF; |
182 | div |= 0xffff << 16; |
183 | } else { |
184 | div = 0xffff; |
185 | div |= value << 16; |
186 | } |
187 | |
188 | if (divider->flags & CLK_DIVIDER_POWER_OF_TWO) |
189 | div = __ffs(div); |
190 | |
191 | ret = zynqmp_pm_clock_setdivider(clock_id: clk_id, divider: div); |
192 | |
193 | if (ret) |
194 | pr_debug("%s() set divider failed for %s, ret = %d\n" , |
195 | __func__, clk_name, ret); |
196 | |
197 | return ret; |
198 | } |
199 | |
200 | static const struct clk_ops zynqmp_clk_divider_ops = { |
201 | .recalc_rate = zynqmp_clk_divider_recalc_rate, |
202 | .round_rate = zynqmp_clk_divider_round_rate, |
203 | .set_rate = zynqmp_clk_divider_set_rate, |
204 | }; |
205 | |
206 | static const struct clk_ops zynqmp_clk_divider_ro_ops = { |
207 | .recalc_rate = zynqmp_clk_divider_recalc_rate, |
208 | .round_rate = zynqmp_clk_divider_round_rate, |
209 | }; |
210 | |
211 | /** |
212 | * zynqmp_clk_get_max_divisor() - Get maximum supported divisor from firmware. |
213 | * @clk_id: Id of clock |
214 | * @type: Divider type |
215 | * |
216 | * Return: Maximum divisor of a clock if query data is successful |
217 | * U16_MAX in case of query data is not success |
218 | */ |
219 | static u32 zynqmp_clk_get_max_divisor(u32 clk_id, u32 type) |
220 | { |
221 | struct zynqmp_pm_query_data qdata = {0}; |
222 | u32 ret_payload[PAYLOAD_ARG_CNT]; |
223 | int ret; |
224 | |
225 | qdata.qid = PM_QID_CLOCK_GET_MAX_DIVISOR; |
226 | qdata.arg1 = clk_id; |
227 | qdata.arg2 = type; |
228 | ret = zynqmp_pm_query_data(qdata, out: ret_payload); |
229 | /* |
230 | * To maintain backward compatibility return maximum possible value |
231 | * (0xFFFF) if query for max divisor is not successful. |
232 | */ |
233 | if (ret) |
234 | return U16_MAX; |
235 | |
236 | return ret_payload[1]; |
237 | } |
238 | |
239 | static inline unsigned long zynqmp_clk_map_divider_ccf_flags( |
240 | const u32 zynqmp_type_flag) |
241 | { |
242 | unsigned long ccf_flag = 0; |
243 | |
244 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ONE_BASED) |
245 | ccf_flag |= CLK_DIVIDER_ONE_BASED; |
246 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_POWER_OF_TWO) |
247 | ccf_flag |= CLK_DIVIDER_POWER_OF_TWO; |
248 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ALLOW_ZERO) |
249 | ccf_flag |= CLK_DIVIDER_ALLOW_ZERO; |
250 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_POWER_OF_TWO) |
251 | ccf_flag |= CLK_DIVIDER_HIWORD_MASK; |
252 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ROUND_CLOSEST) |
253 | ccf_flag |= CLK_DIVIDER_ROUND_CLOSEST; |
254 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_READ_ONLY) |
255 | ccf_flag |= CLK_DIVIDER_READ_ONLY; |
256 | if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_MAX_AT_ZERO) |
257 | ccf_flag |= CLK_DIVIDER_MAX_AT_ZERO; |
258 | |
259 | return ccf_flag; |
260 | } |
261 | |
262 | /** |
263 | * zynqmp_clk_register_divider() - Register a divider clock |
264 | * @name: Name of this clock |
265 | * @clk_id: Id of clock |
266 | * @parents: Name of this clock's parents |
267 | * @num_parents: Number of parents |
268 | * @nodes: Clock topology node |
269 | * |
270 | * Return: clock hardware to registered clock divider |
271 | */ |
272 | struct clk_hw *zynqmp_clk_register_divider(const char *name, |
273 | u32 clk_id, |
274 | const char * const *parents, |
275 | u8 num_parents, |
276 | const struct clock_topology *nodes) |
277 | { |
278 | struct zynqmp_clk_divider *div; |
279 | struct clk_hw *hw; |
280 | struct clk_init_data init; |
281 | int ret; |
282 | |
283 | /* allocate the divider */ |
284 | div = kzalloc(size: sizeof(*div), GFP_KERNEL); |
285 | if (!div) |
286 | return ERR_PTR(error: -ENOMEM); |
287 | |
288 | init.name = name; |
289 | if (nodes->type_flag & CLK_DIVIDER_READ_ONLY) |
290 | init.ops = &zynqmp_clk_divider_ro_ops; |
291 | else |
292 | init.ops = &zynqmp_clk_divider_ops; |
293 | |
294 | init.flags = zynqmp_clk_map_common_ccf_flags(zynqmp_flag: nodes->flag); |
295 | |
296 | init.parent_names = parents; |
297 | init.num_parents = 1; |
298 | |
299 | /* struct clk_divider assignments */ |
300 | div->is_frac = !!((nodes->flag & CLK_FRAC) | |
301 | (nodes->custom_type_flag & CUSTOM_FLAG_CLK_FRAC)); |
302 | div->flags = zynqmp_clk_map_divider_ccf_flags(zynqmp_type_flag: nodes->type_flag); |
303 | div->hw.init = &init; |
304 | div->clk_id = clk_id; |
305 | div->div_type = nodes->type; |
306 | |
307 | /* |
308 | * To achieve best possible rate, maximum limit of divider is required |
309 | * while computation. |
310 | */ |
311 | div->max_div = zynqmp_clk_get_max_divisor(clk_id, type: nodes->type); |
312 | |
313 | hw = &div->hw; |
314 | ret = clk_hw_register(NULL, hw); |
315 | if (ret) { |
316 | kfree(objp: div); |
317 | hw = ERR_PTR(error: ret); |
318 | } |
319 | |
320 | return hw; |
321 | } |
322 | |