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; |
bcd61c0f | 185 | |
50c6a503 | 186 | f = qcom_find_freq(f, rate); |
bcd61c0f SB |
187 | if (!f) |
188 | return -EINVAL; | |
189 | ||
190 | clk_flags = __clk_get_flags(hw->clk); | |
646cafc6 | 191 | p = clk_get_parent_by_index(hw->clk, f->src); |
bcd61c0f SB |
192 | if (clk_flags & CLK_SET_RATE_PARENT) { |
193 | if (f->pre_div) { | |
194 | rate /= 2; | |
195 | rate *= f->pre_div + 1; | |
196 | } | |
197 | ||
198 | if (f->n) { | |
199 | u64 tmp = rate; | |
200 | tmp = tmp * f->n; | |
201 | do_div(tmp, f->m); | |
202 | rate = tmp; | |
203 | } | |
204 | } else { | |
646cafc6 | 205 | rate = __clk_get_rate(p); |
bcd61c0f | 206 | } |
646cafc6 | 207 | *p_hw = __clk_get_hw(p); |
bcd61c0f SB |
208 | *p_rate = rate; |
209 | ||
210 | return f->freq; | |
211 | } | |
212 | ||
213 | static long clk_rcg2_determine_rate(struct clk_hw *hw, unsigned long rate, | |
1c8e6004 | 214 | unsigned long min_rate, unsigned long max_rate, |
646cafc6 | 215 | unsigned long *p_rate, struct clk_hw **p) |
bcd61c0f SB |
216 | { |
217 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
218 | ||
219 | return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, p_rate, p); | |
220 | } | |
221 | ||
99cbd064 | 222 | static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f) |
bcd61c0f | 223 | { |
bcd61c0f | 224 | u32 cfg, mask; |
293d2e97 GD |
225 | struct clk_hw *hw = &rcg->clkr.hw; |
226 | int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src); | |
227 | ||
228 | if (index < 0) | |
229 | return index; | |
bcd61c0f | 230 | |
bcd61c0f SB |
231 | if (rcg->mnd_width && f->n) { |
232 | mask = BIT(rcg->mnd_width) - 1; | |
99cbd064 SB |
233 | ret = regmap_update_bits(rcg->clkr.regmap, |
234 | rcg->cmd_rcgr + M_REG, mask, f->m); | |
bcd61c0f SB |
235 | if (ret) |
236 | return ret; | |
237 | ||
99cbd064 SB |
238 | ret = regmap_update_bits(rcg->clkr.regmap, |
239 | rcg->cmd_rcgr + N_REG, mask, ~(f->n - f->m)); | |
bcd61c0f SB |
240 | if (ret) |
241 | return ret; | |
242 | ||
99cbd064 SB |
243 | ret = regmap_update_bits(rcg->clkr.regmap, |
244 | rcg->cmd_rcgr + D_REG, mask, ~f->n); | |
bcd61c0f SB |
245 | if (ret) |
246 | return ret; | |
247 | } | |
248 | ||
249 | mask = BIT(rcg->hid_width) - 1; | |
250 | mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK; | |
251 | cfg = f->pre_div << CFG_SRC_DIV_SHIFT; | |
293d2e97 | 252 | cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT; |
0b21503d | 253 | if (rcg->mnd_width && f->n && (f->m != f->n)) |
bcd61c0f | 254 | cfg |= CFG_MODE_DUAL_EDGE; |
99cbd064 SB |
255 | ret = regmap_update_bits(rcg->clkr.regmap, |
256 | rcg->cmd_rcgr + CFG_REG, mask, cfg); | |
bcd61c0f SB |
257 | if (ret) |
258 | return ret; | |
259 | ||
260 | return update_config(rcg); | |
261 | } | |
262 | ||
99cbd064 SB |
263 | static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate) |
264 | { | |
265 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
266 | const struct freq_tbl *f; | |
267 | ||
50c6a503 | 268 | f = qcom_find_freq(rcg->freq_tbl, rate); |
99cbd064 SB |
269 | if (!f) |
270 | return -EINVAL; | |
271 | ||
272 | return clk_rcg2_configure(rcg, f); | |
273 | } | |
274 | ||
bcd61c0f SB |
275 | static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate, |
276 | unsigned long parent_rate) | |
277 | { | |
278 | return __clk_rcg2_set_rate(hw, rate); | |
279 | } | |
280 | ||
281 | static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw, | |
282 | unsigned long rate, unsigned long parent_rate, u8 index) | |
283 | { | |
284 | return __clk_rcg2_set_rate(hw, rate); | |
285 | } | |
286 | ||
287 | const struct clk_ops clk_rcg2_ops = { | |
288 | .is_enabled = clk_rcg2_is_enabled, | |
289 | .get_parent = clk_rcg2_get_parent, | |
290 | .set_parent = clk_rcg2_set_parent, | |
291 | .recalc_rate = clk_rcg2_recalc_rate, | |
292 | .determine_rate = clk_rcg2_determine_rate, | |
293 | .set_rate = clk_rcg2_set_rate, | |
294 | .set_rate_and_parent = clk_rcg2_set_rate_and_parent, | |
295 | }; | |
296 | EXPORT_SYMBOL_GPL(clk_rcg2_ops); | |
99cbd064 SB |
297 | |
298 | struct frac_entry { | |
299 | int num; | |
300 | int den; | |
301 | }; | |
302 | ||
303 | static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */ | |
304 | { 52, 295 }, /* 119 M */ | |
305 | { 11, 57 }, /* 130.25 M */ | |
306 | { 63, 307 }, /* 138.50 M */ | |
307 | { 11, 50 }, /* 148.50 M */ | |
308 | { 47, 206 }, /* 154 M */ | |
309 | { 31, 100 }, /* 205.25 M */ | |
310 | { 107, 269 }, /* 268.50 M */ | |
311 | { }, | |
312 | }; | |
313 | ||
314 | static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */ | |
315 | { 31, 211 }, /* 119 M */ | |
316 | { 32, 199 }, /* 130.25 M */ | |
317 | { 63, 307 }, /* 138.50 M */ | |
318 | { 11, 60 }, /* 148.50 M */ | |
319 | { 50, 263 }, /* 154 M */ | |
320 | { 31, 120 }, /* 205.25 M */ | |
321 | { 119, 359 }, /* 268.50 M */ | |
322 | { }, | |
323 | }; | |
324 | ||
325 | static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate, | |
326 | unsigned long parent_rate) | |
327 | { | |
328 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
329 | struct freq_tbl f = *rcg->freq_tbl; | |
330 | const struct frac_entry *frac; | |
331 | int delta = 100000; | |
332 | s64 src_rate = parent_rate; | |
333 | s64 request; | |
334 | u32 mask = BIT(rcg->hid_width) - 1; | |
335 | u32 hid_div; | |
336 | ||
337 | if (src_rate == 810000000) | |
338 | frac = frac_table_810m; | |
339 | else | |
340 | frac = frac_table_675m; | |
341 | ||
342 | for (; frac->num; frac++) { | |
343 | request = rate; | |
344 | request *= frac->den; | |
345 | request = div_s64(request, frac->num); | |
346 | if ((src_rate < (request - delta)) || | |
347 | (src_rate > (request + delta))) | |
348 | continue; | |
349 | ||
350 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
351 | &hid_div); | |
352 | f.pre_div = hid_div; | |
353 | f.pre_div >>= CFG_SRC_DIV_SHIFT; | |
354 | f.pre_div &= mask; | |
355 | f.m = frac->num; | |
356 | f.n = frac->den; | |
357 | ||
358 | return clk_rcg2_configure(rcg, &f); | |
359 | } | |
360 | ||
361 | return -EINVAL; | |
362 | } | |
363 | ||
364 | static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw, | |
365 | unsigned long rate, unsigned long parent_rate, u8 index) | |
366 | { | |
367 | /* Parent index is set statically in frequency table */ | |
368 | return clk_edp_pixel_set_rate(hw, rate, parent_rate); | |
369 | } | |
370 | ||
371 | static long clk_edp_pixel_determine_rate(struct clk_hw *hw, unsigned long rate, | |
1c8e6004 TV |
372 | unsigned long min_rate, |
373 | unsigned long max_rate, | |
646cafc6 | 374 | unsigned long *p_rate, struct clk_hw **p) |
99cbd064 SB |
375 | { |
376 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
377 | const struct freq_tbl *f = rcg->freq_tbl; | |
378 | const struct frac_entry *frac; | |
379 | int delta = 100000; | |
380 | s64 src_rate = *p_rate; | |
381 | s64 request; | |
382 | u32 mask = BIT(rcg->hid_width) - 1; | |
383 | u32 hid_div; | |
384 | ||
385 | /* Force the correct parent */ | |
646cafc6 | 386 | *p = __clk_get_hw(clk_get_parent_by_index(hw->clk, f->src)); |
99cbd064 SB |
387 | |
388 | if (src_rate == 810000000) | |
389 | frac = frac_table_810m; | |
390 | else | |
391 | frac = frac_table_675m; | |
392 | ||
393 | for (; frac->num; frac++) { | |
394 | request = rate; | |
395 | request *= frac->den; | |
396 | request = div_s64(request, frac->num); | |
397 | if ((src_rate < (request - delta)) || | |
398 | (src_rate > (request + delta))) | |
399 | continue; | |
400 | ||
401 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
402 | &hid_div); | |
403 | hid_div >>= CFG_SRC_DIV_SHIFT; | |
404 | hid_div &= mask; | |
405 | ||
406 | return calc_rate(src_rate, frac->num, frac->den, !!frac->den, | |
407 | hid_div); | |
408 | } | |
409 | ||
410 | return -EINVAL; | |
411 | } | |
412 | ||
413 | const struct clk_ops clk_edp_pixel_ops = { | |
414 | .is_enabled = clk_rcg2_is_enabled, | |
415 | .get_parent = clk_rcg2_get_parent, | |
416 | .set_parent = clk_rcg2_set_parent, | |
417 | .recalc_rate = clk_rcg2_recalc_rate, | |
418 | .set_rate = clk_edp_pixel_set_rate, | |
419 | .set_rate_and_parent = clk_edp_pixel_set_rate_and_parent, | |
420 | .determine_rate = clk_edp_pixel_determine_rate, | |
421 | }; | |
422 | EXPORT_SYMBOL_GPL(clk_edp_pixel_ops); | |
423 | ||
424 | static long clk_byte_determine_rate(struct clk_hw *hw, unsigned long rate, | |
1c8e6004 | 425 | unsigned long min_rate, unsigned long max_rate, |
646cafc6 | 426 | unsigned long *p_rate, struct clk_hw **p_hw) |
99cbd064 SB |
427 | { |
428 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
429 | const struct freq_tbl *f = rcg->freq_tbl; | |
430 | unsigned long parent_rate, div; | |
431 | u32 mask = BIT(rcg->hid_width) - 1; | |
646cafc6 | 432 | struct clk *p; |
99cbd064 SB |
433 | |
434 | if (rate == 0) | |
435 | return -EINVAL; | |
436 | ||
646cafc6 TV |
437 | p = clk_get_parent_by_index(hw->clk, f->src); |
438 | *p_hw = __clk_get_hw(p); | |
439 | *p_rate = parent_rate = __clk_round_rate(p, rate); | |
99cbd064 SB |
440 | |
441 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; | |
442 | div = min_t(u32, div, mask); | |
443 | ||
444 | return calc_rate(parent_rate, 0, 0, 0, div); | |
445 | } | |
446 | ||
447 | static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate, | |
448 | unsigned long parent_rate) | |
449 | { | |
450 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
451 | struct freq_tbl f = *rcg->freq_tbl; | |
452 | unsigned long div; | |
453 | u32 mask = BIT(rcg->hid_width) - 1; | |
454 | ||
455 | div = DIV_ROUND_UP((2 * parent_rate), rate) - 1; | |
456 | div = min_t(u32, div, mask); | |
457 | ||
458 | f.pre_div = div; | |
459 | ||
460 | return clk_rcg2_configure(rcg, &f); | |
461 | } | |
462 | ||
463 | static int clk_byte_set_rate_and_parent(struct clk_hw *hw, | |
464 | unsigned long rate, unsigned long parent_rate, u8 index) | |
465 | { | |
466 | /* Parent index is set statically in frequency table */ | |
467 | return clk_byte_set_rate(hw, rate, parent_rate); | |
468 | } | |
469 | ||
470 | const struct clk_ops clk_byte_ops = { | |
471 | .is_enabled = clk_rcg2_is_enabled, | |
472 | .get_parent = clk_rcg2_get_parent, | |
473 | .set_parent = clk_rcg2_set_parent, | |
474 | .recalc_rate = clk_rcg2_recalc_rate, | |
475 | .set_rate = clk_byte_set_rate, | |
476 | .set_rate_and_parent = clk_byte_set_rate_and_parent, | |
477 | .determine_rate = clk_byte_determine_rate, | |
478 | }; | |
479 | EXPORT_SYMBOL_GPL(clk_byte_ops); | |
480 | ||
481 | static const struct frac_entry frac_table_pixel[] = { | |
482 | { 3, 8 }, | |
483 | { 2, 9 }, | |
484 | { 4, 9 }, | |
485 | { 1, 1 }, | |
486 | { } | |
487 | }; | |
488 | ||
489 | static long clk_pixel_determine_rate(struct clk_hw *hw, unsigned long rate, | |
1c8e6004 TV |
490 | unsigned long min_rate, |
491 | unsigned long max_rate, | |
646cafc6 | 492 | unsigned long *p_rate, struct clk_hw **p) |
99cbd064 SB |
493 | { |
494 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
495 | unsigned long request, src_rate; | |
496 | int delta = 100000; | |
497 | const struct freq_tbl *f = rcg->freq_tbl; | |
498 | const struct frac_entry *frac = frac_table_pixel; | |
646cafc6 TV |
499 | struct clk *parent = clk_get_parent_by_index(hw->clk, f->src); |
500 | ||
501 | *p = __clk_get_hw(parent); | |
99cbd064 SB |
502 | |
503 | for (; frac->num; frac++) { | |
504 | request = (rate * frac->den) / frac->num; | |
505 | ||
506 | src_rate = __clk_round_rate(parent, request); | |
507 | if ((src_rate < (request - delta)) || | |
508 | (src_rate > (request + delta))) | |
509 | continue; | |
510 | ||
511 | *p_rate = src_rate; | |
512 | return (src_rate * frac->num) / frac->den; | |
513 | } | |
514 | ||
515 | return -EINVAL; | |
516 | } | |
517 | ||
518 | static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate, | |
519 | unsigned long parent_rate) | |
520 | { | |
521 | struct clk_rcg2 *rcg = to_clk_rcg2(hw); | |
522 | struct freq_tbl f = *rcg->freq_tbl; | |
523 | const struct frac_entry *frac = frac_table_pixel; | |
524 | unsigned long request, src_rate; | |
525 | int delta = 100000; | |
526 | u32 mask = BIT(rcg->hid_width) - 1; | |
527 | u32 hid_div; | |
528 | struct clk *parent = clk_get_parent_by_index(hw->clk, f.src); | |
529 | ||
530 | for (; frac->num; frac++) { | |
531 | request = (rate * frac->den) / frac->num; | |
532 | ||
533 | src_rate = __clk_round_rate(parent, request); | |
534 | if ((src_rate < (request - delta)) || | |
535 | (src_rate > (request + delta))) | |
536 | continue; | |
537 | ||
538 | regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, | |
539 | &hid_div); | |
540 | f.pre_div = hid_div; | |
541 | f.pre_div >>= CFG_SRC_DIV_SHIFT; | |
542 | f.pre_div &= mask; | |
543 | f.m = frac->num; | |
544 | f.n = frac->den; | |
545 | ||
546 | return clk_rcg2_configure(rcg, &f); | |
547 | } | |
548 | return -EINVAL; | |
549 | } | |
550 | ||
551 | static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, | |
552 | unsigned long parent_rate, u8 index) | |
553 | { | |
554 | /* Parent index is set statically in frequency table */ | |
555 | return clk_pixel_set_rate(hw, rate, parent_rate); | |
556 | } | |
557 | ||
558 | const struct clk_ops clk_pixel_ops = { | |
559 | .is_enabled = clk_rcg2_is_enabled, | |
560 | .get_parent = clk_rcg2_get_parent, | |
561 | .set_parent = clk_rcg2_set_parent, | |
562 | .recalc_rate = clk_rcg2_recalc_rate, | |
563 | .set_rate = clk_pixel_set_rate, | |
564 | .set_rate_and_parent = clk_pixel_set_rate_and_parent, | |
565 | .determine_rate = clk_pixel_determine_rate, | |
566 | }; | |
567 | EXPORT_SYMBOL_GPL(clk_pixel_ops); |