1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2013, 2018, The Linux Foundation. All rights reserved.
4 */
5
6 #include <linux/kernel.h>
7 #include <linux/bitops.h>
8 #include <linux/err.h>
9 #include <linux/bug.h>
10 #include <linux/export.h>
11 #include <linux/clk-provider.h>
12 #include <linux/delay.h>
13 #include <linux/rational.h>
14 #include <linux/regmap.h>
15 #include <linux/math64.h>
16 #include <linux/minmax.h>
17 #include <linux/slab.h>
18
19 #include <asm/div64.h>
20
21 #include "clk-rcg.h"
22 #include "common.h"
23
24 #define CMD_REG 0x0
25 #define CMD_UPDATE BIT(0)
26 #define CMD_ROOT_EN BIT(1)
27 #define CMD_DIRTY_CFG BIT(4)
28 #define CMD_DIRTY_N BIT(5)
29 #define CMD_DIRTY_M BIT(6)
30 #define CMD_DIRTY_D BIT(7)
31 #define CMD_ROOT_OFF BIT(31)
32
33 #define CFG_REG 0x4
34 #define CFG_SRC_DIV_SHIFT 0
35 #define CFG_SRC_SEL_SHIFT 8
36 #define CFG_SRC_SEL_MASK (0x7 << CFG_SRC_SEL_SHIFT)
37 #define CFG_MODE_SHIFT 12
38 #define CFG_MODE_MASK (0x3 << CFG_MODE_SHIFT)
39 #define CFG_MODE_DUAL_EDGE (0x2 << CFG_MODE_SHIFT)
40 #define CFG_HW_CLK_CTRL_MASK BIT(20)
41
42 #define M_REG 0x8
43 #define N_REG 0xc
44 #define D_REG 0x10
45
46 #define RCG_CFG_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + CFG_REG)
47 #define RCG_M_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + M_REG)
48 #define RCG_N_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + N_REG)
49 #define RCG_D_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + D_REG)
50
51 /* Dynamic Frequency Scaling */
52 #define MAX_PERF_LEVEL 8
53 #define SE_CMD_DFSR_OFFSET 0x14
54 #define SE_CMD_DFS_EN BIT(0)
55 #define SE_PERF_DFSR(level) (0x1c + 0x4 * (level))
56 #define SE_PERF_M_DFSR(level) (0x5c + 0x4 * (level))
57 #define SE_PERF_N_DFSR(level) (0x9c + 0x4 * (level))
58
59 enum freq_policy {
60 FLOOR,
61 CEIL,
62 };
63
clk_rcg2_is_enabled(struct clk_hw * hw)64 static int clk_rcg2_is_enabled(struct clk_hw *hw)
65 {
66 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
67 u32 cmd;
68 int ret;
69
70 ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd);
71 if (ret)
72 return ret;
73
74 return (cmd & CMD_ROOT_OFF) == 0;
75 }
76
__clk_rcg2_get_parent(struct clk_hw * hw,u32 cfg)77 static u8 __clk_rcg2_get_parent(struct clk_hw *hw, u32 cfg)
78 {
79 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
80 int num_parents = clk_hw_get_num_parents(hw);
81 int i;
82
83 cfg &= CFG_SRC_SEL_MASK;
84 cfg >>= CFG_SRC_SEL_SHIFT;
85
86 for (i = 0; i < num_parents; i++)
87 if (cfg == rcg->parent_map[i].cfg)
88 return i;
89
90 pr_debug("%s: Clock %s has invalid parent, using default.\n",
91 __func__, clk_hw_get_name(hw));
92 return 0;
93 }
94
clk_rcg2_get_parent(struct clk_hw * hw)95 static u8 clk_rcg2_get_parent(struct clk_hw *hw)
96 {
97 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
98 u32 cfg;
99 int ret;
100
101 ret = regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);
102 if (ret) {
103 pr_debug("%s: Unable to read CFG register for %s\n",
104 __func__, clk_hw_get_name(hw));
105 return 0;
106 }
107
108 return __clk_rcg2_get_parent(hw, cfg);
109 }
110
update_config(struct clk_rcg2 * rcg)111 static int update_config(struct clk_rcg2 *rcg)
112 {
113 int count, ret;
114 u32 cmd;
115 struct clk_hw *hw = &rcg->clkr.hw;
116 const char *name = clk_hw_get_name(hw);
117
118 ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
119 CMD_UPDATE, CMD_UPDATE);
120 if (ret)
121 return ret;
122
123 /* Wait for update to take effect */
124 for (count = 500; count > 0; count--) {
125 ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd);
126 if (ret)
127 return ret;
128 if (!(cmd & CMD_UPDATE))
129 return 0;
130 udelay(1);
131 }
132
133 WARN(1, "%s: rcg didn't update its configuration.", name);
134 return -EBUSY;
135 }
136
clk_rcg2_set_parent(struct clk_hw * hw,u8 index)137 static int clk_rcg2_set_parent(struct clk_hw *hw, u8 index)
138 {
139 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
140 int ret;
141 u32 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
142
143 ret = regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg),
144 CFG_SRC_SEL_MASK, cfg);
145 if (ret)
146 return ret;
147
148 return update_config(rcg);
149 }
150
151 /*
152 * Calculate m/n:d rate
153 *
154 * parent_rate m
155 * rate = ----------- x ---
156 * hid_div n
157 */
158 static unsigned long
calc_rate(unsigned long rate,u32 m,u32 n,u32 mode,u32 hid_div)159 calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 hid_div)
160 {
161 if (hid_div)
162 rate = mult_frac(rate, 2, hid_div + 1);
163
164 if (mode)
165 rate = mult_frac(rate, m, n);
166
167 return rate;
168 }
169
170 static unsigned long
__clk_rcg2_recalc_rate(struct clk_hw * hw,unsigned long parent_rate,u32 cfg)171 __clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate, u32 cfg)
172 {
173 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
174 u32 hid_div, m = 0, n = 0, mode = 0, mask;
175
176 if (rcg->mnd_width) {
177 mask = BIT(rcg->mnd_width) - 1;
178 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);
179 m &= mask;
180 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), &n);
181 n = ~n;
182 n &= mask;
183 n += m;
184 mode = cfg & CFG_MODE_MASK;
185 mode >>= CFG_MODE_SHIFT;
186 }
187
188 mask = BIT(rcg->hid_width) - 1;
189 hid_div = cfg >> CFG_SRC_DIV_SHIFT;
190 hid_div &= mask;
191
192 return calc_rate(parent_rate, m, n, mode, hid_div);
193 }
194
195 static unsigned long
clk_rcg2_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)196 clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
197 {
198 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
199 u32 cfg;
200
201 regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);
202
203 return __clk_rcg2_recalc_rate(hw, parent_rate, cfg);
204 }
205
_freq_tbl_determine_rate(struct clk_hw * hw,const struct freq_tbl * f,struct clk_rate_request * req,enum freq_policy policy)206 static int _freq_tbl_determine_rate(struct clk_hw *hw, const struct freq_tbl *f,
207 struct clk_rate_request *req,
208 enum freq_policy policy)
209 {
210 unsigned long clk_flags, rate = req->rate;
211 struct clk_hw *p;
212 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
213 int index;
214
215 switch (policy) {
216 case FLOOR:
217 f = qcom_find_freq_floor(f, rate);
218 break;
219 case CEIL:
220 f = qcom_find_freq(f, rate);
221 break;
222 default:
223 return -EINVAL;
224 }
225
226 if (!f)
227 return -EINVAL;
228
229 index = qcom_find_src_index(hw, rcg->parent_map, f->src);
230 if (index < 0)
231 return index;
232
233 clk_flags = clk_hw_get_flags(hw);
234 p = clk_hw_get_parent_by_index(hw, index);
235 if (!p)
236 return -EINVAL;
237
238 if (clk_flags & CLK_SET_RATE_PARENT) {
239 rate = f->freq;
240 if (f->pre_div) {
241 if (!rate)
242 rate = req->rate;
243 rate /= 2;
244 rate *= f->pre_div + 1;
245 }
246
247 if (f->n) {
248 u64 tmp = rate;
249 tmp = tmp * f->n;
250 do_div(tmp, f->m);
251 rate = tmp;
252 }
253 } else {
254 rate = clk_hw_get_rate(p);
255 }
256 req->best_parent_hw = p;
257 req->best_parent_rate = rate;
258 req->rate = f->freq;
259
260 return 0;
261 }
262
263 static const struct freq_conf *
__clk_rcg2_select_conf(struct clk_hw * hw,const struct freq_multi_tbl * f,unsigned long req_rate)264 __clk_rcg2_select_conf(struct clk_hw *hw, const struct freq_multi_tbl *f,
265 unsigned long req_rate)
266 {
267 unsigned long rate_diff, best_rate_diff = ULONG_MAX;
268 const struct freq_conf *conf, *best_conf = NULL;
269 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
270 const char *name = clk_hw_get_name(hw);
271 unsigned long parent_rate, rate;
272 struct clk_hw *p;
273 int index, i;
274
275 /* Exit early if only one config is defined */
276 if (f->num_confs == 1) {
277 best_conf = f->confs;
278 goto exit;
279 }
280
281 /* Search in each provided config the one that is near the wanted rate */
282 for (i = 0, conf = f->confs; i < f->num_confs; i++, conf++) {
283 index = qcom_find_src_index(hw, rcg->parent_map, conf->src);
284 if (index < 0)
285 continue;
286
287 p = clk_hw_get_parent_by_index(hw, index);
288 if (!p)
289 continue;
290
291 parent_rate = clk_hw_get_rate(p);
292 rate = calc_rate(parent_rate, conf->n, conf->m, conf->n, conf->pre_div);
293
294 if (rate == req_rate) {
295 best_conf = conf;
296 goto exit;
297 }
298
299 rate_diff = abs_diff(req_rate, rate);
300 if (rate_diff < best_rate_diff) {
301 best_rate_diff = rate_diff;
302 best_conf = conf;
303 }
304 }
305
306 /*
307 * Very unlikely. Warn if we couldn't find a correct config
308 * due to parent not found in every config.
309 */
310 if (unlikely(!best_conf)) {
311 WARN(1, "%s: can't find a configuration for rate %lu\n",
312 name, req_rate);
313 return ERR_PTR(-EINVAL);
314 }
315
316 exit:
317 return best_conf;
318 }
319
_freq_tbl_fm_determine_rate(struct clk_hw * hw,const struct freq_multi_tbl * f,struct clk_rate_request * req)320 static int _freq_tbl_fm_determine_rate(struct clk_hw *hw, const struct freq_multi_tbl *f,
321 struct clk_rate_request *req)
322 {
323 unsigned long clk_flags, rate = req->rate;
324 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
325 const struct freq_conf *conf;
326 struct clk_hw *p;
327 int index;
328
329 f = qcom_find_freq_multi(f, rate);
330 if (!f || !f->confs)
331 return -EINVAL;
332
333 conf = __clk_rcg2_select_conf(hw, f, rate);
334 if (IS_ERR(conf))
335 return PTR_ERR(conf);
336 index = qcom_find_src_index(hw, rcg->parent_map, conf->src);
337 if (index < 0)
338 return index;
339
340 clk_flags = clk_hw_get_flags(hw);
341 p = clk_hw_get_parent_by_index(hw, index);
342 if (!p)
343 return -EINVAL;
344
345 if (clk_flags & CLK_SET_RATE_PARENT) {
346 rate = f->freq;
347 if (conf->pre_div) {
348 if (!rate)
349 rate = req->rate;
350 rate /= 2;
351 rate *= conf->pre_div + 1;
352 }
353
354 if (conf->n) {
355 u64 tmp = rate;
356
357 tmp = tmp * conf->n;
358 do_div(tmp, conf->m);
359 rate = tmp;
360 }
361 } else {
362 rate = clk_hw_get_rate(p);
363 }
364
365 req->best_parent_hw = p;
366 req->best_parent_rate = rate;
367 req->rate = f->freq;
368
369 return 0;
370 }
371
clk_rcg2_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)372 static int clk_rcg2_determine_rate(struct clk_hw *hw,
373 struct clk_rate_request *req)
374 {
375 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
376
377 return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, CEIL);
378 }
379
clk_rcg2_determine_floor_rate(struct clk_hw * hw,struct clk_rate_request * req)380 static int clk_rcg2_determine_floor_rate(struct clk_hw *hw,
381 struct clk_rate_request *req)
382 {
383 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
384
385 return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, FLOOR);
386 }
387
clk_rcg2_fm_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)388 static int clk_rcg2_fm_determine_rate(struct clk_hw *hw,
389 struct clk_rate_request *req)
390 {
391 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
392
393 return _freq_tbl_fm_determine_rate(hw, rcg->freq_multi_tbl, req);
394 }
395
__clk_rcg2_configure(struct clk_rcg2 * rcg,const struct freq_tbl * f,u32 * _cfg)396 static int __clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f,
397 u32 *_cfg)
398 {
399 u32 cfg, mask, d_val, not2d_val, n_minus_m;
400 struct clk_hw *hw = &rcg->clkr.hw;
401 int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src);
402
403 if (index < 0)
404 return index;
405
406 if (rcg->mnd_width && f->n) {
407 mask = BIT(rcg->mnd_width) - 1;
408 ret = regmap_update_bits(rcg->clkr.regmap,
409 RCG_M_OFFSET(rcg), mask, f->m);
410 if (ret)
411 return ret;
412
413 ret = regmap_update_bits(rcg->clkr.regmap,
414 RCG_N_OFFSET(rcg), mask, ~(f->n - f->m));
415 if (ret)
416 return ret;
417
418 /* Calculate 2d value */
419 d_val = f->n;
420
421 n_minus_m = f->n - f->m;
422 n_minus_m *= 2;
423
424 d_val = clamp_t(u32, d_val, f->m, n_minus_m);
425 not2d_val = ~d_val & mask;
426
427 ret = regmap_update_bits(rcg->clkr.regmap,
428 RCG_D_OFFSET(rcg), mask, not2d_val);
429 if (ret)
430 return ret;
431 }
432
433 mask = BIT(rcg->hid_width) - 1;
434 mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK | CFG_HW_CLK_CTRL_MASK;
435 cfg = f->pre_div << CFG_SRC_DIV_SHIFT;
436 cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
437 if (rcg->mnd_width && f->n && (f->m != f->n))
438 cfg |= CFG_MODE_DUAL_EDGE;
439 if (rcg->hw_clk_ctrl)
440 cfg |= CFG_HW_CLK_CTRL_MASK;
441
442 *_cfg &= ~mask;
443 *_cfg |= cfg;
444
445 return 0;
446 }
447
clk_rcg2_configure(struct clk_rcg2 * rcg,const struct freq_tbl * f)448 static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f)
449 {
450 u32 cfg;
451 int ret;
452
453 ret = regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);
454 if (ret)
455 return ret;
456
457 ret = __clk_rcg2_configure(rcg, f, &cfg);
458 if (ret)
459 return ret;
460
461 ret = regmap_write(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), cfg);
462 if (ret)
463 return ret;
464
465 return update_config(rcg);
466 }
467
__clk_rcg2_set_rate(struct clk_hw * hw,unsigned long rate,enum freq_policy policy)468 static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
469 enum freq_policy policy)
470 {
471 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
472 const struct freq_tbl *f;
473
474 switch (policy) {
475 case FLOOR:
476 f = qcom_find_freq_floor(rcg->freq_tbl, rate);
477 break;
478 case CEIL:
479 f = qcom_find_freq(rcg->freq_tbl, rate);
480 break;
481 default:
482 return -EINVAL;
483 }
484
485 if (!f)
486 return -EINVAL;
487
488 return clk_rcg2_configure(rcg, f);
489 }
490
__clk_rcg2_fm_set_rate(struct clk_hw * hw,unsigned long rate)491 static int __clk_rcg2_fm_set_rate(struct clk_hw *hw, unsigned long rate)
492 {
493 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
494 const struct freq_multi_tbl *f;
495 const struct freq_conf *conf;
496 struct freq_tbl f_tbl = {};
497
498 f = qcom_find_freq_multi(rcg->freq_multi_tbl, rate);
499 if (!f || !f->confs)
500 return -EINVAL;
501
502 conf = __clk_rcg2_select_conf(hw, f, rate);
503 if (IS_ERR(conf))
504 return PTR_ERR(conf);
505
506 f_tbl.freq = f->freq;
507 f_tbl.src = conf->src;
508 f_tbl.pre_div = conf->pre_div;
509 f_tbl.m = conf->m;
510 f_tbl.n = conf->n;
511
512 return clk_rcg2_configure(rcg, &f_tbl);
513 }
514
clk_rcg2_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)515 static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
516 unsigned long parent_rate)
517 {
518 return __clk_rcg2_set_rate(hw, rate, CEIL);
519 }
520
clk_rcg2_set_floor_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)521 static int clk_rcg2_set_floor_rate(struct clk_hw *hw, unsigned long rate,
522 unsigned long parent_rate)
523 {
524 return __clk_rcg2_set_rate(hw, rate, FLOOR);
525 }
526
clk_rcg2_fm_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)527 static int clk_rcg2_fm_set_rate(struct clk_hw *hw, unsigned long rate,
528 unsigned long parent_rate)
529 {
530 return __clk_rcg2_fm_set_rate(hw, rate);
531 }
532
clk_rcg2_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)533 static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw,
534 unsigned long rate, unsigned long parent_rate, u8 index)
535 {
536 return __clk_rcg2_set_rate(hw, rate, CEIL);
537 }
538
clk_rcg2_set_floor_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)539 static int clk_rcg2_set_floor_rate_and_parent(struct clk_hw *hw,
540 unsigned long rate, unsigned long parent_rate, u8 index)
541 {
542 return __clk_rcg2_set_rate(hw, rate, FLOOR);
543 }
544
clk_rcg2_fm_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)545 static int clk_rcg2_fm_set_rate_and_parent(struct clk_hw *hw,
546 unsigned long rate, unsigned long parent_rate, u8 index)
547 {
548 return __clk_rcg2_fm_set_rate(hw, rate);
549 }
550
clk_rcg2_get_duty_cycle(struct clk_hw * hw,struct clk_duty * duty)551 static int clk_rcg2_get_duty_cycle(struct clk_hw *hw, struct clk_duty *duty)
552 {
553 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
554 u32 notn_m, n, m, d, not2d, mask;
555
556 if (!rcg->mnd_width) {
557 /* 50 % duty-cycle for Non-MND RCGs */
558 duty->num = 1;
559 duty->den = 2;
560 return 0;
561 }
562
563 regmap_read(rcg->clkr.regmap, RCG_D_OFFSET(rcg), ¬2d);
564 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);
565 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), ¬n_m);
566
567 if (!not2d && !m && !notn_m) {
568 /* 50 % duty-cycle always */
569 duty->num = 1;
570 duty->den = 2;
571 return 0;
572 }
573
574 mask = BIT(rcg->mnd_width) - 1;
575
576 d = ~(not2d) & mask;
577 d = DIV_ROUND_CLOSEST(d, 2);
578
579 n = (~(notn_m) + m) & mask;
580
581 duty->num = d;
582 duty->den = n;
583
584 return 0;
585 }
586
clk_rcg2_set_duty_cycle(struct clk_hw * hw,struct clk_duty * duty)587 static int clk_rcg2_set_duty_cycle(struct clk_hw *hw, struct clk_duty *duty)
588 {
589 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
590 u32 notn_m, n, m, d, not2d, mask, duty_per, cfg;
591 int ret;
592
593 /* Duty-cycle cannot be modified for non-MND RCGs */
594 if (!rcg->mnd_width)
595 return -EINVAL;
596
597 mask = BIT(rcg->mnd_width) - 1;
598
599 regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), ¬n_m);
600 regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);
601 regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);
602
603 /* Duty-cycle cannot be modified if MND divider is in bypass mode. */
604 if (!(cfg & CFG_MODE_MASK))
605 return -EINVAL;
606
607 n = (~(notn_m) + m) & mask;
608
609 duty_per = (duty->num * 100) / duty->den;
610
611 /* Calculate 2d value */
612 d = DIV_ROUND_CLOSEST(n * duty_per * 2, 100);
613
614 /*
615 * Check bit widths of 2d. If D is too big reduce duty cycle.
616 * Also make sure it is never zero.
617 */
618 d = clamp_val(d, 1, mask);
619
620 if ((d / 2) > (n - m))
621 d = (n - m) * 2;
622 else if ((d / 2) < (m / 2))
623 d = m;
624
625 not2d = ~d & mask;
626
627 ret = regmap_update_bits(rcg->clkr.regmap, RCG_D_OFFSET(rcg), mask,
628 not2d);
629 if (ret)
630 return ret;
631
632 return update_config(rcg);
633 }
634
635 const struct clk_ops clk_rcg2_ops = {
636 .is_enabled = clk_rcg2_is_enabled,
637 .get_parent = clk_rcg2_get_parent,
638 .set_parent = clk_rcg2_set_parent,
639 .recalc_rate = clk_rcg2_recalc_rate,
640 .determine_rate = clk_rcg2_determine_rate,
641 .set_rate = clk_rcg2_set_rate,
642 .set_rate_and_parent = clk_rcg2_set_rate_and_parent,
643 .get_duty_cycle = clk_rcg2_get_duty_cycle,
644 .set_duty_cycle = clk_rcg2_set_duty_cycle,
645 };
646 EXPORT_SYMBOL_GPL(clk_rcg2_ops);
647
648 const struct clk_ops clk_rcg2_floor_ops = {
649 .is_enabled = clk_rcg2_is_enabled,
650 .get_parent = clk_rcg2_get_parent,
651 .set_parent = clk_rcg2_set_parent,
652 .recalc_rate = clk_rcg2_recalc_rate,
653 .determine_rate = clk_rcg2_determine_floor_rate,
654 .set_rate = clk_rcg2_set_floor_rate,
655 .set_rate_and_parent = clk_rcg2_set_floor_rate_and_parent,
656 .get_duty_cycle = clk_rcg2_get_duty_cycle,
657 .set_duty_cycle = clk_rcg2_set_duty_cycle,
658 };
659 EXPORT_SYMBOL_GPL(clk_rcg2_floor_ops);
660
661 const struct clk_ops clk_rcg2_fm_ops = {
662 .is_enabled = clk_rcg2_is_enabled,
663 .get_parent = clk_rcg2_get_parent,
664 .set_parent = clk_rcg2_set_parent,
665 .recalc_rate = clk_rcg2_recalc_rate,
666 .determine_rate = clk_rcg2_fm_determine_rate,
667 .set_rate = clk_rcg2_fm_set_rate,
668 .set_rate_and_parent = clk_rcg2_fm_set_rate_and_parent,
669 .get_duty_cycle = clk_rcg2_get_duty_cycle,
670 .set_duty_cycle = clk_rcg2_set_duty_cycle,
671 };
672 EXPORT_SYMBOL_GPL(clk_rcg2_fm_ops);
673
674 const struct clk_ops clk_rcg2_mux_closest_ops = {
675 .determine_rate = __clk_mux_determine_rate_closest,
676 .get_parent = clk_rcg2_get_parent,
677 .set_parent = clk_rcg2_set_parent,
678 };
679 EXPORT_SYMBOL_GPL(clk_rcg2_mux_closest_ops);
680
681 struct frac_entry {
682 int num;
683 int den;
684 };
685
686 static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */
687 { 52, 295 }, /* 119 M */
688 { 11, 57 }, /* 130.25 M */
689 { 63, 307 }, /* 138.50 M */
690 { 11, 50 }, /* 148.50 M */
691 { 47, 206 }, /* 154 M */
692 { 31, 100 }, /* 205.25 M */
693 { 107, 269 }, /* 268.50 M */
694 { },
695 };
696
697 static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */
698 { 31, 211 }, /* 119 M */
699 { 32, 199 }, /* 130.25 M */
700 { 63, 307 }, /* 138.50 M */
701 { 11, 60 }, /* 148.50 M */
702 { 50, 263 }, /* 154 M */
703 { 31, 120 }, /* 205.25 M */
704 { 119, 359 }, /* 268.50 M */
705 { },
706 };
707
clk_edp_pixel_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)708 static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
709 unsigned long parent_rate)
710 {
711 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
712 struct freq_tbl f = *rcg->freq_tbl;
713 const struct frac_entry *frac;
714 int delta = 100000;
715 s64 src_rate = parent_rate;
716 s64 request;
717 u32 mask = BIT(rcg->hid_width) - 1;
718 u32 hid_div;
719
720 if (src_rate == 810000000)
721 frac = frac_table_810m;
722 else
723 frac = frac_table_675m;
724
725 for (; frac->num; frac++) {
726 request = rate;
727 request *= frac->den;
728 request = div_s64(request, frac->num);
729 if ((src_rate < (request - delta)) ||
730 (src_rate > (request + delta)))
731 continue;
732
733 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
734 &hid_div);
735 f.pre_div = hid_div;
736 f.pre_div >>= CFG_SRC_DIV_SHIFT;
737 f.pre_div &= mask;
738 f.m = frac->num;
739 f.n = frac->den;
740
741 return clk_rcg2_configure(rcg, &f);
742 }
743
744 return -EINVAL;
745 }
746
clk_edp_pixel_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)747 static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,
748 unsigned long rate, unsigned long parent_rate, u8 index)
749 {
750 /* Parent index is set statically in frequency table */
751 return clk_edp_pixel_set_rate(hw, rate, parent_rate);
752 }
753
clk_edp_pixel_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)754 static int clk_edp_pixel_determine_rate(struct clk_hw *hw,
755 struct clk_rate_request *req)
756 {
757 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
758 const struct freq_tbl *f = rcg->freq_tbl;
759 const struct frac_entry *frac;
760 int delta = 100000;
761 s64 request;
762 u32 mask = BIT(rcg->hid_width) - 1;
763 u32 hid_div;
764 int index = qcom_find_src_index(hw, rcg->parent_map, f->src);
765
766 /* Force the correct parent */
767 req->best_parent_hw = clk_hw_get_parent_by_index(hw, index);
768 req->best_parent_rate = clk_hw_get_rate(req->best_parent_hw);
769
770 if (req->best_parent_rate == 810000000)
771 frac = frac_table_810m;
772 else
773 frac = frac_table_675m;
774
775 for (; frac->num; frac++) {
776 request = req->rate;
777 request *= frac->den;
778 request = div_s64(request, frac->num);
779 if ((req->best_parent_rate < (request - delta)) ||
780 (req->best_parent_rate > (request + delta)))
781 continue;
782
783 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
784 &hid_div);
785 hid_div >>= CFG_SRC_DIV_SHIFT;
786 hid_div &= mask;
787
788 req->rate = calc_rate(req->best_parent_rate,
789 frac->num, frac->den,
790 !!frac->den, hid_div);
791 return 0;
792 }
793
794 return -EINVAL;
795 }
796
797 const struct clk_ops clk_edp_pixel_ops = {
798 .is_enabled = clk_rcg2_is_enabled,
799 .get_parent = clk_rcg2_get_parent,
800 .set_parent = clk_rcg2_set_parent,
801 .recalc_rate = clk_rcg2_recalc_rate,
802 .set_rate = clk_edp_pixel_set_rate,
803 .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent,
804 .determine_rate = clk_edp_pixel_determine_rate,
805 };
806 EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);
807
clk_byte_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)808 static int clk_byte_determine_rate(struct clk_hw *hw,
809 struct clk_rate_request *req)
810 {
811 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
812 const struct freq_tbl *f = rcg->freq_tbl;
813 int index = qcom_find_src_index(hw, rcg->parent_map, f->src);
814 unsigned long parent_rate, div;
815 u32 mask = BIT(rcg->hid_width) - 1;
816 struct clk_hw *p;
817
818 if (req->rate == 0)
819 return -EINVAL;
820
821 req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index);
822 req->best_parent_rate = parent_rate = clk_hw_round_rate(p, req->rate);
823
824 div = DIV_ROUND_UP((2 * parent_rate), req->rate) - 1;
825 div = min_t(u32, div, mask);
826
827 req->rate = calc_rate(parent_rate, 0, 0, 0, div);
828
829 return 0;
830 }
831
clk_byte_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)832 static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate,
833 unsigned long parent_rate)
834 {
835 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
836 struct freq_tbl f = *rcg->freq_tbl;
837 unsigned long div;
838 u32 mask = BIT(rcg->hid_width) - 1;
839
840 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
841 div = min_t(u32, div, mask);
842
843 f.pre_div = div;
844
845 return clk_rcg2_configure(rcg, &f);
846 }
847
clk_byte_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)848 static int clk_byte_set_rate_and_parent(struct clk_hw *hw,
849 unsigned long rate, unsigned long parent_rate, u8 index)
850 {
851 /* Parent index is set statically in frequency table */
852 return clk_byte_set_rate(hw, rate, parent_rate);
853 }
854
855 const struct clk_ops clk_byte_ops = {
856 .is_enabled = clk_rcg2_is_enabled,
857 .get_parent = clk_rcg2_get_parent,
858 .set_parent = clk_rcg2_set_parent,
859 .recalc_rate = clk_rcg2_recalc_rate,
860 .set_rate = clk_byte_set_rate,
861 .set_rate_and_parent = clk_byte_set_rate_and_parent,
862 .determine_rate = clk_byte_determine_rate,
863 };
864 EXPORT_SYMBOL_GPL(clk_byte_ops);
865
clk_byte2_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)866 static int clk_byte2_determine_rate(struct clk_hw *hw,
867 struct clk_rate_request *req)
868 {
869 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
870 unsigned long parent_rate, div;
871 u32 mask = BIT(rcg->hid_width) - 1;
872 struct clk_hw *p;
873 unsigned long rate = req->rate;
874
875 if (rate == 0)
876 return -EINVAL;
877
878 p = req->best_parent_hw;
879 req->best_parent_rate = parent_rate = clk_hw_round_rate(p, rate);
880
881 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
882 div = min_t(u32, div, mask);
883
884 req->rate = calc_rate(parent_rate, 0, 0, 0, div);
885
886 return 0;
887 }
888
clk_byte2_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)889 static int clk_byte2_set_rate(struct clk_hw *hw, unsigned long rate,
890 unsigned long parent_rate)
891 {
892 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
893 struct freq_tbl f = { 0 };
894 unsigned long div;
895 int i, num_parents = clk_hw_get_num_parents(hw);
896 u32 mask = BIT(rcg->hid_width) - 1;
897 u32 cfg;
898
899 div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
900 div = min_t(u32, div, mask);
901
902 f.pre_div = div;
903
904 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
905 cfg &= CFG_SRC_SEL_MASK;
906 cfg >>= CFG_SRC_SEL_SHIFT;
907
908 for (i = 0; i < num_parents; i++) {
909 if (cfg == rcg->parent_map[i].cfg) {
910 f.src = rcg->parent_map[i].src;
911 return clk_rcg2_configure(rcg, &f);
912 }
913 }
914
915 return -EINVAL;
916 }
917
clk_byte2_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)918 static int clk_byte2_set_rate_and_parent(struct clk_hw *hw,
919 unsigned long rate, unsigned long parent_rate, u8 index)
920 {
921 /* Read the hardware to determine parent during set_rate */
922 return clk_byte2_set_rate(hw, rate, parent_rate);
923 }
924
925 const struct clk_ops clk_byte2_ops = {
926 .is_enabled = clk_rcg2_is_enabled,
927 .get_parent = clk_rcg2_get_parent,
928 .set_parent = clk_rcg2_set_parent,
929 .recalc_rate = clk_rcg2_recalc_rate,
930 .set_rate = clk_byte2_set_rate,
931 .set_rate_and_parent = clk_byte2_set_rate_and_parent,
932 .determine_rate = clk_byte2_determine_rate,
933 };
934 EXPORT_SYMBOL_GPL(clk_byte2_ops);
935
936 static const struct frac_entry frac_table_pixel[] = {
937 { 3, 8 },
938 { 2, 9 },
939 { 4, 9 },
940 { 1, 1 },
941 { 2, 3 },
942 { }
943 };
944
clk_pixel_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)945 static int clk_pixel_determine_rate(struct clk_hw *hw,
946 struct clk_rate_request *req)
947 {
948 unsigned long request, src_rate;
949 int delta = 100000;
950 const struct frac_entry *frac = frac_table_pixel;
951
952 for (; frac->num; frac++) {
953 request = (req->rate * frac->den) / frac->num;
954
955 src_rate = clk_hw_round_rate(req->best_parent_hw, request);
956 if ((src_rate < (request - delta)) ||
957 (src_rate > (request + delta)))
958 continue;
959
960 req->best_parent_rate = src_rate;
961 req->rate = (src_rate * frac->num) / frac->den;
962 return 0;
963 }
964
965 return -EINVAL;
966 }
967
clk_pixel_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)968 static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
969 unsigned long parent_rate)
970 {
971 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
972 struct freq_tbl f = { 0 };
973 const struct frac_entry *frac = frac_table_pixel;
974 unsigned long request;
975 int delta = 100000;
976 u32 mask = BIT(rcg->hid_width) - 1;
977 u32 hid_div, cfg;
978 int i, num_parents = clk_hw_get_num_parents(hw);
979
980 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
981 cfg &= CFG_SRC_SEL_MASK;
982 cfg >>= CFG_SRC_SEL_SHIFT;
983
984 for (i = 0; i < num_parents; i++)
985 if (cfg == rcg->parent_map[i].cfg) {
986 f.src = rcg->parent_map[i].src;
987 break;
988 }
989
990 for (; frac->num; frac++) {
991 request = (rate * frac->den) / frac->num;
992
993 if ((parent_rate < (request - delta)) ||
994 (parent_rate > (request + delta)))
995 continue;
996
997 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
998 &hid_div);
999 f.pre_div = hid_div;
1000 f.pre_div >>= CFG_SRC_DIV_SHIFT;
1001 f.pre_div &= mask;
1002 f.m = frac->num;
1003 f.n = frac->den;
1004
1005 return clk_rcg2_configure(rcg, &f);
1006 }
1007 return -EINVAL;
1008 }
1009
clk_pixel_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)1010 static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
1011 unsigned long parent_rate, u8 index)
1012 {
1013 return clk_pixel_set_rate(hw, rate, parent_rate);
1014 }
1015
1016 const struct clk_ops clk_pixel_ops = {
1017 .is_enabled = clk_rcg2_is_enabled,
1018 .get_parent = clk_rcg2_get_parent,
1019 .set_parent = clk_rcg2_set_parent,
1020 .recalc_rate = clk_rcg2_recalc_rate,
1021 .set_rate = clk_pixel_set_rate,
1022 .set_rate_and_parent = clk_pixel_set_rate_and_parent,
1023 .determine_rate = clk_pixel_determine_rate,
1024 };
1025 EXPORT_SYMBOL_GPL(clk_pixel_ops);
1026
clk_gfx3d_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)1027 static int clk_gfx3d_determine_rate(struct clk_hw *hw,
1028 struct clk_rate_request *req)
1029 {
1030 struct clk_rate_request parent_req = { .min_rate = 0, .max_rate = ULONG_MAX };
1031 struct clk_rcg2_gfx3d *cgfx = to_clk_rcg2_gfx3d(hw);
1032 struct clk_hw *xo, *p0, *p1, *p2;
1033 unsigned long p0_rate;
1034 u8 mux_div = cgfx->div;
1035 int ret;
1036
1037 p0 = cgfx->hws[0];
1038 p1 = cgfx->hws[1];
1039 p2 = cgfx->hws[2];
1040 /*
1041 * This function does ping-pong the RCG between PLLs: if we don't
1042 * have at least one fixed PLL and two variable ones,
1043 * then it's not going to work correctly.
1044 */
1045 if (WARN_ON(!p0 || !p1 || !p2))
1046 return -EINVAL;
1047
1048 xo = clk_hw_get_parent_by_index(hw, 0);
1049 if (req->rate == clk_hw_get_rate(xo)) {
1050 req->best_parent_hw = xo;
1051 return 0;
1052 }
1053
1054 if (mux_div == 0)
1055 mux_div = 1;
1056
1057 parent_req.rate = req->rate * mux_div;
1058
1059 /* This has to be a fixed rate PLL */
1060 p0_rate = clk_hw_get_rate(p0);
1061
1062 if (parent_req.rate == p0_rate) {
1063 req->rate = req->best_parent_rate = p0_rate;
1064 req->best_parent_hw = p0;
1065 return 0;
1066 }
1067
1068 if (req->best_parent_hw == p0) {
1069 /* Are we going back to a previously used rate? */
1070 if (clk_hw_get_rate(p2) == parent_req.rate)
1071 req->best_parent_hw = p2;
1072 else
1073 req->best_parent_hw = p1;
1074 } else if (req->best_parent_hw == p2) {
1075 req->best_parent_hw = p1;
1076 } else {
1077 req->best_parent_hw = p2;
1078 }
1079
1080 clk_hw_get_rate_range(req->best_parent_hw,
1081 &parent_req.min_rate, &parent_req.max_rate);
1082
1083 if (req->min_rate > parent_req.min_rate)
1084 parent_req.min_rate = req->min_rate;
1085
1086 if (req->max_rate < parent_req.max_rate)
1087 parent_req.max_rate = req->max_rate;
1088
1089 ret = __clk_determine_rate(req->best_parent_hw, &parent_req);
1090 if (ret)
1091 return ret;
1092
1093 req->rate = req->best_parent_rate = parent_req.rate;
1094 req->rate /= mux_div;
1095
1096 return 0;
1097 }
1098
clk_gfx3d_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)1099 static int clk_gfx3d_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
1100 unsigned long parent_rate, u8 index)
1101 {
1102 struct clk_rcg2_gfx3d *cgfx = to_clk_rcg2_gfx3d(hw);
1103 struct clk_rcg2 *rcg = &cgfx->rcg;
1104 u32 cfg;
1105 int ret;
1106
1107 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
1108 /* On some targets, the GFX3D RCG may need to divide PLL frequency */
1109 if (cgfx->div > 1)
1110 cfg |= ((2 * cgfx->div) - 1) << CFG_SRC_DIV_SHIFT;
1111
1112 ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg);
1113 if (ret)
1114 return ret;
1115
1116 return update_config(rcg);
1117 }
1118
clk_gfx3d_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)1119 static int clk_gfx3d_set_rate(struct clk_hw *hw, unsigned long rate,
1120 unsigned long parent_rate)
1121 {
1122 /*
1123 * We should never get here; clk_gfx3d_determine_rate() should always
1124 * make us use a different parent than what we're currently using, so
1125 * clk_gfx3d_set_rate_and_parent() should always be called.
1126 */
1127 return 0;
1128 }
1129
1130 const struct clk_ops clk_gfx3d_ops = {
1131 .is_enabled = clk_rcg2_is_enabled,
1132 .get_parent = clk_rcg2_get_parent,
1133 .set_parent = clk_rcg2_set_parent,
1134 .recalc_rate = clk_rcg2_recalc_rate,
1135 .set_rate = clk_gfx3d_set_rate,
1136 .set_rate_and_parent = clk_gfx3d_set_rate_and_parent,
1137 .determine_rate = clk_gfx3d_determine_rate,
1138 };
1139 EXPORT_SYMBOL_GPL(clk_gfx3d_ops);
1140
clk_rcg2_set_force_enable(struct clk_hw * hw)1141 static int clk_rcg2_set_force_enable(struct clk_hw *hw)
1142 {
1143 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1144 const char *name = clk_hw_get_name(hw);
1145 int ret, count;
1146
1147 ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
1148 CMD_ROOT_EN, CMD_ROOT_EN);
1149 if (ret)
1150 return ret;
1151
1152 /* wait for RCG to turn ON */
1153 for (count = 500; count > 0; count--) {
1154 if (clk_rcg2_is_enabled(hw))
1155 return 0;
1156
1157 udelay(1);
1158 }
1159
1160 pr_err("%s: RCG did not turn on\n", name);
1161 return -ETIMEDOUT;
1162 }
1163
clk_rcg2_clear_force_enable(struct clk_hw * hw)1164 static int clk_rcg2_clear_force_enable(struct clk_hw *hw)
1165 {
1166 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1167
1168 return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
1169 CMD_ROOT_EN, 0);
1170 }
1171
1172 static int
clk_rcg2_shared_force_enable_clear(struct clk_hw * hw,const struct freq_tbl * f)1173 clk_rcg2_shared_force_enable_clear(struct clk_hw *hw, const struct freq_tbl *f)
1174 {
1175 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1176 int ret;
1177
1178 ret = clk_rcg2_set_force_enable(hw);
1179 if (ret)
1180 return ret;
1181
1182 ret = clk_rcg2_configure(rcg, f);
1183 if (ret)
1184 return ret;
1185
1186 return clk_rcg2_clear_force_enable(hw);
1187 }
1188
clk_rcg2_shared_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)1189 static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate,
1190 unsigned long parent_rate)
1191 {
1192 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1193 const struct freq_tbl *f;
1194
1195 f = qcom_find_freq(rcg->freq_tbl, rate);
1196 if (!f)
1197 return -EINVAL;
1198
1199 /*
1200 * In case clock is disabled, update the M, N and D registers, cache
1201 * the CFG value in parked_cfg and don't hit the update bit of CMD
1202 * register.
1203 */
1204 if (!clk_hw_is_enabled(hw))
1205 return __clk_rcg2_configure(rcg, f, &rcg->parked_cfg);
1206
1207 return clk_rcg2_shared_force_enable_clear(hw, f);
1208 }
1209
clk_rcg2_shared_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)1210 static int clk_rcg2_shared_set_rate_and_parent(struct clk_hw *hw,
1211 unsigned long rate, unsigned long parent_rate, u8 index)
1212 {
1213 return clk_rcg2_shared_set_rate(hw, rate, parent_rate);
1214 }
1215
clk_rcg2_shared_enable(struct clk_hw * hw)1216 static int clk_rcg2_shared_enable(struct clk_hw *hw)
1217 {
1218 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1219 int ret;
1220
1221 /*
1222 * Set the update bit because required configuration has already
1223 * been written in clk_rcg2_shared_set_rate()
1224 */
1225 ret = clk_rcg2_set_force_enable(hw);
1226 if (ret)
1227 return ret;
1228
1229 /* Write back the stored configuration corresponding to current rate */
1230 ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, rcg->parked_cfg);
1231 if (ret)
1232 return ret;
1233
1234 ret = update_config(rcg);
1235 if (ret)
1236 return ret;
1237
1238 return clk_rcg2_clear_force_enable(hw);
1239 }
1240
clk_rcg2_shared_disable(struct clk_hw * hw)1241 static void clk_rcg2_shared_disable(struct clk_hw *hw)
1242 {
1243 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1244
1245 /*
1246 * Store current configuration as switching to safe source would clear
1247 * the SRC and DIV of CFG register
1248 */
1249 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &rcg->parked_cfg);
1250
1251 /*
1252 * Park the RCG at a safe configuration - sourced off of safe source.
1253 * Force enable and disable the RCG while configuring it to safeguard
1254 * against any update signal coming from the downstream clock.
1255 * The current parent is still prepared and enabled at this point, and
1256 * the safe source is always on while application processor subsystem
1257 * is online. Therefore, the RCG can safely switch its parent.
1258 */
1259 clk_rcg2_set_force_enable(hw);
1260
1261 regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
1262 rcg->safe_src_index << CFG_SRC_SEL_SHIFT);
1263
1264 update_config(rcg);
1265
1266 clk_rcg2_clear_force_enable(hw);
1267 }
1268
clk_rcg2_shared_get_parent(struct clk_hw * hw)1269 static u8 clk_rcg2_shared_get_parent(struct clk_hw *hw)
1270 {
1271 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1272
1273 /* If the shared rcg is parked use the cached cfg instead */
1274 if (!clk_hw_is_enabled(hw))
1275 return __clk_rcg2_get_parent(hw, rcg->parked_cfg);
1276
1277 return clk_rcg2_get_parent(hw);
1278 }
1279
clk_rcg2_shared_set_parent(struct clk_hw * hw,u8 index)1280 static int clk_rcg2_shared_set_parent(struct clk_hw *hw, u8 index)
1281 {
1282 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1283
1284 /* If the shared rcg is parked only update the cached cfg */
1285 if (!clk_hw_is_enabled(hw)) {
1286 rcg->parked_cfg &= ~CFG_SRC_SEL_MASK;
1287 rcg->parked_cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
1288
1289 return 0;
1290 }
1291
1292 return clk_rcg2_set_parent(hw, index);
1293 }
1294
1295 static unsigned long
clk_rcg2_shared_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)1296 clk_rcg2_shared_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1297 {
1298 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1299
1300 /* If the shared rcg is parked use the cached cfg instead */
1301 if (!clk_hw_is_enabled(hw))
1302 return __clk_rcg2_recalc_rate(hw, parent_rate, rcg->parked_cfg);
1303
1304 return clk_rcg2_recalc_rate(hw, parent_rate);
1305 }
1306
1307 const struct clk_ops clk_rcg2_shared_ops = {
1308 .enable = clk_rcg2_shared_enable,
1309 .disable = clk_rcg2_shared_disable,
1310 .get_parent = clk_rcg2_shared_get_parent,
1311 .set_parent = clk_rcg2_shared_set_parent,
1312 .recalc_rate = clk_rcg2_shared_recalc_rate,
1313 .determine_rate = clk_rcg2_determine_rate,
1314 .set_rate = clk_rcg2_shared_set_rate,
1315 .set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent,
1316 };
1317 EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops);
1318
1319 /* Common APIs to be used for DFS based RCGR */
clk_rcg2_dfs_populate_freq(struct clk_hw * hw,unsigned int l,struct freq_tbl * f)1320 static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l,
1321 struct freq_tbl *f)
1322 {
1323 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1324 struct clk_hw *p;
1325 unsigned long prate = 0;
1326 u32 val, mask, cfg, mode, src;
1327 int i, num_parents;
1328
1329 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg);
1330
1331 mask = BIT(rcg->hid_width) - 1;
1332 f->pre_div = 1;
1333 if (cfg & mask)
1334 f->pre_div = cfg & mask;
1335
1336 src = cfg & CFG_SRC_SEL_MASK;
1337 src >>= CFG_SRC_SEL_SHIFT;
1338
1339 num_parents = clk_hw_get_num_parents(hw);
1340 for (i = 0; i < num_parents; i++) {
1341 if (src == rcg->parent_map[i].cfg) {
1342 f->src = rcg->parent_map[i].src;
1343 p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i);
1344 prate = clk_hw_get_rate(p);
1345 }
1346 }
1347
1348 mode = cfg & CFG_MODE_MASK;
1349 mode >>= CFG_MODE_SHIFT;
1350 if (mode) {
1351 mask = BIT(rcg->mnd_width) - 1;
1352 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l),
1353 &val);
1354 val &= mask;
1355 f->m = val;
1356
1357 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l),
1358 &val);
1359 val = ~val;
1360 val &= mask;
1361 val += f->m;
1362 f->n = val;
1363 }
1364
1365 f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div);
1366 }
1367
clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 * rcg)1368 static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg)
1369 {
1370 struct freq_tbl *freq_tbl;
1371 int i;
1372
1373 /* Allocate space for 1 extra since table is NULL terminated */
1374 freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL);
1375 if (!freq_tbl)
1376 return -ENOMEM;
1377 rcg->freq_tbl = freq_tbl;
1378
1379 for (i = 0; i < MAX_PERF_LEVEL; i++)
1380 clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i);
1381
1382 return 0;
1383 }
1384
clk_rcg2_dfs_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)1385 static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw,
1386 struct clk_rate_request *req)
1387 {
1388 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1389 int ret;
1390
1391 if (!rcg->freq_tbl) {
1392 ret = clk_rcg2_dfs_populate_freq_table(rcg);
1393 if (ret) {
1394 pr_err("Failed to update DFS tables for %s\n",
1395 clk_hw_get_name(hw));
1396 return ret;
1397 }
1398 }
1399
1400 return clk_rcg2_determine_rate(hw, req);
1401 }
1402
1403 static unsigned long
clk_rcg2_dfs_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)1404 clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1405 {
1406 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1407 u32 level, mask, cfg, m = 0, n = 0, mode, pre_div;
1408
1409 regmap_read(rcg->clkr.regmap,
1410 rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level);
1411 level &= GENMASK(4, 1);
1412 level >>= 1;
1413
1414 if (rcg->freq_tbl)
1415 return rcg->freq_tbl[level].freq;
1416
1417 /*
1418 * Assume that parent_rate is actually the parent because
1419 * we can't do any better at figuring it out when the table
1420 * hasn't been populated yet. We only populate the table
1421 * in determine_rate because we can't guarantee the parents
1422 * will be registered with the framework until then.
1423 */
1424 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level),
1425 &cfg);
1426
1427 mask = BIT(rcg->hid_width) - 1;
1428 pre_div = 1;
1429 if (cfg & mask)
1430 pre_div = cfg & mask;
1431
1432 mode = cfg & CFG_MODE_MASK;
1433 mode >>= CFG_MODE_SHIFT;
1434 if (mode) {
1435 mask = BIT(rcg->mnd_width) - 1;
1436 regmap_read(rcg->clkr.regmap,
1437 rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m);
1438 m &= mask;
1439
1440 regmap_read(rcg->clkr.regmap,
1441 rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n);
1442 n = ~n;
1443 n &= mask;
1444 n += m;
1445 }
1446
1447 return calc_rate(parent_rate, m, n, mode, pre_div);
1448 }
1449
1450 static const struct clk_ops clk_rcg2_dfs_ops = {
1451 .is_enabled = clk_rcg2_is_enabled,
1452 .get_parent = clk_rcg2_get_parent,
1453 .determine_rate = clk_rcg2_dfs_determine_rate,
1454 .recalc_rate = clk_rcg2_dfs_recalc_rate,
1455 };
1456
clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data * data,struct regmap * regmap)1457 static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data,
1458 struct regmap *regmap)
1459 {
1460 struct clk_rcg2 *rcg = data->rcg;
1461 struct clk_init_data *init = data->init;
1462 u32 val;
1463 int ret;
1464
1465 ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val);
1466 if (ret)
1467 return -EINVAL;
1468
1469 if (!(val & SE_CMD_DFS_EN))
1470 return 0;
1471
1472 /*
1473 * Rate changes with consumer writing a register in
1474 * their own I/O region
1475 */
1476 init->flags |= CLK_GET_RATE_NOCACHE;
1477 init->ops = &clk_rcg2_dfs_ops;
1478
1479 rcg->freq_tbl = NULL;
1480
1481 return 0;
1482 }
1483
qcom_cc_register_rcg_dfs(struct regmap * regmap,const struct clk_rcg_dfs_data * rcgs,size_t len)1484 int qcom_cc_register_rcg_dfs(struct regmap *regmap,
1485 const struct clk_rcg_dfs_data *rcgs, size_t len)
1486 {
1487 int i, ret;
1488
1489 for (i = 0; i < len; i++) {
1490 ret = clk_rcg2_enable_dfs(&rcgs[i], regmap);
1491 if (ret)
1492 return ret;
1493 }
1494
1495 return 0;
1496 }
1497 EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs);
1498
clk_rcg2_dp_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)1499 static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate,
1500 unsigned long parent_rate)
1501 {
1502 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1503 struct freq_tbl f = { 0 };
1504 u32 mask = BIT(rcg->hid_width) - 1;
1505 u32 hid_div, cfg;
1506 int i, num_parents = clk_hw_get_num_parents(hw);
1507 unsigned long num, den;
1508
1509 rational_best_approximation(parent_rate, rate,
1510 GENMASK(rcg->mnd_width - 1, 0),
1511 GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1512
1513 if (!num || !den)
1514 return -EINVAL;
1515
1516 regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
1517 hid_div = cfg;
1518 cfg &= CFG_SRC_SEL_MASK;
1519 cfg >>= CFG_SRC_SEL_SHIFT;
1520
1521 for (i = 0; i < num_parents; i++) {
1522 if (cfg == rcg->parent_map[i].cfg) {
1523 f.src = rcg->parent_map[i].src;
1524 break;
1525 }
1526 }
1527
1528 f.pre_div = hid_div;
1529 f.pre_div >>= CFG_SRC_DIV_SHIFT;
1530 f.pre_div &= mask;
1531
1532 if (num != den) {
1533 f.m = num;
1534 f.n = den;
1535 } else {
1536 f.m = 0;
1537 f.n = 0;
1538 }
1539
1540 return clk_rcg2_configure(rcg, &f);
1541 }
1542
clk_rcg2_dp_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)1543 static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw,
1544 unsigned long rate, unsigned long parent_rate, u8 index)
1545 {
1546 return clk_rcg2_dp_set_rate(hw, rate, parent_rate);
1547 }
1548
clk_rcg2_dp_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)1549 static int clk_rcg2_dp_determine_rate(struct clk_hw *hw,
1550 struct clk_rate_request *req)
1551 {
1552 struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1553 unsigned long num, den;
1554 u64 tmp;
1555
1556 /* Parent rate is a fixed phy link rate */
1557 rational_best_approximation(req->best_parent_rate, req->rate,
1558 GENMASK(rcg->mnd_width - 1, 0),
1559 GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1560
1561 if (!num || !den)
1562 return -EINVAL;
1563
1564 tmp = req->best_parent_rate * num;
1565 do_div(tmp, den);
1566 req->rate = tmp;
1567
1568 return 0;
1569 }
1570
1571 const struct clk_ops clk_dp_ops = {
1572 .is_enabled = clk_rcg2_is_enabled,
1573 .get_parent = clk_rcg2_get_parent,
1574 .set_parent = clk_rcg2_set_parent,
1575 .recalc_rate = clk_rcg2_recalc_rate,
1576 .set_rate = clk_rcg2_dp_set_rate,
1577 .set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent,
1578 .determine_rate = clk_rcg2_dp_determine_rate,
1579 };
1580 EXPORT_SYMBOL_GPL(clk_dp_ops);
1581