1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Zynq UltraScale+ MPSoC Divider support
4  *
5  *  Copyright (C) 2016-2019 Xilinx
6  *
7  * Adjustable divider clock implementation
8  */
9 
10 #include <linux/clk.h>
11 #include <linux/clk-provider.h>
12 #include <linux/slab.h>
13 #include "clk-zynqmp.h"
14 
15 /*
16  * DOC: basic adjustable divider clock that cannot gate
17  *
18  * Traits of this clock:
19  * prepare - clk_prepare only ensures that parents are prepared
20  * enable - clk_enable only ensures that parents are enabled
21  * rate - rate is adjustable.  clk->rate = ceiling(parent->rate / divisor)
22  * parent - fixed parent.  No clk_set_parent support
23  */
24 
25 #define to_zynqmp_clk_divider(_hw)		\
26 	container_of(_hw, struct zynqmp_clk_divider, hw)
27 
28 #define CLK_FRAC		BIT(13) /* has a fractional parent */
29 #define CUSTOM_FLAG_CLK_FRAC	BIT(0) /* has a fractional parent in custom type flag */
30 
31 /**
32  * struct zynqmp_clk_divider - adjustable divider clock
33  * @hw:		handle between common and hardware-specific interfaces
34  * @flags:	Hardware specific flags
35  * @is_frac:	The divider is a fractional divider
36  * @clk_id:	Id of clock
37  * @div_type:	divisor type (TYPE_DIV1 or TYPE_DIV2)
38  * @max_div:	maximum supported divisor (fetched from firmware)
39  */
40 struct zynqmp_clk_divider {
41 	struct clk_hw hw;
42 	u8 flags;
43 	bool is_frac;
44 	u32 clk_id;
45 	u32 div_type;
46 	u16 max_div;
47 };
48 
zynqmp_divider_get_val(unsigned long parent_rate,unsigned long rate,u16 flags)49 static inline int zynqmp_divider_get_val(unsigned long parent_rate,
50 					 unsigned long rate, u16 flags)
51 {
52 	int up, down;
53 	unsigned long up_rate, down_rate;
54 
55 	if (flags & CLK_DIVIDER_POWER_OF_TWO) {
56 		up = DIV_ROUND_UP_ULL((u64)parent_rate, rate);
57 		down = DIV_ROUND_DOWN_ULL((u64)parent_rate, rate);
58 
59 		up = __roundup_pow_of_two(up);
60 		down = __rounddown_pow_of_two(down);
61 
62 		up_rate = DIV_ROUND_UP_ULL((u64)parent_rate, up);
63 		down_rate = DIV_ROUND_UP_ULL((u64)parent_rate, down);
64 
65 		return (rate - up_rate) <= (down_rate - rate) ? up : down;
66 
67 	} else {
68 		return DIV_ROUND_CLOSEST(parent_rate, rate);
69 	}
70 }
71 
72 /**
73  * zynqmp_clk_divider_recalc_rate() - Recalc rate of divider clock
74  * @hw:			handle between common and hardware-specific interfaces
75  * @parent_rate:	rate of parent clock
76  *
77  * Return: 0 on success else error+reason
78  */
zynqmp_clk_divider_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)79 static unsigned long zynqmp_clk_divider_recalc_rate(struct clk_hw *hw,
80 						    unsigned long parent_rate)
81 {
82 	struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw);
83 	const char *clk_name = clk_hw_get_name(hw);
84 	u32 clk_id = divider->clk_id;
85 	u32 div_type = divider->div_type;
86 	u32 div, value;
87 	int ret;
88 
89 	ret = zynqmp_pm_clock_getdivider(clk_id, &div);
90 
91 	if (ret)
92 		pr_debug("%s() get divider failed for %s, ret = %d\n",
93 			 __func__, clk_name, ret);
94 
95 	if (div_type == TYPE_DIV1)
96 		value = div & 0xFFFF;
97 	else
98 		value = div >> 16;
99 
100 	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
101 		value = 1 << value;
102 
103 	if (!value) {
104 		WARN(!(divider->flags & CLK_DIVIDER_ALLOW_ZERO),
105 		     "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
106 		     clk_name);
107 		return parent_rate;
108 	}
109 
110 	return DIV_ROUND_UP_ULL(parent_rate, value);
111 }
112 
zynqmp_get_divider2_val(struct clk_hw * hw,unsigned long rate,struct zynqmp_clk_divider * divider,u32 * bestdiv)113 static void zynqmp_get_divider2_val(struct clk_hw *hw,
114 				    unsigned long rate,
115 				    struct zynqmp_clk_divider *divider,
116 				    u32 *bestdiv)
117 {
118 	int div1;
119 	int div2;
120 	long error = LONG_MAX;
121 	unsigned long div1_prate;
122 	struct clk_hw *div1_parent_hw;
123 	struct zynqmp_clk_divider *pdivider;
124 	struct clk_hw *div2_parent_hw = clk_hw_get_parent(hw);
125 
126 	if (!div2_parent_hw)
127 		return;
128 
129 	pdivider = to_zynqmp_clk_divider(div2_parent_hw);
130 	if (!pdivider)
131 		return;
132 
133 	div1_parent_hw = clk_hw_get_parent(div2_parent_hw);
134 	if (!div1_parent_hw)
135 		return;
136 
137 	div1_prate = clk_hw_get_rate(div1_parent_hw);
138 	*bestdiv = 1;
139 	for (div1 = 1; div1 <= pdivider->max_div;) {
140 		for (div2 = 1; div2 <= divider->max_div;) {
141 			long new_error = ((div1_prate / div1) / div2) - rate;
142 
143 			if (abs(new_error) < abs(error)) {
144 				*bestdiv = div2;
145 				error = new_error;
146 			}
147 			if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
148 				div2 = div2 << 1;
149 			else
150 				div2++;
151 		}
152 		if (pdivider->flags & CLK_DIVIDER_POWER_OF_TWO)
153 			div1 = div1 << 1;
154 		else
155 			div1++;
156 	}
157 }
158 
159 /**
160  * zynqmp_clk_divider_round_rate() - Round rate of divider clock
161  * @hw:			handle between common and hardware-specific interfaces
162  * @rate:		rate of clock to be set
163  * @prate:		rate of parent clock
164  *
165  * Return: 0 on success else error+reason
166  */
zynqmp_clk_divider_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * prate)167 static long zynqmp_clk_divider_round_rate(struct clk_hw *hw,
168 					  unsigned long rate,
169 					  unsigned long *prate)
170 {
171 	struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw);
172 	const char *clk_name = clk_hw_get_name(hw);
173 	u32 clk_id = divider->clk_id;
174 	u32 div_type = divider->div_type;
175 	u32 bestdiv;
176 	int ret;
177 
178 	/* if read only, just return current value */
179 	if (divider->flags & CLK_DIVIDER_READ_ONLY) {
180 		ret = zynqmp_pm_clock_getdivider(clk_id, &bestdiv);
181 
182 		if (ret)
183 			pr_debug("%s() get divider failed for %s, ret = %d\n",
184 				 __func__, clk_name, ret);
185 		if (div_type == TYPE_DIV1)
186 			bestdiv = bestdiv & 0xFFFF;
187 		else
188 			bestdiv  = bestdiv >> 16;
189 
190 		if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
191 			bestdiv = 1 << bestdiv;
192 
193 		return DIV_ROUND_UP_ULL((u64)*prate, bestdiv);
194 	}
195 
196 	bestdiv = zynqmp_divider_get_val(*prate, rate, divider->flags);
197 
198 	/*
199 	 * In case of two divisors, compute best divider values and return
200 	 * divider2 value based on compute value. div1 will  be automatically
201 	 * set to optimum based on required total divider value.
202 	 */
203 	if (div_type == TYPE_DIV2 &&
204 	    (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
205 		zynqmp_get_divider2_val(hw, rate, divider, &bestdiv);
206 	}
207 
208 	if ((clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) && divider->is_frac)
209 		bestdiv = rate % *prate ? 1 : bestdiv;
210 
211 	bestdiv = min_t(u32, bestdiv, divider->max_div);
212 	*prate = rate * bestdiv;
213 
214 	return rate;
215 }
216 
217 /**
218  * zynqmp_clk_divider_set_rate() - Set rate of divider clock
219  * @hw:			handle between common and hardware-specific interfaces
220  * @rate:		rate of clock to be set
221  * @parent_rate:	rate of parent clock
222  *
223  * Return: 0 on success else error+reason
224  */
zynqmp_clk_divider_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)225 static int zynqmp_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
226 				       unsigned long parent_rate)
227 {
228 	struct zynqmp_clk_divider *divider = to_zynqmp_clk_divider(hw);
229 	const char *clk_name = clk_hw_get_name(hw);
230 	u32 clk_id = divider->clk_id;
231 	u32 div_type = divider->div_type;
232 	u32 value, div;
233 	int ret;
234 
235 	value = zynqmp_divider_get_val(parent_rate, rate, divider->flags);
236 	if (div_type == TYPE_DIV1) {
237 		div = value & 0xFFFF;
238 		div |= 0xffff << 16;
239 	} else {
240 		div = 0xffff;
241 		div |= value << 16;
242 	}
243 
244 	if (divider->flags & CLK_DIVIDER_POWER_OF_TWO)
245 		div = __ffs(div);
246 
247 	ret = zynqmp_pm_clock_setdivider(clk_id, div);
248 
249 	if (ret)
250 		pr_debug("%s() set divider failed for %s, ret = %d\n",
251 			 __func__, clk_name, ret);
252 
253 	return ret;
254 }
255 
256 static const struct clk_ops zynqmp_clk_divider_ops = {
257 	.recalc_rate = zynqmp_clk_divider_recalc_rate,
258 	.round_rate = zynqmp_clk_divider_round_rate,
259 	.set_rate = zynqmp_clk_divider_set_rate,
260 };
261 
262 static const struct clk_ops zynqmp_clk_divider_ro_ops = {
263 	.recalc_rate = zynqmp_clk_divider_recalc_rate,
264 	.round_rate = zynqmp_clk_divider_round_rate,
265 };
266 
267 /**
268  * zynqmp_clk_get_max_divisor() - Get maximum supported divisor from firmware.
269  * @clk_id:		Id of clock
270  * @type:		Divider type
271  *
272  * Return: Maximum divisor of a clock if query data is successful
273  *	   U16_MAX in case of query data is not success
274  */
zynqmp_clk_get_max_divisor(u32 clk_id,u32 type)275 static u32 zynqmp_clk_get_max_divisor(u32 clk_id, u32 type)
276 {
277 	struct zynqmp_pm_query_data qdata = {0};
278 	u32 ret_payload[PAYLOAD_ARG_CNT];
279 	int ret;
280 
281 	qdata.qid = PM_QID_CLOCK_GET_MAX_DIVISOR;
282 	qdata.arg1 = clk_id;
283 	qdata.arg2 = type;
284 	ret = zynqmp_pm_query_data(qdata, ret_payload);
285 	/*
286 	 * To maintain backward compatibility return maximum possible value
287 	 * (0xFFFF) if query for max divisor is not successful.
288 	 */
289 	if (ret)
290 		return U16_MAX;
291 
292 	return ret_payload[1];
293 }
294 
zynqmp_clk_map_divider_ccf_flags(const u32 zynqmp_type_flag)295 static inline unsigned long zynqmp_clk_map_divider_ccf_flags(
296 					       const u32 zynqmp_type_flag)
297 {
298 	unsigned long ccf_flag = 0;
299 
300 	if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ONE_BASED)
301 		ccf_flag |= CLK_DIVIDER_ONE_BASED;
302 	if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_POWER_OF_TWO)
303 		ccf_flag |= CLK_DIVIDER_POWER_OF_TWO;
304 	if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ALLOW_ZERO)
305 		ccf_flag |= CLK_DIVIDER_ALLOW_ZERO;
306 	if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_POWER_OF_TWO)
307 		ccf_flag |= CLK_DIVIDER_HIWORD_MASK;
308 	if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_ROUND_CLOSEST)
309 		ccf_flag |= CLK_DIVIDER_ROUND_CLOSEST;
310 	if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_READ_ONLY)
311 		ccf_flag |= CLK_DIVIDER_READ_ONLY;
312 	if (zynqmp_type_flag & ZYNQMP_CLK_DIVIDER_MAX_AT_ZERO)
313 		ccf_flag |= CLK_DIVIDER_MAX_AT_ZERO;
314 
315 	return ccf_flag;
316 }
317 
318 /**
319  * zynqmp_clk_register_divider() - Register a divider clock
320  * @name:		Name of this clock
321  * @clk_id:		Id of clock
322  * @parents:		Name of this clock's parents
323  * @num_parents:	Number of parents
324  * @nodes:		Clock topology node
325  *
326  * Return: clock hardware to registered clock divider
327  */
zynqmp_clk_register_divider(const char * name,u32 clk_id,const char * const * parents,u8 num_parents,const struct clock_topology * nodes)328 struct clk_hw *zynqmp_clk_register_divider(const char *name,
329 					   u32 clk_id,
330 					   const char * const *parents,
331 					   u8 num_parents,
332 					   const struct clock_topology *nodes)
333 {
334 	struct zynqmp_clk_divider *div;
335 	struct clk_hw *hw;
336 	struct clk_init_data init;
337 	int ret;
338 
339 	/* allocate the divider */
340 	div = kzalloc(sizeof(*div), GFP_KERNEL);
341 	if (!div)
342 		return ERR_PTR(-ENOMEM);
343 
344 	init.name = name;
345 	if (nodes->type_flag & CLK_DIVIDER_READ_ONLY)
346 		init.ops = &zynqmp_clk_divider_ro_ops;
347 	else
348 		init.ops = &zynqmp_clk_divider_ops;
349 
350 	init.flags = zynqmp_clk_map_common_ccf_flags(nodes->flag);
351 
352 	init.parent_names = parents;
353 	init.num_parents = 1;
354 
355 	/* struct clk_divider assignments */
356 	div->is_frac = !!((nodes->flag & CLK_FRAC) |
357 			  (nodes->custom_type_flag & CUSTOM_FLAG_CLK_FRAC));
358 	div->flags = zynqmp_clk_map_divider_ccf_flags(nodes->type_flag);
359 	div->hw.init = &init;
360 	div->clk_id = clk_id;
361 	div->div_type = nodes->type;
362 
363 	/*
364 	 * To achieve best possible rate, maximum limit of divider is required
365 	 * while computation.
366 	 */
367 	div->max_div = zynqmp_clk_get_max_divisor(clk_id, nodes->type);
368 
369 	hw = &div->hw;
370 	ret = clk_hw_register(NULL, hw);
371 	if (ret) {
372 		kfree(div);
373 		hw = ERR_PTR(ret);
374 	}
375 
376 	return hw;
377 }
378