Commit | Line | Data |
---|---|---|
1f22f8bb BB |
1 | /* |
2 | * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> | |
3 | * | |
4 | * This program is free software; you can redistribute it and/or modify | |
5 | * it under the terms of the GNU General Public License as published by | |
6 | * the Free Software Foundation; either version 2 of the License, or | |
7 | * (at your option) any later version. | |
8 | * | |
9 | */ | |
10 | ||
11 | #include <linux/clk-provider.h> | |
12 | #include <linux/clkdev.h> | |
13 | #include <linux/clk/at91_pmc.h> | |
14 | #include <linux/of.h> | |
15 | #include <linux/of_address.h> | |
16 | #include <linux/of_irq.h> | |
17 | #include <linux/io.h> | |
18 | #include <linux/wait.h> | |
19 | #include <linux/sched.h> | |
20 | #include <linux/interrupt.h> | |
21 | #include <linux/irq.h> | |
22 | ||
23 | #include "pmc.h" | |
24 | ||
25 | #define PROG_SOURCE_MAX 5 | |
26 | #define PROG_ID_MAX 7 | |
27 | ||
28 | #define PROG_STATUS_MASK(id) (1 << ((id) + 8)) | |
29 | #define PROG_PRES_MASK 0x7 | |
30 | #define PROG_MAX_RM9200_CSS 3 | |
31 | ||
32 | struct clk_programmable_layout { | |
33 | u8 pres_shift; | |
34 | u8 css_mask; | |
35 | u8 have_slck_mck; | |
36 | }; | |
37 | ||
38 | struct clk_programmable { | |
39 | struct clk_hw hw; | |
40 | struct at91_pmc *pmc; | |
41 | unsigned int irq; | |
42 | wait_queue_head_t wait; | |
43 | u8 id; | |
44 | u8 css; | |
45 | u8 pres; | |
46 | u8 slckmck; | |
47 | const struct clk_programmable_layout *layout; | |
48 | }; | |
49 | ||
50 | #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw) | |
51 | ||
52 | ||
53 | static irqreturn_t clk_programmable_irq_handler(int irq, void *dev_id) | |
54 | { | |
55 | struct clk_programmable *prog = (struct clk_programmable *)dev_id; | |
56 | ||
57 | wake_up(&prog->wait); | |
58 | ||
59 | return IRQ_HANDLED; | |
60 | } | |
61 | ||
62 | static int clk_programmable_prepare(struct clk_hw *hw) | |
63 | { | |
64 | u32 tmp; | |
65 | struct clk_programmable *prog = to_clk_programmable(hw); | |
66 | struct at91_pmc *pmc = prog->pmc; | |
67 | const struct clk_programmable_layout *layout = prog->layout; | |
68 | u8 id = prog->id; | |
69 | u32 mask = PROG_STATUS_MASK(id); | |
70 | ||
71 | tmp = prog->css | (prog->pres << layout->pres_shift); | |
72 | if (layout->have_slck_mck && prog->slckmck) | |
73 | tmp |= AT91_PMC_CSSMCK_MCK; | |
74 | ||
75 | pmc_write(pmc, AT91_PMC_PCKR(id), tmp); | |
76 | ||
77 | while (!(pmc_read(pmc, AT91_PMC_SR) & mask)) | |
78 | wait_event(prog->wait, pmc_read(pmc, AT91_PMC_SR) & mask); | |
79 | ||
80 | return 0; | |
81 | } | |
82 | ||
83 | static int clk_programmable_is_ready(struct clk_hw *hw) | |
84 | { | |
85 | struct clk_programmable *prog = to_clk_programmable(hw); | |
86 | struct at91_pmc *pmc = prog->pmc; | |
87 | ||
88 | return !!(pmc_read(pmc, AT91_PMC_SR) & AT91_PMC_PCKR(prog->id)); | |
89 | } | |
90 | ||
91 | static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw, | |
92 | unsigned long parent_rate) | |
93 | { | |
94 | u32 tmp; | |
95 | struct clk_programmable *prog = to_clk_programmable(hw); | |
96 | struct at91_pmc *pmc = prog->pmc; | |
97 | const struct clk_programmable_layout *layout = prog->layout; | |
98 | ||
99 | tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id)); | |
100 | prog->pres = (tmp >> layout->pres_shift) & PROG_PRES_MASK; | |
101 | ||
102 | return parent_rate >> prog->pres; | |
103 | } | |
104 | ||
105 | static long clk_programmable_round_rate(struct clk_hw *hw, unsigned long rate, | |
106 | unsigned long *parent_rate) | |
107 | { | |
108 | unsigned long best_rate = *parent_rate; | |
109 | unsigned long best_diff; | |
110 | unsigned long new_diff; | |
111 | unsigned long cur_rate; | |
112 | int shift = shift; | |
113 | ||
114 | if (rate > *parent_rate) | |
115 | return *parent_rate; | |
116 | else | |
117 | best_diff = *parent_rate - rate; | |
118 | ||
119 | if (!best_diff) | |
120 | return best_rate; | |
121 | ||
122 | for (shift = 1; shift < PROG_PRES_MASK; shift++) { | |
123 | cur_rate = *parent_rate >> shift; | |
124 | ||
125 | if (cur_rate > rate) | |
126 | new_diff = cur_rate - rate; | |
127 | else | |
128 | new_diff = rate - cur_rate; | |
129 | ||
130 | if (!new_diff) | |
131 | return cur_rate; | |
132 | ||
133 | if (new_diff < best_diff) { | |
134 | best_diff = new_diff; | |
135 | best_rate = cur_rate; | |
136 | } | |
137 | ||
138 | if (rate > cur_rate) | |
139 | break; | |
140 | } | |
141 | ||
142 | return best_rate; | |
143 | } | |
144 | ||
145 | static int clk_programmable_set_parent(struct clk_hw *hw, u8 index) | |
146 | { | |
147 | struct clk_programmable *prog = to_clk_programmable(hw); | |
148 | const struct clk_programmable_layout *layout = prog->layout; | |
149 | if (index > layout->css_mask) { | |
150 | if (index > PROG_MAX_RM9200_CSS && layout->have_slck_mck) { | |
151 | prog->css = 0; | |
152 | prog->slckmck = 1; | |
153 | return 0; | |
154 | } else { | |
155 | return -EINVAL; | |
156 | } | |
157 | } | |
158 | ||
159 | prog->css = index; | |
160 | return 0; | |
161 | } | |
162 | ||
163 | static u8 clk_programmable_get_parent(struct clk_hw *hw) | |
164 | { | |
165 | u32 tmp; | |
166 | u8 ret; | |
167 | struct clk_programmable *prog = to_clk_programmable(hw); | |
168 | struct at91_pmc *pmc = prog->pmc; | |
169 | const struct clk_programmable_layout *layout = prog->layout; | |
170 | ||
171 | tmp = pmc_read(pmc, AT91_PMC_PCKR(prog->id)); | |
172 | prog->css = tmp & layout->css_mask; | |
173 | ret = prog->css; | |
174 | if (layout->have_slck_mck) { | |
175 | prog->slckmck = !!(tmp & AT91_PMC_CSSMCK_MCK); | |
176 | if (prog->slckmck && !ret) | |
177 | ret = PROG_MAX_RM9200_CSS + 1; | |
178 | } | |
179 | ||
180 | return ret; | |
181 | } | |
182 | ||
183 | static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate, | |
184 | unsigned long parent_rate) | |
185 | { | |
186 | struct clk_programmable *prog = to_clk_programmable(hw); | |
187 | unsigned long best_rate = parent_rate; | |
188 | unsigned long best_diff; | |
189 | unsigned long new_diff; | |
190 | unsigned long cur_rate; | |
191 | int shift = 0; | |
192 | ||
193 | if (rate > parent_rate) | |
194 | return parent_rate; | |
195 | else | |
196 | best_diff = parent_rate - rate; | |
197 | ||
198 | if (!best_diff) { | |
199 | prog->pres = shift; | |
200 | return 0; | |
201 | } | |
202 | ||
203 | for (shift = 1; shift < PROG_PRES_MASK; shift++) { | |
204 | cur_rate = parent_rate >> shift; | |
205 | ||
206 | if (cur_rate > rate) | |
207 | new_diff = cur_rate - rate; | |
208 | else | |
209 | new_diff = rate - cur_rate; | |
210 | ||
211 | if (!new_diff) | |
212 | break; | |
213 | ||
214 | if (new_diff < best_diff) { | |
215 | best_diff = new_diff; | |
216 | best_rate = cur_rate; | |
217 | } | |
218 | ||
219 | if (rate > cur_rate) | |
220 | break; | |
221 | } | |
222 | ||
223 | prog->pres = shift; | |
224 | return 0; | |
225 | } | |
226 | ||
227 | static const struct clk_ops programmable_ops = { | |
228 | .prepare = clk_programmable_prepare, | |
229 | .is_prepared = clk_programmable_is_ready, | |
230 | .recalc_rate = clk_programmable_recalc_rate, | |
231 | .round_rate = clk_programmable_round_rate, | |
232 | .get_parent = clk_programmable_get_parent, | |
233 | .set_parent = clk_programmable_set_parent, | |
234 | .set_rate = clk_programmable_set_rate, | |
235 | }; | |
236 | ||
237 | static struct clk * __init | |
238 | at91_clk_register_programmable(struct at91_pmc *pmc, unsigned int irq, | |
239 | const char *name, const char **parent_names, | |
240 | u8 num_parents, u8 id, | |
241 | const struct clk_programmable_layout *layout) | |
242 | { | |
243 | int ret; | |
244 | struct clk_programmable *prog; | |
245 | struct clk *clk = NULL; | |
246 | struct clk_init_data init; | |
247 | char irq_name[11]; | |
248 | ||
249 | if (id > PROG_ID_MAX) | |
250 | return ERR_PTR(-EINVAL); | |
251 | ||
252 | prog = kzalloc(sizeof(*prog), GFP_KERNEL); | |
253 | if (!prog) | |
254 | return ERR_PTR(-ENOMEM); | |
255 | ||
256 | init.name = name; | |
257 | init.ops = &programmable_ops; | |
258 | init.parent_names = parent_names; | |
259 | init.num_parents = num_parents; | |
260 | init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; | |
261 | ||
262 | prog->id = id; | |
263 | prog->layout = layout; | |
264 | prog->hw.init = &init; | |
265 | prog->pmc = pmc; | |
266 | prog->irq = irq; | |
267 | init_waitqueue_head(&prog->wait); | |
268 | irq_set_status_flags(prog->irq, IRQ_NOAUTOEN); | |
269 | snprintf(irq_name, sizeof(irq_name), "clk-prog%d", id); | |
270 | ret = request_irq(prog->irq, clk_programmable_irq_handler, | |
271 | IRQF_TRIGGER_HIGH, irq_name, prog); | |
272 | if (ret) | |
273 | return ERR_PTR(ret); | |
274 | ||
275 | clk = clk_register(NULL, &prog->hw); | |
276 | if (IS_ERR(clk)) | |
277 | kfree(prog); | |
278 | ||
279 | return clk; | |
280 | } | |
281 | ||
282 | static const struct clk_programmable_layout at91rm9200_programmable_layout = { | |
283 | .pres_shift = 2, | |
284 | .css_mask = 0x3, | |
285 | .have_slck_mck = 0, | |
286 | }; | |
287 | ||
288 | static const struct clk_programmable_layout at91sam9g45_programmable_layout = { | |
289 | .pres_shift = 2, | |
290 | .css_mask = 0x3, | |
291 | .have_slck_mck = 1, | |
292 | }; | |
293 | ||
294 | static const struct clk_programmable_layout at91sam9x5_programmable_layout = { | |
295 | .pres_shift = 4, | |
296 | .css_mask = 0x7, | |
297 | .have_slck_mck = 0, | |
298 | }; | |
299 | ||
300 | static void __init | |
301 | of_at91_clk_prog_setup(struct device_node *np, struct at91_pmc *pmc, | |
302 | const struct clk_programmable_layout *layout) | |
303 | { | |
304 | int num; | |
305 | u32 id; | |
306 | int i; | |
307 | unsigned int irq; | |
308 | struct clk *clk; | |
309 | int num_parents; | |
310 | const char *parent_names[PROG_SOURCE_MAX]; | |
311 | const char *name; | |
312 | struct device_node *progclknp; | |
313 | ||
314 | num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells"); | |
315 | if (num_parents <= 0 || num_parents > PROG_SOURCE_MAX) | |
316 | return; | |
317 | ||
318 | for (i = 0; i < num_parents; ++i) { | |
319 | parent_names[i] = of_clk_get_parent_name(np, i); | |
320 | if (!parent_names[i]) | |
321 | return; | |
322 | } | |
323 | ||
324 | num = of_get_child_count(np); | |
325 | if (!num || num > (PROG_ID_MAX + 1)) | |
326 | return; | |
327 | ||
328 | for_each_child_of_node(np, progclknp) { | |
329 | if (of_property_read_u32(progclknp, "reg", &id)) | |
330 | continue; | |
331 | ||
332 | if (of_property_read_string(np, "clock-output-names", &name)) | |
333 | name = progclknp->name; | |
334 | ||
335 | irq = irq_of_parse_and_map(progclknp, 0); | |
336 | if (!irq) | |
337 | continue; | |
338 | ||
339 | clk = at91_clk_register_programmable(pmc, irq, name, | |
340 | parent_names, num_parents, | |
341 | id, layout); | |
342 | if (IS_ERR(clk)) | |
343 | continue; | |
344 | ||
345 | of_clk_add_provider(progclknp, of_clk_src_simple_get, clk); | |
346 | } | |
347 | } | |
348 | ||
349 | ||
350 | void __init of_at91rm9200_clk_prog_setup(struct device_node *np, | |
351 | struct at91_pmc *pmc) | |
352 | { | |
353 | of_at91_clk_prog_setup(np, pmc, &at91rm9200_programmable_layout); | |
354 | } | |
355 | ||
356 | void __init of_at91sam9g45_clk_prog_setup(struct device_node *np, | |
357 | struct at91_pmc *pmc) | |
358 | { | |
359 | of_at91_clk_prog_setup(np, pmc, &at91sam9g45_programmable_layout); | |
360 | } | |
361 | ||
362 | void __init of_at91sam9x5_clk_prog_setup(struct device_node *np, | |
363 | struct at91_pmc *pmc) | |
364 | { | |
365 | of_at91_clk_prog_setup(np, pmc, &at91sam9x5_programmable_layout); | |
366 | } |