]>
Commit | Line | Data |
---|---|---|
7ef6f118 | 1 | // SPDX-License-Identifier: GPL-2.0 |
bcd61c0f | 2 | /* |
bdc3bbdd | 3 | * Copyright (c) 2013, 2018, The Linux Foundation. All rights reserved. |
bcd61c0f SB |
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/regmap.h> | |
99cbd064 | 14 | #include <linux/math64.h> |
cc4f6944 | 15 | #include <linux/slab.h> |
bcd61c0f SB |
16 | |
17 | #include <asm/div64.h> | |
18 | ||
19 | #include "clk-rcg.h" | |
50c6a503 | 20 | #include "common.h" |
bcd61c0f SB |
21 | |
22 | #define CMD_REG 0x0 | |
23 | #define CMD_UPDATE BIT(0) | |
24 | #define CMD_ROOT_EN BIT(1) | |
25 | #define CMD_DIRTY_CFG BIT(4) | |
26 | #define CMD_DIRTY_N BIT(5) | |
27 | #define CMD_DIRTY_M BIT(6) | |
28 | #define CMD_DIRTY_D BIT(7) | |
29 | #define CMD_ROOT_OFF BIT(31) | |
30 | ||
31 | #define CFG_REG 0x4 | |
32 | #define CFG_SRC_DIV_SHIFT 0 | |
33 | #define CFG_SRC_SEL_SHIFT 8 | |
34 | #define CFG_SRC_SEL_MASK (0x7 << CFG_SRC_SEL_SHIFT) | |
35 | #define CFG_MODE_SHIFT 12 | |
36 | #define CFG_MODE_MASK (0x3 << CFG_MODE_SHIFT) | |
37 | #define CFG_MODE_DUAL_EDGE (0x2 << CFG_MODE_SHIFT) | |
bdc3bbdd | 38 | #define CFG_HW_CLK_CTRL_MASK BIT(20) |
bcd61c0f SB |
39 | |
40 | #define M_REG 0x8 | |
41 | #define N_REG 0xc | |
42 | #define D_REG 0x10 | |
43 | ||
96dc791d TD |
44 | #define RCG_CFG_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + CFG_REG) |
45 | #define RCG_M_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + M_REG) | |
46 | #define RCG_N_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + N_REG) | |
47 | #define RCG_D_OFFSET(rcg) ((rcg)->cmd_rcgr + (rcg)->cfg_off + D_REG) | |
48 | ||
cc4f6944 TD |
49 | /* Dynamic Frequency Scaling */ |
50 | #define MAX_PERF_LEVEL 8 | |
51 | #define SE_CMD_DFSR_OFFSET 0x14 | |
52 | #define SE_CMD_DFS_EN BIT(0) | |
53 | #define SE_PERF_DFSR(level) (0x1c + 0x4 * (level)) | |
54 | #define SE_PERF_M_DFSR(level) (0x5c + 0x4 * (level)) | |
55 | #define SE_PERF_N_DFSR(level) (0x9c + 0x4 * (level)) | |
56 | ||
081ba802 RN |
57 | enum freq_policy { |
58 | FLOOR, | |
59 | CEIL, | |
60 | }; | |
61 | ||
bcd61c0f SB |
62 | static int clk_rcg2_is_enabled(struct clk_hw *hw) |
63 | { | |
64 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
65 | u32 cmd; | |
66 | int ret; | |
67 | ||
68 | ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd); | |
69 | if (ret) | |
70 | return ret; | |
71 | ||
aa014149 | 72 | return (cmd & CMD_ROOT_OFF) == 0; |
bcd61c0f SB |
73 | } |
74 | ||
75 | static u8 clk_rcg2_get_parent(struct clk_hw *hw) | |
76 | { | |
77 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
497295af | 78 | int num_parents = clk_hw_get_num_parents(hw); |
bcd61c0f SB |
79 | u32 cfg; |
80 | int i, ret; | |
81 | ||
96dc791d | 82 | ret = regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg); |
bcd61c0f | 83 | if (ret) |
7f218978 | 84 | goto err; |
bcd61c0f SB |
85 | |
86 | cfg &= CFG_SRC_SEL_MASK; | |
87 | cfg >>= CFG_SRC_SEL_SHIFT; | |
88 | ||
89 | for (i = 0; i < num_parents; i++) | |
293d2e97 | 90 | if (cfg == rcg->parent_map[i].cfg) |
bcd61c0f SB |
91 | return i; |
92 | ||
7f218978 GD |
93 | err: |
94 | pr_debug("%s: Clock %s has invalid parent, using default.\n", | |
ac269395 | 95 | __func__, clk_hw_get_name(hw)); |
7f218978 | 96 | return 0; |
bcd61c0f SB |
97 | } |
98 | ||
99 | static int update_config(struct clk_rcg2 *rcg) | |
100 | { | |
101 | int count, ret; | |
102 | u32 cmd; | |
103 | struct clk_hw *hw = &rcg->clkr.hw; | |
ac269395 | 104 | const char *name = clk_hw_get_name(hw); |
bcd61c0f SB |
105 | |
106 | ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, | |
107 | CMD_UPDATE, CMD_UPDATE); | |
108 | if (ret) | |
109 | return ret; | |
110 | ||
111 | /* Wait for update to take effect */ | |
112 | for (count = 500; count > 0; count--) { | |
113 | ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd); | |
114 | if (ret) | |
115 | return ret; | |
116 | if (!(cmd & CMD_UPDATE)) | |
117 | return 0; | |
118 | udelay(1); | |
119 | } | |
120 | ||
121 | WARN(1, "%s: rcg didn't update its configuration.", name); | |
122 | return 0; | |
123 | } | |
124 | ||
125 | static int clk_rcg2_set_parent(struct clk_hw *hw, u8 index) | |
126 | { | |
127 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
128 | int ret; | |
293d2e97 | 129 | u32 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; |
bcd61c0f | 130 | |
96dc791d | 131 | ret = regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), |
293d2e97 | 132 | CFG_SRC_SEL_MASK, cfg); |
bcd61c0f SB |
133 | if (ret) |
134 | return ret; | |
135 | ||
136 | return update_config(rcg); | |
137 | } | |
138 | ||
139 | /* | |
140 | * Calculate m/n:d rate | |
141 | * | |
142 | * parent_rate m | |
143 | * rate = ----------- x --- | |
144 | * hid_div n | |
145 | */ | |
146 | static unsigned long | |
147 | calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 hid_div) | |
148 | { | |
149 | if (hid_div) { | |
150 | rate *= 2; | |
151 | rate /= hid_div + 1; | |
152 | } | |
153 | ||
154 | if (mode) { | |
155 | u64 tmp = rate; | |
156 | tmp *= m; | |
157 | do_div(tmp, n); | |
158 | rate = tmp; | |
159 | } | |
160 | ||
161 | return rate; | |
162 | } | |
163 | ||
164 | static unsigned long | |
165 | clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) | |
166 | { | |
167 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
168 | u32 cfg, hid_div, m = 0, n = 0, mode = 0, mask; | |
169 | ||
96dc791d | 170 | regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg); |
bcd61c0f SB |
171 | |
172 | if (rcg->mnd_width) { | |
173 | mask = BIT(rcg->mnd_width) - 1; | |
96dc791d | 174 | regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m); |
bcd61c0f | 175 | m &= mask; |
96dc791d | 176 | regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), &n); |
bcd61c0f SB |
177 | n = ~n; |
178 | n &= mask; | |
179 | n += m; | |
180 | mode = cfg & CFG_MODE_MASK; | |
181 | mode >>= CFG_MODE_SHIFT; | |
182 | } | |
183 | ||
184 | mask = BIT(rcg->hid_width) - 1; | |
185 | hid_div = cfg >> CFG_SRC_DIV_SHIFT; | |
186 | hid_div &= mask; | |
187 | ||
188 | return calc_rate(parent_rate, m, n, mode, hid_div); | |
189 | } | |
190 | ||
081ba802 RN |
191 | static int _freq_tbl_determine_rate(struct clk_hw *hw, const struct freq_tbl *f, |
192 | struct clk_rate_request *req, | |
193 | enum freq_policy policy) | |
bcd61c0f | 194 | { |
0817b62c | 195 | unsigned long clk_flags, rate = req->rate; |
ac269395 | 196 | struct clk_hw *p; |
2f272e7b GD |
197 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
198 | int index; | |
bcd61c0f | 199 | |
081ba802 RN |
200 | switch (policy) { |
201 | case FLOOR: | |
202 | f = qcom_find_freq_floor(f, rate); | |
203 | break; | |
204 | case CEIL: | |
205 | f = qcom_find_freq(f, rate); | |
206 | break; | |
207 | default: | |
208 | return -EINVAL; | |
209 | }; | |
210 | ||
bcd61c0f SB |
211 | if (!f) |
212 | return -EINVAL; | |
213 | ||
2f272e7b GD |
214 | index = qcom_find_src_index(hw, rcg->parent_map, f->src); |
215 | if (index < 0) | |
216 | return index; | |
217 | ||
98d8a60e | 218 | clk_flags = clk_hw_get_flags(hw); |
ac269395 | 219 | p = clk_hw_get_parent_by_index(hw, index); |
bcd61c0f | 220 | if (clk_flags & CLK_SET_RATE_PARENT) { |
c7d2a0eb | 221 | rate = f->freq; |
bcd61c0f SB |
222 | if (f->pre_div) { |
223 | rate /= 2; | |
224 | rate *= f->pre_div + 1; | |
225 | } | |
226 | ||
227 | if (f->n) { | |
228 | u64 tmp = rate; | |
229 | tmp = tmp * f->n; | |
230 | do_div(tmp, f->m); | |
231 | rate = tmp; | |
232 | } | |
233 | } else { | |
ac269395 | 234 | rate = clk_hw_get_rate(p); |
bcd61c0f | 235 | } |
ac269395 | 236 | req->best_parent_hw = p; |
0817b62c BB |
237 | req->best_parent_rate = rate; |
238 | req->rate = f->freq; | |
bcd61c0f | 239 | |
0817b62c | 240 | return 0; |
bcd61c0f SB |
241 | } |
242 | ||
0817b62c BB |
243 | static int clk_rcg2_determine_rate(struct clk_hw *hw, |
244 | struct clk_rate_request *req) | |
bcd61c0f SB |
245 | { |
246 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
247 | ||
081ba802 RN |
248 | return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, CEIL); |
249 | } | |
250 | ||
251 | static int clk_rcg2_determine_floor_rate(struct clk_hw *hw, | |
252 | struct clk_rate_request *req) | |
253 | { | |
254 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
255 | ||
256 | return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, FLOOR); | |
bcd61c0f SB |
257 | } |
258 | ||
7ef6f118 | 259 | static int __clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f) |
bcd61c0f | 260 | { |
bcd61c0f | 261 | u32 cfg, mask; |
293d2e97 GD |
262 | struct clk_hw *hw = &rcg->clkr.hw; |
263 | int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src); | |
264 | ||
265 | if (index < 0) | |
266 | return index; | |
bcd61c0f | 267 | |
bcd61c0f SB |
268 | if (rcg->mnd_width && f->n) { |
269 | mask = BIT(rcg->mnd_width) - 1; | |
99cbd064 | 270 | ret = regmap_update_bits(rcg->clkr.regmap, |
96dc791d | 271 | RCG_M_OFFSET(rcg), mask, f->m); |
bcd61c0f SB |
272 | if (ret) |
273 | return ret; | |
274 | ||
99cbd064 | 275 | ret = regmap_update_bits(rcg->clkr.regmap, |
96dc791d | 276 | RCG_N_OFFSET(rcg), mask, ~(f->n - f->m)); |
bcd61c0f SB |
277 | if (ret) |
278 | return ret; | |
279 | ||
99cbd064 | 280 | ret = regmap_update_bits(rcg->clkr.regmap, |
96dc791d | 281 | RCG_D_OFFSET(rcg), mask, ~f->n); |
bcd61c0f SB |
282 | if (ret) |
283 | return ret; | |
284 | } | |
285 | ||
286 | mask = BIT(rcg->hid_width) - 1; | |
bdc3bbdd | 287 | mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK | CFG_HW_CLK_CTRL_MASK; |
bcd61c0f | 288 | cfg = f->pre_div << CFG_SRC_DIV_SHIFT; |
293d2e97 | 289 | cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; |
0b21503d | 290 | if (rcg->mnd_width && f->n && (f->m != f->n)) |
bcd61c0f | 291 | cfg |= CFG_MODE_DUAL_EDGE; |
96dc791d | 292 | return regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), |
7ef6f118 AN |
293 | mask, cfg); |
294 | } | |
295 | ||
296 | static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f) | |
297 | { | |
298 | int ret; | |
299 | ||
300 | ret = __clk_rcg2_configure(rcg, f); | |
bcd61c0f SB |
301 | if (ret) |
302 | return ret; | |
303 | ||
304 | return update_config(rcg); | |
305 | } | |
306 | ||
081ba802 RN |
307 | static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, |
308 | enum freq_policy policy) | |
99cbd064 SB |
309 | { |
310 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
311 | const struct freq_tbl *f; | |
312 | ||
081ba802 RN |
313 | switch (policy) { |
314 | case FLOOR: | |
315 | f = qcom_find_freq_floor(rcg->freq_tbl, rate); | |
316 | break; | |
317 | case CEIL: | |
318 | f = qcom_find_freq(rcg->freq_tbl, rate); | |
319 | break; | |
320 | default: | |
321 | return -EINVAL; | |
322 | }; | |
323 | ||
99cbd064 SB |
324 | if (!f) |
325 | return -EINVAL; | |
326 | ||
327 | return clk_rcg2_configure(rcg, f); | |
328 | } | |
329 | ||
bcd61c0f SB |
330 | static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, |
331 | unsigned long parent_rate) | |
332 | { | |
081ba802 RN |
333 | return __clk_rcg2_set_rate(hw, rate, CEIL); |
334 | } | |
335 | ||
336 | static int clk_rcg2_set_floor_rate(struct clk_hw *hw, unsigned long rate, | |
337 | unsigned long parent_rate) | |
338 | { | |
339 | return __clk_rcg2_set_rate(hw, rate, FLOOR); | |
bcd61c0f SB |
340 | } |
341 | ||
342 | static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw, | |
343 | unsigned long rate, unsigned long parent_rate, u8 index) | |
344 | { | |
081ba802 RN |
345 | return __clk_rcg2_set_rate(hw, rate, CEIL); |
346 | } | |
347 | ||
348 | static int clk_rcg2_set_floor_rate_and_parent(struct clk_hw *hw, | |
349 | unsigned long rate, unsigned long parent_rate, u8 index) | |
350 | { | |
351 | return __clk_rcg2_set_rate(hw, rate, FLOOR); | |
bcd61c0f SB |
352 | } |
353 | ||
354 | const struct clk_ops clk_rcg2_ops = { | |
355 | .is_enabled = clk_rcg2_is_enabled, | |
356 | .get_parent = clk_rcg2_get_parent, | |
357 | .set_parent = clk_rcg2_set_parent, | |
358 | .recalc_rate = clk_rcg2_recalc_rate, | |
359 | .determine_rate = clk_rcg2_determine_rate, | |
360 | .set_rate = clk_rcg2_set_rate, | |
361 | .set_rate_and_parent = clk_rcg2_set_rate_and_parent, | |
362 | }; | |
363 | EXPORT_SYMBOL_GPL(clk_rcg2_ops); | |
99cbd064 | 364 | |
081ba802 RN |
365 | const struct clk_ops clk_rcg2_floor_ops = { |
366 | .is_enabled = clk_rcg2_is_enabled, | |
367 | .get_parent = clk_rcg2_get_parent, | |
368 | .set_parent = clk_rcg2_set_parent, | |
369 | .recalc_rate = clk_rcg2_recalc_rate, | |
370 | .determine_rate = clk_rcg2_determine_floor_rate, | |
371 | .set_rate = clk_rcg2_set_floor_rate, | |
372 | .set_rate_and_parent = clk_rcg2_set_floor_rate_and_parent, | |
373 | }; | |
374 | EXPORT_SYMBOL_GPL(clk_rcg2_floor_ops); | |
375 | ||
99cbd064 SB |
376 | struct frac_entry { |
377 | int num; | |
378 | int den; | |
379 | }; | |
380 | ||
381 | static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */ | |
382 | { 52, 295 }, /* 119 M */ | |
383 | { 11, 57 }, /* 130.25 M */ | |
384 | { 63, 307 }, /* 138.50 M */ | |
385 | { 11, 50 }, /* 148.50 M */ | |
386 | { 47, 206 }, /* 154 M */ | |
387 | { 31, 100 }, /* 205.25 M */ | |
388 | { 107, 269 }, /* 268.50 M */ | |
389 | { }, | |
390 | }; | |
391 | ||
392 | static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */ | |
393 | { 31, 211 }, /* 119 M */ | |
394 | { 32, 199 }, /* 130.25 M */ | |
395 | { 63, 307 }, /* 138.50 M */ | |
396 | { 11, 60 }, /* 148.50 M */ | |
397 | { 50, 263 }, /* 154 M */ | |
398 | { 31, 120 }, /* 205.25 M */ | |
399 | { 119, 359 }, /* 268.50 M */ | |
400 | { }, | |
401 | }; | |
402 | ||
403 | static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate, | |
404 | unsigned long parent_rate) | |
405 | { | |
406 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
407 | struct freq_tbl f = *rcg->freq_tbl; | |
408 | const struct frac_entry *frac; | |
409 | int delta = 100000; | |
410 | s64 src_rate = parent_rate; | |
411 | s64 request; | |
412 | u32 mask = BIT(rcg->hid_width) - 1; | |
413 | u32 hid_div; | |
414 | ||
415 | if (src_rate == 810000000) | |
416 | frac = frac_table_810m; | |
417 | else | |
418 | frac = frac_table_675m; | |
419 | ||
420 | for (; frac->num; frac++) { | |
421 | request = rate; | |
422 | request *= frac->den; | |
423 | request = div_s64(request, frac->num); | |
424 | if ((src_rate < (request - delta)) || | |
425 | (src_rate > (request + delta))) | |
426 | continue; | |
427 | ||
428 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
429 | &hid_div); | |
430 | f.pre_div = hid_div; | |
431 | f.pre_div >>= CFG_SRC_DIV_SHIFT; | |
432 | f.pre_div &= mask; | |
433 | f.m = frac->num; | |
434 | f.n = frac->den; | |
435 | ||
436 | return clk_rcg2_configure(rcg, &f); | |
437 | } | |
438 | ||
439 | return -EINVAL; | |
440 | } | |
441 | ||
442 | static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw, | |
443 | unsigned long rate, unsigned long parent_rate, u8 index) | |
444 | { | |
445 | /* Parent index is set statically in frequency table */ | |
446 | return clk_edp_pixel_set_rate(hw, rate, parent_rate); | |
447 | } | |
448 | ||
0817b62c BB |
449 | static int clk_edp_pixel_determine_rate(struct clk_hw *hw, |
450 | struct clk_rate_request *req) | |
99cbd064 SB |
451 | { |
452 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
453 | const struct freq_tbl *f = rcg->freq_tbl; | |
454 | const struct frac_entry *frac; | |
455 | int delta = 100000; | |
99cbd064 SB |
456 | s64 request; |
457 | u32 mask = BIT(rcg->hid_width) - 1; | |
458 | u32 hid_div; | |
2f272e7b | 459 | int index = qcom_find_src_index(hw, rcg->parent_map, f->src); |
99cbd064 SB |
460 | |
461 | /* Force the correct parent */ | |
ac269395 SB |
462 | req->best_parent_hw = clk_hw_get_parent_by_index(hw, index); |
463 | req->best_parent_rate = clk_hw_get_rate(req->best_parent_hw); | |
99cbd064 | 464 | |
0817b62c | 465 | if (req->best_parent_rate == 810000000) |
99cbd064 SB |
466 | frac = frac_table_810m; |
467 | else | |
468 | frac = frac_table_675m; | |
469 | ||
470 | for (; frac->num; frac++) { | |
0817b62c | 471 | request = req->rate; |
99cbd064 SB |
472 | request *= frac->den; |
473 | request = div_s64(request, frac->num); | |
0817b62c BB |
474 | if ((req->best_parent_rate < (request - delta)) || |
475 | (req->best_parent_rate > (request + delta))) | |
99cbd064 SB |
476 | continue; |
477 | ||
478 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
479 | &hid_div); | |
480 | hid_div >>= CFG_SRC_DIV_SHIFT; | |
481 | hid_div &= mask; | |
482 | ||
0817b62c BB |
483 | req->rate = calc_rate(req->best_parent_rate, |
484 | frac->num, frac->den, | |
485 | !!frac->den, hid_div); | |
486 | return 0; | |
99cbd064 SB |
487 | } |
488 | ||
489 | return -EINVAL; | |
490 | } | |
491 | ||
492 | const struct clk_ops clk_edp_pixel_ops = { | |
493 | .is_enabled = clk_rcg2_is_enabled, | |
494 | .get_parent = clk_rcg2_get_parent, | |
495 | .set_parent = clk_rcg2_set_parent, | |
496 | .recalc_rate = clk_rcg2_recalc_rate, | |
497 | .set_rate = clk_edp_pixel_set_rate, | |
498 | .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent, | |
499 | .determine_rate = clk_edp_pixel_determine_rate, | |
500 | }; | |
501 | EXPORT_SYMBOL_GPL(clk_edp_pixel_ops); | |
502 | ||
0817b62c BB |
503 | static int clk_byte_determine_rate(struct clk_hw *hw, |
504 | struct clk_rate_request *req) | |
99cbd064 SB |
505 | { |
506 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
507 | const struct freq_tbl *f = rcg->freq_tbl; | |
2f272e7b | 508 | int index = qcom_find_src_index(hw, rcg->parent_map, f->src); |
99cbd064 SB |
509 | unsigned long parent_rate, div; |
510 | u32 mask = BIT(rcg->hid_width) - 1; | |
ac269395 | 511 | struct clk_hw *p; |
99cbd064 | 512 | |
0817b62c | 513 | if (req->rate == 0) |
99cbd064 SB |
514 | return -EINVAL; |
515 | ||
ac269395 SB |
516 | req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index); |
517 | req->best_parent_rate = parent_rate = clk_hw_round_rate(p, req->rate); | |
99cbd064 | 518 | |
0817b62c | 519 | div = DIV_ROUND_UP((2 * parent_rate), req->rate) - 1; |
99cbd064 SB |
520 | div = min_t(u32, div, mask); |
521 | ||
0817b62c BB |
522 | req->rate = calc_rate(parent_rate, 0, 0, 0, div); |
523 | ||
524 | return 0; | |
99cbd064 SB |
525 | } |
526 | ||
527 | static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate, | |
528 | unsigned long parent_rate) | |
529 | { | |
530 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
531 | struct freq_tbl f = *rcg->freq_tbl; | |
532 | unsigned long div; | |
533 | u32 mask = BIT(rcg->hid_width) - 1; | |
534 | ||
535 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; | |
536 | div = min_t(u32, div, mask); | |
537 | ||
538 | f.pre_div = div; | |
539 | ||
540 | return clk_rcg2_configure(rcg, &f); | |
541 | } | |
542 | ||
543 | static int clk_byte_set_rate_and_parent(struct clk_hw *hw, | |
544 | unsigned long rate, unsigned long parent_rate, u8 index) | |
545 | { | |
546 | /* Parent index is set statically in frequency table */ | |
547 | return clk_byte_set_rate(hw, rate, parent_rate); | |
548 | } | |
549 | ||
550 | const struct clk_ops clk_byte_ops = { | |
551 | .is_enabled = clk_rcg2_is_enabled, | |
552 | .get_parent = clk_rcg2_get_parent, | |
553 | .set_parent = clk_rcg2_set_parent, | |
554 | .recalc_rate = clk_rcg2_recalc_rate, | |
555 | .set_rate = clk_byte_set_rate, | |
556 | .set_rate_and_parent = clk_byte_set_rate_and_parent, | |
557 | .determine_rate = clk_byte_determine_rate, | |
558 | }; | |
559 | EXPORT_SYMBOL_GPL(clk_byte_ops); | |
560 | ||
8ee9c7de SB |
561 | static int clk_byte2_determine_rate(struct clk_hw *hw, |
562 | struct clk_rate_request *req) | |
563 | { | |
564 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
565 | unsigned long parent_rate, div; | |
566 | u32 mask = BIT(rcg->hid_width) - 1; | |
567 | struct clk_hw *p; | |
568 | unsigned long rate = req->rate; | |
569 | ||
570 | if (rate == 0) | |
571 | return -EINVAL; | |
572 | ||
573 | p = req->best_parent_hw; | |
574 | req->best_parent_rate = parent_rate = clk_hw_round_rate(p, rate); | |
575 | ||
576 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; | |
577 | div = min_t(u32, div, mask); | |
578 | ||
579 | req->rate = calc_rate(parent_rate, 0, 0, 0, div); | |
580 | ||
581 | return 0; | |
582 | } | |
583 | ||
584 | static int clk_byte2_set_rate(struct clk_hw *hw, unsigned long rate, | |
585 | unsigned long parent_rate) | |
586 | { | |
587 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
588 | struct freq_tbl f = { 0 }; | |
589 | unsigned long div; | |
590 | int i, num_parents = clk_hw_get_num_parents(hw); | |
591 | u32 mask = BIT(rcg->hid_width) - 1; | |
592 | u32 cfg; | |
593 | ||
594 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; | |
595 | div = min_t(u32, div, mask); | |
596 | ||
597 | f.pre_div = div; | |
598 | ||
599 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg); | |
600 | cfg &= CFG_SRC_SEL_MASK; | |
601 | cfg >>= CFG_SRC_SEL_SHIFT; | |
602 | ||
603 | for (i = 0; i < num_parents; i++) { | |
604 | if (cfg == rcg->parent_map[i].cfg) { | |
605 | f.src = rcg->parent_map[i].src; | |
606 | return clk_rcg2_configure(rcg, &f); | |
607 | } | |
608 | } | |
609 | ||
610 | return -EINVAL; | |
611 | } | |
612 | ||
613 | static int clk_byte2_set_rate_and_parent(struct clk_hw *hw, | |
614 | unsigned long rate, unsigned long parent_rate, u8 index) | |
615 | { | |
616 | /* Read the hardware to determine parent during set_rate */ | |
617 | return clk_byte2_set_rate(hw, rate, parent_rate); | |
618 | } | |
619 | ||
620 | const struct clk_ops clk_byte2_ops = { | |
621 | .is_enabled = clk_rcg2_is_enabled, | |
622 | .get_parent = clk_rcg2_get_parent, | |
623 | .set_parent = clk_rcg2_set_parent, | |
624 | .recalc_rate = clk_rcg2_recalc_rate, | |
625 | .set_rate = clk_byte2_set_rate, | |
626 | .set_rate_and_parent = clk_byte2_set_rate_and_parent, | |
627 | .determine_rate = clk_byte2_determine_rate, | |
628 | }; | |
629 | EXPORT_SYMBOL_GPL(clk_byte2_ops); | |
630 | ||
99cbd064 SB |
631 | static const struct frac_entry frac_table_pixel[] = { |
632 | { 3, 8 }, | |
633 | { 2, 9 }, | |
634 | { 4, 9 }, | |
635 | { 1, 1 }, | |
636 | { } | |
637 | }; | |
638 | ||
0817b62c BB |
639 | static int clk_pixel_determine_rate(struct clk_hw *hw, |
640 | struct clk_rate_request *req) | |
99cbd064 | 641 | { |
99cbd064 SB |
642 | unsigned long request, src_rate; |
643 | int delta = 100000; | |
99cbd064 | 644 | const struct frac_entry *frac = frac_table_pixel; |
99cbd064 SB |
645 | |
646 | for (; frac->num; frac++) { | |
0817b62c | 647 | request = (req->rate * frac->den) / frac->num; |
99cbd064 | 648 | |
ac269395 | 649 | src_rate = clk_hw_round_rate(req->best_parent_hw, request); |
99cbd064 SB |
650 | if ((src_rate < (request - delta)) || |
651 | (src_rate > (request + delta))) | |
652 | continue; | |
653 | ||
0817b62c BB |
654 | req->best_parent_rate = src_rate; |
655 | req->rate = (src_rate * frac->num) / frac->den; | |
656 | return 0; | |
99cbd064 SB |
657 | } |
658 | ||
659 | return -EINVAL; | |
660 | } | |
661 | ||
662 | static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate, | |
663 | unsigned long parent_rate) | |
664 | { | |
665 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
8ee9c7de | 666 | struct freq_tbl f = { 0 }; |
99cbd064 | 667 | const struct frac_entry *frac = frac_table_pixel; |
6d451367 | 668 | unsigned long request; |
99cbd064 SB |
669 | int delta = 100000; |
670 | u32 mask = BIT(rcg->hid_width) - 1; | |
8ee9c7de SB |
671 | u32 hid_div, cfg; |
672 | int i, num_parents = clk_hw_get_num_parents(hw); | |
673 | ||
674 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg); | |
675 | cfg &= CFG_SRC_SEL_MASK; | |
676 | cfg >>= CFG_SRC_SEL_SHIFT; | |
677 | ||
678 | for (i = 0; i < num_parents; i++) | |
679 | if (cfg == rcg->parent_map[i].cfg) { | |
680 | f.src = rcg->parent_map[i].src; | |
681 | break; | |
682 | } | |
99cbd064 SB |
683 | |
684 | for (; frac->num; frac++) { | |
685 | request = (rate * frac->den) / frac->num; | |
686 | ||
6d451367 HL |
687 | if ((parent_rate < (request - delta)) || |
688 | (parent_rate > (request + delta))) | |
99cbd064 SB |
689 | continue; |
690 | ||
691 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
692 | &hid_div); | |
693 | f.pre_div = hid_div; | |
694 | f.pre_div >>= CFG_SRC_DIV_SHIFT; | |
695 | f.pre_div &= mask; | |
696 | f.m = frac->num; | |
697 | f.n = frac->den; | |
698 | ||
699 | return clk_rcg2_configure(rcg, &f); | |
700 | } | |
701 | return -EINVAL; | |
702 | } | |
703 | ||
704 | static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, | |
705 | unsigned long parent_rate, u8 index) | |
706 | { | |
99cbd064 SB |
707 | return clk_pixel_set_rate(hw, rate, parent_rate); |
708 | } | |
709 | ||
710 | const struct clk_ops clk_pixel_ops = { | |
711 | .is_enabled = clk_rcg2_is_enabled, | |
712 | .get_parent = clk_rcg2_get_parent, | |
713 | .set_parent = clk_rcg2_set_parent, | |
714 | .recalc_rate = clk_rcg2_recalc_rate, | |
715 | .set_rate = clk_pixel_set_rate, | |
716 | .set_rate_and_parent = clk_pixel_set_rate_and_parent, | |
717 | .determine_rate = clk_pixel_determine_rate, | |
718 | }; | |
719 | EXPORT_SYMBOL_GPL(clk_pixel_ops); | |
55213e1a SB |
720 | |
721 | static int clk_gfx3d_determine_rate(struct clk_hw *hw, | |
722 | struct clk_rate_request *req) | |
723 | { | |
724 | struct clk_rate_request parent_req = { }; | |
725 | struct clk_hw *p2, *p8, *p9, *xo; | |
726 | unsigned long p9_rate; | |
727 | int ret; | |
728 | ||
729 | xo = clk_hw_get_parent_by_index(hw, 0); | |
730 | if (req->rate == clk_hw_get_rate(xo)) { | |
731 | req->best_parent_hw = xo; | |
732 | return 0; | |
733 | } | |
734 | ||
735 | p9 = clk_hw_get_parent_by_index(hw, 2); | |
736 | p2 = clk_hw_get_parent_by_index(hw, 3); | |
737 | p8 = clk_hw_get_parent_by_index(hw, 4); | |
738 | ||
739 | /* PLL9 is a fixed rate PLL */ | |
740 | p9_rate = clk_hw_get_rate(p9); | |
741 | ||
742 | parent_req.rate = req->rate = min(req->rate, p9_rate); | |
743 | if (req->rate == p9_rate) { | |
744 | req->rate = req->best_parent_rate = p9_rate; | |
745 | req->best_parent_hw = p9; | |
746 | return 0; | |
747 | } | |
748 | ||
749 | if (req->best_parent_hw == p9) { | |
750 | /* Are we going back to a previously used rate? */ | |
751 | if (clk_hw_get_rate(p8) == req->rate) | |
752 | req->best_parent_hw = p8; | |
753 | else | |
754 | req->best_parent_hw = p2; | |
755 | } else if (req->best_parent_hw == p8) { | |
756 | req->best_parent_hw = p2; | |
757 | } else { | |
758 | req->best_parent_hw = p8; | |
759 | } | |
760 | ||
761 | ret = __clk_determine_rate(req->best_parent_hw, &parent_req); | |
762 | if (ret) | |
763 | return ret; | |
764 | ||
765 | req->rate = req->best_parent_rate = parent_req.rate; | |
766 | ||
767 | return 0; | |
768 | } | |
769 | ||
770 | static int clk_gfx3d_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, | |
771 | unsigned long parent_rate, u8 index) | |
772 | { | |
773 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
774 | u32 cfg; | |
775 | int ret; | |
776 | ||
777 | /* Just mux it, we don't use the division or m/n hardware */ | |
778 | cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; | |
779 | ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg); | |
780 | if (ret) | |
781 | return ret; | |
782 | ||
783 | return update_config(rcg); | |
784 | } | |
785 | ||
786 | static int clk_gfx3d_set_rate(struct clk_hw *hw, unsigned long rate, | |
787 | unsigned long parent_rate) | |
788 | { | |
789 | /* | |
790 | * We should never get here; clk_gfx3d_determine_rate() should always | |
791 | * make us use a different parent than what we're currently using, so | |
792 | * clk_gfx3d_set_rate_and_parent() should always be called. | |
793 | */ | |
794 | return 0; | |
795 | } | |
796 | ||
797 | const struct clk_ops clk_gfx3d_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_gfx3d_set_rate, | |
803 | .set_rate_and_parent = clk_gfx3d_set_rate_and_parent, | |
804 | .determine_rate = clk_gfx3d_determine_rate, | |
805 | }; | |
806 | EXPORT_SYMBOL_GPL(clk_gfx3d_ops); | |
7ef6f118 AN |
807 | |
808 | static int clk_rcg2_set_force_enable(struct clk_hw *hw) | |
809 | { | |
810 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
811 | const char *name = clk_hw_get_name(hw); | |
812 | int ret, count; | |
813 | ||
814 | ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, | |
815 | CMD_ROOT_EN, CMD_ROOT_EN); | |
816 | if (ret) | |
817 | return ret; | |
818 | ||
819 | /* wait for RCG to turn ON */ | |
820 | for (count = 500; count > 0; count--) { | |
821 | if (clk_rcg2_is_enabled(hw)) | |
822 | return 0; | |
823 | ||
824 | udelay(1); | |
825 | } | |
826 | ||
827 | pr_err("%s: RCG did not turn on\n", name); | |
828 | return -ETIMEDOUT; | |
829 | } | |
830 | ||
831 | static int clk_rcg2_clear_force_enable(struct clk_hw *hw) | |
832 | { | |
833 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
834 | ||
835 | return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, | |
836 | CMD_ROOT_EN, 0); | |
837 | } | |
838 | ||
839 | static int | |
840 | clk_rcg2_shared_force_enable_clear(struct clk_hw *hw, const struct freq_tbl *f) | |
841 | { | |
842 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
843 | int ret; | |
844 | ||
845 | ret = clk_rcg2_set_force_enable(hw); | |
846 | if (ret) | |
847 | return ret; | |
848 | ||
849 | ret = clk_rcg2_configure(rcg, f); | |
850 | if (ret) | |
851 | return ret; | |
852 | ||
853 | return clk_rcg2_clear_force_enable(hw); | |
854 | } | |
855 | ||
856 | static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate, | |
857 | unsigned long parent_rate) | |
858 | { | |
859 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
860 | const struct freq_tbl *f; | |
861 | ||
862 | f = qcom_find_freq(rcg->freq_tbl, rate); | |
863 | if (!f) | |
864 | return -EINVAL; | |
865 | ||
866 | /* | |
867 | * In case clock is disabled, update the CFG, M, N and D registers | |
868 | * and don't hit the update bit of CMD register. | |
869 | */ | |
870 | if (!__clk_is_enabled(hw->clk)) | |
871 | return __clk_rcg2_configure(rcg, f); | |
872 | ||
873 | return clk_rcg2_shared_force_enable_clear(hw, f); | |
874 | } | |
875 | ||
876 | static int clk_rcg2_shared_set_rate_and_parent(struct clk_hw *hw, | |
877 | unsigned long rate, unsigned long parent_rate, u8 index) | |
878 | { | |
879 | return clk_rcg2_shared_set_rate(hw, rate, parent_rate); | |
880 | } | |
881 | ||
882 | static int clk_rcg2_shared_enable(struct clk_hw *hw) | |
883 | { | |
884 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
885 | int ret; | |
886 | ||
887 | /* | |
888 | * Set the update bit because required configuration has already | |
889 | * been written in clk_rcg2_shared_set_rate() | |
890 | */ | |
891 | ret = clk_rcg2_set_force_enable(hw); | |
892 | if (ret) | |
893 | return ret; | |
894 | ||
895 | ret = update_config(rcg); | |
896 | if (ret) | |
897 | return ret; | |
898 | ||
899 | return clk_rcg2_clear_force_enable(hw); | |
900 | } | |
901 | ||
902 | static void clk_rcg2_shared_disable(struct clk_hw *hw) | |
903 | { | |
904 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
905 | u32 cfg; | |
906 | ||
907 | /* | |
908 | * Store current configuration as switching to safe source would clear | |
909 | * the SRC and DIV of CFG register | |
910 | */ | |
911 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg); | |
912 | ||
913 | /* | |
914 | * Park the RCG at a safe configuration - sourced off of safe source. | |
915 | * Force enable and disable the RCG while configuring it to safeguard | |
916 | * against any update signal coming from the downstream clock. | |
917 | * The current parent is still prepared and enabled at this point, and | |
918 | * the safe source is always on while application processor subsystem | |
919 | * is online. Therefore, the RCG can safely switch its parent. | |
920 | */ | |
921 | clk_rcg2_set_force_enable(hw); | |
922 | ||
923 | regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
924 | rcg->safe_src_index << CFG_SRC_SEL_SHIFT); | |
925 | ||
926 | update_config(rcg); | |
927 | ||
928 | clk_rcg2_clear_force_enable(hw); | |
929 | ||
930 | /* Write back the stored configuration corresponding to current rate */ | |
931 | regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg); | |
932 | } | |
933 | ||
934 | const struct clk_ops clk_rcg2_shared_ops = { | |
935 | .enable = clk_rcg2_shared_enable, | |
936 | .disable = clk_rcg2_shared_disable, | |
937 | .get_parent = clk_rcg2_get_parent, | |
938 | .set_parent = clk_rcg2_set_parent, | |
939 | .recalc_rate = clk_rcg2_recalc_rate, | |
940 | .determine_rate = clk_rcg2_determine_rate, | |
941 | .set_rate = clk_rcg2_shared_set_rate, | |
942 | .set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent, | |
943 | }; | |
944 | EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops); | |
cc4f6944 TD |
945 | |
946 | /* Common APIs to be used for DFS based RCGR */ | |
947 | static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l, | |
948 | struct freq_tbl *f) | |
949 | { | |
950 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
951 | struct clk_hw *p; | |
952 | unsigned long prate = 0; | |
953 | u32 val, mask, cfg, mode; | |
954 | int i, num_parents; | |
955 | ||
956 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg); | |
957 | ||
958 | mask = BIT(rcg->hid_width) - 1; | |
959 | f->pre_div = 1; | |
960 | if (cfg & mask) | |
961 | f->pre_div = cfg & mask; | |
962 | ||
963 | cfg &= CFG_SRC_SEL_MASK; | |
964 | cfg >>= CFG_SRC_SEL_SHIFT; | |
965 | ||
966 | num_parents = clk_hw_get_num_parents(hw); | |
967 | for (i = 0; i < num_parents; i++) { | |
968 | if (cfg == rcg->parent_map[i].cfg) { | |
969 | f->src = rcg->parent_map[i].src; | |
970 | p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i); | |
971 | prate = clk_hw_get_rate(p); | |
972 | } | |
973 | } | |
974 | ||
975 | mode = cfg & CFG_MODE_MASK; | |
976 | mode >>= CFG_MODE_SHIFT; | |
977 | if (mode) { | |
978 | mask = BIT(rcg->mnd_width) - 1; | |
979 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l), | |
980 | &val); | |
981 | val &= mask; | |
982 | f->m = val; | |
983 | ||
984 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l), | |
985 | &val); | |
986 | val = ~val; | |
987 | val &= mask; | |
988 | val += f->m; | |
989 | f->n = val; | |
990 | } | |
991 | ||
992 | f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div); | |
993 | } | |
994 | ||
995 | static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg) | |
996 | { | |
997 | struct freq_tbl *freq_tbl; | |
998 | int i; | |
999 | ||
2bdb2a1c DA |
1000 | /* Allocate space for 1 extra since table is NULL terminated */ |
1001 | freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL); | |
cc4f6944 TD |
1002 | if (!freq_tbl) |
1003 | return -ENOMEM; | |
1004 | rcg->freq_tbl = freq_tbl; | |
1005 | ||
1006 | for (i = 0; i < MAX_PERF_LEVEL; i++) | |
1007 | clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i); | |
1008 | ||
1009 | return 0; | |
1010 | } | |
1011 | ||
1012 | static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw, | |
1013 | struct clk_rate_request *req) | |
1014 | { | |
1015 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
1016 | int ret; | |
1017 | ||
1018 | if (!rcg->freq_tbl) { | |
1019 | ret = clk_rcg2_dfs_populate_freq_table(rcg); | |
1020 | if (ret) { | |
1021 | pr_err("Failed to update DFS tables for %s\n", | |
1022 | clk_hw_get_name(hw)); | |
1023 | return ret; | |
1024 | } | |
1025 | } | |
1026 | ||
1027 | return clk_rcg2_determine_rate(hw, req); | |
1028 | } | |
1029 | ||
1030 | static unsigned long | |
1031 | clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) | |
1032 | { | |
1033 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
1034 | u32 level, mask, cfg, m = 0, n = 0, mode, pre_div; | |
1035 | ||
1036 | regmap_read(rcg->clkr.regmap, | |
1037 | rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level); | |
1038 | level &= GENMASK(4, 1); | |
1039 | level >>= 1; | |
1040 | ||
1041 | if (rcg->freq_tbl) | |
1042 | return rcg->freq_tbl[level].freq; | |
1043 | ||
1044 | /* | |
1045 | * Assume that parent_rate is actually the parent because | |
1046 | * we can't do any better at figuring it out when the table | |
1047 | * hasn't been populated yet. We only populate the table | |
1048 | * in determine_rate because we can't guarantee the parents | |
1049 | * will be registered with the framework until then. | |
1050 | */ | |
1051 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level), | |
1052 | &cfg); | |
1053 | ||
1054 | mask = BIT(rcg->hid_width) - 1; | |
1055 | pre_div = 1; | |
1056 | if (cfg & mask) | |
1057 | pre_div = cfg & mask; | |
1058 | ||
1059 | mode = cfg & CFG_MODE_MASK; | |
1060 | mode >>= CFG_MODE_SHIFT; | |
1061 | if (mode) { | |
1062 | mask = BIT(rcg->mnd_width) - 1; | |
1063 | regmap_read(rcg->clkr.regmap, | |
1064 | rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m); | |
1065 | m &= mask; | |
1066 | ||
1067 | regmap_read(rcg->clkr.regmap, | |
1068 | rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n); | |
1069 | n = ~n; | |
1070 | n &= mask; | |
1071 | n += m; | |
1072 | } | |
1073 | ||
1074 | return calc_rate(parent_rate, m, n, mode, pre_div); | |
1075 | } | |
1076 | ||
1077 | static const struct clk_ops clk_rcg2_dfs_ops = { | |
1078 | .is_enabled = clk_rcg2_is_enabled, | |
1079 | .get_parent = clk_rcg2_get_parent, | |
1080 | .determine_rate = clk_rcg2_dfs_determine_rate, | |
1081 | .recalc_rate = clk_rcg2_dfs_recalc_rate, | |
1082 | }; | |
1083 | ||
1084 | static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data, | |
1085 | struct regmap *regmap) | |
1086 | { | |
1087 | struct clk_rcg2 *rcg = data->rcg; | |
1088 | struct clk_init_data *init = data->init; | |
1089 | u32 val; | |
1090 | int ret; | |
1091 | ||
1092 | ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val); | |
1093 | if (ret) | |
1094 | return -EINVAL; | |
1095 | ||
1096 | if (!(val & SE_CMD_DFS_EN)) | |
1097 | return 0; | |
1098 | ||
1099 | /* | |
1100 | * Rate changes with consumer writing a register in | |
1101 | * their own I/O region | |
1102 | */ | |
1103 | init->flags |= CLK_GET_RATE_NOCACHE; | |
1104 | init->ops = &clk_rcg2_dfs_ops; | |
1105 | ||
1106 | rcg->freq_tbl = NULL; | |
1107 | ||
1108 | pr_debug("DFS registered for clk %s\n", init->name); | |
1109 | ||
1110 | return 0; | |
1111 | } | |
1112 | ||
1113 | int qcom_cc_register_rcg_dfs(struct regmap *regmap, | |
1114 | const struct clk_rcg_dfs_data *rcgs, size_t len) | |
1115 | { | |
1116 | int i, ret; | |
1117 | ||
1118 | for (i = 0; i < len; i++) { | |
1119 | ret = clk_rcg2_enable_dfs(&rcgs[i], regmap); | |
1120 | if (ret) { | |
1121 | const char *name = rcgs[i].init->name; | |
1122 | ||
1123 | pr_err("DFS register failed for clk %s\n", name); | |
1124 | return ret; | |
1125 | } | |
1126 | } | |
1127 | ||
1128 | return 0; | |
1129 | } | |
1130 | EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs); |