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 
clk_rcg2_is_enabled(struct clk_hw * hw)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 
clk_rcg2_get_parent(struct clk_hw * hw)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 
update_config(struct clk_rcg2 * rcg)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 
clk_rcg2_set_parent(struct clk_hw * hw,u8 index)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
calc_rate(unsigned long rate,u32 m,u32 n,u32 mode,u32 hid_div)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
clk_rcg2_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)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 
_freq_tbl_determine_rate(struct clk_hw * hw,const struct freq_tbl * f,struct clk_rate_request * req,enum freq_policy policy)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 
clk_rcg2_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)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 
clk_rcg2_determine_floor_rate(struct clk_hw * hw,struct clk_rate_request * req)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 
__clk_rcg2_configure(struct clk_rcg2 * rcg,const struct freq_tbl * f)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 
clk_rcg2_configure(struct clk_rcg2 * rcg,const struct freq_tbl * f)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 
__clk_rcg2_set_rate(struct clk_hw * hw,unsigned long rate,enum freq_policy policy)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 
clk_rcg2_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)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 
clk_rcg2_set_floor_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)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 
clk_rcg2_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)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 
clk_rcg2_set_floor_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)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 const struct clk_ops clk_rcg2_ops = {
361 	.is_enabled = clk_rcg2_is_enabled,
362 	.get_parent = clk_rcg2_get_parent,
363 	.set_parent = clk_rcg2_set_parent,
364 	.recalc_rate = clk_rcg2_recalc_rate,
365 	.determine_rate = clk_rcg2_determine_rate,
366 	.set_rate = clk_rcg2_set_rate,
367 	.set_rate_and_parent = clk_rcg2_set_rate_and_parent,
368 };
369 EXPORT_SYMBOL_GPL(clk_rcg2_ops);
370 
371 const struct clk_ops clk_rcg2_floor_ops = {
372 	.is_enabled = clk_rcg2_is_enabled,
373 	.get_parent = clk_rcg2_get_parent,
374 	.set_parent = clk_rcg2_set_parent,
375 	.recalc_rate = clk_rcg2_recalc_rate,
376 	.determine_rate = clk_rcg2_determine_floor_rate,
377 	.set_rate = clk_rcg2_set_floor_rate,
378 	.set_rate_and_parent = clk_rcg2_set_floor_rate_and_parent,
379 };
380 EXPORT_SYMBOL_GPL(clk_rcg2_floor_ops);
381 
382 struct frac_entry {
383 	int num;
384 	int den;
385 };
386 
387 static const struct frac_entry frac_table_675m[] = {	/* link rate of 270M */
388 	{ 52, 295 },	/* 119 M */
389 	{ 11, 57 },	/* 130.25 M */
390 	{ 63, 307 },	/* 138.50 M */
391 	{ 11, 50 },	/* 148.50 M */
392 	{ 47, 206 },	/* 154 M */
393 	{ 31, 100 },	/* 205.25 M */
394 	{ 107, 269 },	/* 268.50 M */
395 	{ },
396 };
397 
398 static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */
399 	{ 31, 211 },	/* 119 M */
400 	{ 32, 199 },	/* 130.25 M */
401 	{ 63, 307 },	/* 138.50 M */
402 	{ 11, 60 },	/* 148.50 M */
403 	{ 50, 263 },	/* 154 M */
404 	{ 31, 120 },	/* 205.25 M */
405 	{ 119, 359 },	/* 268.50 M */
406 	{ },
407 };
408 
clk_edp_pixel_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)409 static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
410 			      unsigned long parent_rate)
411 {
412 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
413 	struct freq_tbl f = *rcg->freq_tbl;
414 	const struct frac_entry *frac;
415 	int delta = 100000;
416 	s64 src_rate = parent_rate;
417 	s64 request;
418 	u32 mask = BIT(rcg->hid_width) - 1;
419 	u32 hid_div;
420 
421 	if (src_rate == 810000000)
422 		frac = frac_table_810m;
423 	else
424 		frac = frac_table_675m;
425 
426 	for (; frac->num; frac++) {
427 		request = rate;
428 		request *= frac->den;
429 		request = div_s64(request, frac->num);
430 		if ((src_rate < (request - delta)) ||
431 		    (src_rate > (request + delta)))
432 			continue;
433 
434 		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
435 				&hid_div);
436 		f.pre_div = hid_div;
437 		f.pre_div >>= CFG_SRC_DIV_SHIFT;
438 		f.pre_div &= mask;
439 		f.m = frac->num;
440 		f.n = frac->den;
441 
442 		return clk_rcg2_configure(rcg, &f);
443 	}
444 
445 	return -EINVAL;
446 }
447 
clk_edp_pixel_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)448 static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,
449 		unsigned long rate, unsigned long parent_rate, u8 index)
450 {
451 	/* Parent index is set statically in frequency table */
452 	return clk_edp_pixel_set_rate(hw, rate, parent_rate);
453 }
454 
clk_edp_pixel_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)455 static int clk_edp_pixel_determine_rate(struct clk_hw *hw,
456 					struct clk_rate_request *req)
457 {
458 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
459 	const struct freq_tbl *f = rcg->freq_tbl;
460 	const struct frac_entry *frac;
461 	int delta = 100000;
462 	s64 request;
463 	u32 mask = BIT(rcg->hid_width) - 1;
464 	u32 hid_div;
465 	int index = qcom_find_src_index(hw, rcg->parent_map, f->src);
466 
467 	/* Force the correct parent */
468 	req->best_parent_hw = clk_hw_get_parent_by_index(hw, index);
469 	req->best_parent_rate = clk_hw_get_rate(req->best_parent_hw);
470 
471 	if (req->best_parent_rate == 810000000)
472 		frac = frac_table_810m;
473 	else
474 		frac = frac_table_675m;
475 
476 	for (; frac->num; frac++) {
477 		request = req->rate;
478 		request *= frac->den;
479 		request = div_s64(request, frac->num);
480 		if ((req->best_parent_rate < (request - delta)) ||
481 		    (req->best_parent_rate > (request + delta)))
482 			continue;
483 
484 		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
485 				&hid_div);
486 		hid_div >>= CFG_SRC_DIV_SHIFT;
487 		hid_div &= mask;
488 
489 		req->rate = calc_rate(req->best_parent_rate,
490 				      frac->num, frac->den,
491 				      !!frac->den, hid_div);
492 		return 0;
493 	}
494 
495 	return -EINVAL;
496 }
497 
498 const struct clk_ops clk_edp_pixel_ops = {
499 	.is_enabled = clk_rcg2_is_enabled,
500 	.get_parent = clk_rcg2_get_parent,
501 	.set_parent = clk_rcg2_set_parent,
502 	.recalc_rate = clk_rcg2_recalc_rate,
503 	.set_rate = clk_edp_pixel_set_rate,
504 	.set_rate_and_parent = clk_edp_pixel_set_rate_and_parent,
505 	.determine_rate = clk_edp_pixel_determine_rate,
506 };
507 EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);
508 
clk_byte_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)509 static int clk_byte_determine_rate(struct clk_hw *hw,
510 				   struct clk_rate_request *req)
511 {
512 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
513 	const struct freq_tbl *f = rcg->freq_tbl;
514 	int index = qcom_find_src_index(hw, rcg->parent_map, f->src);
515 	unsigned long parent_rate, div;
516 	u32 mask = BIT(rcg->hid_width) - 1;
517 	struct clk_hw *p;
518 
519 	if (req->rate == 0)
520 		return -EINVAL;
521 
522 	req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index);
523 	req->best_parent_rate = parent_rate = clk_hw_round_rate(p, req->rate);
524 
525 	div = DIV_ROUND_UP((2 * parent_rate), req->rate) - 1;
526 	div = min_t(u32, div, mask);
527 
528 	req->rate = calc_rate(parent_rate, 0, 0, 0, div);
529 
530 	return 0;
531 }
532 
clk_byte_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)533 static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate,
534 			 unsigned long parent_rate)
535 {
536 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
537 	struct freq_tbl f = *rcg->freq_tbl;
538 	unsigned long div;
539 	u32 mask = BIT(rcg->hid_width) - 1;
540 
541 	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
542 	div = min_t(u32, div, mask);
543 
544 	f.pre_div = div;
545 
546 	return clk_rcg2_configure(rcg, &f);
547 }
548 
clk_byte_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)549 static int clk_byte_set_rate_and_parent(struct clk_hw *hw,
550 		unsigned long rate, unsigned long parent_rate, u8 index)
551 {
552 	/* Parent index is set statically in frequency table */
553 	return clk_byte_set_rate(hw, rate, parent_rate);
554 }
555 
556 const struct clk_ops clk_byte_ops = {
557 	.is_enabled = clk_rcg2_is_enabled,
558 	.get_parent = clk_rcg2_get_parent,
559 	.set_parent = clk_rcg2_set_parent,
560 	.recalc_rate = clk_rcg2_recalc_rate,
561 	.set_rate = clk_byte_set_rate,
562 	.set_rate_and_parent = clk_byte_set_rate_and_parent,
563 	.determine_rate = clk_byte_determine_rate,
564 };
565 EXPORT_SYMBOL_GPL(clk_byte_ops);
566 
clk_byte2_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)567 static int clk_byte2_determine_rate(struct clk_hw *hw,
568 				    struct clk_rate_request *req)
569 {
570 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
571 	unsigned long parent_rate, div;
572 	u32 mask = BIT(rcg->hid_width) - 1;
573 	struct clk_hw *p;
574 	unsigned long rate = req->rate;
575 
576 	if (rate == 0)
577 		return -EINVAL;
578 
579 	p = req->best_parent_hw;
580 	req->best_parent_rate = parent_rate = clk_hw_round_rate(p, rate);
581 
582 	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
583 	div = min_t(u32, div, mask);
584 
585 	req->rate = calc_rate(parent_rate, 0, 0, 0, div);
586 
587 	return 0;
588 }
589 
clk_byte2_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)590 static int clk_byte2_set_rate(struct clk_hw *hw, unsigned long rate,
591 			 unsigned long parent_rate)
592 {
593 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
594 	struct freq_tbl f = { 0 };
595 	unsigned long div;
596 	int i, num_parents = clk_hw_get_num_parents(hw);
597 	u32 mask = BIT(rcg->hid_width) - 1;
598 	u32 cfg;
599 
600 	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
601 	div = min_t(u32, div, mask);
602 
603 	f.pre_div = div;
604 
605 	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
606 	cfg &= CFG_SRC_SEL_MASK;
607 	cfg >>= CFG_SRC_SEL_SHIFT;
608 
609 	for (i = 0; i < num_parents; i++) {
610 		if (cfg == rcg->parent_map[i].cfg) {
611 			f.src = rcg->parent_map[i].src;
612 			return clk_rcg2_configure(rcg, &f);
613 		}
614 	}
615 
616 	return -EINVAL;
617 }
618 
clk_byte2_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)619 static int clk_byte2_set_rate_and_parent(struct clk_hw *hw,
620 		unsigned long rate, unsigned long parent_rate, u8 index)
621 {
622 	/* Read the hardware to determine parent during set_rate */
623 	return clk_byte2_set_rate(hw, rate, parent_rate);
624 }
625 
626 const struct clk_ops clk_byte2_ops = {
627 	.is_enabled = clk_rcg2_is_enabled,
628 	.get_parent = clk_rcg2_get_parent,
629 	.set_parent = clk_rcg2_set_parent,
630 	.recalc_rate = clk_rcg2_recalc_rate,
631 	.set_rate = clk_byte2_set_rate,
632 	.set_rate_and_parent = clk_byte2_set_rate_and_parent,
633 	.determine_rate = clk_byte2_determine_rate,
634 };
635 EXPORT_SYMBOL_GPL(clk_byte2_ops);
636 
637 static const struct frac_entry frac_table_pixel[] = {
638 	{ 3, 8 },
639 	{ 2, 9 },
640 	{ 4, 9 },
641 	{ 1, 1 },
642 	{ }
643 };
644 
clk_pixel_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)645 static int clk_pixel_determine_rate(struct clk_hw *hw,
646 				    struct clk_rate_request *req)
647 {
648 	unsigned long request, src_rate;
649 	int delta = 100000;
650 	const struct frac_entry *frac = frac_table_pixel;
651 
652 	for (; frac->num; frac++) {
653 		request = (req->rate * frac->den) / frac->num;
654 
655 		src_rate = clk_hw_round_rate(req->best_parent_hw, request);
656 		if ((src_rate < (request - delta)) ||
657 			(src_rate > (request + delta)))
658 			continue;
659 
660 		req->best_parent_rate = src_rate;
661 		req->rate = (src_rate * frac->num) / frac->den;
662 		return 0;
663 	}
664 
665 	return -EINVAL;
666 }
667 
clk_pixel_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)668 static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
669 		unsigned long parent_rate)
670 {
671 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
672 	struct freq_tbl f = { 0 };
673 	const struct frac_entry *frac = frac_table_pixel;
674 	unsigned long request;
675 	int delta = 100000;
676 	u32 mask = BIT(rcg->hid_width) - 1;
677 	u32 hid_div, cfg;
678 	int i, num_parents = clk_hw_get_num_parents(hw);
679 
680 	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
681 	cfg &= CFG_SRC_SEL_MASK;
682 	cfg >>= CFG_SRC_SEL_SHIFT;
683 
684 	for (i = 0; i < num_parents; i++)
685 		if (cfg == rcg->parent_map[i].cfg) {
686 			f.src = rcg->parent_map[i].src;
687 			break;
688 		}
689 
690 	for (; frac->num; frac++) {
691 		request = (rate * frac->den) / frac->num;
692 
693 		if ((parent_rate < (request - delta)) ||
694 			(parent_rate > (request + delta)))
695 			continue;
696 
697 		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
698 				&hid_div);
699 		f.pre_div = hid_div;
700 		f.pre_div >>= CFG_SRC_DIV_SHIFT;
701 		f.pre_div &= mask;
702 		f.m = frac->num;
703 		f.n = frac->den;
704 
705 		return clk_rcg2_configure(rcg, &f);
706 	}
707 	return -EINVAL;
708 }
709 
clk_pixel_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)710 static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
711 		unsigned long parent_rate, u8 index)
712 {
713 	return clk_pixel_set_rate(hw, rate, parent_rate);
714 }
715 
716 const struct clk_ops clk_pixel_ops = {
717 	.is_enabled = clk_rcg2_is_enabled,
718 	.get_parent = clk_rcg2_get_parent,
719 	.set_parent = clk_rcg2_set_parent,
720 	.recalc_rate = clk_rcg2_recalc_rate,
721 	.set_rate = clk_pixel_set_rate,
722 	.set_rate_and_parent = clk_pixel_set_rate_and_parent,
723 	.determine_rate = clk_pixel_determine_rate,
724 };
725 EXPORT_SYMBOL_GPL(clk_pixel_ops);
726 
clk_gfx3d_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)727 static int clk_gfx3d_determine_rate(struct clk_hw *hw,
728 				    struct clk_rate_request *req)
729 {
730 	struct clk_rate_request parent_req = { };
731 	struct clk_hw *p2, *p8, *p9, *xo;
732 	unsigned long p9_rate;
733 	int ret;
734 
735 	xo = clk_hw_get_parent_by_index(hw, 0);
736 	if (req->rate == clk_hw_get_rate(xo)) {
737 		req->best_parent_hw = xo;
738 		return 0;
739 	}
740 
741 	p9 = clk_hw_get_parent_by_index(hw, 2);
742 	p2 = clk_hw_get_parent_by_index(hw, 3);
743 	p8 = clk_hw_get_parent_by_index(hw, 4);
744 
745 	/* PLL9 is a fixed rate PLL */
746 	p9_rate = clk_hw_get_rate(p9);
747 
748 	parent_req.rate = req->rate = min(req->rate, p9_rate);
749 	if (req->rate == p9_rate) {
750 		req->rate = req->best_parent_rate = p9_rate;
751 		req->best_parent_hw = p9;
752 		return 0;
753 	}
754 
755 	if (req->best_parent_hw == p9) {
756 		/* Are we going back to a previously used rate? */
757 		if (clk_hw_get_rate(p8) == req->rate)
758 			req->best_parent_hw = p8;
759 		else
760 			req->best_parent_hw = p2;
761 	} else if (req->best_parent_hw == p8) {
762 		req->best_parent_hw = p2;
763 	} else {
764 		req->best_parent_hw = p8;
765 	}
766 
767 	ret = __clk_determine_rate(req->best_parent_hw, &parent_req);
768 	if (ret)
769 		return ret;
770 
771 	req->rate = req->best_parent_rate = parent_req.rate;
772 
773 	return 0;
774 }
775 
clk_gfx3d_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)776 static int clk_gfx3d_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
777 		unsigned long parent_rate, u8 index)
778 {
779 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
780 	u32 cfg;
781 	int ret;
782 
783 	/* Just mux it, we don't use the division or m/n hardware */
784 	cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
785 	ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg);
786 	if (ret)
787 		return ret;
788 
789 	return update_config(rcg);
790 }
791 
clk_gfx3d_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)792 static int clk_gfx3d_set_rate(struct clk_hw *hw, unsigned long rate,
793 			      unsigned long parent_rate)
794 {
795 	/*
796 	 * We should never get here; clk_gfx3d_determine_rate() should always
797 	 * make us use a different parent than what we're currently using, so
798 	 * clk_gfx3d_set_rate_and_parent() should always be called.
799 	 */
800 	return 0;
801 }
802 
803 const struct clk_ops clk_gfx3d_ops = {
804 	.is_enabled = clk_rcg2_is_enabled,
805 	.get_parent = clk_rcg2_get_parent,
806 	.set_parent = clk_rcg2_set_parent,
807 	.recalc_rate = clk_rcg2_recalc_rate,
808 	.set_rate = clk_gfx3d_set_rate,
809 	.set_rate_and_parent = clk_gfx3d_set_rate_and_parent,
810 	.determine_rate = clk_gfx3d_determine_rate,
811 };
812 EXPORT_SYMBOL_GPL(clk_gfx3d_ops);
813 
clk_rcg2_set_force_enable(struct clk_hw * hw)814 static int clk_rcg2_set_force_enable(struct clk_hw *hw)
815 {
816 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
817 	const char *name = clk_hw_get_name(hw);
818 	int ret, count;
819 
820 	ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
821 				 CMD_ROOT_EN, CMD_ROOT_EN);
822 	if (ret)
823 		return ret;
824 
825 	/* wait for RCG to turn ON */
826 	for (count = 500; count > 0; count--) {
827 		if (clk_rcg2_is_enabled(hw))
828 			return 0;
829 
830 		udelay(1);
831 	}
832 
833 	pr_err("%s: RCG did not turn on\n", name);
834 	return -ETIMEDOUT;
835 }
836 
clk_rcg2_clear_force_enable(struct clk_hw * hw)837 static int clk_rcg2_clear_force_enable(struct clk_hw *hw)
838 {
839 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
840 
841 	return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
842 					CMD_ROOT_EN, 0);
843 }
844 
845 static int
clk_rcg2_shared_force_enable_clear(struct clk_hw * hw,const struct freq_tbl * f)846 clk_rcg2_shared_force_enable_clear(struct clk_hw *hw, const struct freq_tbl *f)
847 {
848 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
849 	int ret;
850 
851 	ret = clk_rcg2_set_force_enable(hw);
852 	if (ret)
853 		return ret;
854 
855 	ret = clk_rcg2_configure(rcg, f);
856 	if (ret)
857 		return ret;
858 
859 	return clk_rcg2_clear_force_enable(hw);
860 }
861 
clk_rcg2_shared_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)862 static int clk_rcg2_shared_set_rate(struct clk_hw *hw, unsigned long rate,
863 				    unsigned long parent_rate)
864 {
865 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
866 	const struct freq_tbl *f;
867 
868 	f = qcom_find_freq(rcg->freq_tbl, rate);
869 	if (!f)
870 		return -EINVAL;
871 
872 	/*
873 	 * In case clock is disabled, update the CFG, M, N and D registers
874 	 * and don't hit the update bit of CMD register.
875 	 */
876 	if (!__clk_is_enabled(hw->clk))
877 		return __clk_rcg2_configure(rcg, f);
878 
879 	return clk_rcg2_shared_force_enable_clear(hw, f);
880 }
881 
clk_rcg2_shared_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)882 static int clk_rcg2_shared_set_rate_and_parent(struct clk_hw *hw,
883 		unsigned long rate, unsigned long parent_rate, u8 index)
884 {
885 	return clk_rcg2_shared_set_rate(hw, rate, parent_rate);
886 }
887 
clk_rcg2_shared_enable(struct clk_hw * hw)888 static int clk_rcg2_shared_enable(struct clk_hw *hw)
889 {
890 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
891 	int ret;
892 
893 	/*
894 	 * Set the update bit because required configuration has already
895 	 * been written in clk_rcg2_shared_set_rate()
896 	 */
897 	ret = clk_rcg2_set_force_enable(hw);
898 	if (ret)
899 		return ret;
900 
901 	ret = update_config(rcg);
902 	if (ret)
903 		return ret;
904 
905 	return clk_rcg2_clear_force_enable(hw);
906 }
907 
clk_rcg2_shared_disable(struct clk_hw * hw)908 static void clk_rcg2_shared_disable(struct clk_hw *hw)
909 {
910 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
911 	u32 cfg;
912 
913 	/*
914 	 * Store current configuration as switching to safe source would clear
915 	 * the SRC and DIV of CFG register
916 	 */
917 	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
918 
919 	/*
920 	 * Park the RCG at a safe configuration - sourced off of safe source.
921 	 * Force enable and disable the RCG while configuring it to safeguard
922 	 * against any update signal coming from the downstream clock.
923 	 * The current parent is still prepared and enabled at this point, and
924 	 * the safe source is always on while application processor subsystem
925 	 * is online. Therefore, the RCG can safely switch its parent.
926 	 */
927 	clk_rcg2_set_force_enable(hw);
928 
929 	regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
930 		     rcg->safe_src_index << CFG_SRC_SEL_SHIFT);
931 
932 	update_config(rcg);
933 
934 	clk_rcg2_clear_force_enable(hw);
935 
936 	/* Write back the stored configuration corresponding to current rate */
937 	regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg);
938 }
939 
940 const struct clk_ops clk_rcg2_shared_ops = {
941 	.enable = clk_rcg2_shared_enable,
942 	.disable = clk_rcg2_shared_disable,
943 	.get_parent = clk_rcg2_get_parent,
944 	.set_parent = clk_rcg2_set_parent,
945 	.recalc_rate = clk_rcg2_recalc_rate,
946 	.determine_rate = clk_rcg2_determine_rate,
947 	.set_rate = clk_rcg2_shared_set_rate,
948 	.set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent,
949 };
950 EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops);
951 
952 /* Common APIs to be used for DFS based RCGR */
clk_rcg2_dfs_populate_freq(struct clk_hw * hw,unsigned int l,struct freq_tbl * f)953 static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l,
954 				       struct freq_tbl *f)
955 {
956 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
957 	struct clk_hw *p;
958 	unsigned long prate = 0;
959 	u32 val, mask, cfg, mode, src;
960 	int i, num_parents;
961 
962 	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg);
963 
964 	mask = BIT(rcg->hid_width) - 1;
965 	f->pre_div = 1;
966 	if (cfg & mask)
967 		f->pre_div = cfg & mask;
968 
969 	src = cfg & CFG_SRC_SEL_MASK;
970 	src >>= CFG_SRC_SEL_SHIFT;
971 
972 	num_parents = clk_hw_get_num_parents(hw);
973 	for (i = 0; i < num_parents; i++) {
974 		if (src == rcg->parent_map[i].cfg) {
975 			f->src = rcg->parent_map[i].src;
976 			p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i);
977 			prate = clk_hw_get_rate(p);
978 		}
979 	}
980 
981 	mode = cfg & CFG_MODE_MASK;
982 	mode >>= CFG_MODE_SHIFT;
983 	if (mode) {
984 		mask = BIT(rcg->mnd_width) - 1;
985 		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l),
986 			    &val);
987 		val &= mask;
988 		f->m = val;
989 
990 		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l),
991 			    &val);
992 		val = ~val;
993 		val &= mask;
994 		val += f->m;
995 		f->n = val;
996 	}
997 
998 	f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div);
999 }
1000 
clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 * rcg)1001 static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg)
1002 {
1003 	struct freq_tbl *freq_tbl;
1004 	int i;
1005 
1006 	/* Allocate space for 1 extra since table is NULL terminated */
1007 	freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL);
1008 	if (!freq_tbl)
1009 		return -ENOMEM;
1010 	rcg->freq_tbl = freq_tbl;
1011 
1012 	for (i = 0; i < MAX_PERF_LEVEL; i++)
1013 		clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i);
1014 
1015 	return 0;
1016 }
1017 
clk_rcg2_dfs_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)1018 static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw,
1019 				   struct clk_rate_request *req)
1020 {
1021 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1022 	int ret;
1023 
1024 	if (!rcg->freq_tbl) {
1025 		ret = clk_rcg2_dfs_populate_freq_table(rcg);
1026 		if (ret) {
1027 			pr_err("Failed to update DFS tables for %s\n",
1028 					clk_hw_get_name(hw));
1029 			return ret;
1030 		}
1031 	}
1032 
1033 	return clk_rcg2_determine_rate(hw, req);
1034 }
1035 
1036 static unsigned long
clk_rcg2_dfs_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)1037 clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1038 {
1039 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1040 	u32 level, mask, cfg, m = 0, n = 0, mode, pre_div;
1041 
1042 	regmap_read(rcg->clkr.regmap,
1043 		    rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level);
1044 	level &= GENMASK(4, 1);
1045 	level >>= 1;
1046 
1047 	if (rcg->freq_tbl)
1048 		return rcg->freq_tbl[level].freq;
1049 
1050 	/*
1051 	 * Assume that parent_rate is actually the parent because
1052 	 * we can't do any better at figuring it out when the table
1053 	 * hasn't been populated yet. We only populate the table
1054 	 * in determine_rate because we can't guarantee the parents
1055 	 * will be registered with the framework until then.
1056 	 */
1057 	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level),
1058 		    &cfg);
1059 
1060 	mask = BIT(rcg->hid_width) - 1;
1061 	pre_div = 1;
1062 	if (cfg & mask)
1063 		pre_div = cfg & mask;
1064 
1065 	mode = cfg & CFG_MODE_MASK;
1066 	mode >>= CFG_MODE_SHIFT;
1067 	if (mode) {
1068 		mask = BIT(rcg->mnd_width) - 1;
1069 		regmap_read(rcg->clkr.regmap,
1070 			    rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m);
1071 		m &= mask;
1072 
1073 		regmap_read(rcg->clkr.regmap,
1074 			    rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n);
1075 		n = ~n;
1076 		n &= mask;
1077 		n += m;
1078 	}
1079 
1080 	return calc_rate(parent_rate, m, n, mode, pre_div);
1081 }
1082 
1083 static const struct clk_ops clk_rcg2_dfs_ops = {
1084 	.is_enabled = clk_rcg2_is_enabled,
1085 	.get_parent = clk_rcg2_get_parent,
1086 	.determine_rate = clk_rcg2_dfs_determine_rate,
1087 	.recalc_rate = clk_rcg2_dfs_recalc_rate,
1088 };
1089 
clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data * data,struct regmap * regmap)1090 static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data,
1091 			       struct regmap *regmap)
1092 {
1093 	struct clk_rcg2 *rcg = data->rcg;
1094 	struct clk_init_data *init = data->init;
1095 	u32 val;
1096 	int ret;
1097 
1098 	ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val);
1099 	if (ret)
1100 		return -EINVAL;
1101 
1102 	if (!(val & SE_CMD_DFS_EN))
1103 		return 0;
1104 
1105 	/*
1106 	 * Rate changes with consumer writing a register in
1107 	 * their own I/O region
1108 	 */
1109 	init->flags |= CLK_GET_RATE_NOCACHE;
1110 	init->ops = &clk_rcg2_dfs_ops;
1111 
1112 	rcg->freq_tbl = NULL;
1113 
1114 	return 0;
1115 }
1116 
qcom_cc_register_rcg_dfs(struct regmap * regmap,const struct clk_rcg_dfs_data * rcgs,size_t len)1117 int qcom_cc_register_rcg_dfs(struct regmap *regmap,
1118 			     const struct clk_rcg_dfs_data *rcgs, size_t len)
1119 {
1120 	int i, ret;
1121 
1122 	for (i = 0; i < len; i++) {
1123 		ret = clk_rcg2_enable_dfs(&rcgs[i], regmap);
1124 		if (ret)
1125 			return ret;
1126 	}
1127 
1128 	return 0;
1129 }
1130 EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs);
1131 
clk_rcg2_dp_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)1132 static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate,
1133 			unsigned long parent_rate)
1134 {
1135 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1136 	struct freq_tbl f = { 0 };
1137 	u32 mask = BIT(rcg->hid_width) - 1;
1138 	u32 hid_div, cfg;
1139 	int i, num_parents = clk_hw_get_num_parents(hw);
1140 	unsigned long num, den;
1141 
1142 	rational_best_approximation(parent_rate, rate,
1143 			GENMASK(rcg->mnd_width - 1, 0),
1144 			GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1145 
1146 	if (!num || !den)
1147 		return -EINVAL;
1148 
1149 	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
1150 	hid_div = cfg;
1151 	cfg &= CFG_SRC_SEL_MASK;
1152 	cfg >>= CFG_SRC_SEL_SHIFT;
1153 
1154 	for (i = 0; i < num_parents; i++) {
1155 		if (cfg == rcg->parent_map[i].cfg) {
1156 			f.src = rcg->parent_map[i].src;
1157 			break;
1158 		}
1159 	}
1160 
1161 	f.pre_div = hid_div;
1162 	f.pre_div >>= CFG_SRC_DIV_SHIFT;
1163 	f.pre_div &= mask;
1164 
1165 	if (num != den) {
1166 		f.m = num;
1167 		f.n = den;
1168 	} else {
1169 		f.m = 0;
1170 		f.n = 0;
1171 	}
1172 
1173 	return clk_rcg2_configure(rcg, &f);
1174 }
1175 
clk_rcg2_dp_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)1176 static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw,
1177 		unsigned long rate, unsigned long parent_rate, u8 index)
1178 {
1179 	return clk_rcg2_dp_set_rate(hw, rate, parent_rate);
1180 }
1181 
clk_rcg2_dp_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)1182 static int clk_rcg2_dp_determine_rate(struct clk_hw *hw,
1183 				struct clk_rate_request *req)
1184 {
1185 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1186 	unsigned long num, den;
1187 	u64 tmp;
1188 
1189 	/* Parent rate is a fixed phy link rate */
1190 	rational_best_approximation(req->best_parent_rate, req->rate,
1191 			GENMASK(rcg->mnd_width - 1, 0),
1192 			GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1193 
1194 	if (!num || !den)
1195 		return -EINVAL;
1196 
1197 	tmp = req->best_parent_rate * num;
1198 	do_div(tmp, den);
1199 	req->rate = tmp;
1200 
1201 	return 0;
1202 }
1203 
1204 const struct clk_ops clk_dp_ops = {
1205 	.is_enabled = clk_rcg2_is_enabled,
1206 	.get_parent = clk_rcg2_get_parent,
1207 	.set_parent = clk_rcg2_set_parent,
1208 	.recalc_rate = clk_rcg2_recalc_rate,
1209 	.set_rate = clk_rcg2_dp_set_rate,
1210 	.set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent,
1211 	.determine_rate = clk_rcg2_dp_determine_rate,
1212 };
1213 EXPORT_SYMBOL_GPL(clk_dp_ops);
1214