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