xref: /openbmc/linux/drivers/clk/clk-composite.c (revision 8795a739)
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/err.h>
8 #include <linux/slab.h>
9 
10 static u8 clk_composite_get_parent(struct clk_hw *hw)
11 {
12 	struct clk_composite *composite = to_clk_composite(hw);
13 	const struct clk_ops *mux_ops = composite->mux_ops;
14 	struct clk_hw *mux_hw = composite->mux_hw;
15 
16 	__clk_hw_set_clk(mux_hw, hw);
17 
18 	return mux_ops->get_parent(mux_hw);
19 }
20 
21 static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
22 {
23 	struct clk_composite *composite = to_clk_composite(hw);
24 	const struct clk_ops *mux_ops = composite->mux_ops;
25 	struct clk_hw *mux_hw = composite->mux_hw;
26 
27 	__clk_hw_set_clk(mux_hw, hw);
28 
29 	return mux_ops->set_parent(mux_hw, index);
30 }
31 
32 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
33 					    unsigned long parent_rate)
34 {
35 	struct clk_composite *composite = to_clk_composite(hw);
36 	const struct clk_ops *rate_ops = composite->rate_ops;
37 	struct clk_hw *rate_hw = composite->rate_hw;
38 
39 	__clk_hw_set_clk(rate_hw, hw);
40 
41 	return rate_ops->recalc_rate(rate_hw, parent_rate);
42 }
43 
44 static int clk_composite_determine_rate(struct clk_hw *hw,
45 					struct clk_rate_request *req)
46 {
47 	struct clk_composite *composite = to_clk_composite(hw);
48 	const struct clk_ops *rate_ops = composite->rate_ops;
49 	const struct clk_ops *mux_ops = composite->mux_ops;
50 	struct clk_hw *rate_hw = composite->rate_hw;
51 	struct clk_hw *mux_hw = composite->mux_hw;
52 	struct clk_hw *parent;
53 	unsigned long parent_rate;
54 	long tmp_rate, best_rate = 0;
55 	unsigned long rate_diff;
56 	unsigned long best_rate_diff = ULONG_MAX;
57 	long rate;
58 	int i;
59 
60 	if (rate_hw && rate_ops && rate_ops->determine_rate) {
61 		__clk_hw_set_clk(rate_hw, hw);
62 		return rate_ops->determine_rate(rate_hw, req);
63 	} else if (rate_hw && rate_ops && rate_ops->round_rate &&
64 		   mux_hw && mux_ops && mux_ops->set_parent) {
65 		req->best_parent_hw = NULL;
66 
67 		if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {
68 			parent = clk_hw_get_parent(mux_hw);
69 			req->best_parent_hw = parent;
70 			req->best_parent_rate = clk_hw_get_rate(parent);
71 
72 			rate = rate_ops->round_rate(rate_hw, req->rate,
73 						    &req->best_parent_rate);
74 			if (rate < 0)
75 				return rate;
76 
77 			req->rate = rate;
78 			return 0;
79 		}
80 
81 		for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) {
82 			parent = clk_hw_get_parent_by_index(mux_hw, i);
83 			if (!parent)
84 				continue;
85 
86 			parent_rate = clk_hw_get_rate(parent);
87 
88 			tmp_rate = rate_ops->round_rate(rate_hw, req->rate,
89 							&parent_rate);
90 			if (tmp_rate < 0)
91 				continue;
92 
93 			rate_diff = abs(req->rate - tmp_rate);
94 
95 			if (!rate_diff || !req->best_parent_hw
96 				       || best_rate_diff > rate_diff) {
97 				req->best_parent_hw = parent;
98 				req->best_parent_rate = parent_rate;
99 				best_rate_diff = rate_diff;
100 				best_rate = tmp_rate;
101 			}
102 
103 			if (!rate_diff)
104 				return 0;
105 		}
106 
107 		req->rate = best_rate;
108 		return 0;
109 	} else if (mux_hw && mux_ops && mux_ops->determine_rate) {
110 		__clk_hw_set_clk(mux_hw, hw);
111 		return mux_ops->determine_rate(mux_hw, req);
112 	} else {
113 		pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
114 		return -EINVAL;
115 	}
116 }
117 
118 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
119 				  unsigned long *prate)
120 {
121 	struct clk_composite *composite = to_clk_composite(hw);
122 	const struct clk_ops *rate_ops = composite->rate_ops;
123 	struct clk_hw *rate_hw = composite->rate_hw;
124 
125 	__clk_hw_set_clk(rate_hw, hw);
126 
127 	return rate_ops->round_rate(rate_hw, rate, prate);
128 }
129 
130 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
131 			       unsigned long parent_rate)
132 {
133 	struct clk_composite *composite = to_clk_composite(hw);
134 	const struct clk_ops *rate_ops = composite->rate_ops;
135 	struct clk_hw *rate_hw = composite->rate_hw;
136 
137 	__clk_hw_set_clk(rate_hw, hw);
138 
139 	return rate_ops->set_rate(rate_hw, rate, parent_rate);
140 }
141 
142 static int clk_composite_set_rate_and_parent(struct clk_hw *hw,
143 					     unsigned long rate,
144 					     unsigned long parent_rate,
145 					     u8 index)
146 {
147 	struct clk_composite *composite = to_clk_composite(hw);
148 	const struct clk_ops *rate_ops = composite->rate_ops;
149 	const struct clk_ops *mux_ops = composite->mux_ops;
150 	struct clk_hw *rate_hw = composite->rate_hw;
151 	struct clk_hw *mux_hw = composite->mux_hw;
152 	unsigned long temp_rate;
153 
154 	__clk_hw_set_clk(rate_hw, hw);
155 	__clk_hw_set_clk(mux_hw, hw);
156 
157 	temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate);
158 	if (temp_rate > rate) {
159 		rate_ops->set_rate(rate_hw, rate, parent_rate);
160 		mux_ops->set_parent(mux_hw, index);
161 	} else {
162 		mux_ops->set_parent(mux_hw, index);
163 		rate_ops->set_rate(rate_hw, rate, parent_rate);
164 	}
165 
166 	return 0;
167 }
168 
169 static int clk_composite_is_enabled(struct clk_hw *hw)
170 {
171 	struct clk_composite *composite = to_clk_composite(hw);
172 	const struct clk_ops *gate_ops = composite->gate_ops;
173 	struct clk_hw *gate_hw = composite->gate_hw;
174 
175 	__clk_hw_set_clk(gate_hw, hw);
176 
177 	return gate_ops->is_enabled(gate_hw);
178 }
179 
180 static int clk_composite_enable(struct clk_hw *hw)
181 {
182 	struct clk_composite *composite = to_clk_composite(hw);
183 	const struct clk_ops *gate_ops = composite->gate_ops;
184 	struct clk_hw *gate_hw = composite->gate_hw;
185 
186 	__clk_hw_set_clk(gate_hw, hw);
187 
188 	return gate_ops->enable(gate_hw);
189 }
190 
191 static void clk_composite_disable(struct clk_hw *hw)
192 {
193 	struct clk_composite *composite = to_clk_composite(hw);
194 	const struct clk_ops *gate_ops = composite->gate_ops;
195 	struct clk_hw *gate_hw = composite->gate_hw;
196 
197 	__clk_hw_set_clk(gate_hw, hw);
198 
199 	gate_ops->disable(gate_hw);
200 }
201 
202 struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name,
203 			const char * const *parent_names, int num_parents,
204 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
205 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
206 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
207 			unsigned long flags)
208 {
209 	struct clk_hw *hw;
210 	struct clk_init_data init;
211 	struct clk_composite *composite;
212 	struct clk_ops *clk_composite_ops;
213 	int ret;
214 
215 	composite = kzalloc(sizeof(*composite), GFP_KERNEL);
216 	if (!composite)
217 		return ERR_PTR(-ENOMEM);
218 
219 	init.name = name;
220 	init.flags = flags;
221 	init.parent_names = parent_names;
222 	init.num_parents = num_parents;
223 	hw = &composite->hw;
224 
225 	clk_composite_ops = &composite->ops;
226 
227 	if (mux_hw && mux_ops) {
228 		if (!mux_ops->get_parent) {
229 			hw = ERR_PTR(-EINVAL);
230 			goto err;
231 		}
232 
233 		composite->mux_hw = mux_hw;
234 		composite->mux_ops = mux_ops;
235 		clk_composite_ops->get_parent = clk_composite_get_parent;
236 		if (mux_ops->set_parent)
237 			clk_composite_ops->set_parent = clk_composite_set_parent;
238 		if (mux_ops->determine_rate)
239 			clk_composite_ops->determine_rate = clk_composite_determine_rate;
240 	}
241 
242 	if (rate_hw && rate_ops) {
243 		if (!rate_ops->recalc_rate) {
244 			hw = ERR_PTR(-EINVAL);
245 			goto err;
246 		}
247 		clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
248 
249 		if (rate_ops->determine_rate)
250 			clk_composite_ops->determine_rate =
251 				clk_composite_determine_rate;
252 		else if (rate_ops->round_rate)
253 			clk_composite_ops->round_rate =
254 				clk_composite_round_rate;
255 
256 		/* .set_rate requires either .round_rate or .determine_rate */
257 		if (rate_ops->set_rate) {
258 			if (rate_ops->determine_rate || rate_ops->round_rate)
259 				clk_composite_ops->set_rate =
260 						clk_composite_set_rate;
261 			else
262 				WARN(1, "%s: missing round_rate op is required\n",
263 						__func__);
264 		}
265 
266 		composite->rate_hw = rate_hw;
267 		composite->rate_ops = rate_ops;
268 	}
269 
270 	if (mux_hw && mux_ops && rate_hw && rate_ops) {
271 		if (mux_ops->set_parent && rate_ops->set_rate)
272 			clk_composite_ops->set_rate_and_parent =
273 			clk_composite_set_rate_and_parent;
274 	}
275 
276 	if (gate_hw && gate_ops) {
277 		if (!gate_ops->is_enabled || !gate_ops->enable ||
278 		    !gate_ops->disable) {
279 			hw = ERR_PTR(-EINVAL);
280 			goto err;
281 		}
282 
283 		composite->gate_hw = gate_hw;
284 		composite->gate_ops = gate_ops;
285 		clk_composite_ops->is_enabled = clk_composite_is_enabled;
286 		clk_composite_ops->enable = clk_composite_enable;
287 		clk_composite_ops->disable = clk_composite_disable;
288 	}
289 
290 	init.ops = clk_composite_ops;
291 	composite->hw.init = &init;
292 
293 	ret = clk_hw_register(dev, hw);
294 	if (ret) {
295 		hw = ERR_PTR(ret);
296 		goto err;
297 	}
298 
299 	if (composite->mux_hw)
300 		composite->mux_hw->clk = hw->clk;
301 
302 	if (composite->rate_hw)
303 		composite->rate_hw->clk = hw->clk;
304 
305 	if (composite->gate_hw)
306 		composite->gate_hw->clk = hw->clk;
307 
308 	return hw;
309 
310 err:
311 	kfree(composite);
312 	return hw;
313 }
314 
315 struct clk *clk_register_composite(struct device *dev, const char *name,
316 			const char * const *parent_names, int num_parents,
317 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
318 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
319 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
320 			unsigned long flags)
321 {
322 	struct clk_hw *hw;
323 
324 	hw = clk_hw_register_composite(dev, name, parent_names, num_parents,
325 			mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops,
326 			flags);
327 	if (IS_ERR(hw))
328 		return ERR_CAST(hw);
329 	return hw->clk;
330 }
331 
332 void clk_unregister_composite(struct clk *clk)
333 {
334 	struct clk_composite *composite;
335 	struct clk_hw *hw;
336 
337 	hw = __clk_get_hw(clk);
338 	if (!hw)
339 		return;
340 
341 	composite = to_clk_composite(hw);
342 
343 	clk_unregister(clk);
344 	kfree(composite);
345 }
346