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 | |
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(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CMD_REG, val: &cmd); |
71 | if (ret) |
72 | return ret; |
73 | |
74 | return (cmd & CMD_ROOT_OFF) == 0; |
75 | } |
76 | |
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 | |
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(map: rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), val: &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 | |
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(map: rcg->clkr.regmap, reg: 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(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CMD_REG, val: &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 | |
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(map: rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), |
144 | CFG_SRC_SEL_MASK, val: 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 |
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 |
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(map: rcg->clkr.regmap, RCG_M_OFFSET(rcg), val: &m); |
179 | m &= mask; |
180 | regmap_read(map: rcg->clkr.regmap, RCG_N_OFFSET(rcg), val: &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(rate: parent_rate, m, n, mode, hid_div); |
193 | } |
194 | |
195 | static unsigned long |
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(map: rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), val: &cfg); |
202 | |
203 | return __clk_rcg2_recalc_rate(hw, parent_rate, cfg); |
204 | } |
205 | |
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, map: rcg->parent_map, src: 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(hw: 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 int clk_rcg2_determine_rate(struct clk_hw *hw, |
264 | struct clk_rate_request *req) |
265 | { |
266 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
267 | |
268 | return _freq_tbl_determine_rate(hw, f: rcg->freq_tbl, req, policy: CEIL); |
269 | } |
270 | |
271 | static int clk_rcg2_determine_floor_rate(struct clk_hw *hw, |
272 | struct clk_rate_request *req) |
273 | { |
274 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
275 | |
276 | return _freq_tbl_determine_rate(hw, f: rcg->freq_tbl, req, policy: FLOOR); |
277 | } |
278 | |
279 | static int __clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f, |
280 | u32 *_cfg) |
281 | { |
282 | u32 cfg, mask, d_val, not2d_val, n_minus_m; |
283 | struct clk_hw *hw = &rcg->clkr.hw; |
284 | int ret, index = qcom_find_src_index(hw, map: rcg->parent_map, src: f->src); |
285 | |
286 | if (index < 0) |
287 | return index; |
288 | |
289 | if (rcg->mnd_width && f->n) { |
290 | mask = BIT(rcg->mnd_width) - 1; |
291 | ret = regmap_update_bits(map: rcg->clkr.regmap, |
292 | RCG_M_OFFSET(rcg), mask, val: f->m); |
293 | if (ret) |
294 | return ret; |
295 | |
296 | ret = regmap_update_bits(map: rcg->clkr.regmap, |
297 | RCG_N_OFFSET(rcg), mask, val: ~(f->n - f->m)); |
298 | if (ret) |
299 | return ret; |
300 | |
301 | /* Calculate 2d value */ |
302 | d_val = f->n; |
303 | |
304 | n_minus_m = f->n - f->m; |
305 | n_minus_m *= 2; |
306 | |
307 | d_val = clamp_t(u32, d_val, f->m, n_minus_m); |
308 | not2d_val = ~d_val & mask; |
309 | |
310 | ret = regmap_update_bits(map: rcg->clkr.regmap, |
311 | RCG_D_OFFSET(rcg), mask, val: not2d_val); |
312 | if (ret) |
313 | return ret; |
314 | } |
315 | |
316 | mask = BIT(rcg->hid_width) - 1; |
317 | mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK | CFG_HW_CLK_CTRL_MASK; |
318 | cfg = f->pre_div << CFG_SRC_DIV_SHIFT; |
319 | cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; |
320 | if (rcg->mnd_width && f->n && (f->m != f->n)) |
321 | cfg |= CFG_MODE_DUAL_EDGE; |
322 | if (rcg->hw_clk_ctrl) |
323 | cfg |= CFG_HW_CLK_CTRL_MASK; |
324 | |
325 | *_cfg &= ~mask; |
326 | *_cfg |= cfg; |
327 | |
328 | return 0; |
329 | } |
330 | |
331 | static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f) |
332 | { |
333 | u32 cfg; |
334 | int ret; |
335 | |
336 | ret = regmap_read(map: rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), val: &cfg); |
337 | if (ret) |
338 | return ret; |
339 | |
340 | ret = __clk_rcg2_configure(rcg, f, cfg: &cfg); |
341 | if (ret) |
342 | return ret; |
343 | |
344 | ret = regmap_write(map: rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), val: cfg); |
345 | if (ret) |
346 | return ret; |
347 | |
348 | return update_config(rcg); |
349 | } |
350 | |
351 | static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, |
352 | enum freq_policy policy) |
353 | { |
354 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
355 | const struct freq_tbl *f; |
356 | |
357 | switch (policy) { |
358 | case FLOOR: |
359 | f = qcom_find_freq_floor(f: rcg->freq_tbl, rate); |
360 | break; |
361 | case CEIL: |
362 | f = qcom_find_freq(f: rcg->freq_tbl, rate); |
363 | break; |
364 | default: |
365 | return -EINVAL; |
366 | } |
367 | |
368 | if (!f) |
369 | return -EINVAL; |
370 | |
371 | return clk_rcg2_configure(rcg, f); |
372 | } |
373 | |
374 | static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, |
375 | unsigned long parent_rate) |
376 | { |
377 | return __clk_rcg2_set_rate(hw, rate, policy: CEIL); |
378 | } |
379 | |
380 | static int clk_rcg2_set_floor_rate(struct clk_hw *hw, unsigned long rate, |
381 | unsigned long parent_rate) |
382 | { |
383 | return __clk_rcg2_set_rate(hw, rate, policy: FLOOR); |
384 | } |
385 | |
386 | static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw, |
387 | unsigned long rate, unsigned long parent_rate, u8 index) |
388 | { |
389 | return __clk_rcg2_set_rate(hw, rate, policy: CEIL); |
390 | } |
391 | |
392 | static int clk_rcg2_set_floor_rate_and_parent(struct clk_hw *hw, |
393 | unsigned long rate, unsigned long parent_rate, u8 index) |
394 | { |
395 | return __clk_rcg2_set_rate(hw, rate, policy: FLOOR); |
396 | } |
397 | |
398 | static int clk_rcg2_get_duty_cycle(struct clk_hw *hw, struct clk_duty *duty) |
399 | { |
400 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
401 | u32 notn_m, n, m, d, not2d, mask; |
402 | |
403 | if (!rcg->mnd_width) { |
404 | /* 50 % duty-cycle for Non-MND RCGs */ |
405 | duty->num = 1; |
406 | duty->den = 2; |
407 | return 0; |
408 | } |
409 | |
410 | regmap_read(map: rcg->clkr.regmap, RCG_D_OFFSET(rcg), val: ¬2d); |
411 | regmap_read(map: rcg->clkr.regmap, RCG_M_OFFSET(rcg), val: &m); |
412 | regmap_read(map: rcg->clkr.regmap, RCG_N_OFFSET(rcg), val: ¬n_m); |
413 | |
414 | if (!not2d && !m && !notn_m) { |
415 | /* 50 % duty-cycle always */ |
416 | duty->num = 1; |
417 | duty->den = 2; |
418 | return 0; |
419 | } |
420 | |
421 | mask = BIT(rcg->mnd_width) - 1; |
422 | |
423 | d = ~(not2d) & mask; |
424 | d = DIV_ROUND_CLOSEST(d, 2); |
425 | |
426 | n = (~(notn_m) + m) & mask; |
427 | |
428 | duty->num = d; |
429 | duty->den = n; |
430 | |
431 | return 0; |
432 | } |
433 | |
434 | static int clk_rcg2_set_duty_cycle(struct clk_hw *hw, struct clk_duty *duty) |
435 | { |
436 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
437 | u32 notn_m, n, m, d, not2d, mask, duty_per, cfg; |
438 | int ret; |
439 | |
440 | /* Duty-cycle cannot be modified for non-MND RCGs */ |
441 | if (!rcg->mnd_width) |
442 | return -EINVAL; |
443 | |
444 | mask = BIT(rcg->mnd_width) - 1; |
445 | |
446 | regmap_read(map: rcg->clkr.regmap, RCG_N_OFFSET(rcg), val: ¬n_m); |
447 | regmap_read(map: rcg->clkr.regmap, RCG_M_OFFSET(rcg), val: &m); |
448 | regmap_read(map: rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), val: &cfg); |
449 | |
450 | /* Duty-cycle cannot be modified if MND divider is in bypass mode. */ |
451 | if (!(cfg & CFG_MODE_MASK)) |
452 | return -EINVAL; |
453 | |
454 | n = (~(notn_m) + m) & mask; |
455 | |
456 | duty_per = (duty->num * 100) / duty->den; |
457 | |
458 | /* Calculate 2d value */ |
459 | d = DIV_ROUND_CLOSEST(n * duty_per * 2, 100); |
460 | |
461 | /* |
462 | * Check bit widths of 2d. If D is too big reduce duty cycle. |
463 | * Also make sure it is never zero. |
464 | */ |
465 | d = clamp_val(d, 1, mask); |
466 | |
467 | if ((d / 2) > (n - m)) |
468 | d = (n - m) * 2; |
469 | else if ((d / 2) < (m / 2)) |
470 | d = m; |
471 | |
472 | not2d = ~d & mask; |
473 | |
474 | ret = regmap_update_bits(map: rcg->clkr.regmap, RCG_D_OFFSET(rcg), mask, |
475 | val: not2d); |
476 | if (ret) |
477 | return ret; |
478 | |
479 | return update_config(rcg); |
480 | } |
481 | |
482 | const struct clk_ops clk_rcg2_ops = { |
483 | .is_enabled = clk_rcg2_is_enabled, |
484 | .get_parent = clk_rcg2_get_parent, |
485 | .set_parent = clk_rcg2_set_parent, |
486 | .recalc_rate = clk_rcg2_recalc_rate, |
487 | .determine_rate = clk_rcg2_determine_rate, |
488 | .set_rate = clk_rcg2_set_rate, |
489 | .set_rate_and_parent = clk_rcg2_set_rate_and_parent, |
490 | .get_duty_cycle = clk_rcg2_get_duty_cycle, |
491 | .set_duty_cycle = clk_rcg2_set_duty_cycle, |
492 | }; |
493 | EXPORT_SYMBOL_GPL(clk_rcg2_ops); |
494 | |
495 | const struct clk_ops clk_rcg2_floor_ops = { |
496 | .is_enabled = clk_rcg2_is_enabled, |
497 | .get_parent = clk_rcg2_get_parent, |
498 | .set_parent = clk_rcg2_set_parent, |
499 | .recalc_rate = clk_rcg2_recalc_rate, |
500 | .determine_rate = clk_rcg2_determine_floor_rate, |
501 | .set_rate = clk_rcg2_set_floor_rate, |
502 | .set_rate_and_parent = clk_rcg2_set_floor_rate_and_parent, |
503 | .get_duty_cycle = clk_rcg2_get_duty_cycle, |
504 | .set_duty_cycle = clk_rcg2_set_duty_cycle, |
505 | }; |
506 | EXPORT_SYMBOL_GPL(clk_rcg2_floor_ops); |
507 | |
508 | const struct clk_ops clk_rcg2_mux_closest_ops = { |
509 | .determine_rate = __clk_mux_determine_rate_closest, |
510 | .get_parent = clk_rcg2_get_parent, |
511 | .set_parent = clk_rcg2_set_parent, |
512 | }; |
513 | EXPORT_SYMBOL_GPL(clk_rcg2_mux_closest_ops); |
514 | |
515 | struct frac_entry { |
516 | int num; |
517 | int den; |
518 | }; |
519 | |
520 | static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */ |
521 | { 52, 295 }, /* 119 M */ |
522 | { 11, 57 }, /* 130.25 M */ |
523 | { 63, 307 }, /* 138.50 M */ |
524 | { 11, 50 }, /* 148.50 M */ |
525 | { 47, 206 }, /* 154 M */ |
526 | { 31, 100 }, /* 205.25 M */ |
527 | { 107, 269 }, /* 268.50 M */ |
528 | { }, |
529 | }; |
530 | |
531 | static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */ |
532 | { 31, 211 }, /* 119 M */ |
533 | { 32, 199 }, /* 130.25 M */ |
534 | { 63, 307 }, /* 138.50 M */ |
535 | { 11, 60 }, /* 148.50 M */ |
536 | { 50, 263 }, /* 154 M */ |
537 | { 31, 120 }, /* 205.25 M */ |
538 | { 119, 359 }, /* 268.50 M */ |
539 | { }, |
540 | }; |
541 | |
542 | static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate, |
543 | unsigned long parent_rate) |
544 | { |
545 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
546 | struct freq_tbl f = *rcg->freq_tbl; |
547 | const struct frac_entry *frac; |
548 | int delta = 100000; |
549 | s64 src_rate = parent_rate; |
550 | s64 request; |
551 | u32 mask = BIT(rcg->hid_width) - 1; |
552 | u32 hid_div; |
553 | |
554 | if (src_rate == 810000000) |
555 | frac = frac_table_810m; |
556 | else |
557 | frac = frac_table_675m; |
558 | |
559 | for (; frac->num; frac++) { |
560 | request = rate; |
561 | request *= frac->den; |
562 | request = div_s64(dividend: request, divisor: frac->num); |
563 | if ((src_rate < (request - delta)) || |
564 | (src_rate > (request + delta))) |
565 | continue; |
566 | |
567 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, |
568 | val: &hid_div); |
569 | f.pre_div = hid_div; |
570 | f.pre_div >>= CFG_SRC_DIV_SHIFT; |
571 | f.pre_div &= mask; |
572 | f.m = frac->num; |
573 | f.n = frac->den; |
574 | |
575 | return clk_rcg2_configure(rcg, f: &f); |
576 | } |
577 | |
578 | return -EINVAL; |
579 | } |
580 | |
581 | static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw, |
582 | unsigned long rate, unsigned long parent_rate, u8 index) |
583 | { |
584 | /* Parent index is set statically in frequency table */ |
585 | return clk_edp_pixel_set_rate(hw, rate, parent_rate); |
586 | } |
587 | |
588 | static int clk_edp_pixel_determine_rate(struct clk_hw *hw, |
589 | struct clk_rate_request *req) |
590 | { |
591 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
592 | const struct freq_tbl *f = rcg->freq_tbl; |
593 | const struct frac_entry *frac; |
594 | int delta = 100000; |
595 | s64 request; |
596 | u32 mask = BIT(rcg->hid_width) - 1; |
597 | u32 hid_div; |
598 | int index = qcom_find_src_index(hw, map: rcg->parent_map, src: f->src); |
599 | |
600 | /* Force the correct parent */ |
601 | req->best_parent_hw = clk_hw_get_parent_by_index(hw, index); |
602 | req->best_parent_rate = clk_hw_get_rate(hw: req->best_parent_hw); |
603 | |
604 | if (req->best_parent_rate == 810000000) |
605 | frac = frac_table_810m; |
606 | else |
607 | frac = frac_table_675m; |
608 | |
609 | for (; frac->num; frac++) { |
610 | request = req->rate; |
611 | request *= frac->den; |
612 | request = div_s64(dividend: request, divisor: frac->num); |
613 | if ((req->best_parent_rate < (request - delta)) || |
614 | (req->best_parent_rate > (request + delta))) |
615 | continue; |
616 | |
617 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, |
618 | val: &hid_div); |
619 | hid_div >>= CFG_SRC_DIV_SHIFT; |
620 | hid_div &= mask; |
621 | |
622 | req->rate = calc_rate(rate: req->best_parent_rate, |
623 | m: frac->num, n: frac->den, |
624 | mode: !!frac->den, hid_div); |
625 | return 0; |
626 | } |
627 | |
628 | return -EINVAL; |
629 | } |
630 | |
631 | const struct clk_ops clk_edp_pixel_ops = { |
632 | .is_enabled = clk_rcg2_is_enabled, |
633 | .get_parent = clk_rcg2_get_parent, |
634 | .set_parent = clk_rcg2_set_parent, |
635 | .recalc_rate = clk_rcg2_recalc_rate, |
636 | .set_rate = clk_edp_pixel_set_rate, |
637 | .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent, |
638 | .determine_rate = clk_edp_pixel_determine_rate, |
639 | }; |
640 | EXPORT_SYMBOL_GPL(clk_edp_pixel_ops); |
641 | |
642 | static int clk_byte_determine_rate(struct clk_hw *hw, |
643 | struct clk_rate_request *req) |
644 | { |
645 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
646 | const struct freq_tbl *f = rcg->freq_tbl; |
647 | int index = qcom_find_src_index(hw, map: rcg->parent_map, src: f->src); |
648 | unsigned long parent_rate, div; |
649 | u32 mask = BIT(rcg->hid_width) - 1; |
650 | struct clk_hw *p; |
651 | |
652 | if (req->rate == 0) |
653 | return -EINVAL; |
654 | |
655 | req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index); |
656 | req->best_parent_rate = parent_rate = clk_hw_round_rate(hw: p, rate: req->rate); |
657 | |
658 | div = DIV_ROUND_UP((2 * parent_rate), req->rate) - 1; |
659 | div = min_t(u32, div, mask); |
660 | |
661 | req->rate = calc_rate(rate: parent_rate, m: 0, n: 0, mode: 0, hid_div: div); |
662 | |
663 | return 0; |
664 | } |
665 | |
666 | static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate, |
667 | unsigned long parent_rate) |
668 | { |
669 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
670 | struct freq_tbl f = *rcg->freq_tbl; |
671 | unsigned long div; |
672 | u32 mask = BIT(rcg->hid_width) - 1; |
673 | |
674 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; |
675 | div = min_t(u32, div, mask); |
676 | |
677 | f.pre_div = div; |
678 | |
679 | return clk_rcg2_configure(rcg, f: &f); |
680 | } |
681 | |
682 | static int clk_byte_set_rate_and_parent(struct clk_hw *hw, |
683 | unsigned long rate, unsigned long parent_rate, u8 index) |
684 | { |
685 | /* Parent index is set statically in frequency table */ |
686 | return clk_byte_set_rate(hw, rate, parent_rate); |
687 | } |
688 | |
689 | const struct clk_ops clk_byte_ops = { |
690 | .is_enabled = clk_rcg2_is_enabled, |
691 | .get_parent = clk_rcg2_get_parent, |
692 | .set_parent = clk_rcg2_set_parent, |
693 | .recalc_rate = clk_rcg2_recalc_rate, |
694 | .set_rate = clk_byte_set_rate, |
695 | .set_rate_and_parent = clk_byte_set_rate_and_parent, |
696 | .determine_rate = clk_byte_determine_rate, |
697 | }; |
698 | EXPORT_SYMBOL_GPL(clk_byte_ops); |
699 | |
700 | static int clk_byte2_determine_rate(struct clk_hw *hw, |
701 | struct clk_rate_request *req) |
702 | { |
703 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
704 | unsigned long parent_rate, div; |
705 | u32 mask = BIT(rcg->hid_width) - 1; |
706 | struct clk_hw *p; |
707 | unsigned long rate = req->rate; |
708 | |
709 | if (rate == 0) |
710 | return -EINVAL; |
711 | |
712 | p = req->best_parent_hw; |
713 | req->best_parent_rate = parent_rate = clk_hw_round_rate(hw: p, rate); |
714 | |
715 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; |
716 | div = min_t(u32, div, mask); |
717 | |
718 | req->rate = calc_rate(rate: parent_rate, m: 0, n: 0, mode: 0, hid_div: div); |
719 | |
720 | return 0; |
721 | } |
722 | |
723 | static int clk_byte2_set_rate(struct clk_hw *hw, unsigned long rate, |
724 | unsigned long parent_rate) |
725 | { |
726 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
727 | struct freq_tbl f = { 0 }; |
728 | unsigned long div; |
729 | int i, num_parents = clk_hw_get_num_parents(hw); |
730 | u32 mask = BIT(rcg->hid_width) - 1; |
731 | u32 cfg; |
732 | |
733 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; |
734 | div = min_t(u32, div, mask); |
735 | |
736 | f.pre_div = div; |
737 | |
738 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, val: &cfg); |
739 | cfg &= CFG_SRC_SEL_MASK; |
740 | cfg >>= CFG_SRC_SEL_SHIFT; |
741 | |
742 | for (i = 0; i < num_parents; i++) { |
743 | if (cfg == rcg->parent_map[i].cfg) { |
744 | f.src = rcg->parent_map[i].src; |
745 | return clk_rcg2_configure(rcg, f: &f); |
746 | } |
747 | } |
748 | |
749 | return -EINVAL; |
750 | } |
751 | |
752 | static int clk_byte2_set_rate_and_parent(struct clk_hw *hw, |
753 | unsigned long rate, unsigned long parent_rate, u8 index) |
754 | { |
755 | /* Read the hardware to determine parent during set_rate */ |
756 | return clk_byte2_set_rate(hw, rate, parent_rate); |
757 | } |
758 | |
759 | const struct clk_ops clk_byte2_ops = { |
760 | .is_enabled = clk_rcg2_is_enabled, |
761 | .get_parent = clk_rcg2_get_parent, |
762 | .set_parent = clk_rcg2_set_parent, |
763 | .recalc_rate = clk_rcg2_recalc_rate, |
764 | .set_rate = clk_byte2_set_rate, |
765 | .set_rate_and_parent = clk_byte2_set_rate_and_parent, |
766 | .determine_rate = clk_byte2_determine_rate, |
767 | }; |
768 | EXPORT_SYMBOL_GPL(clk_byte2_ops); |
769 | |
770 | static const struct frac_entry frac_table_pixel[] = { |
771 | { .num: 3, .den: 8 }, |
772 | { 2, 9 }, |
773 | { 4, 9 }, |
774 | { 1, 1 }, |
775 | { 2, 3 }, |
776 | { } |
777 | }; |
778 | |
779 | static int clk_pixel_determine_rate(struct clk_hw *hw, |
780 | struct clk_rate_request *req) |
781 | { |
782 | unsigned long request, src_rate; |
783 | int delta = 100000; |
784 | const struct frac_entry *frac = frac_table_pixel; |
785 | |
786 | for (; frac->num; frac++) { |
787 | request = (req->rate * frac->den) / frac->num; |
788 | |
789 | src_rate = clk_hw_round_rate(hw: req->best_parent_hw, rate: request); |
790 | if ((src_rate < (request - delta)) || |
791 | (src_rate > (request + delta))) |
792 | continue; |
793 | |
794 | req->best_parent_rate = src_rate; |
795 | req->rate = (src_rate * frac->num) / frac->den; |
796 | return 0; |
797 | } |
798 | |
799 | return -EINVAL; |
800 | } |
801 | |
802 | static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate, |
803 | unsigned long parent_rate) |
804 | { |
805 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
806 | struct freq_tbl f = { 0 }; |
807 | const struct frac_entry *frac = frac_table_pixel; |
808 | unsigned long request; |
809 | int delta = 100000; |
810 | u32 mask = BIT(rcg->hid_width) - 1; |
811 | u32 hid_div, cfg; |
812 | int i, num_parents = clk_hw_get_num_parents(hw); |
813 | |
814 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, val: &cfg); |
815 | cfg &= CFG_SRC_SEL_MASK; |
816 | cfg >>= CFG_SRC_SEL_SHIFT; |
817 | |
818 | for (i = 0; i < num_parents; i++) |
819 | if (cfg == rcg->parent_map[i].cfg) { |
820 | f.src = rcg->parent_map[i].src; |
821 | break; |
822 | } |
823 | |
824 | for (; frac->num; frac++) { |
825 | request = (rate * frac->den) / frac->num; |
826 | |
827 | if ((parent_rate < (request - delta)) || |
828 | (parent_rate > (request + delta))) |
829 | continue; |
830 | |
831 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, |
832 | val: &hid_div); |
833 | f.pre_div = hid_div; |
834 | f.pre_div >>= CFG_SRC_DIV_SHIFT; |
835 | f.pre_div &= mask; |
836 | f.m = frac->num; |
837 | f.n = frac->den; |
838 | |
839 | return clk_rcg2_configure(rcg, f: &f); |
840 | } |
841 | return -EINVAL; |
842 | } |
843 | |
844 | static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, |
845 | unsigned long parent_rate, u8 index) |
846 | { |
847 | return clk_pixel_set_rate(hw, rate, parent_rate); |
848 | } |
849 | |
850 | const struct clk_ops clk_pixel_ops = { |
851 | .is_enabled = clk_rcg2_is_enabled, |
852 | .get_parent = clk_rcg2_get_parent, |
853 | .set_parent = clk_rcg2_set_parent, |
854 | .recalc_rate = clk_rcg2_recalc_rate, |
855 | .set_rate = clk_pixel_set_rate, |
856 | .set_rate_and_parent = clk_pixel_set_rate_and_parent, |
857 | .determine_rate = clk_pixel_determine_rate, |
858 | }; |
859 | EXPORT_SYMBOL_GPL(clk_pixel_ops); |
860 | |
861 | static int clk_gfx3d_determine_rate(struct clk_hw *hw, |
862 | struct clk_rate_request *req) |
863 | { |
864 | struct clk_rate_request parent_req = { .min_rate = 0, .max_rate = ULONG_MAX }; |
865 | struct clk_rcg2_gfx3d *cgfx = to_clk_rcg2_gfx3d(hw); |
866 | struct clk_hw *xo, *p0, *p1, *p2; |
867 | unsigned long p0_rate; |
868 | u8 mux_div = cgfx->div; |
869 | int ret; |
870 | |
871 | p0 = cgfx->hws[0]; |
872 | p1 = cgfx->hws[1]; |
873 | p2 = cgfx->hws[2]; |
874 | /* |
875 | * This function does ping-pong the RCG between PLLs: if we don't |
876 | * have at least one fixed PLL and two variable ones, |
877 | * then it's not going to work correctly. |
878 | */ |
879 | if (WARN_ON(!p0 || !p1 || !p2)) |
880 | return -EINVAL; |
881 | |
882 | xo = clk_hw_get_parent_by_index(hw, index: 0); |
883 | if (req->rate == clk_hw_get_rate(hw: xo)) { |
884 | req->best_parent_hw = xo; |
885 | return 0; |
886 | } |
887 | |
888 | if (mux_div == 0) |
889 | mux_div = 1; |
890 | |
891 | parent_req.rate = req->rate * mux_div; |
892 | |
893 | /* This has to be a fixed rate PLL */ |
894 | p0_rate = clk_hw_get_rate(hw: p0); |
895 | |
896 | if (parent_req.rate == p0_rate) { |
897 | req->rate = req->best_parent_rate = p0_rate; |
898 | req->best_parent_hw = p0; |
899 | return 0; |
900 | } |
901 | |
902 | if (req->best_parent_hw == p0) { |
903 | /* Are we going back to a previously used rate? */ |
904 | if (clk_hw_get_rate(hw: p2) == parent_req.rate) |
905 | req->best_parent_hw = p2; |
906 | else |
907 | req->best_parent_hw = p1; |
908 | } else if (req->best_parent_hw == p2) { |
909 | req->best_parent_hw = p1; |
910 | } else { |
911 | req->best_parent_hw = p2; |
912 | } |
913 | |
914 | clk_hw_get_rate_range(hw: req->best_parent_hw, |
915 | min_rate: &parent_req.min_rate, max_rate: &parent_req.max_rate); |
916 | |
917 | if (req->min_rate > parent_req.min_rate) |
918 | parent_req.min_rate = req->min_rate; |
919 | |
920 | if (req->max_rate < parent_req.max_rate) |
921 | parent_req.max_rate = req->max_rate; |
922 | |
923 | ret = __clk_determine_rate(core: req->best_parent_hw, req: &parent_req); |
924 | if (ret) |
925 | return ret; |
926 | |
927 | req->rate = req->best_parent_rate = parent_req.rate; |
928 | req->rate /= mux_div; |
929 | |
930 | return 0; |
931 | } |
932 | |
933 | static int clk_gfx3d_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, |
934 | unsigned long parent_rate, u8 index) |
935 | { |
936 | struct clk_rcg2_gfx3d *cgfx = to_clk_rcg2_gfx3d(hw); |
937 | struct clk_rcg2 *rcg = &cgfx->rcg; |
938 | u32 cfg; |
939 | int ret; |
940 | |
941 | cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; |
942 | /* On some targets, the GFX3D RCG may need to divide PLL frequency */ |
943 | if (cgfx->div > 1) |
944 | cfg |= ((2 * cgfx->div) - 1) << CFG_SRC_DIV_SHIFT; |
945 | |
946 | ret = regmap_write(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, val: cfg); |
947 | if (ret) |
948 | return ret; |
949 | |
950 | return update_config(rcg); |
951 | } |
952 | |
953 | static int clk_gfx3d_set_rate(struct clk_hw *hw, unsigned long rate, |
954 | unsigned long parent_rate) |
955 | { |
956 | /* |
957 | * We should never get here; clk_gfx3d_determine_rate() should always |
958 | * make us use a different parent than what we're currently using, so |
959 | * clk_gfx3d_set_rate_and_parent() should always be called. |
960 | */ |
961 | return 0; |
962 | } |
963 | |
964 | const struct clk_ops clk_gfx3d_ops = { |
965 | .is_enabled = clk_rcg2_is_enabled, |
966 | .get_parent = clk_rcg2_get_parent, |
967 | .set_parent = clk_rcg2_set_parent, |
968 | .recalc_rate = clk_rcg2_recalc_rate, |
969 | .set_rate = clk_gfx3d_set_rate, |
970 | .set_rate_and_parent = clk_gfx3d_set_rate_and_parent, |
971 | .determine_rate = clk_gfx3d_determine_rate, |
972 | }; |
973 | EXPORT_SYMBOL_GPL(clk_gfx3d_ops); |
974 | |
975 | static int clk_rcg2_set_force_enable(struct clk_hw *hw) |
976 | { |
977 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
978 | const char *name = clk_hw_get_name(hw); |
979 | int ret, count; |
980 | |
981 | ret = regmap_update_bits(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CMD_REG, |
982 | CMD_ROOT_EN, CMD_ROOT_EN); |
983 | if (ret) |
984 | return ret; |
985 | |
986 | /* wait for RCG to turn ON */ |
987 | for (count = 500; count > 0; count--) { |
988 | if (clk_rcg2_is_enabled(hw)) |
989 | return 0; |
990 | |
991 | udelay(1); |
992 | } |
993 | |
994 | pr_err("%s: RCG did not turn on\n" , name); |
995 | return -ETIMEDOUT; |
996 | } |
997 | |
998 | static int clk_rcg2_clear_force_enable(struct clk_hw *hw) |
999 | { |
1000 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1001 | |
1002 | return regmap_update_bits(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CMD_REG, |
1003 | CMD_ROOT_EN, val: 0); |
1004 | } |
1005 | |
1006 | static int |
1007 | clk_rcg2_shared_force_enable_clear(struct clk_hw *hw, const struct freq_tbl *f) |
1008 | { |
1009 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1010 | int ret; |
1011 | |
1012 | ret = clk_rcg2_set_force_enable(hw); |
1013 | if (ret) |
1014 | return ret; |
1015 | |
1016 | ret = clk_rcg2_configure(rcg, f); |
1017 | if (ret) |
1018 | return ret; |
1019 | |
1020 | return clk_rcg2_clear_force_enable(hw); |
1021 | } |
1022 | |
1023 | static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate, |
1024 | unsigned long parent_rate) |
1025 | { |
1026 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1027 | const struct freq_tbl *f; |
1028 | |
1029 | f = qcom_find_freq(f: rcg->freq_tbl, rate); |
1030 | if (!f) |
1031 | return -EINVAL; |
1032 | |
1033 | /* |
1034 | * In case clock is disabled, update the M, N and D registers, cache |
1035 | * the CFG value in parked_cfg and don't hit the update bit of CMD |
1036 | * register. |
1037 | */ |
1038 | if (!clk_hw_is_enabled(hw)) |
1039 | return __clk_rcg2_configure(rcg, f, cfg: &rcg->parked_cfg); |
1040 | |
1041 | return clk_rcg2_shared_force_enable_clear(hw, f); |
1042 | } |
1043 | |
1044 | static int clk_rcg2_shared_set_rate_and_parent(struct clk_hw *hw, |
1045 | unsigned long rate, unsigned long parent_rate, u8 index) |
1046 | { |
1047 | return clk_rcg2_shared_set_rate(hw, rate, parent_rate); |
1048 | } |
1049 | |
1050 | static int clk_rcg2_shared_enable(struct clk_hw *hw) |
1051 | { |
1052 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1053 | int ret; |
1054 | |
1055 | /* |
1056 | * Set the update bit because required configuration has already |
1057 | * been written in clk_rcg2_shared_set_rate() |
1058 | */ |
1059 | ret = clk_rcg2_set_force_enable(hw); |
1060 | if (ret) |
1061 | return ret; |
1062 | |
1063 | /* Write back the stored configuration corresponding to current rate */ |
1064 | ret = regmap_write(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, val: rcg->parked_cfg); |
1065 | if (ret) |
1066 | return ret; |
1067 | |
1068 | ret = update_config(rcg); |
1069 | if (ret) |
1070 | return ret; |
1071 | |
1072 | return clk_rcg2_clear_force_enable(hw); |
1073 | } |
1074 | |
1075 | static void clk_rcg2_shared_disable(struct clk_hw *hw) |
1076 | { |
1077 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1078 | |
1079 | /* |
1080 | * Store current configuration as switching to safe source would clear |
1081 | * the SRC and DIV of CFG register |
1082 | */ |
1083 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, val: &rcg->parked_cfg); |
1084 | |
1085 | /* |
1086 | * Park the RCG at a safe configuration - sourced off of safe source. |
1087 | * Force enable and disable the RCG while configuring it to safeguard |
1088 | * against any update signal coming from the downstream clock. |
1089 | * The current parent is still prepared and enabled at this point, and |
1090 | * the safe source is always on while application processor subsystem |
1091 | * is online. Therefore, the RCG can safely switch its parent. |
1092 | */ |
1093 | clk_rcg2_set_force_enable(hw); |
1094 | |
1095 | regmap_write(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, |
1096 | val: rcg->safe_src_index << CFG_SRC_SEL_SHIFT); |
1097 | |
1098 | update_config(rcg); |
1099 | |
1100 | clk_rcg2_clear_force_enable(hw); |
1101 | } |
1102 | |
1103 | static u8 clk_rcg2_shared_get_parent(struct clk_hw *hw) |
1104 | { |
1105 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1106 | |
1107 | /* If the shared rcg is parked use the cached cfg instead */ |
1108 | if (!clk_hw_is_enabled(hw)) |
1109 | return __clk_rcg2_get_parent(hw, cfg: rcg->parked_cfg); |
1110 | |
1111 | return clk_rcg2_get_parent(hw); |
1112 | } |
1113 | |
1114 | static int clk_rcg2_shared_set_parent(struct clk_hw *hw, u8 index) |
1115 | { |
1116 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1117 | |
1118 | /* If the shared rcg is parked only update the cached cfg */ |
1119 | if (!clk_hw_is_enabled(hw)) { |
1120 | rcg->parked_cfg &= ~CFG_SRC_SEL_MASK; |
1121 | rcg->parked_cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; |
1122 | |
1123 | return 0; |
1124 | } |
1125 | |
1126 | return clk_rcg2_set_parent(hw, index); |
1127 | } |
1128 | |
1129 | static unsigned long |
1130 | clk_rcg2_shared_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) |
1131 | { |
1132 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1133 | |
1134 | /* If the shared rcg is parked use the cached cfg instead */ |
1135 | if (!clk_hw_is_enabled(hw)) |
1136 | return __clk_rcg2_recalc_rate(hw, parent_rate, cfg: rcg->parked_cfg); |
1137 | |
1138 | return clk_rcg2_recalc_rate(hw, parent_rate); |
1139 | } |
1140 | |
1141 | const struct clk_ops clk_rcg2_shared_ops = { |
1142 | .enable = clk_rcg2_shared_enable, |
1143 | .disable = clk_rcg2_shared_disable, |
1144 | .get_parent = clk_rcg2_shared_get_parent, |
1145 | .set_parent = clk_rcg2_shared_set_parent, |
1146 | .recalc_rate = clk_rcg2_shared_recalc_rate, |
1147 | .determine_rate = clk_rcg2_determine_rate, |
1148 | .set_rate = clk_rcg2_shared_set_rate, |
1149 | .set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent, |
1150 | }; |
1151 | EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops); |
1152 | |
1153 | /* Common APIs to be used for DFS based RCGR */ |
1154 | static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l, |
1155 | struct freq_tbl *f) |
1156 | { |
1157 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1158 | struct clk_hw *p; |
1159 | unsigned long prate = 0; |
1160 | u32 val, mask, cfg, mode, src; |
1161 | int i, num_parents; |
1162 | |
1163 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + SE_PERF_DFSR(l), val: &cfg); |
1164 | |
1165 | mask = BIT(rcg->hid_width) - 1; |
1166 | f->pre_div = 1; |
1167 | if (cfg & mask) |
1168 | f->pre_div = cfg & mask; |
1169 | |
1170 | src = cfg & CFG_SRC_SEL_MASK; |
1171 | src >>= CFG_SRC_SEL_SHIFT; |
1172 | |
1173 | num_parents = clk_hw_get_num_parents(hw); |
1174 | for (i = 0; i < num_parents; i++) { |
1175 | if (src == rcg->parent_map[i].cfg) { |
1176 | f->src = rcg->parent_map[i].src; |
1177 | p = clk_hw_get_parent_by_index(hw: &rcg->clkr.hw, index: i); |
1178 | prate = clk_hw_get_rate(hw: p); |
1179 | } |
1180 | } |
1181 | |
1182 | mode = cfg & CFG_MODE_MASK; |
1183 | mode >>= CFG_MODE_SHIFT; |
1184 | if (mode) { |
1185 | mask = BIT(rcg->mnd_width) - 1; |
1186 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + SE_PERF_M_DFSR(l), |
1187 | val: &val); |
1188 | val &= mask; |
1189 | f->m = val; |
1190 | |
1191 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + SE_PERF_N_DFSR(l), |
1192 | val: &val); |
1193 | val = ~val; |
1194 | val &= mask; |
1195 | val += f->m; |
1196 | f->n = val; |
1197 | } |
1198 | |
1199 | f->freq = calc_rate(rate: prate, m: f->m, n: f->n, mode, hid_div: f->pre_div); |
1200 | } |
1201 | |
1202 | static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg) |
1203 | { |
1204 | struct freq_tbl *freq_tbl; |
1205 | int i; |
1206 | |
1207 | /* Allocate space for 1 extra since table is NULL terminated */ |
1208 | freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, size: sizeof(*freq_tbl), GFP_KERNEL); |
1209 | if (!freq_tbl) |
1210 | return -ENOMEM; |
1211 | rcg->freq_tbl = freq_tbl; |
1212 | |
1213 | for (i = 0; i < MAX_PERF_LEVEL; i++) |
1214 | clk_rcg2_dfs_populate_freq(hw: &rcg->clkr.hw, l: i, f: freq_tbl + i); |
1215 | |
1216 | return 0; |
1217 | } |
1218 | |
1219 | static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw, |
1220 | struct clk_rate_request *req) |
1221 | { |
1222 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1223 | int ret; |
1224 | |
1225 | if (!rcg->freq_tbl) { |
1226 | ret = clk_rcg2_dfs_populate_freq_table(rcg); |
1227 | if (ret) { |
1228 | pr_err("Failed to update DFS tables for %s\n" , |
1229 | clk_hw_get_name(hw)); |
1230 | return ret; |
1231 | } |
1232 | } |
1233 | |
1234 | return clk_rcg2_determine_rate(hw, req); |
1235 | } |
1236 | |
1237 | static unsigned long |
1238 | clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) |
1239 | { |
1240 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1241 | u32 level, mask, cfg, m = 0, n = 0, mode, pre_div; |
1242 | |
1243 | regmap_read(map: rcg->clkr.regmap, |
1244 | reg: rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, val: &level); |
1245 | level &= GENMASK(4, 1); |
1246 | level >>= 1; |
1247 | |
1248 | if (rcg->freq_tbl) |
1249 | return rcg->freq_tbl[level].freq; |
1250 | |
1251 | /* |
1252 | * Assume that parent_rate is actually the parent because |
1253 | * we can't do any better at figuring it out when the table |
1254 | * hasn't been populated yet. We only populate the table |
1255 | * in determine_rate because we can't guarantee the parents |
1256 | * will be registered with the framework until then. |
1257 | */ |
1258 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + SE_PERF_DFSR(level), |
1259 | val: &cfg); |
1260 | |
1261 | mask = BIT(rcg->hid_width) - 1; |
1262 | pre_div = 1; |
1263 | if (cfg & mask) |
1264 | pre_div = cfg & mask; |
1265 | |
1266 | mode = cfg & CFG_MODE_MASK; |
1267 | mode >>= CFG_MODE_SHIFT; |
1268 | if (mode) { |
1269 | mask = BIT(rcg->mnd_width) - 1; |
1270 | regmap_read(map: rcg->clkr.regmap, |
1271 | reg: rcg->cmd_rcgr + SE_PERF_M_DFSR(level), val: &m); |
1272 | m &= mask; |
1273 | |
1274 | regmap_read(map: rcg->clkr.regmap, |
1275 | reg: rcg->cmd_rcgr + SE_PERF_N_DFSR(level), val: &n); |
1276 | n = ~n; |
1277 | n &= mask; |
1278 | n += m; |
1279 | } |
1280 | |
1281 | return calc_rate(rate: parent_rate, m, n, mode, hid_div: pre_div); |
1282 | } |
1283 | |
1284 | static const struct clk_ops clk_rcg2_dfs_ops = { |
1285 | .is_enabled = clk_rcg2_is_enabled, |
1286 | .get_parent = clk_rcg2_get_parent, |
1287 | .determine_rate = clk_rcg2_dfs_determine_rate, |
1288 | .recalc_rate = clk_rcg2_dfs_recalc_rate, |
1289 | }; |
1290 | |
1291 | static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data, |
1292 | struct regmap *regmap) |
1293 | { |
1294 | struct clk_rcg2 *rcg = data->rcg; |
1295 | struct clk_init_data *init = data->init; |
1296 | u32 val; |
1297 | int ret; |
1298 | |
1299 | ret = regmap_read(map: regmap, reg: rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, val: &val); |
1300 | if (ret) |
1301 | return -EINVAL; |
1302 | |
1303 | if (!(val & SE_CMD_DFS_EN)) |
1304 | return 0; |
1305 | |
1306 | /* |
1307 | * Rate changes with consumer writing a register in |
1308 | * their own I/O region |
1309 | */ |
1310 | init->flags |= CLK_GET_RATE_NOCACHE; |
1311 | init->ops = &clk_rcg2_dfs_ops; |
1312 | |
1313 | rcg->freq_tbl = NULL; |
1314 | |
1315 | return 0; |
1316 | } |
1317 | |
1318 | int qcom_cc_register_rcg_dfs(struct regmap *regmap, |
1319 | const struct clk_rcg_dfs_data *rcgs, size_t len) |
1320 | { |
1321 | int i, ret; |
1322 | |
1323 | for (i = 0; i < len; i++) { |
1324 | ret = clk_rcg2_enable_dfs(data: &rcgs[i], regmap); |
1325 | if (ret) |
1326 | return ret; |
1327 | } |
1328 | |
1329 | return 0; |
1330 | } |
1331 | EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs); |
1332 | |
1333 | static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate, |
1334 | unsigned long parent_rate) |
1335 | { |
1336 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1337 | struct freq_tbl f = { 0 }; |
1338 | u32 mask = BIT(rcg->hid_width) - 1; |
1339 | u32 hid_div, cfg; |
1340 | int i, num_parents = clk_hw_get_num_parents(hw); |
1341 | unsigned long num, den; |
1342 | |
1343 | rational_best_approximation(given_numerator: parent_rate, given_denominator: rate, |
1344 | GENMASK(rcg->mnd_width - 1, 0), |
1345 | GENMASK(rcg->mnd_width - 1, 0), best_numerator: &den, best_denominator: &num); |
1346 | |
1347 | if (!num || !den) |
1348 | return -EINVAL; |
1349 | |
1350 | regmap_read(map: rcg->clkr.regmap, reg: rcg->cmd_rcgr + CFG_REG, val: &cfg); |
1351 | hid_div = cfg; |
1352 | cfg &= CFG_SRC_SEL_MASK; |
1353 | cfg >>= CFG_SRC_SEL_SHIFT; |
1354 | |
1355 | for (i = 0; i < num_parents; i++) { |
1356 | if (cfg == rcg->parent_map[i].cfg) { |
1357 | f.src = rcg->parent_map[i].src; |
1358 | break; |
1359 | } |
1360 | } |
1361 | |
1362 | f.pre_div = hid_div; |
1363 | f.pre_div >>= CFG_SRC_DIV_SHIFT; |
1364 | f.pre_div &= mask; |
1365 | |
1366 | if (num != den) { |
1367 | f.m = num; |
1368 | f.n = den; |
1369 | } else { |
1370 | f.m = 0; |
1371 | f.n = 0; |
1372 | } |
1373 | |
1374 | return clk_rcg2_configure(rcg, f: &f); |
1375 | } |
1376 | |
1377 | static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw, |
1378 | unsigned long rate, unsigned long parent_rate, u8 index) |
1379 | { |
1380 | return clk_rcg2_dp_set_rate(hw, rate, parent_rate); |
1381 | } |
1382 | |
1383 | static int clk_rcg2_dp_determine_rate(struct clk_hw *hw, |
1384 | struct clk_rate_request *req) |
1385 | { |
1386 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
1387 | unsigned long num, den; |
1388 | u64 tmp; |
1389 | |
1390 | /* Parent rate is a fixed phy link rate */ |
1391 | rational_best_approximation(given_numerator: req->best_parent_rate, given_denominator: req->rate, |
1392 | GENMASK(rcg->mnd_width - 1, 0), |
1393 | GENMASK(rcg->mnd_width - 1, 0), best_numerator: &den, best_denominator: &num); |
1394 | |
1395 | if (!num || !den) |
1396 | return -EINVAL; |
1397 | |
1398 | tmp = req->best_parent_rate * num; |
1399 | do_div(tmp, den); |
1400 | req->rate = tmp; |
1401 | |
1402 | return 0; |
1403 | } |
1404 | |
1405 | const struct clk_ops clk_dp_ops = { |
1406 | .is_enabled = clk_rcg2_is_enabled, |
1407 | .get_parent = clk_rcg2_get_parent, |
1408 | .set_parent = clk_rcg2_set_parent, |
1409 | .recalc_rate = clk_rcg2_recalc_rate, |
1410 | .set_rate = clk_rcg2_dp_set_rate, |
1411 | .set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent, |
1412 | .determine_rate = clk_rcg2_dp_determine_rate, |
1413 | }; |
1414 | EXPORT_SYMBOL_GPL(clk_dp_ops); |
1415 | |