xref: /linux/drivers/clk/clk-composite.c (revision c79c3c34f75d72a066e292b10aa50fc758c97c89) 
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (c) 2013 NVIDIA CORPORATION.  All rights reserved.
4  */
5 
6 #include <linux/clk-provider.h>
7 #include <linux/device.h>
8 #include <linux/err.h>
9 #include <linux/slab.h>
10 
11 static u8 clk_composite_get_parent(struct clk_hw *hw)
12 {
13 	struct clk_composite *composite = to_clk_composite(hw);
14 	const struct clk_ops *mux_ops = composite->mux_ops;
15 	struct clk_hw *mux_hw = composite->mux_hw;
16 
17 	__clk_hw_set_clk(mux_hw, hw);
18 
19 	return mux_ops->get_parent(mux_hw);
20 }
21 
22 static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
23 {
24 	struct clk_composite *composite = to_clk_composite(hw);
25 	const struct clk_ops *mux_ops = composite->mux_ops;
26 	struct clk_hw *mux_hw = composite->mux_hw;
27 
28 	__clk_hw_set_clk(mux_hw, hw);
29 
30 	return mux_ops->set_parent(mux_hw, index);
31 }
32 
33 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
34 					    unsigned long parent_rate)
35 {
36 	struct clk_composite *composite = to_clk_composite(hw);
37 	const struct clk_ops *rate_ops = composite->rate_ops;
38 	struct clk_hw *rate_hw = composite->rate_hw;
39 
40 	__clk_hw_set_clk(rate_hw, hw);
41 
42 	return rate_ops->recalc_rate(rate_hw, parent_rate);
43 }
44 
45 static int clk_composite_determine_rate(struct clk_hw *hw,
46 					struct clk_rate_request *req)
47 {
48 	struct clk_composite *composite = to_clk_composite(hw);
49 	const struct clk_ops *rate_ops = composite->rate_ops;
50 	const struct clk_ops *mux_ops = composite->mux_ops;
51 	struct clk_hw *rate_hw = composite->rate_hw;
52 	struct clk_hw *mux_hw = composite->mux_hw;
53 	struct clk_hw *parent;
54 	unsigned long parent_rate;
55 	long tmp_rate, best_rate = 0;
56 	unsigned long rate_diff;
57 	unsigned long best_rate_diff = ULONG_MAX;
58 	long rate;
59 	int i;
60 
61 	if (rate_hw && rate_ops && rate_ops->determine_rate) {
62 		__clk_hw_set_clk(rate_hw, hw);
63 		return rate_ops->determine_rate(rate_hw, req);
64 	} else if (rate_hw && rate_ops && rate_ops->round_rate &&
65 		   mux_hw && mux_ops && mux_ops->set_parent) {
66 		req->best_parent_hw = NULL;
67 
68 		if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {
69 			parent = clk_hw_get_parent(mux_hw);
70 			req->best_parent_hw = parent;
71 			req->best_parent_rate = clk_hw_get_rate(parent);
72 
73 			rate = rate_ops->round_rate(rate_hw, req->rate,
74 						    &req->best_parent_rate);
75 			if (rate < 0)
76 				return rate;
77 
78 			req->rate = rate;
79 			return 0;
80 		}
81 
82 		for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) {
83 			parent = clk_hw_get_parent_by_index(mux_hw, i);
84 			if (!parent)
85 				continue;
86 
87 			parent_rate = clk_hw_get_rate(parent);
88 
89 			tmp_rate = rate_ops->round_rate(rate_hw, req->rate,
90 							&parent_rate);
91 			if (tmp_rate < 0)
92 				continue;
93 
94 			rate_diff = abs(req->rate - tmp_rate);
95 
96 			if (!rate_diff || !req->best_parent_hw
97 				       || best_rate_diff > rate_diff) {
98 				req->best_parent_hw = parent;
99 				req->best_parent_rate = parent_rate;
100 				best_rate_diff = rate_diff;
101 				best_rate = tmp_rate;
102 			}
103 
104 			if (!rate_diff)
105 				return 0;
106 		}
107 
108 		req->rate = best_rate;
109 		return 0;
110 	} else if (mux_hw && mux_ops && mux_ops->determine_rate) {
111 		__clk_hw_set_clk(mux_hw, hw);
112 		return mux_ops->determine_rate(mux_hw, req);
113 	} else {
114 		pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
115 		return -EINVAL;
116 	}
117 }
118 
119 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
120 				  unsigned long *prate)
121 {
122 	struct clk_composite *composite = to_clk_composite(hw);
123 	const struct clk_ops *rate_ops = composite->rate_ops;
124 	struct clk_hw *rate_hw = composite->rate_hw;
125 
126 	__clk_hw_set_clk(rate_hw, hw);
127 
128 	return rate_ops->round_rate(rate_hw, rate, prate);
129 }
130 
131 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
132 			       unsigned long parent_rate)
133 {
134 	struct clk_composite *composite = to_clk_composite(hw);
135 	const struct clk_ops *rate_ops = composite->rate_ops;
136 	struct clk_hw *rate_hw = composite->rate_hw;
137 
138 	__clk_hw_set_clk(rate_hw, hw);
139 
140 	return rate_ops->set_rate(rate_hw, rate, parent_rate);
141 }
142 
143 static int clk_composite_set_rate_and_parent(struct clk_hw *hw,
144 					     unsigned long rate,
145 					     unsigned long parent_rate,
146 					     u8 index)
147 {
148 	struct clk_composite *composite = to_clk_composite(hw);
149 	const struct clk_ops *rate_ops = composite->rate_ops;
150 	const struct clk_ops *mux_ops = composite->mux_ops;
151 	struct clk_hw *rate_hw = composite->rate_hw;
152 	struct clk_hw *mux_hw = composite->mux_hw;
153 	unsigned long temp_rate;
154 
155 	__clk_hw_set_clk(rate_hw, hw);
156 	__clk_hw_set_clk(mux_hw, hw);
157 
158 	temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate);
159 	if (temp_rate > rate) {
160 		rate_ops->set_rate(rate_hw, rate, parent_rate);
161 		mux_ops->set_parent(mux_hw, index);
162 	} else {
163 		mux_ops->set_parent(mux_hw, index);
164 		rate_ops->set_rate(rate_hw, rate, parent_rate);
165 	}
166 
167 	return 0;
168 }
169 
170 static int clk_composite_is_enabled(struct clk_hw *hw)
171 {
172 	struct clk_composite *composite = to_clk_composite(hw);
173 	const struct clk_ops *gate_ops = composite->gate_ops;
174 	struct clk_hw *gate_hw = composite->gate_hw;
175 
176 	__clk_hw_set_clk(gate_hw, hw);
177 
178 	return gate_ops->is_enabled(gate_hw);
179 }
180 
181 static int clk_composite_enable(struct clk_hw *hw)
182 {
183 	struct clk_composite *composite = to_clk_composite(hw);
184 	const struct clk_ops *gate_ops = composite->gate_ops;
185 	struct clk_hw *gate_hw = composite->gate_hw;
186 
187 	__clk_hw_set_clk(gate_hw, hw);
188 
189 	return gate_ops->enable(gate_hw);
190 }
191 
192 static void clk_composite_disable(struct clk_hw *hw)
193 {
194 	struct clk_composite *composite = to_clk_composite(hw);
195 	const struct clk_ops *gate_ops = composite->gate_ops;
196 	struct clk_hw *gate_hw = composite->gate_hw;
197 
198 	__clk_hw_set_clk(gate_hw, hw);
199 
200 	gate_ops->disable(gate_hw);
201 }
202 
203 static struct clk_hw *__clk_hw_register_composite(struct device *dev,
204 			const char *name, const char * const *parent_names,
205 			const struct clk_parent_data *pdata, int num_parents,
206 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
207 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
208 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
209 			unsigned long flags)
210 {
211 	struct clk_hw *hw;
212 	struct clk_init_data init = {};
213 	struct clk_composite *composite;
214 	struct clk_ops *clk_composite_ops;
215 	int ret;
216 
217 	composite = kzalloc(sizeof(*composite), GFP_KERNEL);
218 	if (!composite)
219 		return ERR_PTR(-ENOMEM);
220 
221 	init.name = name;
222 	init.flags = flags;
223 	if (parent_names)
224 		init.parent_names = parent_names;
225 	else
226 		init.parent_data = pdata;
227 	init.num_parents = num_parents;
228 	hw = &composite->hw;
229 
230 	clk_composite_ops = &composite->ops;
231 
232 	if (mux_hw && mux_ops) {
233 		if (!mux_ops->get_parent) {
234 			hw = ERR_PTR(-EINVAL);
235 			goto err;
236 		}
237 
238 		composite->mux_hw = mux_hw;
239 		composite->mux_ops = mux_ops;
240 		clk_composite_ops->get_parent = clk_composite_get_parent;
241 		if (mux_ops->set_parent)
242 			clk_composite_ops->set_parent = clk_composite_set_parent;
243 		if (mux_ops->determine_rate)
244 			clk_composite_ops->determine_rate = clk_composite_determine_rate;
245 	}
246 
247 	if (rate_hw && rate_ops) {
248 		if (!rate_ops->recalc_rate) {
249 			hw = ERR_PTR(-EINVAL);
250 			goto err;
251 		}
252 		clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
253 
254 		if (rate_ops->determine_rate)
255 			clk_composite_ops->determine_rate =
256 				clk_composite_determine_rate;
257 		else if (rate_ops->round_rate)
258 			clk_composite_ops->round_rate =
259 				clk_composite_round_rate;
260 
261 		/* .set_rate requires either .round_rate or .determine_rate */
262 		if (rate_ops->set_rate) {
263 			if (rate_ops->determine_rate || rate_ops->round_rate)
264 				clk_composite_ops->set_rate =
265 						clk_composite_set_rate;
266 			else
267 				WARN(1, "%s: missing round_rate op is required\n",
268 						__func__);
269 		}
270 
271 		composite->rate_hw = rate_hw;
272 		composite->rate_ops = rate_ops;
273 	}
274 
275 	if (mux_hw && mux_ops && rate_hw && rate_ops) {
276 		if (mux_ops->set_parent && rate_ops->set_rate)
277 			clk_composite_ops->set_rate_and_parent =
278 			clk_composite_set_rate_and_parent;
279 	}
280 
281 	if (gate_hw && gate_ops) {
282 		if (!gate_ops->is_enabled || !gate_ops->enable ||
283 		    !gate_ops->disable) {
284 			hw = ERR_PTR(-EINVAL);
285 			goto err;
286 		}
287 
288 		composite->gate_hw = gate_hw;
289 		composite->gate_ops = gate_ops;
290 		clk_composite_ops->is_enabled = clk_composite_is_enabled;
291 		clk_composite_ops->enable = clk_composite_enable;
292 		clk_composite_ops->disable = clk_composite_disable;
293 	}
294 
295 	init.ops = clk_composite_ops;
296 	composite->hw.init = &init;
297 
298 	ret = clk_hw_register(dev, hw);
299 	if (ret) {
300 		hw = ERR_PTR(ret);
301 		goto err;
302 	}
303 
304 	if (composite->mux_hw)
305 		composite->mux_hw->clk = hw->clk;
306 
307 	if (composite->rate_hw)
308 		composite->rate_hw->clk = hw->clk;
309 
310 	if (composite->gate_hw)
311 		composite->gate_hw->clk = hw->clk;
312 
313 	return hw;
314 
315 err:
316 	kfree(composite);
317 	return hw;
318 }
319 
320 struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name,
321 			const char * const *parent_names, int num_parents,
322 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
323 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
324 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
325 			unsigned long flags)
326 {
327 	return __clk_hw_register_composite(dev, name, parent_names, NULL,
328 					   num_parents, mux_hw, mux_ops,
329 					   rate_hw, rate_ops, gate_hw,
330 					   gate_ops, flags);
331 }
332 EXPORT_SYMBOL_GPL(clk_hw_register_composite);
333 
334 struct clk_hw *clk_hw_register_composite_pdata(struct device *dev,
335 			const char *name,
336 			const struct clk_parent_data *parent_data,
337 			int num_parents,
338 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
339 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
340 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
341 			unsigned long flags)
342 {
343 	return __clk_hw_register_composite(dev, name, NULL, parent_data,
344 					   num_parents, mux_hw, mux_ops,
345 					   rate_hw, rate_ops, gate_hw,
346 					   gate_ops, flags);
347 }
348 
349 struct clk *clk_register_composite(struct device *dev, const char *name,
350 			const char * const *parent_names, int num_parents,
351 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
352 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
353 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
354 			unsigned long flags)
355 {
356 	struct clk_hw *hw;
357 
358 	hw = clk_hw_register_composite(dev, name, parent_names, num_parents,
359 			mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops,
360 			flags);
361 	if (IS_ERR(hw))
362 		return ERR_CAST(hw);
363 	return hw->clk;
364 }
365 
366 struct clk *clk_register_composite_pdata(struct device *dev, const char *name,
367 			const struct clk_parent_data *parent_data,
368 			int num_parents,
369 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
370 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
371 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
372 			unsigned long flags)
373 {
374 	struct clk_hw *hw;
375 
376 	hw = clk_hw_register_composite_pdata(dev, name, parent_data,
377 			num_parents, mux_hw, mux_ops, rate_hw, rate_ops,
378 			gate_hw, gate_ops, flags);
379 	if (IS_ERR(hw))
380 		return ERR_CAST(hw);
381 	return hw->clk;
382 }
383 
384 void clk_unregister_composite(struct clk *clk)
385 {
386 	struct clk_composite *composite;
387 	struct clk_hw *hw;
388 
389 	hw = __clk_get_hw(clk);
390 	if (!hw)
391 		return;
392 
393 	composite = to_clk_composite(hw);
394 
395 	clk_unregister(clk);
396 	kfree(composite);
397 }
398 
399 void clk_hw_unregister_composite(struct clk_hw *hw)
400 {
401 	struct clk_composite *composite;
402 
403 	composite = to_clk_composite(hw);
404 
405 	clk_hw_unregister(hw);
406 	kfree(composite);
407 }
408 EXPORT_SYMBOL_GPL(clk_hw_unregister_composite);
409 
410 static void devm_clk_hw_release_composite(struct device *dev, void *res)
411 {
412 	clk_hw_unregister_composite(*(struct clk_hw **)res);
413 }
414 
415 static struct clk_hw *__devm_clk_hw_register_composite(struct device *dev,
416 			const char *name, const char * const *parent_names,
417 			const struct clk_parent_data *pdata, int num_parents,
418 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
419 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
420 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
421 			unsigned long flags)
422 {
423 	struct clk_hw **ptr, *hw;
424 
425 	ptr = devres_alloc(devm_clk_hw_release_composite, sizeof(*ptr),
426 			   GFP_KERNEL);
427 	if (!ptr)
428 		return ERR_PTR(-ENOMEM);
429 
430 	hw = __clk_hw_register_composite(dev, name, parent_names, pdata,
431 					 num_parents, mux_hw, mux_ops, rate_hw,
432 					 rate_ops, gate_hw, gate_ops, flags);
433 
434 	if (!IS_ERR(hw)) {
435 		*ptr = hw;
436 		devres_add(dev, ptr);
437 	} else {
438 		devres_free(ptr);
439 	}
440 
441 	return hw;
442 }
443 
444 struct clk_hw *devm_clk_hw_register_composite_pdata(struct device *dev,
445 			const char *name,
446 			const struct clk_parent_data *parent_data,
447 			int num_parents,
448 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
449 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
450 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
451 			unsigned long flags)
452 {
453 	return __devm_clk_hw_register_composite(dev, name, NULL, parent_data,
454 						num_parents, mux_hw, mux_ops,
455 						rate_hw, rate_ops, gate_hw,
456 						gate_ops, flags);
457 }
458