1
2
3
4
5
6
7
8#include <linux/clk.h>
9#include <linux/clk-provider.h>
10#include <linux/module.h>
11#include <linux/slab.h>
12#include <linux/io.h>
13#include <linux/err.h>
14#include <linux/string.h>
15#include <linux/of.h>
16#include <linux/of_address.h>
17
18struct clkgen_data {
19 unsigned long flags;
20 bool mode;
21};
22
23struct flexgen {
24 struct clk_hw hw;
25
26
27 struct clk_mux mux;
28
29 struct clk_gate pgate;
30
31 struct clk_divider pdiv;
32
33 struct clk_gate fgate;
34
35 struct clk_divider fdiv;
36
37 struct clk_gate sync;
38
39 bool control_mode;
40};
41
42#define to_flexgen(_hw) container_of(_hw, struct flexgen, hw)
43#define to_clk_gate(_hw) container_of(_hw, struct clk_gate, hw)
44
45static int flexgen_enable(struct clk_hw *hw)
46{
47 struct flexgen *flexgen = to_flexgen(hw);
48 struct clk_hw *pgate_hw = &flexgen->pgate.hw;
49 struct clk_hw *fgate_hw = &flexgen->fgate.hw;
50
51 __clk_hw_set_clk(pgate_hw, hw);
52 __clk_hw_set_clk(fgate_hw, hw);
53
54 clk_gate_ops.enable(pgate_hw);
55
56 clk_gate_ops.enable(fgate_hw);
57
58 pr_debug("%s: flexgen output enabled\n", clk_hw_get_name(hw));
59 return 0;
60}
61
62static void flexgen_disable(struct clk_hw *hw)
63{
64 struct flexgen *flexgen = to_flexgen(hw);
65 struct clk_hw *fgate_hw = &flexgen->fgate.hw;
66
67
68 __clk_hw_set_clk(fgate_hw, hw);
69
70 clk_gate_ops.disable(fgate_hw);
71
72 pr_debug("%s: flexgen output disabled\n", clk_hw_get_name(hw));
73}
74
75static int flexgen_is_enabled(struct clk_hw *hw)
76{
77 struct flexgen *flexgen = to_flexgen(hw);
78 struct clk_hw *fgate_hw = &flexgen->fgate.hw;
79
80 __clk_hw_set_clk(fgate_hw, hw);
81
82 if (!clk_gate_ops.is_enabled(fgate_hw))
83 return 0;
84
85 return 1;
86}
87
88static u8 flexgen_get_parent(struct clk_hw *hw)
89{
90 struct flexgen *flexgen = to_flexgen(hw);
91 struct clk_hw *mux_hw = &flexgen->mux.hw;
92
93 __clk_hw_set_clk(mux_hw, hw);
94
95 return clk_mux_ops.get_parent(mux_hw);
96}
97
98static int flexgen_set_parent(struct clk_hw *hw, u8 index)
99{
100 struct flexgen *flexgen = to_flexgen(hw);
101 struct clk_hw *mux_hw = &flexgen->mux.hw;
102
103 __clk_hw_set_clk(mux_hw, hw);
104
105 return clk_mux_ops.set_parent(mux_hw, index);
106}
107
108static inline unsigned long
109clk_best_div(unsigned long parent_rate, unsigned long rate)
110{
111 return parent_rate / rate + ((rate > (2*(parent_rate % rate))) ? 0 : 1);
112}
113
114static long flexgen_round_rate(struct clk_hw *hw, unsigned long rate,
115 unsigned long *prate)
116{
117 unsigned long div;
118
119
120 div = clk_best_div(*prate, rate);
121
122 if (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) {
123 *prate = rate * div;
124 return rate;
125 }
126
127 return *prate / div;
128}
129
130static unsigned long flexgen_recalc_rate(struct clk_hw *hw,
131 unsigned long parent_rate)
132{
133 struct flexgen *flexgen = to_flexgen(hw);
134 struct clk_hw *pdiv_hw = &flexgen->pdiv.hw;
135 struct clk_hw *fdiv_hw = &flexgen->fdiv.hw;
136 unsigned long mid_rate;
137
138 __clk_hw_set_clk(pdiv_hw, hw);
139 __clk_hw_set_clk(fdiv_hw, hw);
140
141 mid_rate = clk_divider_ops.recalc_rate(pdiv_hw, parent_rate);
142
143 return clk_divider_ops.recalc_rate(fdiv_hw, mid_rate);
144}
145
146static int flexgen_set_rate(struct clk_hw *hw, unsigned long rate,
147 unsigned long parent_rate)
148{
149 struct flexgen *flexgen = to_flexgen(hw);
150 struct clk_hw *pdiv_hw = &flexgen->pdiv.hw;
151 struct clk_hw *fdiv_hw = &flexgen->fdiv.hw;
152 struct clk_hw *sync_hw = &flexgen->sync.hw;
153 struct clk_gate *config = to_clk_gate(sync_hw);
154 unsigned long div = 0;
155 int ret = 0;
156 u32 reg;
157
158 __clk_hw_set_clk(pdiv_hw, hw);
159 __clk_hw_set_clk(fdiv_hw, hw);
160
161 if (flexgen->control_mode) {
162 reg = readl(config->reg);
163 reg &= ~BIT(config->bit_idx);
164 writel(reg, config->reg);
165 }
166
167 div = clk_best_div(parent_rate, rate);
168
169
170
171
172
173
174
175 if (div <= 64) {
176 clk_divider_ops.set_rate(pdiv_hw, parent_rate, parent_rate);
177 ret = clk_divider_ops.set_rate(fdiv_hw, rate, rate * div);
178 } else {
179 clk_divider_ops.set_rate(fdiv_hw, parent_rate, parent_rate);
180 ret = clk_divider_ops.set_rate(pdiv_hw, rate, rate * div);
181 }
182
183 return ret;
184}
185
186static const struct clk_ops flexgen_ops = {
187 .enable = flexgen_enable,
188 .disable = flexgen_disable,
189 .is_enabled = flexgen_is_enabled,
190 .get_parent = flexgen_get_parent,
191 .set_parent = flexgen_set_parent,
192 .round_rate = flexgen_round_rate,
193 .recalc_rate = flexgen_recalc_rate,
194 .set_rate = flexgen_set_rate,
195};
196
197static struct clk *clk_register_flexgen(const char *name,
198 const char **parent_names, u8 num_parents,
199 void __iomem *reg, spinlock_t *lock, u32 idx,
200 unsigned long flexgen_flags, bool mode) {
201 struct flexgen *fgxbar;
202 struct clk *clk;
203 struct clk_init_data init;
204 u32 xbar_shift;
205 void __iomem *xbar_reg, *fdiv_reg;
206
207 fgxbar = kzalloc(sizeof(struct flexgen), GFP_KERNEL);
208 if (!fgxbar)
209 return ERR_PTR(-ENOMEM);
210
211 init.name = name;
212 init.ops = &flexgen_ops;
213 init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE | flexgen_flags;
214 init.parent_names = parent_names;
215 init.num_parents = num_parents;
216
217 xbar_reg = reg + 0x18 + (idx & ~0x3);
218 xbar_shift = (idx % 4) * 0x8;
219 fdiv_reg = reg + 0x164 + idx * 4;
220
221
222 fgxbar->mux.lock = lock;
223 fgxbar->mux.mask = BIT(6) - 1;
224 fgxbar->mux.reg = xbar_reg;
225 fgxbar->mux.shift = xbar_shift;
226 fgxbar->mux.table = NULL;
227
228
229
230 fgxbar->pgate.lock = lock;
231 fgxbar->pgate.reg = xbar_reg;
232 fgxbar->pgate.bit_idx = xbar_shift + 6;
233
234
235 fgxbar->pdiv.lock = lock;
236 fgxbar->pdiv.reg = reg + 0x58 + idx * 4;
237 fgxbar->pdiv.width = 10;
238
239
240 fgxbar->fgate.lock = lock;
241 fgxbar->fgate.reg = fdiv_reg;
242 fgxbar->fgate.bit_idx = 6;
243
244
245 fgxbar->fdiv.lock = lock;
246 fgxbar->fdiv.reg = fdiv_reg;
247 fgxbar->fdiv.width = 6;
248
249
250 fgxbar->sync.lock = lock;
251 fgxbar->sync.reg = fdiv_reg;
252 fgxbar->sync.bit_idx = 7;
253
254 fgxbar->control_mode = mode;
255
256 fgxbar->hw.init = &init;
257
258 clk = clk_register(NULL, &fgxbar->hw);
259 if (IS_ERR(clk))
260 kfree(fgxbar);
261 else
262 pr_debug("%s: parent %s rate %u\n",
263 __clk_get_name(clk),
264 __clk_get_name(clk_get_parent(clk)),
265 (unsigned int)clk_get_rate(clk));
266 return clk;
267}
268
269static const char ** __init flexgen_get_parents(struct device_node *np,
270 int *num_parents)
271{
272 const char **parents;
273 unsigned int nparents;
274
275 nparents = of_clk_get_parent_count(np);
276 if (WARN_ON(!nparents))
277 return NULL;
278
279 parents = kcalloc(nparents, sizeof(const char *), GFP_KERNEL);
280 if (!parents)
281 return NULL;
282
283 *num_parents = of_clk_parent_fill(np, parents, nparents);
284
285 return parents;
286}
287
288static const struct clkgen_data clkgen_audio = {
289 .flags = CLK_SET_RATE_PARENT,
290};
291
292static const struct clkgen_data clkgen_video = {
293 .flags = CLK_SET_RATE_PARENT,
294 .mode = 1,
295};
296
297static const struct of_device_id flexgen_of_match[] = {
298 {
299 .compatible = "st,flexgen-audio",
300 .data = &clkgen_audio,
301 },
302 {
303 .compatible = "st,flexgen-video",
304 .data = &clkgen_video,
305 },
306 {}
307};
308
309static void __init st_of_flexgen_setup(struct device_node *np)
310{
311 struct device_node *pnode;
312 void __iomem *reg;
313 struct clk_onecell_data *clk_data;
314 const char **parents;
315 int num_parents, i;
316 spinlock_t *rlock = NULL;
317 const struct of_device_id *match;
318 struct clkgen_data *data = NULL;
319 unsigned long flex_flags = 0;
320 int ret;
321 bool clk_mode = 0;
322
323 pnode = of_get_parent(np);
324 if (!pnode)
325 return;
326
327 reg = of_iomap(pnode, 0);
328 if (!reg)
329 return;
330
331 parents = flexgen_get_parents(np, &num_parents);
332 if (!parents) {
333 iounmap(reg);
334 return;
335 }
336
337 match = of_match_node(flexgen_of_match, np);
338 if (match) {
339 data = (struct clkgen_data *)match->data;
340 flex_flags = data->flags;
341 clk_mode = data->mode;
342 }
343
344 clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
345 if (!clk_data)
346 goto err;
347
348 ret = of_property_count_strings(np, "clock-output-names");
349 if (ret <= 0) {
350 pr_err("%s: Failed to get number of output clocks (%d)",
351 __func__, clk_data->clk_num);
352 goto err;
353 }
354 clk_data->clk_num = ret;
355
356 clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
357 GFP_KERNEL);
358 if (!clk_data->clks)
359 goto err;
360
361 rlock = kzalloc(sizeof(spinlock_t), GFP_KERNEL);
362 if (!rlock)
363 goto err;
364
365 spin_lock_init(rlock);
366
367 for (i = 0; i < clk_data->clk_num; i++) {
368 struct clk *clk;
369 const char *clk_name;
370
371 if (of_property_read_string_index(np, "clock-output-names",
372 i, &clk_name)) {
373 break;
374 }
375
376 of_clk_detect_critical(np, i, &flex_flags);
377
378
379
380
381 if (*clk_name == '\0')
382 continue;
383
384 clk = clk_register_flexgen(clk_name, parents, num_parents,
385 reg, rlock, i, flex_flags, clk_mode);
386
387 if (IS_ERR(clk))
388 goto err;
389
390 clk_data->clks[i] = clk;
391 }
392
393 kfree(parents);
394 of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
395
396 return;
397
398err:
399 iounmap(reg);
400 if (clk_data)
401 kfree(clk_data->clks);
402 kfree(clk_data);
403 kfree(parents);
404 kfree(rlock);
405}
406CLK_OF_DECLARE(flexgen, "st,flexgen", st_of_flexgen_setup);
407
