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