1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * mmp mix(div and mux) clock operation source file |
4 | * |
5 | * Copyright (C) 2014 Marvell |
6 | * Chao Xie <chao.xie@marvell.com> |
7 | */ |
8 | |
9 | #include <linux/clk-provider.h> |
10 | #include <linux/slab.h> |
11 | #include <linux/io.h> |
12 | #include <linux/err.h> |
13 | |
14 | #include "clk.h" |
15 | |
16 | /* |
17 | * The mix clock is a clock combined mux and div type clock. |
18 | * Because the div field and mux field need to be set at same |
19 | * time, we can not divide it into 2 types of clock |
20 | */ |
21 | |
22 | #define to_clk_mix(hw) container_of(hw, struct mmp_clk_mix, hw) |
23 | |
24 | static unsigned int _get_maxdiv(struct mmp_clk_mix *mix) |
25 | { |
26 | unsigned int div_mask = (1 << mix->reg_info.width_div) - 1; |
27 | unsigned int maxdiv = 0; |
28 | struct clk_div_table *clkt; |
29 | |
30 | if (mix->div_flags & CLK_DIVIDER_ONE_BASED) |
31 | return div_mask; |
32 | if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO) |
33 | return 1 << div_mask; |
34 | if (mix->div_table) { |
35 | for (clkt = mix->div_table; clkt->div; clkt++) |
36 | if (clkt->div > maxdiv) |
37 | maxdiv = clkt->div; |
38 | return maxdiv; |
39 | } |
40 | return div_mask + 1; |
41 | } |
42 | |
43 | static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val) |
44 | { |
45 | struct clk_div_table *clkt; |
46 | |
47 | if (mix->div_flags & CLK_DIVIDER_ONE_BASED) |
48 | return val; |
49 | if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO) |
50 | return 1 << val; |
51 | if (mix->div_table) { |
52 | for (clkt = mix->div_table; clkt->div; clkt++) |
53 | if (clkt->val == val) |
54 | return clkt->div; |
55 | if (clkt->div == 0) |
56 | return 0; |
57 | } |
58 | return val + 1; |
59 | } |
60 | |
61 | static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val) |
62 | { |
63 | int num_parents = clk_hw_get_num_parents(hw: &mix->hw); |
64 | int i; |
65 | |
66 | if (mix->mux_flags & CLK_MUX_INDEX_BIT) |
67 | return ffs(val) - 1; |
68 | if (mix->mux_flags & CLK_MUX_INDEX_ONE) |
69 | return val - 1; |
70 | if (mix->mux_table) { |
71 | for (i = 0; i < num_parents; i++) |
72 | if (mix->mux_table[i] == val) |
73 | return i; |
74 | if (i == num_parents) |
75 | return 0; |
76 | } |
77 | |
78 | return val; |
79 | } |
80 | static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div) |
81 | { |
82 | struct clk_div_table *clkt; |
83 | |
84 | if (mix->div_flags & CLK_DIVIDER_ONE_BASED) |
85 | return div; |
86 | if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO) |
87 | return __ffs(div); |
88 | if (mix->div_table) { |
89 | for (clkt = mix->div_table; clkt->div; clkt++) |
90 | if (clkt->div == div) |
91 | return clkt->val; |
92 | if (clkt->div == 0) |
93 | return 0; |
94 | } |
95 | |
96 | return div - 1; |
97 | } |
98 | |
99 | static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux) |
100 | { |
101 | if (mix->mux_table) |
102 | return mix->mux_table[mux]; |
103 | |
104 | return mux; |
105 | } |
106 | |
107 | static void _filter_clk_table(struct mmp_clk_mix *mix, |
108 | struct mmp_clk_mix_clk_table *table, |
109 | unsigned int table_size) |
110 | { |
111 | int i; |
112 | struct mmp_clk_mix_clk_table *item; |
113 | struct clk_hw *parent, *hw; |
114 | unsigned long parent_rate; |
115 | |
116 | hw = &mix->hw; |
117 | |
118 | for (i = 0; i < table_size; i++) { |
119 | item = &table[i]; |
120 | parent = clk_hw_get_parent_by_index(hw, index: item->parent_index); |
121 | parent_rate = clk_hw_get_rate(hw: parent); |
122 | if (parent_rate % item->rate) { |
123 | item->valid = 0; |
124 | } else { |
125 | item->divisor = parent_rate / item->rate; |
126 | item->valid = 1; |
127 | } |
128 | } |
129 | } |
130 | |
131 | static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val, |
132 | unsigned int change_mux, unsigned int change_div) |
133 | { |
134 | struct mmp_clk_mix_reg_info *ri = &mix->reg_info; |
135 | u8 width, shift; |
136 | u32 mux_div, fc_req; |
137 | int ret, timeout = 50; |
138 | unsigned long flags = 0; |
139 | |
140 | if (!change_mux && !change_div) |
141 | return -EINVAL; |
142 | |
143 | if (mix->lock) |
144 | spin_lock_irqsave(mix->lock, flags); |
145 | |
146 | if (mix->type == MMP_CLK_MIX_TYPE_V1 |
147 | || mix->type == MMP_CLK_MIX_TYPE_V2) |
148 | mux_div = readl(addr: ri->reg_clk_ctrl); |
149 | else |
150 | mux_div = readl(addr: ri->reg_clk_sel); |
151 | |
152 | if (change_div) { |
153 | width = ri->width_div; |
154 | shift = ri->shift_div; |
155 | mux_div &= ~MMP_CLK_BITS_MASK(width, shift); |
156 | mux_div |= MMP_CLK_BITS_SET_VAL(div_val, width, shift); |
157 | } |
158 | |
159 | if (change_mux) { |
160 | width = ri->width_mux; |
161 | shift = ri->shift_mux; |
162 | mux_div &= ~MMP_CLK_BITS_MASK(width, shift); |
163 | mux_div |= MMP_CLK_BITS_SET_VAL(mux_val, width, shift); |
164 | } |
165 | |
166 | if (mix->type == MMP_CLK_MIX_TYPE_V1) { |
167 | writel(val: mux_div, addr: ri->reg_clk_ctrl); |
168 | } else if (mix->type == MMP_CLK_MIX_TYPE_V2) { |
169 | mux_div |= (1 << ri->bit_fc); |
170 | writel(val: mux_div, addr: ri->reg_clk_ctrl); |
171 | |
172 | do { |
173 | fc_req = readl(addr: ri->reg_clk_ctrl); |
174 | timeout--; |
175 | if (!(fc_req & (1 << ri->bit_fc))) |
176 | break; |
177 | } while (timeout); |
178 | |
179 | if (timeout == 0) { |
180 | pr_err("%s:%s cannot do frequency change\n" , |
181 | __func__, clk_hw_get_name(&mix->hw)); |
182 | ret = -EBUSY; |
183 | goto error; |
184 | } |
185 | } else { |
186 | fc_req = readl(addr: ri->reg_clk_ctrl); |
187 | fc_req |= 1 << ri->bit_fc; |
188 | writel(val: fc_req, addr: ri->reg_clk_ctrl); |
189 | writel(val: mux_div, addr: ri->reg_clk_sel); |
190 | fc_req &= ~(1 << ri->bit_fc); |
191 | } |
192 | |
193 | ret = 0; |
194 | error: |
195 | if (mix->lock) |
196 | spin_unlock_irqrestore(lock: mix->lock, flags); |
197 | |
198 | return ret; |
199 | } |
200 | |
201 | static int mmp_clk_mix_determine_rate(struct clk_hw *hw, |
202 | struct clk_rate_request *req) |
203 | { |
204 | struct mmp_clk_mix *mix = to_clk_mix(hw); |
205 | struct mmp_clk_mix_clk_table *item; |
206 | struct clk_hw *parent, *parent_best; |
207 | unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best; |
208 | unsigned long gap, gap_best; |
209 | u32 div_val_max; |
210 | unsigned int div; |
211 | int i, j; |
212 | |
213 | |
214 | mix_rate_best = 0; |
215 | parent_rate_best = 0; |
216 | gap_best = ULONG_MAX; |
217 | parent_best = NULL; |
218 | |
219 | if (mix->table) { |
220 | for (i = 0; i < mix->table_size; i++) { |
221 | item = &mix->table[i]; |
222 | if (item->valid == 0) |
223 | continue; |
224 | parent = clk_hw_get_parent_by_index(hw, |
225 | index: item->parent_index); |
226 | parent_rate = clk_hw_get_rate(hw: parent); |
227 | mix_rate = parent_rate / item->divisor; |
228 | gap = abs(mix_rate - req->rate); |
229 | if (!parent_best || gap < gap_best) { |
230 | parent_best = parent; |
231 | parent_rate_best = parent_rate; |
232 | mix_rate_best = mix_rate; |
233 | gap_best = gap; |
234 | if (gap_best == 0) |
235 | goto found; |
236 | } |
237 | } |
238 | } else { |
239 | for (i = 0; i < clk_hw_get_num_parents(hw); i++) { |
240 | parent = clk_hw_get_parent_by_index(hw, index: i); |
241 | parent_rate = clk_hw_get_rate(hw: parent); |
242 | div_val_max = _get_maxdiv(mix); |
243 | for (j = 0; j < div_val_max; j++) { |
244 | div = _get_div(mix, val: j); |
245 | mix_rate = parent_rate / div; |
246 | gap = abs(mix_rate - req->rate); |
247 | if (!parent_best || gap < gap_best) { |
248 | parent_best = parent; |
249 | parent_rate_best = parent_rate; |
250 | mix_rate_best = mix_rate; |
251 | gap_best = gap; |
252 | if (gap_best == 0) |
253 | goto found; |
254 | } |
255 | } |
256 | } |
257 | } |
258 | |
259 | found: |
260 | if (!parent_best) |
261 | return -EINVAL; |
262 | |
263 | req->best_parent_rate = parent_rate_best; |
264 | req->best_parent_hw = parent_best; |
265 | req->rate = mix_rate_best; |
266 | |
267 | return 0; |
268 | } |
269 | |
270 | static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw, |
271 | unsigned long rate, |
272 | unsigned long parent_rate, |
273 | u8 index) |
274 | { |
275 | struct mmp_clk_mix *mix = to_clk_mix(hw); |
276 | unsigned int div; |
277 | u32 div_val, mux_val; |
278 | |
279 | div = parent_rate / rate; |
280 | div_val = _get_div_val(mix, div); |
281 | mux_val = _get_mux_val(mix, mux: index); |
282 | |
283 | return _set_rate(mix, mux_val, div_val, change_mux: 1, change_div: 1); |
284 | } |
285 | |
286 | static u8 mmp_clk_mix_get_parent(struct clk_hw *hw) |
287 | { |
288 | struct mmp_clk_mix *mix = to_clk_mix(hw); |
289 | struct mmp_clk_mix_reg_info *ri = &mix->reg_info; |
290 | unsigned long flags = 0; |
291 | u32 mux_div = 0; |
292 | u8 width, shift; |
293 | u32 mux_val; |
294 | |
295 | if (mix->lock) |
296 | spin_lock_irqsave(mix->lock, flags); |
297 | |
298 | if (mix->type == MMP_CLK_MIX_TYPE_V1 |
299 | || mix->type == MMP_CLK_MIX_TYPE_V2) |
300 | mux_div = readl(addr: ri->reg_clk_ctrl); |
301 | else |
302 | mux_div = readl(addr: ri->reg_clk_sel); |
303 | |
304 | if (mix->lock) |
305 | spin_unlock_irqrestore(lock: mix->lock, flags); |
306 | |
307 | width = mix->reg_info.width_mux; |
308 | shift = mix->reg_info.shift_mux; |
309 | |
310 | mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift); |
311 | |
312 | return _get_mux(mix, val: mux_val); |
313 | } |
314 | |
315 | static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw, |
316 | unsigned long parent_rate) |
317 | { |
318 | struct mmp_clk_mix *mix = to_clk_mix(hw); |
319 | struct mmp_clk_mix_reg_info *ri = &mix->reg_info; |
320 | unsigned long flags = 0; |
321 | u32 mux_div = 0; |
322 | u8 width, shift; |
323 | unsigned int div; |
324 | |
325 | if (mix->lock) |
326 | spin_lock_irqsave(mix->lock, flags); |
327 | |
328 | if (mix->type == MMP_CLK_MIX_TYPE_V1 |
329 | || mix->type == MMP_CLK_MIX_TYPE_V2) |
330 | mux_div = readl(addr: ri->reg_clk_ctrl); |
331 | else |
332 | mux_div = readl(addr: ri->reg_clk_sel); |
333 | |
334 | if (mix->lock) |
335 | spin_unlock_irqrestore(lock: mix->lock, flags); |
336 | |
337 | width = mix->reg_info.width_div; |
338 | shift = mix->reg_info.shift_div; |
339 | |
340 | div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift)); |
341 | |
342 | return parent_rate / div; |
343 | } |
344 | |
345 | static int mmp_clk_set_parent(struct clk_hw *hw, u8 index) |
346 | { |
347 | struct mmp_clk_mix *mix = to_clk_mix(hw); |
348 | struct mmp_clk_mix_clk_table *item; |
349 | int i; |
350 | u32 div_val, mux_val; |
351 | |
352 | if (mix->table) { |
353 | for (i = 0; i < mix->table_size; i++) { |
354 | item = &mix->table[i]; |
355 | if (item->valid == 0) |
356 | continue; |
357 | if (item->parent_index == index) |
358 | break; |
359 | } |
360 | if (i < mix->table_size) { |
361 | div_val = _get_div_val(mix, div: item->divisor); |
362 | mux_val = _get_mux_val(mix, mux: item->parent_index); |
363 | } else |
364 | return -EINVAL; |
365 | } else { |
366 | mux_val = _get_mux_val(mix, mux: index); |
367 | div_val = 0; |
368 | } |
369 | |
370 | return _set_rate(mix, mux_val, div_val, change_mux: 1, change_div: div_val ? 1 : 0); |
371 | } |
372 | |
373 | static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate, |
374 | unsigned long best_parent_rate) |
375 | { |
376 | struct mmp_clk_mix *mix = to_clk_mix(hw); |
377 | struct mmp_clk_mix_clk_table *item; |
378 | unsigned long parent_rate; |
379 | unsigned int best_divisor; |
380 | struct clk_hw *parent; |
381 | int i; |
382 | |
383 | best_divisor = best_parent_rate / rate; |
384 | |
385 | if (mix->table) { |
386 | for (i = 0; i < mix->table_size; i++) { |
387 | item = &mix->table[i]; |
388 | if (item->valid == 0) |
389 | continue; |
390 | parent = clk_hw_get_parent_by_index(hw, |
391 | index: item->parent_index); |
392 | parent_rate = clk_hw_get_rate(hw: parent); |
393 | if (parent_rate == best_parent_rate |
394 | && item->divisor == best_divisor) |
395 | break; |
396 | } |
397 | if (i < mix->table_size) |
398 | return _set_rate(mix, |
399 | mux_val: _get_mux_val(mix, mux: item->parent_index), |
400 | div_val: _get_div_val(mix, div: item->divisor), |
401 | change_mux: 1, change_div: 1); |
402 | else |
403 | return -EINVAL; |
404 | } else { |
405 | for (i = 0; i < clk_hw_get_num_parents(hw); i++) { |
406 | parent = clk_hw_get_parent_by_index(hw, index: i); |
407 | parent_rate = clk_hw_get_rate(hw: parent); |
408 | if (parent_rate == best_parent_rate) |
409 | break; |
410 | } |
411 | if (i < clk_hw_get_num_parents(hw)) |
412 | return _set_rate(mix, mux_val: _get_mux_val(mix, mux: i), |
413 | div_val: _get_div_val(mix, div: best_divisor), change_mux: 1, change_div: 1); |
414 | else |
415 | return -EINVAL; |
416 | } |
417 | } |
418 | |
419 | static int mmp_clk_mix_init(struct clk_hw *hw) |
420 | { |
421 | struct mmp_clk_mix *mix = to_clk_mix(hw); |
422 | |
423 | if (mix->table) |
424 | _filter_clk_table(mix, table: mix->table, table_size: mix->table_size); |
425 | |
426 | return 0; |
427 | } |
428 | |
429 | const struct clk_ops mmp_clk_mix_ops = { |
430 | .determine_rate = mmp_clk_mix_determine_rate, |
431 | .set_rate_and_parent = mmp_clk_mix_set_rate_and_parent, |
432 | .set_rate = mmp_clk_set_rate, |
433 | .set_parent = mmp_clk_set_parent, |
434 | .get_parent = mmp_clk_mix_get_parent, |
435 | .recalc_rate = mmp_clk_mix_recalc_rate, |
436 | .init = mmp_clk_mix_init, |
437 | }; |
438 | |
439 | struct clk *mmp_clk_register_mix(struct device *dev, |
440 | const char *name, |
441 | const char * const *parent_names, |
442 | u8 num_parents, |
443 | unsigned long flags, |
444 | struct mmp_clk_mix_config *config, |
445 | spinlock_t *lock) |
446 | { |
447 | struct mmp_clk_mix *mix; |
448 | struct clk *clk; |
449 | struct clk_init_data init; |
450 | size_t table_bytes; |
451 | |
452 | mix = kzalloc(size: sizeof(*mix), GFP_KERNEL); |
453 | if (!mix) |
454 | return ERR_PTR(error: -ENOMEM); |
455 | |
456 | init.name = name; |
457 | init.flags = flags | CLK_GET_RATE_NOCACHE; |
458 | init.parent_names = parent_names; |
459 | init.num_parents = num_parents; |
460 | init.ops = &mmp_clk_mix_ops; |
461 | |
462 | memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info)); |
463 | if (config->table) { |
464 | table_bytes = sizeof(*config->table) * config->table_size; |
465 | mix->table = kmemdup(p: config->table, size: table_bytes, GFP_KERNEL); |
466 | if (!mix->table) |
467 | goto free_mix; |
468 | |
469 | mix->table_size = config->table_size; |
470 | } |
471 | |
472 | if (config->mux_table) { |
473 | table_bytes = sizeof(u32) * num_parents; |
474 | mix->mux_table = kmemdup(p: config->mux_table, size: table_bytes, |
475 | GFP_KERNEL); |
476 | if (!mix->mux_table) { |
477 | kfree(objp: mix->table); |
478 | goto free_mix; |
479 | } |
480 | } |
481 | |
482 | mix->div_flags = config->div_flags; |
483 | mix->mux_flags = config->mux_flags; |
484 | mix->lock = lock; |
485 | mix->hw.init = &init; |
486 | |
487 | if (config->reg_info.bit_fc >= 32) |
488 | mix->type = MMP_CLK_MIX_TYPE_V1; |
489 | else if (config->reg_info.reg_clk_sel) |
490 | mix->type = MMP_CLK_MIX_TYPE_V3; |
491 | else |
492 | mix->type = MMP_CLK_MIX_TYPE_V2; |
493 | clk = clk_register(dev, hw: &mix->hw); |
494 | |
495 | if (IS_ERR(ptr: clk)) { |
496 | kfree(objp: mix->mux_table); |
497 | kfree(objp: mix->table); |
498 | kfree(objp: mix); |
499 | } |
500 | |
501 | return clk; |
502 | |
503 | free_mix: |
504 | kfree(objp: mix); |
505 | return ERR_PTR(error: -ENOMEM); |
506 | } |
507 | |