1 /*
2  * Copyright (c) 2013 NVIDIA CORPORATION.  All rights reserved.
3  *
4  * This program is free software; you can redistribute it and/or modify it
5  * under the terms and conditions of the GNU General Public License,
6  * version 2, as published by the Free Software Foundation.
7  *
8  * This program is distributed in the hope it will be useful, but WITHOUT
9  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
10  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
11  * more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
15  */
16 
17 #include <linux/clk.h>
18 #include <linux/clk-provider.h>
19 #include <linux/err.h>
20 #include <linux/slab.h>
21 
clk_composite_get_parent(struct clk_hw * hw)22 static u8 clk_composite_get_parent(struct clk_hw *hw)
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->get_parent(mux_hw);
31 }
32 
clk_composite_set_parent(struct clk_hw * hw,u8 index)33 static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
34 {
35 	struct clk_composite *composite = to_clk_composite(hw);
36 	const struct clk_ops *mux_ops = composite->mux_ops;
37 	struct clk_hw *mux_hw = composite->mux_hw;
38 
39 	__clk_hw_set_clk(mux_hw, hw);
40 
41 	return mux_ops->set_parent(mux_hw, index);
42 }
43 
clk_composite_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)44 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
45 					    unsigned long parent_rate)
46 {
47 	struct clk_composite *composite = to_clk_composite(hw);
48 	const struct clk_ops *rate_ops = composite->rate_ops;
49 	struct clk_hw *rate_hw = composite->rate_hw;
50 
51 	__clk_hw_set_clk(rate_hw, hw);
52 
53 	return rate_ops->recalc_rate(rate_hw, parent_rate);
54 }
55 
clk_composite_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)56 static int clk_composite_determine_rate(struct clk_hw *hw,
57 					struct clk_rate_request *req)
58 {
59 	struct clk_composite *composite = to_clk_composite(hw);
60 	const struct clk_ops *rate_ops = composite->rate_ops;
61 	const struct clk_ops *mux_ops = composite->mux_ops;
62 	struct clk_hw *rate_hw = composite->rate_hw;
63 	struct clk_hw *mux_hw = composite->mux_hw;
64 	struct clk_hw *parent;
65 	unsigned long parent_rate;
66 	long tmp_rate, best_rate = 0;
67 	unsigned long rate_diff;
68 	unsigned long best_rate_diff = ULONG_MAX;
69 	long rate;
70 	int i;
71 
72 	if (rate_hw && rate_ops && rate_ops->determine_rate) {
73 		__clk_hw_set_clk(rate_hw, hw);
74 		return rate_ops->determine_rate(rate_hw, req);
75 	} else if (rate_hw && rate_ops && rate_ops->round_rate &&
76 		   mux_hw && mux_ops && mux_ops->set_parent) {
77 		req->best_parent_hw = NULL;
78 
79 		if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {
80 			parent = clk_hw_get_parent(mux_hw);
81 			req->best_parent_hw = parent;
82 			req->best_parent_rate = clk_hw_get_rate(parent);
83 
84 			rate = rate_ops->round_rate(rate_hw, req->rate,
85 						    &req->best_parent_rate);
86 			if (rate < 0)
87 				return rate;
88 
89 			req->rate = rate;
90 			return 0;
91 		}
92 
93 		for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) {
94 			parent = clk_hw_get_parent_by_index(mux_hw, i);
95 			if (!parent)
96 				continue;
97 
98 			parent_rate = clk_hw_get_rate(parent);
99 
100 			tmp_rate = rate_ops->round_rate(rate_hw, req->rate,
101 							&parent_rate);
102 			if (tmp_rate < 0)
103 				continue;
104 
105 			rate_diff = abs(req->rate - tmp_rate);
106 
107 			if (!rate_diff || !req->best_parent_hw
108 				       || best_rate_diff > rate_diff) {
109 				req->best_parent_hw = parent;
110 				req->best_parent_rate = parent_rate;
111 				best_rate_diff = rate_diff;
112 				best_rate = tmp_rate;
113 			}
114 
115 			if (!rate_diff)
116 				return 0;
117 		}
118 
119 		req->rate = best_rate;
120 		return 0;
121 	} else if (mux_hw && mux_ops && mux_ops->determine_rate) {
122 		__clk_hw_set_clk(mux_hw, hw);
123 		return mux_ops->determine_rate(mux_hw, req);
124 	} else {
125 		pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
126 		return -EINVAL;
127 	}
128 }
129 
clk_composite_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * prate)130 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
131 				  unsigned long *prate)
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->round_rate(rate_hw, rate, prate);
140 }
141 
clk_composite_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)142 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
143 			       unsigned long parent_rate)
144 {
145 	struct clk_composite *composite = to_clk_composite(hw);
146 	const struct clk_ops *rate_ops = composite->rate_ops;
147 	struct clk_hw *rate_hw = composite->rate_hw;
148 
149 	__clk_hw_set_clk(rate_hw, hw);
150 
151 	return rate_ops->set_rate(rate_hw, rate, parent_rate);
152 }
153 
clk_composite_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)154 static int clk_composite_set_rate_and_parent(struct clk_hw *hw,
155 					     unsigned long rate,
156 					     unsigned long parent_rate,
157 					     u8 index)
158 {
159 	struct clk_composite *composite = to_clk_composite(hw);
160 	const struct clk_ops *rate_ops = composite->rate_ops;
161 	const struct clk_ops *mux_ops = composite->mux_ops;
162 	struct clk_hw *rate_hw = composite->rate_hw;
163 	struct clk_hw *mux_hw = composite->mux_hw;
164 	unsigned long temp_rate;
165 
166 	__clk_hw_set_clk(rate_hw, hw);
167 	__clk_hw_set_clk(mux_hw, hw);
168 
169 	temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate);
170 	if (temp_rate > rate) {
171 		rate_ops->set_rate(rate_hw, rate, parent_rate);
172 		mux_ops->set_parent(mux_hw, index);
173 	} else {
174 		mux_ops->set_parent(mux_hw, index);
175 		rate_ops->set_rate(rate_hw, rate, parent_rate);
176 	}
177 
178 	return 0;
179 }
180 
clk_composite_is_enabled(struct clk_hw * hw)181 static int clk_composite_is_enabled(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->is_enabled(gate_hw);
190 }
191 
clk_composite_enable(struct clk_hw * hw)192 static int clk_composite_enable(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 	return gate_ops->enable(gate_hw);
201 }
202 
clk_composite_disable(struct clk_hw * hw)203 static void clk_composite_disable(struct clk_hw *hw)
204 {
205 	struct clk_composite *composite = to_clk_composite(hw);
206 	const struct clk_ops *gate_ops = composite->gate_ops;
207 	struct clk_hw *gate_hw = composite->gate_hw;
208 
209 	__clk_hw_set_clk(gate_hw, hw);
210 
211 	gate_ops->disable(gate_hw);
212 }
213 
clk_hw_register_composite(struct device * dev,const char * name,const char * const * parent_names,int num_parents,struct clk_hw * mux_hw,const struct clk_ops * mux_ops,struct clk_hw * rate_hw,const struct clk_ops * rate_ops,struct clk_hw * gate_hw,const struct clk_ops * gate_ops,unsigned long flags)214 struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name,
215 			const char * const *parent_names, int num_parents,
216 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
217 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
218 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
219 			unsigned long flags)
220 {
221 	struct clk_hw *hw;
222 	struct clk_init_data init;
223 	struct clk_composite *composite;
224 	struct clk_ops *clk_composite_ops;
225 	int ret;
226 
227 	composite = kzalloc(sizeof(*composite), GFP_KERNEL);
228 	if (!composite)
229 		return ERR_PTR(-ENOMEM);
230 
231 	init.name = name;
232 	init.flags = flags | CLK_IS_BASIC;
233 	init.parent_names = parent_names;
234 	init.num_parents = num_parents;
235 	hw = &composite->hw;
236 
237 	clk_composite_ops = &composite->ops;
238 
239 	if (mux_hw && mux_ops) {
240 		if (!mux_ops->get_parent) {
241 			hw = ERR_PTR(-EINVAL);
242 			goto err;
243 		}
244 
245 		composite->mux_hw = mux_hw;
246 		composite->mux_ops = mux_ops;
247 		clk_composite_ops->get_parent = clk_composite_get_parent;
248 		if (mux_ops->set_parent)
249 			clk_composite_ops->set_parent = clk_composite_set_parent;
250 		if (mux_ops->determine_rate)
251 			clk_composite_ops->determine_rate = clk_composite_determine_rate;
252 	}
253 
254 	if (rate_hw && rate_ops) {
255 		if (!rate_ops->recalc_rate) {
256 			hw = ERR_PTR(-EINVAL);
257 			goto err;
258 		}
259 		clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
260 
261 		if (rate_ops->determine_rate)
262 			clk_composite_ops->determine_rate =
263 				clk_composite_determine_rate;
264 		else if (rate_ops->round_rate)
265 			clk_composite_ops->round_rate =
266 				clk_composite_round_rate;
267 
268 		/* .set_rate requires either .round_rate or .determine_rate */
269 		if (rate_ops->set_rate) {
270 			if (rate_ops->determine_rate || rate_ops->round_rate)
271 				clk_composite_ops->set_rate =
272 						clk_composite_set_rate;
273 			else
274 				WARN(1, "%s: missing round_rate op is required\n",
275 						__func__);
276 		}
277 
278 		composite->rate_hw = rate_hw;
279 		composite->rate_ops = rate_ops;
280 	}
281 
282 	if (mux_hw && mux_ops && rate_hw && rate_ops) {
283 		if (mux_ops->set_parent && rate_ops->set_rate)
284 			clk_composite_ops->set_rate_and_parent =
285 			clk_composite_set_rate_and_parent;
286 	}
287 
288 	if (gate_hw && gate_ops) {
289 		if (!gate_ops->is_enabled || !gate_ops->enable ||
290 		    !gate_ops->disable) {
291 			hw = ERR_PTR(-EINVAL);
292 			goto err;
293 		}
294 
295 		composite->gate_hw = gate_hw;
296 		composite->gate_ops = gate_ops;
297 		clk_composite_ops->is_enabled = clk_composite_is_enabled;
298 		clk_composite_ops->enable = clk_composite_enable;
299 		clk_composite_ops->disable = clk_composite_disable;
300 	}
301 
302 	init.ops = clk_composite_ops;
303 	composite->hw.init = &init;
304 
305 	ret = clk_hw_register(dev, hw);
306 	if (ret) {
307 		hw = ERR_PTR(ret);
308 		goto err;
309 	}
310 
311 	if (composite->mux_hw)
312 		composite->mux_hw->clk = hw->clk;
313 
314 	if (composite->rate_hw)
315 		composite->rate_hw->clk = hw->clk;
316 
317 	if (composite->gate_hw)
318 		composite->gate_hw->clk = hw->clk;
319 
320 	return hw;
321 
322 err:
323 	kfree(composite);
324 	return hw;
325 }
326 
clk_register_composite(struct device * dev,const char * name,const char * const * parent_names,int num_parents,struct clk_hw * mux_hw,const struct clk_ops * mux_ops,struct clk_hw * rate_hw,const struct clk_ops * rate_ops,struct clk_hw * gate_hw,const struct clk_ops * gate_ops,unsigned long flags)327 struct clk *clk_register_composite(struct device *dev, const char *name,
328 			const char * const *parent_names, int num_parents,
329 			struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
330 			struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
331 			struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
332 			unsigned long flags)
333 {
334 	struct clk_hw *hw;
335 
336 	hw = clk_hw_register_composite(dev, name, parent_names, num_parents,
337 			mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops,
338 			flags);
339 	if (IS_ERR(hw))
340 		return ERR_CAST(hw);
341 	return hw->clk;
342 }
343 
clk_unregister_composite(struct clk * clk)344 void clk_unregister_composite(struct clk *clk)
345 {
346 	struct clk_composite *composite;
347 	struct clk_hw *hw;
348 
349 	hw = __clk_get_hw(clk);
350 	if (!hw)
351 		return;
352 
353 	composite = to_clk_composite(hw);
354 
355 	clk_unregister(clk);
356 	kfree(composite);
357 }
358