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