]>
Commit | Line | Data |
---|---|---|
bcd61c0f SB |
1 | /* |
2 | * Copyright (c) 2013, The Linux Foundation. All rights reserved. | |
3 | * | |
4 | * This software is licensed under the terms of the GNU General Public | |
5 | * License version 2, as published by the Free Software Foundation, and | |
6 | * may be copied, distributed, and modified under those terms. | |
7 | * | |
8 | * This program is distributed in the hope that it will be useful, | |
9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
11 | * GNU General Public License for more details. | |
12 | */ | |
13 | ||
14 | #include <linux/kernel.h> | |
15 | #include <linux/bitops.h> | |
16 | #include <linux/err.h> | |
17 | #include <linux/bug.h> | |
18 | #include <linux/export.h> | |
19 | #include <linux/clk-provider.h> | |
20 | #include <linux/delay.h> | |
21 | #include <linux/regmap.h> | |
99cbd064 | 22 | #include <linux/math64.h> |
bcd61c0f SB |
23 | |
24 | #include <asm/div64.h> | |
25 | ||
26 | #include "clk-rcg.h" | |
50c6a503 | 27 | #include "common.h" |
bcd61c0f SB |
28 | |
29 | #define CMD_REG 0x0 | |
30 | #define CMD_UPDATE BIT(0) | |
31 | #define CMD_ROOT_EN BIT(1) | |
32 | #define CMD_DIRTY_CFG BIT(4) | |
33 | #define CMD_DIRTY_N BIT(5) | |
34 | #define CMD_DIRTY_M BIT(6) | |
35 | #define CMD_DIRTY_D BIT(7) | |
36 | #define CMD_ROOT_OFF BIT(31) | |
37 | ||
38 | #define CFG_REG 0x4 | |
39 | #define CFG_SRC_DIV_SHIFT 0 | |
40 | #define CFG_SRC_SEL_SHIFT 8 | |
41 | #define CFG_SRC_SEL_MASK (0x7 << CFG_SRC_SEL_SHIFT) | |
42 | #define CFG_MODE_SHIFT 12 | |
43 | #define CFG_MODE_MASK (0x3 << CFG_MODE_SHIFT) | |
44 | #define CFG_MODE_DUAL_EDGE (0x2 << CFG_MODE_SHIFT) | |
45 | ||
46 | #define M_REG 0x8 | |
47 | #define N_REG 0xc | |
48 | #define D_REG 0x10 | |
49 | ||
50 | static int clk_rcg2_is_enabled(struct clk_hw *hw) | |
51 | { | |
52 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
53 | u32 cmd; | |
54 | int ret; | |
55 | ||
56 | ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd); | |
57 | if (ret) | |
58 | return ret; | |
59 | ||
aa014149 | 60 | return (cmd & CMD_ROOT_OFF) == 0; |
bcd61c0f SB |
61 | } |
62 | ||
63 | static u8 clk_rcg2_get_parent(struct clk_hw *hw) | |
64 | { | |
65 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
66 | int num_parents = __clk_get_num_parents(hw->clk); | |
67 | u32 cfg; | |
68 | int i, ret; | |
69 | ||
70 | ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg); | |
71 | if (ret) | |
7f218978 | 72 | goto err; |
bcd61c0f SB |
73 | |
74 | cfg &= CFG_SRC_SEL_MASK; | |
75 | cfg >>= CFG_SRC_SEL_SHIFT; | |
76 | ||
77 | for (i = 0; i < num_parents; i++) | |
293d2e97 | 78 | if (cfg == rcg->parent_map[i].cfg) |
bcd61c0f SB |
79 | return i; |
80 | ||
7f218978 GD |
81 | err: |
82 | pr_debug("%s: Clock %s has invalid parent, using default.\n", | |
83 | __func__, __clk_get_name(hw->clk)); | |
84 | return 0; | |
bcd61c0f SB |
85 | } |
86 | ||
87 | static int update_config(struct clk_rcg2 *rcg) | |
88 | { | |
89 | int count, ret; | |
90 | u32 cmd; | |
91 | struct clk_hw *hw = &rcg->clkr.hw; | |
92 | const char *name = __clk_get_name(hw->clk); | |
93 | ||
94 | ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, | |
95 | CMD_UPDATE, CMD_UPDATE); | |
96 | if (ret) | |
97 | return ret; | |
98 | ||
99 | /* Wait for update to take effect */ | |
100 | for (count = 500; count > 0; count--) { | |
101 | ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd); | |
102 | if (ret) | |
103 | return ret; | |
104 | if (!(cmd & CMD_UPDATE)) | |
105 | return 0; | |
106 | udelay(1); | |
107 | } | |
108 | ||
109 | WARN(1, "%s: rcg didn't update its configuration.", name); | |
110 | return 0; | |
111 | } | |
112 | ||
113 | static int clk_rcg2_set_parent(struct clk_hw *hw, u8 index) | |
114 | { | |
115 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
116 | int ret; | |
293d2e97 | 117 | u32 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; |
bcd61c0f SB |
118 | |
119 | ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
293d2e97 | 120 | CFG_SRC_SEL_MASK, cfg); |
bcd61c0f SB |
121 | if (ret) |
122 | return ret; | |
123 | ||
124 | return update_config(rcg); | |
125 | } | |
126 | ||
127 | /* | |
128 | * Calculate m/n:d rate | |
129 | * | |
130 | * parent_rate m | |
131 | * rate = ----------- x --- | |
132 | * hid_div n | |
133 | */ | |
134 | static unsigned long | |
135 | calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 hid_div) | |
136 | { | |
137 | if (hid_div) { | |
138 | rate *= 2; | |
139 | rate /= hid_div + 1; | |
140 | } | |
141 | ||
142 | if (mode) { | |
143 | u64 tmp = rate; | |
144 | tmp *= m; | |
145 | do_div(tmp, n); | |
146 | rate = tmp; | |
147 | } | |
148 | ||
149 | return rate; | |
150 | } | |
151 | ||
152 | static unsigned long | |
153 | clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) | |
154 | { | |
155 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
156 | u32 cfg, hid_div, m = 0, n = 0, mode = 0, mask; | |
157 | ||
158 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg); | |
159 | ||
160 | if (rcg->mnd_width) { | |
161 | mask = BIT(rcg->mnd_width) - 1; | |
162 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + M_REG, &m); | |
163 | m &= mask; | |
164 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + N_REG, &n); | |
165 | n = ~n; | |
166 | n &= mask; | |
167 | n += m; | |
168 | mode = cfg & CFG_MODE_MASK; | |
169 | mode >>= CFG_MODE_SHIFT; | |
170 | } | |
171 | ||
172 | mask = BIT(rcg->hid_width) - 1; | |
173 | hid_div = cfg >> CFG_SRC_DIV_SHIFT; | |
174 | hid_div &= mask; | |
175 | ||
176 | return calc_rate(parent_rate, m, n, mode, hid_div); | |
177 | } | |
178 | ||
bcd61c0f SB |
179 | static long _freq_tbl_determine_rate(struct clk_hw *hw, |
180 | const struct freq_tbl *f, unsigned long rate, | |
646cafc6 | 181 | unsigned long *p_rate, struct clk_hw **p_hw) |
bcd61c0f SB |
182 | { |
183 | unsigned long clk_flags; | |
646cafc6 | 184 | struct clk *p; |
2f272e7b GD |
185 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); |
186 | int index; | |
bcd61c0f | 187 | |
50c6a503 | 188 | f = qcom_find_freq(f, rate); |
bcd61c0f SB |
189 | if (!f) |
190 | return -EINVAL; | |
191 | ||
2f272e7b GD |
192 | index = qcom_find_src_index(hw, rcg->parent_map, f->src); |
193 | if (index < 0) | |
194 | return index; | |
195 | ||
bcd61c0f | 196 | clk_flags = __clk_get_flags(hw->clk); |
2f272e7b | 197 | p = clk_get_parent_by_index(hw->clk, index); |
bcd61c0f SB |
198 | if (clk_flags & CLK_SET_RATE_PARENT) { |
199 | if (f->pre_div) { | |
200 | rate /= 2; | |
201 | rate *= f->pre_div + 1; | |
202 | } | |
203 | ||
204 | if (f->n) { | |
205 | u64 tmp = rate; | |
206 | tmp = tmp * f->n; | |
207 | do_div(tmp, f->m); | |
208 | rate = tmp; | |
209 | } | |
210 | } else { | |
646cafc6 | 211 | rate = __clk_get_rate(p); |
bcd61c0f | 212 | } |
646cafc6 | 213 | *p_hw = __clk_get_hw(p); |
bcd61c0f SB |
214 | *p_rate = rate; |
215 | ||
216 | return f->freq; | |
217 | } | |
218 | ||
219 | static long clk_rcg2_determine_rate(struct clk_hw *hw, unsigned long rate, | |
1c8e6004 | 220 | unsigned long min_rate, unsigned long max_rate, |
646cafc6 | 221 | unsigned long *p_rate, struct clk_hw **p) |
bcd61c0f SB |
222 | { |
223 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
224 | ||
225 | return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, p_rate, p); | |
226 | } | |
227 | ||
99cbd064 | 228 | static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f) |
bcd61c0f | 229 | { |
bcd61c0f | 230 | u32 cfg, mask; |
293d2e97 GD |
231 | struct clk_hw *hw = &rcg->clkr.hw; |
232 | int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src); | |
233 | ||
234 | if (index < 0) | |
235 | return index; | |
bcd61c0f | 236 | |
bcd61c0f SB |
237 | if (rcg->mnd_width && f->n) { |
238 | mask = BIT(rcg->mnd_width) - 1; | |
99cbd064 SB |
239 | ret = regmap_update_bits(rcg->clkr.regmap, |
240 | rcg->cmd_rcgr + M_REG, mask, f->m); | |
bcd61c0f SB |
241 | if (ret) |
242 | return ret; | |
243 | ||
99cbd064 SB |
244 | ret = regmap_update_bits(rcg->clkr.regmap, |
245 | rcg->cmd_rcgr + N_REG, mask, ~(f->n - f->m)); | |
bcd61c0f SB |
246 | if (ret) |
247 | return ret; | |
248 | ||
99cbd064 SB |
249 | ret = regmap_update_bits(rcg->clkr.regmap, |
250 | rcg->cmd_rcgr + D_REG, mask, ~f->n); | |
bcd61c0f SB |
251 | if (ret) |
252 | return ret; | |
253 | } | |
254 | ||
255 | mask = BIT(rcg->hid_width) - 1; | |
256 | mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK; | |
257 | cfg = f->pre_div << CFG_SRC_DIV_SHIFT; | |
293d2e97 | 258 | cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; |
0b21503d | 259 | if (rcg->mnd_width && f->n && (f->m != f->n)) |
bcd61c0f | 260 | cfg |= CFG_MODE_DUAL_EDGE; |
99cbd064 SB |
261 | ret = regmap_update_bits(rcg->clkr.regmap, |
262 | rcg->cmd_rcgr + CFG_REG, mask, cfg); | |
bcd61c0f SB |
263 | if (ret) |
264 | return ret; | |
265 | ||
266 | return update_config(rcg); | |
267 | } | |
268 | ||
99cbd064 SB |
269 | static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate) |
270 | { | |
271 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
272 | const struct freq_tbl *f; | |
273 | ||
50c6a503 | 274 | f = qcom_find_freq(rcg->freq_tbl, rate); |
99cbd064 SB |
275 | if (!f) |
276 | return -EINVAL; | |
277 | ||
278 | return clk_rcg2_configure(rcg, f); | |
279 | } | |
280 | ||
bcd61c0f SB |
281 | static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, |
282 | unsigned long parent_rate) | |
283 | { | |
284 | return __clk_rcg2_set_rate(hw, rate); | |
285 | } | |
286 | ||
287 | static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw, | |
288 | unsigned long rate, unsigned long parent_rate, u8 index) | |
289 | { | |
290 | return __clk_rcg2_set_rate(hw, rate); | |
291 | } | |
292 | ||
293 | const struct clk_ops clk_rcg2_ops = { | |
294 | .is_enabled = clk_rcg2_is_enabled, | |
295 | .get_parent = clk_rcg2_get_parent, | |
296 | .set_parent = clk_rcg2_set_parent, | |
297 | .recalc_rate = clk_rcg2_recalc_rate, | |
298 | .determine_rate = clk_rcg2_determine_rate, | |
299 | .set_rate = clk_rcg2_set_rate, | |
300 | .set_rate_and_parent = clk_rcg2_set_rate_and_parent, | |
301 | }; | |
302 | EXPORT_SYMBOL_GPL(clk_rcg2_ops); | |
99cbd064 SB |
303 | |
304 | struct frac_entry { | |
305 | int num; | |
306 | int den; | |
307 | }; | |
308 | ||
309 | static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */ | |
310 | { 52, 295 }, /* 119 M */ | |
311 | { 11, 57 }, /* 130.25 M */ | |
312 | { 63, 307 }, /* 138.50 M */ | |
313 | { 11, 50 }, /* 148.50 M */ | |
314 | { 47, 206 }, /* 154 M */ | |
315 | { 31, 100 }, /* 205.25 M */ | |
316 | { 107, 269 }, /* 268.50 M */ | |
317 | { }, | |
318 | }; | |
319 | ||
320 | static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */ | |
321 | { 31, 211 }, /* 119 M */ | |
322 | { 32, 199 }, /* 130.25 M */ | |
323 | { 63, 307 }, /* 138.50 M */ | |
324 | { 11, 60 }, /* 148.50 M */ | |
325 | { 50, 263 }, /* 154 M */ | |
326 | { 31, 120 }, /* 205.25 M */ | |
327 | { 119, 359 }, /* 268.50 M */ | |
328 | { }, | |
329 | }; | |
330 | ||
331 | static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate, | |
332 | unsigned long parent_rate) | |
333 | { | |
334 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
335 | struct freq_tbl f = *rcg->freq_tbl; | |
336 | const struct frac_entry *frac; | |
337 | int delta = 100000; | |
338 | s64 src_rate = parent_rate; | |
339 | s64 request; | |
340 | u32 mask = BIT(rcg->hid_width) - 1; | |
341 | u32 hid_div; | |
342 | ||
343 | if (src_rate == 810000000) | |
344 | frac = frac_table_810m; | |
345 | else | |
346 | frac = frac_table_675m; | |
347 | ||
348 | for (; frac->num; frac++) { | |
349 | request = rate; | |
350 | request *= frac->den; | |
351 | request = div_s64(request, frac->num); | |
352 | if ((src_rate < (request - delta)) || | |
353 | (src_rate > (request + delta))) | |
354 | continue; | |
355 | ||
356 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
357 | &hid_div); | |
358 | f.pre_div = hid_div; | |
359 | f.pre_div >>= CFG_SRC_DIV_SHIFT; | |
360 | f.pre_div &= mask; | |
361 | f.m = frac->num; | |
362 | f.n = frac->den; | |
363 | ||
364 | return clk_rcg2_configure(rcg, &f); | |
365 | } | |
366 | ||
367 | return -EINVAL; | |
368 | } | |
369 | ||
370 | static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw, | |
371 | unsigned long rate, unsigned long parent_rate, u8 index) | |
372 | { | |
373 | /* Parent index is set statically in frequency table */ | |
374 | return clk_edp_pixel_set_rate(hw, rate, parent_rate); | |
375 | } | |
376 | ||
377 | static long clk_edp_pixel_determine_rate(struct clk_hw *hw, unsigned long rate, | |
1c8e6004 TV |
378 | unsigned long min_rate, |
379 | unsigned long max_rate, | |
646cafc6 | 380 | unsigned long *p_rate, struct clk_hw **p) |
99cbd064 SB |
381 | { |
382 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
383 | const struct freq_tbl *f = rcg->freq_tbl; | |
384 | const struct frac_entry *frac; | |
385 | int delta = 100000; | |
386 | s64 src_rate = *p_rate; | |
387 | s64 request; | |
388 | u32 mask = BIT(rcg->hid_width) - 1; | |
389 | u32 hid_div; | |
2f272e7b | 390 | int index = qcom_find_src_index(hw, rcg->parent_map, f->src); |
99cbd064 SB |
391 | |
392 | /* Force the correct parent */ | |
2f272e7b | 393 | *p = __clk_get_hw(clk_get_parent_by_index(hw->clk, index)); |
99cbd064 SB |
394 | |
395 | if (src_rate == 810000000) | |
396 | frac = frac_table_810m; | |
397 | else | |
398 | frac = frac_table_675m; | |
399 | ||
400 | for (; frac->num; frac++) { | |
401 | request = rate; | |
402 | request *= frac->den; | |
403 | request = div_s64(request, frac->num); | |
404 | if ((src_rate < (request - delta)) || | |
405 | (src_rate > (request + delta))) | |
406 | continue; | |
407 | ||
408 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
409 | &hid_div); | |
410 | hid_div >>= CFG_SRC_DIV_SHIFT; | |
411 | hid_div &= mask; | |
412 | ||
413 | return calc_rate(src_rate, frac->num, frac->den, !!frac->den, | |
414 | hid_div); | |
415 | } | |
416 | ||
417 | return -EINVAL; | |
418 | } | |
419 | ||
420 | const struct clk_ops clk_edp_pixel_ops = { | |
421 | .is_enabled = clk_rcg2_is_enabled, | |
422 | .get_parent = clk_rcg2_get_parent, | |
423 | .set_parent = clk_rcg2_set_parent, | |
424 | .recalc_rate = clk_rcg2_recalc_rate, | |
425 | .set_rate = clk_edp_pixel_set_rate, | |
426 | .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent, | |
427 | .determine_rate = clk_edp_pixel_determine_rate, | |
428 | }; | |
429 | EXPORT_SYMBOL_GPL(clk_edp_pixel_ops); | |
430 | ||
431 | static long clk_byte_determine_rate(struct clk_hw *hw, unsigned long rate, | |
1c8e6004 | 432 | unsigned long min_rate, unsigned long max_rate, |
646cafc6 | 433 | unsigned long *p_rate, struct clk_hw **p_hw) |
99cbd064 SB |
434 | { |
435 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
436 | const struct freq_tbl *f = rcg->freq_tbl; | |
2f272e7b | 437 | int index = qcom_find_src_index(hw, rcg->parent_map, f->src); |
99cbd064 SB |
438 | unsigned long parent_rate, div; |
439 | u32 mask = BIT(rcg->hid_width) - 1; | |
646cafc6 | 440 | struct clk *p; |
99cbd064 SB |
441 | |
442 | if (rate == 0) | |
443 | return -EINVAL; | |
444 | ||
2f272e7b | 445 | p = clk_get_parent_by_index(hw->clk, index); |
646cafc6 TV |
446 | *p_hw = __clk_get_hw(p); |
447 | *p_rate = parent_rate = __clk_round_rate(p, rate); | |
99cbd064 SB |
448 | |
449 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; | |
450 | div = min_t(u32, div, mask); | |
451 | ||
452 | return calc_rate(parent_rate, 0, 0, 0, div); | |
453 | } | |
454 | ||
455 | static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate, | |
456 | unsigned long parent_rate) | |
457 | { | |
458 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
459 | struct freq_tbl f = *rcg->freq_tbl; | |
460 | unsigned long div; | |
461 | u32 mask = BIT(rcg->hid_width) - 1; | |
462 | ||
463 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; | |
464 | div = min_t(u32, div, mask); | |
465 | ||
466 | f.pre_div = div; | |
467 | ||
468 | return clk_rcg2_configure(rcg, &f); | |
469 | } | |
470 | ||
471 | static int clk_byte_set_rate_and_parent(struct clk_hw *hw, | |
472 | unsigned long rate, unsigned long parent_rate, u8 index) | |
473 | { | |
474 | /* Parent index is set statically in frequency table */ | |
475 | return clk_byte_set_rate(hw, rate, parent_rate); | |
476 | } | |
477 | ||
478 | const struct clk_ops clk_byte_ops = { | |
479 | .is_enabled = clk_rcg2_is_enabled, | |
480 | .get_parent = clk_rcg2_get_parent, | |
481 | .set_parent = clk_rcg2_set_parent, | |
482 | .recalc_rate = clk_rcg2_recalc_rate, | |
483 | .set_rate = clk_byte_set_rate, | |
484 | .set_rate_and_parent = clk_byte_set_rate_and_parent, | |
485 | .determine_rate = clk_byte_determine_rate, | |
486 | }; | |
487 | EXPORT_SYMBOL_GPL(clk_byte_ops); | |
488 | ||
489 | static const struct frac_entry frac_table_pixel[] = { | |
490 | { 3, 8 }, | |
491 | { 2, 9 }, | |
492 | { 4, 9 }, | |
493 | { 1, 1 }, | |
494 | { } | |
495 | }; | |
496 | ||
497 | static long clk_pixel_determine_rate(struct clk_hw *hw, unsigned long rate, | |
1c8e6004 TV |
498 | unsigned long min_rate, |
499 | unsigned long max_rate, | |
646cafc6 | 500 | unsigned long *p_rate, struct clk_hw **p) |
99cbd064 SB |
501 | { |
502 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
503 | unsigned long request, src_rate; | |
504 | int delta = 100000; | |
505 | const struct freq_tbl *f = rcg->freq_tbl; | |
506 | const struct frac_entry *frac = frac_table_pixel; | |
2f272e7b GD |
507 | int index = qcom_find_src_index(hw, rcg->parent_map, f->src); |
508 | struct clk *parent = clk_get_parent_by_index(hw->clk, index); | |
646cafc6 TV |
509 | |
510 | *p = __clk_get_hw(parent); | |
99cbd064 SB |
511 | |
512 | for (; frac->num; frac++) { | |
513 | request = (rate * frac->den) / frac->num; | |
514 | ||
515 | src_rate = __clk_round_rate(parent, request); | |
516 | if ((src_rate < (request - delta)) || | |
517 | (src_rate > (request + delta))) | |
518 | continue; | |
519 | ||
520 | *p_rate = src_rate; | |
521 | return (src_rate * frac->num) / frac->den; | |
522 | } | |
523 | ||
524 | return -EINVAL; | |
525 | } | |
526 | ||
527 | static int clk_pixel_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 | const struct frac_entry *frac = frac_table_pixel; | |
533 | unsigned long request, src_rate; | |
534 | int delta = 100000; | |
535 | u32 mask = BIT(rcg->hid_width) - 1; | |
536 | u32 hid_div; | |
2f272e7b GD |
537 | int index = qcom_find_src_index(hw, rcg->parent_map, f.src); |
538 | struct clk *parent = clk_get_parent_by_index(hw->clk, index); | |
99cbd064 SB |
539 | |
540 | for (; frac->num; frac++) { | |
541 | request = (rate * frac->den) / frac->num; | |
542 | ||
543 | src_rate = __clk_round_rate(parent, request); | |
544 | if ((src_rate < (request - delta)) || | |
545 | (src_rate > (request + delta))) | |
546 | continue; | |
547 | ||
548 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
549 | &hid_div); | |
550 | f.pre_div = hid_div; | |
551 | f.pre_div >>= CFG_SRC_DIV_SHIFT; | |
552 | f.pre_div &= mask; | |
553 | f.m = frac->num; | |
554 | f.n = frac->den; | |
555 | ||
556 | return clk_rcg2_configure(rcg, &f); | |
557 | } | |
558 | return -EINVAL; | |
559 | } | |
560 | ||
561 | static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, | |
562 | unsigned long parent_rate, u8 index) | |
563 | { | |
564 | /* Parent index is set statically in frequency table */ | |
565 | return clk_pixel_set_rate(hw, rate, parent_rate); | |
566 | } | |
567 | ||
568 | const struct clk_ops clk_pixel_ops = { | |
569 | .is_enabled = clk_rcg2_is_enabled, | |
570 | .get_parent = clk_rcg2_get_parent, | |
571 | .set_parent = clk_rcg2_set_parent, | |
572 | .recalc_rate = clk_rcg2_recalc_rate, | |
573 | .set_rate = clk_pixel_set_rate, | |
574 | .set_rate_and_parent = clk_pixel_set_rate_and_parent, | |
575 | .determine_rate = clk_pixel_determine_rate, | |
576 | }; | |
577 | EXPORT_SYMBOL_GPL(clk_pixel_ops); |