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
_get_maxdiv(struct mmp_clk_mix * mix)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
_get_div(struct mmp_clk_mix * mix,unsigned int val)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
_get_mux(struct mmp_clk_mix * mix,unsigned int val)61 static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val)
62 {
63 int num_parents = clk_hw_get_num_parents(&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 }
_get_div_val(struct mmp_clk_mix * mix,unsigned int div)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
_get_mux_val(struct mmp_clk_mix * mix,unsigned int mux)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
_filter_clk_table(struct mmp_clk_mix * mix,struct mmp_clk_mix_clk_table * table,unsigned int table_size)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, item->parent_index);
121 parent_rate = clk_hw_get_rate(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
_set_rate(struct mmp_clk_mix * mix,u32 mux_val,u32 div_val,unsigned int change_mux,unsigned int change_div)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(ri->reg_clk_ctrl);
149 else
150 mux_div = readl(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(mux_div, ri->reg_clk_ctrl);
168 } else if (mix->type == MMP_CLK_MIX_TYPE_V2) {
169 mux_div |= (1 << ri->bit_fc);
170 writel(mux_div, ri->reg_clk_ctrl);
171
172 do {
173 fc_req = readl(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(ri->reg_clk_ctrl);
187 fc_req |= 1 << ri->bit_fc;
188 writel(fc_req, ri->reg_clk_ctrl);
189 writel(mux_div, 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(mix->lock, flags);
197
198 return ret;
199 }
200
mmp_clk_mix_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)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 item->parent_index);
226 parent_rate = clk_hw_get_rate(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, i);
241 parent_rate = clk_hw_get_rate(parent);
242 div_val_max = _get_maxdiv(mix);
243 for (j = 0; j < div_val_max; j++) {
244 div = _get_div(mix, 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
mmp_clk_mix_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)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, index);
282
283 return _set_rate(mix, mux_val, div_val, 1, 1);
284 }
285
mmp_clk_mix_get_parent(struct clk_hw * hw)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(ri->reg_clk_ctrl);
301 else
302 mux_div = readl(ri->reg_clk_sel);
303
304 if (mix->lock)
305 spin_unlock_irqrestore(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, mux_val);
313 }
314
mmp_clk_mix_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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(ri->reg_clk_ctrl);
331 else
332 mux_div = readl(ri->reg_clk_sel);
333
334 if (mix->lock)
335 spin_unlock_irqrestore(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
mmp_clk_set_parent(struct clk_hw * hw,u8 index)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, item->divisor);
362 mux_val = _get_mux_val(mix, item->parent_index);
363 } else
364 return -EINVAL;
365 } else {
366 mux_val = _get_mux_val(mix, index);
367 div_val = 0;
368 }
369
370 return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0);
371 }
372
mmp_clk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long best_parent_rate)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 item->parent_index);
392 parent_rate = clk_hw_get_rate(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 _get_mux_val(mix, item->parent_index),
400 _get_div_val(mix, item->divisor),
401 1, 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, i);
407 parent_rate = clk_hw_get_rate(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, _get_mux_val(mix, i),
413 _get_div_val(mix, best_divisor), 1, 1);
414 else
415 return -EINVAL;
416 }
417 }
418
mmp_clk_mix_init(struct clk_hw * hw)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, mix->table, 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
mmp_clk_register_mix(struct device * dev,const char * name,const char * const * parent_names,u8 num_parents,unsigned long flags,struct mmp_clk_mix_config * config,spinlock_t * lock)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(sizeof(*mix), GFP_KERNEL);
453 if (!mix)
454 return ERR_PTR(-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(config->table, 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(config->mux_table, table_bytes,
475 GFP_KERNEL);
476 if (!mix->mux_table) {
477 kfree(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, &mix->hw);
494
495 if (IS_ERR(clk)) {
496 kfree(mix->mux_table);
497 kfree(mix->table);
498 kfree(mix);
499 }
500
501 return clk;
502
503 free_mix:
504 kfree(mix);
505 return ERR_PTR(-ENOMEM);
506 }
507