1
2
3
4
5
6#include <linux/clk-provider.h>
7#include <linux/clkdev.h>
8#include <linux/clk/at91_pmc.h>
9#include <linux/of.h>
10#include <linux/mfd/syscon.h>
11#include <linux/regmap.h>
12
13#include "pmc.h"
14
15#define MASTER_PRES_MASK 0x7
16#define MASTER_PRES_MAX MASTER_PRES_MASK
17#define MASTER_DIV_SHIFT 8
18#define MASTER_DIV_MASK 0x3
19
20#define PMC_MCR 0x30
21#define PMC_MCR_ID_MSK GENMASK(3, 0)
22#define PMC_MCR_CMD BIT(7)
23#define PMC_MCR_DIV GENMASK(10, 8)
24#define PMC_MCR_CSS GENMASK(20, 16)
25#define PMC_MCR_CSS_SHIFT (16)
26#define PMC_MCR_EN BIT(28)
27
28#define PMC_MCR_ID(x) ((x) & PMC_MCR_ID_MSK)
29
30#define MASTER_MAX_ID 4
31
32#define to_clk_master(hw) container_of(hw, struct clk_master, hw)
33
34struct clk_master {
35 struct clk_hw hw;
36 struct regmap *regmap;
37 spinlock_t *lock;
38 const struct clk_master_layout *layout;
39 const struct clk_master_characteristics *characteristics;
40 u32 *mux_table;
41 u32 mckr;
42 int chg_pid;
43 u8 id;
44 u8 parent;
45 u8 div;
46};
47
48static inline bool clk_master_ready(struct clk_master *master)
49{
50 unsigned int bit = master->id ? AT91_PMC_MCKXRDY : AT91_PMC_MCKRDY;
51 unsigned int status;
52
53 regmap_read(master->regmap, AT91_PMC_SR, &status);
54
55 return !!(status & bit);
56}
57
58static int clk_master_prepare(struct clk_hw *hw)
59{
60 struct clk_master *master = to_clk_master(hw);
61
62 while (!clk_master_ready(master))
63 cpu_relax();
64
65 return 0;
66}
67
68static int clk_master_is_prepared(struct clk_hw *hw)
69{
70 struct clk_master *master = to_clk_master(hw);
71
72 return clk_master_ready(master);
73}
74
75static unsigned long clk_master_recalc_rate(struct clk_hw *hw,
76 unsigned long parent_rate)
77{
78 u8 pres;
79 u8 div;
80 unsigned long rate = parent_rate;
81 struct clk_master *master = to_clk_master(hw);
82 const struct clk_master_layout *layout = master->layout;
83 const struct clk_master_characteristics *characteristics =
84 master->characteristics;
85 unsigned int mckr;
86
87 regmap_read(master->regmap, master->layout->offset, &mckr);
88 mckr &= layout->mask;
89
90 pres = (mckr >> layout->pres_shift) & MASTER_PRES_MASK;
91 div = (mckr >> MASTER_DIV_SHIFT) & MASTER_DIV_MASK;
92
93 if (characteristics->have_div3_pres && pres == MASTER_PRES_MAX)
94 rate /= 3;
95 else
96 rate >>= pres;
97
98 rate /= characteristics->divisors[div];
99
100 if (rate < characteristics->output.min)
101 pr_warn("master clk is underclocked");
102 else if (rate > characteristics->output.max)
103 pr_warn("master clk is overclocked");
104
105 return rate;
106}
107
108static u8 clk_master_get_parent(struct clk_hw *hw)
109{
110 struct clk_master *master = to_clk_master(hw);
111 unsigned int mckr;
112
113 regmap_read(master->regmap, master->layout->offset, &mckr);
114
115 return mckr & AT91_PMC_CSS;
116}
117
118static const struct clk_ops master_ops = {
119 .prepare = clk_master_prepare,
120 .is_prepared = clk_master_is_prepared,
121 .recalc_rate = clk_master_recalc_rate,
122 .get_parent = clk_master_get_parent,
123};
124
125struct clk_hw * __init
126at91_clk_register_master(struct regmap *regmap,
127 const char *name, int num_parents,
128 const char **parent_names,
129 const struct clk_master_layout *layout,
130 const struct clk_master_characteristics *characteristics)
131{
132 struct clk_master *master;
133 struct clk_init_data init;
134 struct clk_hw *hw;
135 int ret;
136
137 if (!name || !num_parents || !parent_names)
138 return ERR_PTR(-EINVAL);
139
140 master = kzalloc(sizeof(*master), GFP_KERNEL);
141 if (!master)
142 return ERR_PTR(-ENOMEM);
143
144 init.name = name;
145 init.ops = &master_ops;
146 init.parent_names = parent_names;
147 init.num_parents = num_parents;
148 init.flags = 0;
149
150 master->hw.init = &init;
151 master->layout = layout;
152 master->characteristics = characteristics;
153 master->regmap = regmap;
154
155 hw = &master->hw;
156 ret = clk_hw_register(NULL, &master->hw);
157 if (ret) {
158 kfree(master);
159 hw = ERR_PTR(ret);
160 }
161
162 return hw;
163}
164
165static unsigned long
166clk_sama7g5_master_recalc_rate(struct clk_hw *hw,
167 unsigned long parent_rate)
168{
169 struct clk_master *master = to_clk_master(hw);
170
171 return DIV_ROUND_CLOSEST_ULL(parent_rate, (1 << master->div));
172}
173
174static void clk_sama7g5_master_best_diff(struct clk_rate_request *req,
175 struct clk_hw *parent,
176 unsigned long parent_rate,
177 long *best_rate,
178 long *best_diff,
179 u32 div)
180{
181 unsigned long tmp_rate, tmp_diff;
182
183 if (div == MASTER_PRES_MAX)
184 tmp_rate = parent_rate / 3;
185 else
186 tmp_rate = parent_rate >> div;
187
188 tmp_diff = abs(req->rate - tmp_rate);
189
190 if (*best_diff < 0 || *best_diff >= tmp_diff) {
191 *best_rate = tmp_rate;
192 *best_diff = tmp_diff;
193 req->best_parent_rate = parent_rate;
194 req->best_parent_hw = parent;
195 }
196}
197
198static int clk_sama7g5_master_determine_rate(struct clk_hw *hw,
199 struct clk_rate_request *req)
200{
201 struct clk_master *master = to_clk_master(hw);
202 struct clk_rate_request req_parent = *req;
203 struct clk_hw *parent;
204 long best_rate = LONG_MIN, best_diff = LONG_MIN;
205 unsigned long parent_rate;
206 unsigned int div, i;
207
208
209 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
210 parent = clk_hw_get_parent_by_index(hw, i);
211 if (!parent)
212 continue;
213
214 parent_rate = clk_hw_get_rate(parent);
215 if (!parent_rate)
216 continue;
217
218 for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
219 clk_sama7g5_master_best_diff(req, parent, parent_rate,
220 &best_rate, &best_diff,
221 div);
222 if (!best_diff)
223 break;
224 }
225
226 if (!best_diff)
227 break;
228 }
229
230
231 if (master->chg_pid < 0)
232 goto end;
233
234 parent = clk_hw_get_parent_by_index(hw, master->chg_pid);
235 if (!parent)
236 goto end;
237
238 for (div = 0; div < MASTER_PRES_MAX + 1; div++) {
239 if (div == MASTER_PRES_MAX)
240 req_parent.rate = req->rate * 3;
241 else
242 req_parent.rate = req->rate << div;
243
244 if (__clk_determine_rate(parent, &req_parent))
245 continue;
246
247 clk_sama7g5_master_best_diff(req, parent, req_parent.rate,
248 &best_rate, &best_diff, div);
249
250 if (!best_diff)
251 break;
252 }
253
254end:
255 pr_debug("MCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
256 __func__, best_rate,
257 __clk_get_name((req->best_parent_hw)->clk),
258 req->best_parent_rate);
259
260 if (best_rate < 0)
261 return -EINVAL;
262
263 req->rate = best_rate;
264
265 return 0;
266}
267
268static u8 clk_sama7g5_master_get_parent(struct clk_hw *hw)
269{
270 struct clk_master *master = to_clk_master(hw);
271 unsigned long flags;
272 u8 index;
273
274 spin_lock_irqsave(master->lock, flags);
275 index = clk_mux_val_to_index(&master->hw, master->mux_table, 0,
276 master->parent);
277 spin_unlock_irqrestore(master->lock, flags);
278
279 return index;
280}
281
282static int clk_sama7g5_master_set_parent(struct clk_hw *hw, u8 index)
283{
284 struct clk_master *master = to_clk_master(hw);
285 unsigned long flags;
286
287 if (index >= clk_hw_get_num_parents(hw))
288 return -EINVAL;
289
290 spin_lock_irqsave(master->lock, flags);
291 master->parent = clk_mux_index_to_val(master->mux_table, 0, index);
292 spin_unlock_irqrestore(master->lock, flags);
293
294 return 0;
295}
296
297static int clk_sama7g5_master_enable(struct clk_hw *hw)
298{
299 struct clk_master *master = to_clk_master(hw);
300 unsigned long flags;
301 unsigned int val, cparent;
302
303 spin_lock_irqsave(master->lock, flags);
304
305 regmap_write(master->regmap, PMC_MCR, PMC_MCR_ID(master->id));
306 regmap_read(master->regmap, PMC_MCR, &val);
307 regmap_update_bits(master->regmap, PMC_MCR,
308 PMC_MCR_EN | PMC_MCR_CSS | PMC_MCR_DIV |
309 PMC_MCR_CMD | PMC_MCR_ID_MSK,
310 PMC_MCR_EN | (master->parent << PMC_MCR_CSS_SHIFT) |
311 (master->div << MASTER_DIV_SHIFT) |
312 PMC_MCR_CMD | PMC_MCR_ID(master->id));
313
314 cparent = (val & PMC_MCR_CSS) >> PMC_MCR_CSS_SHIFT;
315
316
317 while ((cparent != master->parent) && !clk_master_ready(master))
318 cpu_relax();
319
320 spin_unlock_irqrestore(master->lock, flags);
321
322 return 0;
323}
324
325static void clk_sama7g5_master_disable(struct clk_hw *hw)
326{
327 struct clk_master *master = to_clk_master(hw);
328 unsigned long flags;
329
330 spin_lock_irqsave(master->lock, flags);
331
332 regmap_write(master->regmap, PMC_MCR, master->id);
333 regmap_update_bits(master->regmap, PMC_MCR,
334 PMC_MCR_EN | PMC_MCR_CMD | PMC_MCR_ID_MSK,
335 PMC_MCR_CMD | PMC_MCR_ID(master->id));
336
337 spin_unlock_irqrestore(master->lock, flags);
338}
339
340static int clk_sama7g5_master_is_enabled(struct clk_hw *hw)
341{
342 struct clk_master *master = to_clk_master(hw);
343 unsigned long flags;
344 unsigned int val;
345
346 spin_lock_irqsave(master->lock, flags);
347
348 regmap_write(master->regmap, PMC_MCR, master->id);
349 regmap_read(master->regmap, PMC_MCR, &val);
350
351 spin_unlock_irqrestore(master->lock, flags);
352
353 return !!(val & PMC_MCR_EN);
354}
355
356static int clk_sama7g5_master_set_rate(struct clk_hw *hw, unsigned long rate,
357 unsigned long parent_rate)
358{
359 struct clk_master *master = to_clk_master(hw);
360 unsigned long div, flags;
361
362 div = DIV_ROUND_CLOSEST(parent_rate, rate);
363 if ((div > (1 << (MASTER_PRES_MAX - 1))) || (div & (div - 1)))
364 return -EINVAL;
365
366 if (div == 3)
367 div = MASTER_PRES_MAX;
368 else
369 div = ffs(div) - 1;
370
371 spin_lock_irqsave(master->lock, flags);
372 master->div = div;
373 spin_unlock_irqrestore(master->lock, flags);
374
375 return 0;
376}
377
378static const struct clk_ops sama7g5_master_ops = {
379 .enable = clk_sama7g5_master_enable,
380 .disable = clk_sama7g5_master_disable,
381 .is_enabled = clk_sama7g5_master_is_enabled,
382 .recalc_rate = clk_sama7g5_master_recalc_rate,
383 .determine_rate = clk_sama7g5_master_determine_rate,
384 .set_rate = clk_sama7g5_master_set_rate,
385 .get_parent = clk_sama7g5_master_get_parent,
386 .set_parent = clk_sama7g5_master_set_parent,
387};
388
389struct clk_hw * __init
390at91_clk_sama7g5_register_master(struct regmap *regmap,
391 const char *name, int num_parents,
392 const char **parent_names,
393 u32 *mux_table,
394 spinlock_t *lock, u8 id,
395 bool critical, int chg_pid)
396{
397 struct clk_master *master;
398 struct clk_hw *hw;
399 struct clk_init_data init;
400 unsigned long flags;
401 unsigned int val;
402 int ret;
403
404 if (!name || !num_parents || !parent_names || !mux_table ||
405 !lock || id > MASTER_MAX_ID)
406 return ERR_PTR(-EINVAL);
407
408 master = kzalloc(sizeof(*master), GFP_KERNEL);
409 if (!master)
410 return ERR_PTR(-ENOMEM);
411
412 init.name = name;
413 init.ops = &sama7g5_master_ops;
414 init.parent_names = parent_names;
415 init.num_parents = num_parents;
416 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
417 if (chg_pid >= 0)
418 init.flags |= CLK_SET_RATE_PARENT;
419 if (critical)
420 init.flags |= CLK_IS_CRITICAL;
421
422 master->hw.init = &init;
423 master->regmap = regmap;
424 master->id = id;
425 master->chg_pid = chg_pid;
426 master->lock = lock;
427 master->mux_table = mux_table;
428
429 spin_lock_irqsave(master->lock, flags);
430 regmap_write(master->regmap, PMC_MCR, master->id);
431 regmap_read(master->regmap, PMC_MCR, &val);
432 master->parent = (val & PMC_MCR_CSS) >> PMC_MCR_CSS_SHIFT;
433 master->div = (val & PMC_MCR_DIV) >> MASTER_DIV_SHIFT;
434 spin_unlock_irqrestore(master->lock, flags);
435
436 hw = &master->hw;
437 ret = clk_hw_register(NULL, &master->hw);
438 if (ret) {
439 kfree(master);
440 hw = ERR_PTR(ret);
441 }
442
443 return hw;
444}
445
446const struct clk_master_layout at91rm9200_master_layout = {
447 .mask = 0x31F,
448 .pres_shift = 2,
449 .offset = AT91_PMC_MCKR,
450};
451
452const struct clk_master_layout at91sam9x5_master_layout = {
453 .mask = 0x373,
454 .pres_shift = 4,
455 .offset = AT91_PMC_MCKR,
456};
457
