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
22 #define to_clk_composite(_hw) container_of(_hw, struct clk_composite, hw)
23
clk_composite_get_parent(struct clk_hw * hw)24 static u8 clk_composite_get_parent(struct clk_hw *hw)
25 {
26 struct clk_composite *composite = to_clk_composite(hw);
27 const struct clk_ops *mux_ops = composite->mux_ops;
28 struct clk_hw *mux_hw = composite->mux_hw;
29
30 mux_hw->clk = hw->clk;
31
32 return mux_ops->get_parent(mux_hw);
33 }
34
clk_composite_set_parent(struct clk_hw * hw,u8 index)35 static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
36 {
37 struct clk_composite *composite = to_clk_composite(hw);
38 const struct clk_ops *mux_ops = composite->mux_ops;
39 struct clk_hw *mux_hw = composite->mux_hw;
40
41 mux_hw->clk = hw->clk;
42
43 return mux_ops->set_parent(mux_hw, index);
44 }
45
clk_composite_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)46 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
47 unsigned long parent_rate)
48 {
49 struct clk_composite *composite = to_clk_composite(hw);
50 const struct clk_ops *rate_ops = composite->rate_ops;
51 struct clk_hw *rate_hw = composite->rate_hw;
52
53 rate_hw->clk = hw->clk;
54
55 return rate_ops->recalc_rate(rate_hw, parent_rate);
56 }
57
clk_composite_determine_rate(struct clk_hw * hw,unsigned long rate,unsigned long * best_parent_rate,struct clk ** best_parent_p)58 static long clk_composite_determine_rate(struct clk_hw *hw, unsigned long rate,
59 unsigned long *best_parent_rate,
60 struct clk **best_parent_p)
61 {
62 struct clk_composite *composite = to_clk_composite(hw);
63 const struct clk_ops *rate_ops = composite->rate_ops;
64 const struct clk_ops *mux_ops = composite->mux_ops;
65 struct clk_hw *rate_hw = composite->rate_hw;
66 struct clk_hw *mux_hw = composite->mux_hw;
67 struct clk *parent;
68 unsigned long parent_rate;
69 long tmp_rate, best_rate = 0;
70 unsigned long rate_diff;
71 unsigned long best_rate_diff = ULONG_MAX;
72 int i;
73
74 if (rate_hw && rate_ops && rate_ops->determine_rate) {
75 rate_hw->clk = hw->clk;
76 return rate_ops->determine_rate(rate_hw, rate, best_parent_rate,
77 best_parent_p);
78 } else if (rate_hw && rate_ops && rate_ops->round_rate &&
79 mux_hw && mux_ops && mux_ops->set_parent) {
80 *best_parent_p = NULL;
81
82 if (__clk_get_flags(hw->clk) & CLK_SET_RATE_NO_REPARENT) {
83 *best_parent_p = clk_get_parent(mux_hw->clk);
84 *best_parent_rate = __clk_get_rate(*best_parent_p);
85
86 return rate_ops->round_rate(rate_hw, rate,
87 best_parent_rate);
88 }
89
90 for (i = 0; i < __clk_get_num_parents(mux_hw->clk); i++) {
91 parent = clk_get_parent_by_index(mux_hw->clk, i);
92 if (!parent)
93 continue;
94
95 parent_rate = __clk_get_rate(parent);
96
97 tmp_rate = rate_ops->round_rate(rate_hw, rate,
98 &parent_rate);
99 if (tmp_rate < 0)
100 continue;
101
102 rate_diff = abs(rate - tmp_rate);
103
104 if (!rate_diff || !*best_parent_p
105 || best_rate_diff > rate_diff) {
106 *best_parent_p = parent;
107 *best_parent_rate = parent_rate;
108 best_rate_diff = rate_diff;
109 best_rate = tmp_rate;
110 }
111
112 if (!rate_diff)
113 return rate;
114 }
115
116 return best_rate;
117 } else if (mux_hw && mux_ops && mux_ops->determine_rate) {
118 mux_hw->clk = hw->clk;
119 return mux_ops->determine_rate(mux_hw, rate, best_parent_rate,
120 best_parent_p);
121 } else {
122 pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
123 return 0;
124 }
125 }
126
clk_composite_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * prate)127 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
128 unsigned long *prate)
129 {
130 struct clk_composite *composite = to_clk_composite(hw);
131 const struct clk_ops *rate_ops = composite->rate_ops;
132 struct clk_hw *rate_hw = composite->rate_hw;
133
134 rate_hw->clk = hw->clk;
135
136 return rate_ops->round_rate(rate_hw, rate, prate);
137 }
138
clk_composite_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)139 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
140 unsigned long parent_rate)
141 {
142 struct clk_composite *composite = to_clk_composite(hw);
143 const struct clk_ops *rate_ops = composite->rate_ops;
144 struct clk_hw *rate_hw = composite->rate_hw;
145
146 rate_hw->clk = hw->clk;
147
148 return rate_ops->set_rate(rate_hw, rate, parent_rate);
149 }
150
clk_composite_is_enabled(struct clk_hw * hw)151 static int clk_composite_is_enabled(struct clk_hw *hw)
152 {
153 struct clk_composite *composite = to_clk_composite(hw);
154 const struct clk_ops *gate_ops = composite->gate_ops;
155 struct clk_hw *gate_hw = composite->gate_hw;
156
157 gate_hw->clk = hw->clk;
158
159 return gate_ops->is_enabled(gate_hw);
160 }
161
clk_composite_enable(struct clk_hw * hw)162 static int clk_composite_enable(struct clk_hw *hw)
163 {
164 struct clk_composite *composite = to_clk_composite(hw);
165 const struct clk_ops *gate_ops = composite->gate_ops;
166 struct clk_hw *gate_hw = composite->gate_hw;
167
168 gate_hw->clk = hw->clk;
169
170 return gate_ops->enable(gate_hw);
171 }
172
clk_composite_disable(struct clk_hw * hw)173 static void clk_composite_disable(struct clk_hw *hw)
174 {
175 struct clk_composite *composite = to_clk_composite(hw);
176 const struct clk_ops *gate_ops = composite->gate_ops;
177 struct clk_hw *gate_hw = composite->gate_hw;
178
179 gate_hw->clk = hw->clk;
180
181 gate_ops->disable(gate_hw);
182 }
183
clk_register_composite(struct device * dev,const char * name,const char ** 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)184 struct clk *clk_register_composite(struct device *dev, const char *name,
185 const char **parent_names, int num_parents,
186 struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
187 struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
188 struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
189 unsigned long flags)
190 {
191 struct clk *clk;
192 struct clk_init_data init;
193 struct clk_composite *composite;
194 struct clk_ops *clk_composite_ops;
195
196 composite = kzalloc(sizeof(*composite), GFP_KERNEL);
197 if (!composite) {
198 pr_err("%s: could not allocate composite clk\n", __func__);
199 return ERR_PTR(-ENOMEM);
200 }
201
202 init.name = name;
203 init.flags = flags | CLK_IS_BASIC;
204 init.parent_names = parent_names;
205 init.num_parents = num_parents;
206
207 clk_composite_ops = &composite->ops;
208
209 if (mux_hw && mux_ops) {
210 if (!mux_ops->get_parent) {
211 clk = ERR_PTR(-EINVAL);
212 goto err;
213 }
214
215 composite->mux_hw = mux_hw;
216 composite->mux_ops = mux_ops;
217 clk_composite_ops->get_parent = clk_composite_get_parent;
218 if (mux_ops->set_parent)
219 clk_composite_ops->set_parent = clk_composite_set_parent;
220 if (mux_ops->determine_rate)
221 clk_composite_ops->determine_rate = clk_composite_determine_rate;
222 }
223
224 if (rate_hw && rate_ops) {
225 if (!rate_ops->recalc_rate) {
226 clk = ERR_PTR(-EINVAL);
227 goto err;
228 }
229 clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
230
231 if (rate_ops->determine_rate)
232 clk_composite_ops->determine_rate =
233 clk_composite_determine_rate;
234 else if (rate_ops->round_rate)
235 clk_composite_ops->round_rate =
236 clk_composite_round_rate;
237
238 /* .set_rate requires either .round_rate or .determine_rate */
239 if (rate_ops->set_rate) {
240 if (rate_ops->determine_rate || rate_ops->round_rate)
241 clk_composite_ops->set_rate =
242 clk_composite_set_rate;
243 else
244 WARN(1, "%s: missing round_rate op is required\n",
245 __func__);
246 }
247
248 composite->rate_hw = rate_hw;
249 composite->rate_ops = rate_ops;
250 }
251
252 if (gate_hw && gate_ops) {
253 if (!gate_ops->is_enabled || !gate_ops->enable ||
254 !gate_ops->disable) {
255 clk = ERR_PTR(-EINVAL);
256 goto err;
257 }
258
259 composite->gate_hw = gate_hw;
260 composite->gate_ops = gate_ops;
261 clk_composite_ops->is_enabled = clk_composite_is_enabled;
262 clk_composite_ops->enable = clk_composite_enable;
263 clk_composite_ops->disable = clk_composite_disable;
264 }
265
266 init.ops = clk_composite_ops;
267 composite->hw.init = &init;
268
269 clk = clk_register(dev, &composite->hw);
270 if (IS_ERR(clk))
271 goto err;
272
273 if (composite->mux_hw)
274 composite->mux_hw->clk = clk;
275
276 if (composite->rate_hw)
277 composite->rate_hw->clk = clk;
278
279 if (composite->gate_hw)
280 composite->gate_hw->clk = clk;
281
282 return clk;
283
284 err:
285 kfree(composite);
286 return clk;
287 }
288