Warning: That file was not part of the compilation database. It may have many parsing errors.
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/mfd/syscon.h> |
16 | #include <linux/regmap.h> |
17 | |
18 | #include "pmc.h" |
19 | |
20 | DEFINE_SPINLOCK(pmc_pcr_lock); |
21 | |
22 | #define PERIPHERAL_ID_MIN 2 |
23 | #define PERIPHERAL_ID_MAX 31 |
24 | #define PERIPHERAL_MASK(id) (1 << ((id) & PERIPHERAL_ID_MAX)) |
25 | |
26 | #define PERIPHERAL_RSHIFT_MASK 0x3 |
27 | #define PERIPHERAL_RSHIFT(val) (((val) >> 16) & PERIPHERAL_RSHIFT_MASK) |
28 | |
29 | #define PERIPHERAL_MAX_SHIFT 3 |
30 | |
31 | struct clk_peripheral { |
32 | struct clk_hw hw; |
33 | struct regmap *regmap; |
34 | u32 id; |
35 | }; |
36 | |
37 | #define to_clk_peripheral(hw) container_of(hw, struct clk_peripheral, hw) |
38 | |
39 | struct clk_sam9x5_peripheral { |
40 | struct clk_hw hw; |
41 | struct regmap *regmap; |
42 | struct clk_range range; |
43 | spinlock_t *lock; |
44 | u32 id; |
45 | u32 div; |
46 | bool auto_div; |
47 | }; |
48 | |
49 | #define to_clk_sam9x5_peripheral(hw) \ |
50 | container_of(hw, struct clk_sam9x5_peripheral, hw) |
51 | |
52 | static int clk_peripheral_enable(struct clk_hw *hw) |
53 | { |
54 | struct clk_peripheral *periph = to_clk_peripheral(hw); |
55 | int offset = AT91_PMC_PCER; |
56 | u32 id = periph->id; |
57 | |
58 | if (id < PERIPHERAL_ID_MIN) |
59 | return 0; |
60 | if (id > PERIPHERAL_ID_MAX) |
61 | offset = AT91_PMC_PCER1; |
62 | regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id)); |
63 | |
64 | return 0; |
65 | } |
66 | |
67 | static void clk_peripheral_disable(struct clk_hw *hw) |
68 | { |
69 | struct clk_peripheral *periph = to_clk_peripheral(hw); |
70 | int offset = AT91_PMC_PCDR; |
71 | u32 id = periph->id; |
72 | |
73 | if (id < PERIPHERAL_ID_MIN) |
74 | return; |
75 | if (id > PERIPHERAL_ID_MAX) |
76 | offset = AT91_PMC_PCDR1; |
77 | regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id)); |
78 | } |
79 | |
80 | static int clk_peripheral_is_enabled(struct clk_hw *hw) |
81 | { |
82 | struct clk_peripheral *periph = to_clk_peripheral(hw); |
83 | int offset = AT91_PMC_PCSR; |
84 | unsigned int status; |
85 | u32 id = periph->id; |
86 | |
87 | if (id < PERIPHERAL_ID_MIN) |
88 | return 1; |
89 | if (id > PERIPHERAL_ID_MAX) |
90 | offset = AT91_PMC_PCSR1; |
91 | regmap_read(periph->regmap, offset, &status); |
92 | |
93 | return status & PERIPHERAL_MASK(id) ? 1 : 0; |
94 | } |
95 | |
96 | static const struct clk_ops peripheral_ops = { |
97 | .enable = clk_peripheral_enable, |
98 | .disable = clk_peripheral_disable, |
99 | .is_enabled = clk_peripheral_is_enabled, |
100 | }; |
101 | |
102 | struct clk_hw * __init |
103 | at91_clk_register_peripheral(struct regmap *regmap, const char *name, |
104 | const char *parent_name, u32 id) |
105 | { |
106 | struct clk_peripheral *periph; |
107 | struct clk_init_data init; |
108 | struct clk_hw *hw; |
109 | int ret; |
110 | |
111 | if (!name || !parent_name || id > PERIPHERAL_ID_MAX) |
112 | return ERR_PTR(-EINVAL); |
113 | |
114 | periph = kzalloc(sizeof(*periph), GFP_KERNEL); |
115 | if (!periph) |
116 | return ERR_PTR(-ENOMEM); |
117 | |
118 | init.name = name; |
119 | init.ops = &peripheral_ops; |
120 | init.parent_names = (parent_name ? &parent_name : NULL); |
121 | init.num_parents = (parent_name ? 1 : 0); |
122 | init.flags = 0; |
123 | |
124 | periph->id = id; |
125 | periph->hw.init = &init; |
126 | periph->regmap = regmap; |
127 | |
128 | hw = &periph->hw; |
129 | ret = clk_hw_register(NULL, &periph->hw); |
130 | if (ret) { |
131 | kfree(periph); |
132 | hw = ERR_PTR(ret); |
133 | } |
134 | |
135 | return hw; |
136 | } |
137 | |
138 | static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph) |
139 | { |
140 | struct clk_hw *parent; |
141 | unsigned long parent_rate; |
142 | int shift = 0; |
143 | |
144 | if (!periph->auto_div) |
145 | return; |
146 | |
147 | if (periph->range.max) { |
148 | parent = clk_hw_get_parent_by_index(&periph->hw, 0); |
149 | parent_rate = clk_hw_get_rate(parent); |
150 | if (!parent_rate) |
151 | return; |
152 | |
153 | for (; shift < PERIPHERAL_MAX_SHIFT; shift++) { |
154 | if (parent_rate >> shift <= periph->range.max) |
155 | break; |
156 | } |
157 | } |
158 | |
159 | periph->auto_div = false; |
160 | periph->div = shift; |
161 | } |
162 | |
163 | static int clk_sam9x5_peripheral_enable(struct clk_hw *hw) |
164 | { |
165 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
166 | unsigned long flags; |
167 | |
168 | if (periph->id < PERIPHERAL_ID_MIN) |
169 | return 0; |
170 | |
171 | spin_lock_irqsave(periph->lock, flags); |
172 | regmap_write(periph->regmap, AT91_PMC_PCR, |
173 | (periph->id & AT91_PMC_PCR_PID_MASK)); |
174 | regmap_update_bits(periph->regmap, AT91_PMC_PCR, |
175 | AT91_PMC_PCR_DIV_MASK | AT91_PMC_PCR_CMD | |
176 | AT91_PMC_PCR_EN, |
177 | AT91_PMC_PCR_DIV(periph->div) | |
178 | AT91_PMC_PCR_CMD | |
179 | AT91_PMC_PCR_EN); |
180 | spin_unlock_irqrestore(periph->lock, flags); |
181 | |
182 | return 0; |
183 | } |
184 | |
185 | static void clk_sam9x5_peripheral_disable(struct clk_hw *hw) |
186 | { |
187 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
188 | unsigned long flags; |
189 | |
190 | if (periph->id < PERIPHERAL_ID_MIN) |
191 | return; |
192 | |
193 | spin_lock_irqsave(periph->lock, flags); |
194 | regmap_write(periph->regmap, AT91_PMC_PCR, |
195 | (periph->id & AT91_PMC_PCR_PID_MASK)); |
196 | regmap_update_bits(periph->regmap, AT91_PMC_PCR, |
197 | AT91_PMC_PCR_EN | AT91_PMC_PCR_CMD, |
198 | AT91_PMC_PCR_CMD); |
199 | spin_unlock_irqrestore(periph->lock, flags); |
200 | } |
201 | |
202 | static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw) |
203 | { |
204 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
205 | unsigned long flags; |
206 | unsigned int status; |
207 | |
208 | if (periph->id < PERIPHERAL_ID_MIN) |
209 | return 1; |
210 | |
211 | spin_lock_irqsave(periph->lock, flags); |
212 | regmap_write(periph->regmap, AT91_PMC_PCR, |
213 | (periph->id & AT91_PMC_PCR_PID_MASK)); |
214 | regmap_read(periph->regmap, AT91_PMC_PCR, &status); |
215 | spin_unlock_irqrestore(periph->lock, flags); |
216 | |
217 | return status & AT91_PMC_PCR_EN ? 1 : 0; |
218 | } |
219 | |
220 | static unsigned long |
221 | clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw, |
222 | unsigned long parent_rate) |
223 | { |
224 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
225 | unsigned long flags; |
226 | unsigned int status; |
227 | |
228 | if (periph->id < PERIPHERAL_ID_MIN) |
229 | return parent_rate; |
230 | |
231 | spin_lock_irqsave(periph->lock, flags); |
232 | regmap_write(periph->regmap, AT91_PMC_PCR, |
233 | (periph->id & AT91_PMC_PCR_PID_MASK)); |
234 | regmap_read(periph->regmap, AT91_PMC_PCR, &status); |
235 | spin_unlock_irqrestore(periph->lock, flags); |
236 | |
237 | if (status & AT91_PMC_PCR_EN) { |
238 | periph->div = PERIPHERAL_RSHIFT(status); |
239 | periph->auto_div = false; |
240 | } else { |
241 | clk_sam9x5_peripheral_autodiv(periph); |
242 | } |
243 | |
244 | return parent_rate >> periph->div; |
245 | } |
246 | |
247 | static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw, |
248 | unsigned long rate, |
249 | unsigned long *parent_rate) |
250 | { |
251 | int shift = 0; |
252 | unsigned long best_rate; |
253 | unsigned long best_diff; |
254 | unsigned long cur_rate = *parent_rate; |
255 | unsigned long cur_diff; |
256 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
257 | |
258 | if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) |
259 | return *parent_rate; |
260 | |
261 | if (periph->range.max) { |
262 | for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { |
263 | cur_rate = *parent_rate >> shift; |
264 | if (cur_rate <= periph->range.max) |
265 | break; |
266 | } |
267 | } |
268 | |
269 | if (rate >= cur_rate) |
270 | return cur_rate; |
271 | |
272 | best_diff = cur_rate - rate; |
273 | best_rate = cur_rate; |
274 | for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { |
275 | cur_rate = *parent_rate >> shift; |
276 | if (cur_rate < rate) |
277 | cur_diff = rate - cur_rate; |
278 | else |
279 | cur_diff = cur_rate - rate; |
280 | |
281 | if (cur_diff < best_diff) { |
282 | best_diff = cur_diff; |
283 | best_rate = cur_rate; |
284 | } |
285 | |
286 | if (!best_diff || cur_rate < rate) |
287 | break; |
288 | } |
289 | |
290 | return best_rate; |
291 | } |
292 | |
293 | static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw, |
294 | unsigned long rate, |
295 | unsigned long parent_rate) |
296 | { |
297 | int shift; |
298 | struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); |
299 | if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) { |
300 | if (parent_rate == rate) |
301 | return 0; |
302 | else |
303 | return -EINVAL; |
304 | } |
305 | |
306 | if (periph->range.max && rate > periph->range.max) |
307 | return -EINVAL; |
308 | |
309 | for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { |
310 | if (parent_rate >> shift == rate) { |
311 | periph->auto_div = false; |
312 | periph->div = shift; |
313 | return 0; |
314 | } |
315 | } |
316 | |
317 | return -EINVAL; |
318 | } |
319 | |
320 | static const struct clk_ops sam9x5_peripheral_ops = { |
321 | .enable = clk_sam9x5_peripheral_enable, |
322 | .disable = clk_sam9x5_peripheral_disable, |
323 | .is_enabled = clk_sam9x5_peripheral_is_enabled, |
324 | .recalc_rate = clk_sam9x5_peripheral_recalc_rate, |
325 | .round_rate = clk_sam9x5_peripheral_round_rate, |
326 | .set_rate = clk_sam9x5_peripheral_set_rate, |
327 | }; |
328 | |
329 | struct clk_hw * __init |
330 | at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock, |
331 | const char *name, const char *parent_name, |
332 | u32 id, const struct clk_range *range) |
333 | { |
334 | struct clk_sam9x5_peripheral *periph; |
335 | struct clk_init_data init; |
336 | struct clk_hw *hw; |
337 | int ret; |
338 | |
339 | if (!name || !parent_name) |
340 | return ERR_PTR(-EINVAL); |
341 | |
342 | periph = kzalloc(sizeof(*periph), GFP_KERNEL); |
343 | if (!periph) |
344 | return ERR_PTR(-ENOMEM); |
345 | |
346 | init.name = name; |
347 | init.ops = &sam9x5_peripheral_ops; |
348 | init.parent_names = (parent_name ? &parent_name : NULL); |
349 | init.num_parents = (parent_name ? 1 : 0); |
350 | init.flags = 0; |
351 | |
352 | periph->id = id; |
353 | periph->hw.init = &init; |
354 | periph->div = 0; |
355 | periph->regmap = regmap; |
356 | periph->lock = lock; |
357 | periph->auto_div = true; |
358 | periph->range = *range; |
359 | |
360 | hw = &periph->hw; |
361 | ret = clk_hw_register(NULL, &periph->hw); |
362 | if (ret) { |
363 | kfree(periph); |
364 | hw = ERR_PTR(ret); |
365 | } else { |
366 | clk_sam9x5_peripheral_autodiv(periph); |
367 | pmc_register_id(id); |
368 | } |
369 | |
370 | return hw; |
371 | } |
372 |
Warning: That file was not part of the compilation database. It may have many parsing errors.