1 // SPDX-License-Identifier: GPL-2.0+
2 //
3 // OWL factor clock driver
4 //
5 // Copyright (c) 2014 Actions Semi Inc.
6 // Author: David Liu <liuwei@actions-semi.com>
7 //
8 // Copyright (c) 2018 Linaro Ltd.
9 // Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
10 
11 #include <linux/clk-provider.h>
12 #include <linux/regmap.h>
13 
14 #include "owl-factor.h"
15 
_get_table_maxval(const struct clk_factor_table * table)16 static unsigned int _get_table_maxval(const struct clk_factor_table *table)
17 {
18 	unsigned int maxval = 0;
19 	const struct clk_factor_table *clkt;
20 
21 	for (clkt = table; clkt->div; clkt++)
22 		if (clkt->val > maxval)
23 			maxval = clkt->val;
24 	return maxval;
25 }
26 
_get_table_div_mul(const struct clk_factor_table * table,unsigned int val,unsigned int * mul,unsigned int * div)27 static int _get_table_div_mul(const struct clk_factor_table *table,
28 			unsigned int val, unsigned int *mul, unsigned int *div)
29 {
30 	const struct clk_factor_table *clkt;
31 
32 	for (clkt = table; clkt->div; clkt++) {
33 		if (clkt->val == val) {
34 			*mul = clkt->mul;
35 			*div = clkt->div;
36 			return 1;
37 		}
38 	}
39 
40 	return 0;
41 }
42 
_get_table_val(const struct clk_factor_table * table,unsigned long rate,unsigned long parent_rate)43 static unsigned int _get_table_val(const struct clk_factor_table *table,
44 			unsigned long rate, unsigned long parent_rate)
45 {
46 	const struct clk_factor_table *clkt;
47 	int val = -1;
48 	u64 calc_rate;
49 
50 	for (clkt = table; clkt->div; clkt++) {
51 		calc_rate = parent_rate * clkt->mul;
52 		do_div(calc_rate, clkt->div);
53 
54 		if ((unsigned long)calc_rate <= rate) {
55 			val = clkt->val;
56 			break;
57 		}
58 	}
59 
60 	if (val == -1)
61 		val = _get_table_maxval(table);
62 
63 	return val;
64 }
65 
owl_clk_val_best(const struct owl_factor_hw * factor_hw,struct clk_hw * hw,unsigned long rate,unsigned long * best_parent_rate)66 static int owl_clk_val_best(const struct owl_factor_hw *factor_hw,
67 			struct clk_hw *hw, unsigned long rate,
68 			unsigned long *best_parent_rate)
69 {
70 	const struct clk_factor_table *clkt = factor_hw->table;
71 	unsigned long parent_rate, try_parent_rate, best = 0, cur_rate;
72 	unsigned long parent_rate_saved = *best_parent_rate;
73 	int bestval = 0;
74 
75 	if (!rate)
76 		rate = 1;
77 
78 	if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
79 		parent_rate = *best_parent_rate;
80 		bestval = _get_table_val(clkt, rate, parent_rate);
81 		return bestval;
82 	}
83 
84 	for (clkt = factor_hw->table; clkt->div; clkt++) {
85 		try_parent_rate = rate * clkt->div / clkt->mul;
86 
87 		if (try_parent_rate == parent_rate_saved) {
88 			pr_debug("%s: [%d %d %d] found try_parent_rate %ld\n",
89 				__func__, clkt->val, clkt->mul, clkt->div,
90 				try_parent_rate);
91 			/*
92 			 * It's the most ideal case if the requested rate can be
93 			 * divided from parent clock without any need to change
94 			 * parent rate, so return the divider immediately.
95 			 */
96 			*best_parent_rate = parent_rate_saved;
97 			return clkt->val;
98 		}
99 
100 		parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
101 				try_parent_rate);
102 		cur_rate = DIV_ROUND_UP(parent_rate, clkt->div) * clkt->mul;
103 		if (cur_rate <= rate && cur_rate > best) {
104 			bestval = clkt->val;
105 			best = cur_rate;
106 			*best_parent_rate = parent_rate;
107 		}
108 	}
109 
110 	if (!bestval) {
111 		bestval = _get_table_maxval(clkt);
112 		*best_parent_rate = clk_hw_round_rate(
113 				clk_hw_get_parent(hw), 1);
114 	}
115 
116 	return bestval;
117 }
118 
owl_factor_helper_round_rate(struct owl_clk_common * common,const struct owl_factor_hw * factor_hw,unsigned long rate,unsigned long * parent_rate)119 long owl_factor_helper_round_rate(struct owl_clk_common *common,
120 				const struct owl_factor_hw *factor_hw,
121 				unsigned long rate,
122 				unsigned long *parent_rate)
123 {
124 	const struct clk_factor_table *clkt = factor_hw->table;
125 	unsigned int val, mul = 0, div = 1;
126 
127 	val = owl_clk_val_best(factor_hw, &common->hw, rate, parent_rate);
128 	_get_table_div_mul(clkt, val, &mul, &div);
129 
130 	return *parent_rate * mul / div;
131 }
132 
owl_factor_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)133 static long owl_factor_round_rate(struct clk_hw *hw, unsigned long rate,
134 			unsigned long *parent_rate)
135 {
136 	struct owl_factor *factor = hw_to_owl_factor(hw);
137 	struct owl_factor_hw *factor_hw = &factor->factor_hw;
138 
139 	return owl_factor_helper_round_rate(&factor->common, factor_hw,
140 					rate, parent_rate);
141 }
142 
owl_factor_helper_recalc_rate(struct owl_clk_common * common,const struct owl_factor_hw * factor_hw,unsigned long parent_rate)143 unsigned long owl_factor_helper_recalc_rate(struct owl_clk_common *common,
144 					 const struct owl_factor_hw *factor_hw,
145 					 unsigned long parent_rate)
146 {
147 	const struct clk_factor_table *clkt = factor_hw->table;
148 	unsigned long long int rate;
149 	u32 reg, val, mul, div;
150 
151 	div = 0;
152 	mul = 0;
153 
154 	regmap_read(common->regmap, factor_hw->reg, &reg);
155 
156 	val = reg >> factor_hw->shift;
157 	val &= div_mask(factor_hw);
158 
159 	_get_table_div_mul(clkt, val, &mul, &div);
160 	if (!div) {
161 		WARN(!(factor_hw->fct_flags & CLK_DIVIDER_ALLOW_ZERO),
162 			"%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
163 			__clk_get_name(common->hw.clk));
164 		return parent_rate;
165 	}
166 
167 	rate = (unsigned long long int)parent_rate * mul;
168 	do_div(rate, div);
169 
170 	return rate;
171 }
172 
owl_factor_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)173 static unsigned long owl_factor_recalc_rate(struct clk_hw *hw,
174 			unsigned long parent_rate)
175 {
176 	struct owl_factor *factor = hw_to_owl_factor(hw);
177 	struct owl_factor_hw *factor_hw = &factor->factor_hw;
178 	struct owl_clk_common *common = &factor->common;
179 
180 	return owl_factor_helper_recalc_rate(common, factor_hw, parent_rate);
181 }
182 
owl_factor_helper_set_rate(const struct owl_clk_common * common,const struct owl_factor_hw * factor_hw,unsigned long rate,unsigned long parent_rate)183 int owl_factor_helper_set_rate(const struct owl_clk_common *common,
184 				const struct owl_factor_hw *factor_hw,
185 				unsigned long rate,
186 				unsigned long parent_rate)
187 {
188 	u32 val, reg;
189 
190 	val = _get_table_val(factor_hw->table, rate, parent_rate);
191 
192 	if (val > div_mask(factor_hw))
193 		val = div_mask(factor_hw);
194 
195 	regmap_read(common->regmap, factor_hw->reg, &reg);
196 
197 	reg &= ~(div_mask(factor_hw) << factor_hw->shift);
198 	reg |= val << factor_hw->shift;
199 
200 	regmap_write(common->regmap, factor_hw->reg, reg);
201 
202 	return 0;
203 }
204 
owl_factor_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)205 static int owl_factor_set_rate(struct clk_hw *hw, unsigned long rate,
206 			       unsigned long parent_rate)
207 {
208 	struct owl_factor *factor = hw_to_owl_factor(hw);
209 	struct owl_factor_hw *factor_hw = &factor->factor_hw;
210 	struct owl_clk_common *common = &factor->common;
211 
212 	return owl_factor_helper_set_rate(common, factor_hw,
213 					rate, parent_rate);
214 }
215 
216 const struct clk_ops owl_factor_ops = {
217 	.round_rate	= owl_factor_round_rate,
218 	.recalc_rate	= owl_factor_recalc_rate,
219 	.set_rate	= owl_factor_set_rate,
220 };
221