Merge branch 'linus' into genirq
[linux-2.6] / arch / arm / mach-omap2 / clock.c
1 /*
2  *  linux/arch/arm/mach-omap2/clock.c
3  *
4  *  Copyright (C) 2005-2008 Texas Instruments, Inc.
5  *  Copyright (C) 2004-2008 Nokia Corporation
6  *
7  *  Contacts:
8  *  Richard Woodruff <r-woodruff2@ti.com>
9  *  Paul Walmsley
10  *
11  * This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License version 2 as
13  * published by the Free Software Foundation.
14  */
15 #undef DEBUG
16
17 #include <linux/module.h>
18 #include <linux/kernel.h>
19 #include <linux/device.h>
20 #include <linux/list.h>
21 #include <linux/errno.h>
22 #include <linux/delay.h>
23 #include <linux/clk.h>
24 #include <linux/io.h>
25 #include <linux/bitops.h>
26
27 #include <mach/clock.h>
28 #include <mach/sram.h>
29 #include <mach/cpu.h>
30 #include <asm/div64.h>
31
32 #include "memory.h"
33 #include "sdrc.h"
34 #include "clock.h"
35 #include "prm.h"
36 #include "prm-regbits-24xx.h"
37 #include "cm.h"
38 #include "cm-regbits-24xx.h"
39 #include "cm-regbits-34xx.h"
40
41 #define MAX_CLOCK_ENABLE_WAIT           100000
42
43 /* DPLL rate rounding: minimum DPLL multiplier, divider values */
44 #define DPLL_MIN_MULTIPLIER             1
45 #define DPLL_MIN_DIVIDER                1
46
47 /* Possible error results from _dpll_test_mult */
48 #define DPLL_MULT_UNDERFLOW             (1 << 0)
49
50 /*
51  * Scale factor to mitigate roundoff errors in DPLL rate rounding.
52  * The higher the scale factor, the greater the risk of arithmetic overflow,
53  * but the closer the rounded rate to the target rate.  DPLL_SCALE_FACTOR
54  * must be a power of DPLL_SCALE_BASE.
55  */
56 #define DPLL_SCALE_FACTOR               64
57 #define DPLL_SCALE_BASE                 2
58 #define DPLL_ROUNDING_VAL               ((DPLL_SCALE_BASE / 2) * \
59                                          (DPLL_SCALE_FACTOR / DPLL_SCALE_BASE))
60
61 u8 cpu_mask;
62
63 /*-------------------------------------------------------------------------
64  * Omap2 specific clock functions
65  *-------------------------------------------------------------------------*/
66
67 /**
68  * omap2_init_clksel_parent - set a clksel clk's parent field from the hardware
69  * @clk: OMAP clock struct ptr to use
70  *
71  * Given a pointer to a source-selectable struct clk, read the hardware
72  * register and determine what its parent is currently set to.  Update the
73  * clk->parent field with the appropriate clk ptr.
74  */
75 void omap2_init_clksel_parent(struct clk *clk)
76 {
77         const struct clksel *clks;
78         const struct clksel_rate *clkr;
79         u32 r, found = 0;
80
81         if (!clk->clksel)
82                 return;
83
84         r = __raw_readl(clk->clksel_reg) & clk->clksel_mask;
85         r >>= __ffs(clk->clksel_mask);
86
87         for (clks = clk->clksel; clks->parent && !found; clks++) {
88                 for (clkr = clks->rates; clkr->div && !found; clkr++) {
89                         if ((clkr->flags & cpu_mask) && (clkr->val == r)) {
90                                 if (clk->parent != clks->parent) {
91                                         pr_debug("clock: inited %s parent "
92                                                  "to %s (was %s)\n",
93                                                  clk->name, clks->parent->name,
94                                                  ((clk->parent) ?
95                                                   clk->parent->name : "NULL"));
96                                         clk->parent = clks->parent;
97                                 };
98                                 found = 1;
99                         }
100                 }
101         }
102
103         if (!found)
104                 printk(KERN_ERR "clock: init parent: could not find "
105                        "regval %0x for clock %s\n", r,  clk->name);
106
107         return;
108 }
109
110 /* Returns the DPLL rate */
111 u32 omap2_get_dpll_rate(struct clk *clk)
112 {
113         long long dpll_clk;
114         u32 dpll_mult, dpll_div, dpll;
115         struct dpll_data *dd;
116
117         dd = clk->dpll_data;
118         /* REVISIT: What do we return on error? */
119         if (!dd)
120                 return 0;
121
122         dpll = __raw_readl(dd->mult_div1_reg);
123         dpll_mult = dpll & dd->mult_mask;
124         dpll_mult >>= __ffs(dd->mult_mask);
125         dpll_div = dpll & dd->div1_mask;
126         dpll_div >>= __ffs(dd->div1_mask);
127
128         dpll_clk = (long long)clk->parent->rate * dpll_mult;
129         do_div(dpll_clk, dpll_div + 1);
130
131         return dpll_clk;
132 }
133
134 /*
135  * Used for clocks that have the same value as the parent clock,
136  * divided by some factor
137  */
138 void omap2_fixed_divisor_recalc(struct clk *clk)
139 {
140         WARN_ON(!clk->fixed_div);
141
142         clk->rate = clk->parent->rate / clk->fixed_div;
143
144         if (clk->flags & RATE_PROPAGATES)
145                 propagate_rate(clk);
146 }
147
148 /**
149  * omap2_wait_clock_ready - wait for clock to enable
150  * @reg: physical address of clock IDLEST register
151  * @mask: value to mask against to determine if the clock is active
152  * @name: name of the clock (for printk)
153  *
154  * Returns 1 if the clock enabled in time, or 0 if it failed to enable
155  * in roughly MAX_CLOCK_ENABLE_WAIT microseconds.
156  */
157 int omap2_wait_clock_ready(void __iomem *reg, u32 mask, const char *name)
158 {
159         int i = 0;
160         int ena = 0;
161
162         /*
163          * 24xx uses 0 to indicate not ready, and 1 to indicate ready.
164          * 34xx reverses this, just to keep us on our toes
165          */
166         if (cpu_mask & (RATE_IN_242X | RATE_IN_243X)) {
167                 ena = mask;
168         } else if (cpu_mask & RATE_IN_343X) {
169                 ena = 0;
170         }
171
172         /* Wait for lock */
173         while (((__raw_readl(reg) & mask) != ena) &&
174                (i++ < MAX_CLOCK_ENABLE_WAIT)) {
175                 udelay(1);
176         }
177
178         if (i < MAX_CLOCK_ENABLE_WAIT)
179                 pr_debug("Clock %s stable after %d loops\n", name, i);
180         else
181                 printk(KERN_ERR "Clock %s didn't enable in %d tries\n",
182                        name, MAX_CLOCK_ENABLE_WAIT);
183
184
185         return (i < MAX_CLOCK_ENABLE_WAIT) ? 1 : 0;
186 };
187
188
189 /*
190  * Note: We don't need special code here for INVERT_ENABLE
191  * for the time being since INVERT_ENABLE only applies to clocks enabled by
192  * CM_CLKEN_PLL
193  */
194 static void omap2_clk_wait_ready(struct clk *clk)
195 {
196         void __iomem *reg, *other_reg, *st_reg;
197         u32 bit;
198
199         /*
200          * REVISIT: This code is pretty ugly.  It would be nice to generalize
201          * it and pull it into struct clk itself somehow.
202          */
203         reg = clk->enable_reg;
204         if ((((u32)reg & 0xff) >= CM_FCLKEN1) &&
205             (((u32)reg & 0xff) <= OMAP24XX_CM_FCLKEN2))
206                 other_reg = (void __iomem *)(((u32)reg & ~0xf0) | 0x10); /* CM_ICLKEN* */
207         else if ((((u32)reg & 0xff) >= CM_ICLKEN1) &&
208                  (((u32)reg & 0xff) <= OMAP24XX_CM_ICLKEN4))
209                 other_reg = (void __iomem *)(((u32)reg & ~0xf0) | 0x00); /* CM_FCLKEN* */
210         else
211                 return;
212
213         /* REVISIT: What are the appropriate exclusions for 34XX? */
214         /* No check for DSS or cam clocks */
215         if (cpu_is_omap24xx() && ((u32)reg & 0x0f) == 0) { /* CM_{F,I}CLKEN1 */
216                 if (clk->enable_bit == OMAP24XX_EN_DSS2_SHIFT ||
217                     clk->enable_bit == OMAP24XX_EN_DSS1_SHIFT ||
218                     clk->enable_bit == OMAP24XX_EN_CAM_SHIFT)
219                         return;
220         }
221
222         /* REVISIT: What are the appropriate exclusions for 34XX? */
223         /* OMAP3: ignore DSS-mod clocks */
224         if (cpu_is_omap34xx() &&
225             (((u32)reg & ~0xff) == (u32)OMAP_CM_REGADDR(OMAP3430_DSS_MOD, 0) ||
226              ((((u32)reg & ~0xff) == (u32)OMAP_CM_REGADDR(CORE_MOD, 0)) &&
227              clk->enable_bit == OMAP3430_EN_SSI_SHIFT)))
228                 return;
229
230         /* Check if both functional and interface clocks
231          * are running. */
232         bit = 1 << clk->enable_bit;
233         if (!(__raw_readl(other_reg) & bit))
234                 return;
235         st_reg = (void __iomem *)(((u32)other_reg & ~0xf0) | 0x20); /* CM_IDLEST* */
236
237         omap2_wait_clock_ready(st_reg, bit, clk->name);
238 }
239
240 /* Enables clock without considering parent dependencies or use count
241  * REVISIT: Maybe change this to use clk->enable like on omap1?
242  */
243 int _omap2_clk_enable(struct clk *clk)
244 {
245         u32 regval32;
246
247         if (clk->flags & (ALWAYS_ENABLED | PARENT_CONTROLS_CLOCK))
248                 return 0;
249
250         if (clk->enable)
251                 return clk->enable(clk);
252
253         if (unlikely(clk->enable_reg == 0)) {
254                 printk(KERN_ERR "clock.c: Enable for %s without enable code\n",
255                        clk->name);
256                 return 0; /* REVISIT: -EINVAL */
257         }
258
259         regval32 = __raw_readl(clk->enable_reg);
260         if (clk->flags & INVERT_ENABLE)
261                 regval32 &= ~(1 << clk->enable_bit);
262         else
263                 regval32 |= (1 << clk->enable_bit);
264         __raw_writel(regval32, clk->enable_reg);
265         wmb();
266
267         omap2_clk_wait_ready(clk);
268
269         return 0;
270 }
271
272 /* Disables clock without considering parent dependencies or use count */
273 void _omap2_clk_disable(struct clk *clk)
274 {
275         u32 regval32;
276
277         if (clk->flags & (ALWAYS_ENABLED | PARENT_CONTROLS_CLOCK))
278                 return;
279
280         if (clk->disable) {
281                 clk->disable(clk);
282                 return;
283         }
284
285         if (clk->enable_reg == 0) {
286                 /*
287                  * 'Independent' here refers to a clock which is not
288                  * controlled by its parent.
289                  */
290                 printk(KERN_ERR "clock: clk_disable called on independent "
291                        "clock %s which has no enable_reg\n", clk->name);
292                 return;
293         }
294
295         regval32 = __raw_readl(clk->enable_reg);
296         if (clk->flags & INVERT_ENABLE)
297                 regval32 |= (1 << clk->enable_bit);
298         else
299                 regval32 &= ~(1 << clk->enable_bit);
300         __raw_writel(regval32, clk->enable_reg);
301         wmb();
302 }
303
304 void omap2_clk_disable(struct clk *clk)
305 {
306         if (clk->usecount > 0 && !(--clk->usecount)) {
307                 _omap2_clk_disable(clk);
308                 if (likely((u32)clk->parent))
309                         omap2_clk_disable(clk->parent);
310         }
311 }
312
313 int omap2_clk_enable(struct clk *clk)
314 {
315         int ret = 0;
316
317         if (clk->usecount++ == 0) {
318                 if (likely((u32)clk->parent))
319                         ret = omap2_clk_enable(clk->parent);
320
321                 if (unlikely(ret != 0)) {
322                         clk->usecount--;
323                         return ret;
324                 }
325
326                 ret = _omap2_clk_enable(clk);
327
328                 if (unlikely(ret != 0) && clk->parent) {
329                         omap2_clk_disable(clk->parent);
330                         clk->usecount--;
331                 }
332         }
333
334         return ret;
335 }
336
337 /*
338  * Used for clocks that are part of CLKSEL_xyz governed clocks.
339  * REVISIT: Maybe change to use clk->enable() functions like on omap1?
340  */
341 void omap2_clksel_recalc(struct clk *clk)
342 {
343         u32 div = 0;
344
345         pr_debug("clock: recalc'ing clksel clk %s\n", clk->name);
346
347         div = omap2_clksel_get_divisor(clk);
348         if (div == 0)
349                 return;
350
351         if (unlikely(clk->rate == clk->parent->rate / div))
352                 return;
353         clk->rate = clk->parent->rate / div;
354
355         pr_debug("clock: new clock rate is %ld (div %d)\n", clk->rate, div);
356
357         if (unlikely(clk->flags & RATE_PROPAGATES))
358                 propagate_rate(clk);
359 }
360
361 /**
362  * omap2_get_clksel_by_parent - return clksel struct for a given clk & parent
363  * @clk: OMAP struct clk ptr to inspect
364  * @src_clk: OMAP struct clk ptr of the parent clk to search for
365  *
366  * Scan the struct clksel array associated with the clock to find
367  * the element associated with the supplied parent clock address.
368  * Returns a pointer to the struct clksel on success or NULL on error.
369  */
370 const struct clksel *omap2_get_clksel_by_parent(struct clk *clk,
371                                                 struct clk *src_clk)
372 {
373         const struct clksel *clks;
374
375         if (!clk->clksel)
376                 return NULL;
377
378         for (clks = clk->clksel; clks->parent; clks++) {
379                 if (clks->parent == src_clk)
380                         break; /* Found the requested parent */
381         }
382
383         if (!clks->parent) {
384                 printk(KERN_ERR "clock: Could not find parent clock %s in "
385                        "clksel array of clock %s\n", src_clk->name,
386                        clk->name);
387                 return NULL;
388         }
389
390         return clks;
391 }
392
393 /**
394  * omap2_clksel_round_rate_div - find divisor for the given clock and rate
395  * @clk: OMAP struct clk to use
396  * @target_rate: desired clock rate
397  * @new_div: ptr to where we should store the divisor
398  *
399  * Finds 'best' divider value in an array based on the source and target
400  * rates.  The divider array must be sorted with smallest divider first.
401  * Note that this will not work for clocks which are part of CONFIG_PARTICIPANT,
402  * they are only settable as part of virtual_prcm set.
403  *
404  * Returns the rounded clock rate or returns 0xffffffff on error.
405  */
406 u32 omap2_clksel_round_rate_div(struct clk *clk, unsigned long target_rate,
407                                 u32 *new_div)
408 {
409         unsigned long test_rate;
410         const struct clksel *clks;
411         const struct clksel_rate *clkr;
412         u32 last_div = 0;
413
414         printk(KERN_INFO "clock: clksel_round_rate_div: %s target_rate %ld\n",
415                clk->name, target_rate);
416
417         *new_div = 1;
418
419         clks = omap2_get_clksel_by_parent(clk, clk->parent);
420         if (clks == NULL)
421                 return ~0;
422
423         for (clkr = clks->rates; clkr->div; clkr++) {
424                 if (!(clkr->flags & cpu_mask))
425                     continue;
426
427                 /* Sanity check */
428                 if (clkr->div <= last_div)
429                         printk(KERN_ERR "clock: clksel_rate table not sorted "
430                                "for clock %s", clk->name);
431
432                 last_div = clkr->div;
433
434                 test_rate = clk->parent->rate / clkr->div;
435
436                 if (test_rate <= target_rate)
437                         break; /* found it */
438         }
439
440         if (!clkr->div) {
441                 printk(KERN_ERR "clock: Could not find divisor for target "
442                        "rate %ld for clock %s parent %s\n", target_rate,
443                        clk->name, clk->parent->name);
444                 return ~0;
445         }
446
447         *new_div = clkr->div;
448
449         printk(KERN_INFO "clock: new_div = %d, new_rate = %ld\n", *new_div,
450                (clk->parent->rate / clkr->div));
451
452         return (clk->parent->rate / clkr->div);
453 }
454
455 /**
456  * omap2_clksel_round_rate - find rounded rate for the given clock and rate
457  * @clk: OMAP struct clk to use
458  * @target_rate: desired clock rate
459  *
460  * Compatibility wrapper for OMAP clock framework
461  * Finds best target rate based on the source clock and possible dividers.
462  * rates. The divider array must be sorted with smallest divider first.
463  * Note that this will not work for clocks which are part of CONFIG_PARTICIPANT,
464  * they are only settable as part of virtual_prcm set.
465  *
466  * Returns the rounded clock rate or returns 0xffffffff on error.
467  */
468 long omap2_clksel_round_rate(struct clk *clk, unsigned long target_rate)
469 {
470         u32 new_div;
471
472         return omap2_clksel_round_rate_div(clk, target_rate, &new_div);
473 }
474
475
476 /* Given a clock and a rate apply a clock specific rounding function */
477 long omap2_clk_round_rate(struct clk *clk, unsigned long rate)
478 {
479         if (clk->round_rate != 0)
480                 return clk->round_rate(clk, rate);
481
482         if (clk->flags & RATE_FIXED)
483                 printk(KERN_ERR "clock: generic omap2_clk_round_rate called "
484                        "on fixed-rate clock %s\n", clk->name);
485
486         return clk->rate;
487 }
488
489 /**
490  * omap2_clksel_to_divisor() - turn clksel field value into integer divider
491  * @clk: OMAP struct clk to use
492  * @field_val: register field value to find
493  *
494  * Given a struct clk of a rate-selectable clksel clock, and a register field
495  * value to search for, find the corresponding clock divisor.  The register
496  * field value should be pre-masked and shifted down so the LSB is at bit 0
497  * before calling.  Returns 0 on error
498  */
499 u32 omap2_clksel_to_divisor(struct clk *clk, u32 field_val)
500 {
501         const struct clksel *clks;
502         const struct clksel_rate *clkr;
503
504         clks = omap2_get_clksel_by_parent(clk, clk->parent);
505         if (clks == NULL)
506                 return 0;
507
508         for (clkr = clks->rates; clkr->div; clkr++) {
509                 if ((clkr->flags & cpu_mask) && (clkr->val == field_val))
510                         break;
511         }
512
513         if (!clkr->div) {
514                 printk(KERN_ERR "clock: Could not find fieldval %d for "
515                        "clock %s parent %s\n", field_val, clk->name,
516                        clk->parent->name);
517                 return 0;
518         }
519
520         return clkr->div;
521 }
522
523 /**
524  * omap2_divisor_to_clksel() - turn clksel integer divisor into a field value
525  * @clk: OMAP struct clk to use
526  * @div: integer divisor to search for
527  *
528  * Given a struct clk of a rate-selectable clksel clock, and a clock divisor,
529  * find the corresponding register field value.  The return register value is
530  * the value before left-shifting.  Returns 0xffffffff on error
531  */
532 u32 omap2_divisor_to_clksel(struct clk *clk, u32 div)
533 {
534         const struct clksel *clks;
535         const struct clksel_rate *clkr;
536
537         /* should never happen */
538         WARN_ON(div == 0);
539
540         clks = omap2_get_clksel_by_parent(clk, clk->parent);
541         if (clks == NULL)
542                 return 0;
543
544         for (clkr = clks->rates; clkr->div; clkr++) {
545                 if ((clkr->flags & cpu_mask) && (clkr->div == div))
546                         break;
547         }
548
549         if (!clkr->div) {
550                 printk(KERN_ERR "clock: Could not find divisor %d for "
551                        "clock %s parent %s\n", div, clk->name,
552                        clk->parent->name);
553                 return 0;
554         }
555
556         return clkr->val;
557 }
558
559 /**
560  * omap2_get_clksel - find clksel register addr & field mask for a clk
561  * @clk: struct clk to use
562  * @field_mask: ptr to u32 to store the register field mask
563  *
564  * Returns the address of the clksel register upon success or NULL on error.
565  */
566 void __iomem *omap2_get_clksel(struct clk *clk, u32 *field_mask)
567 {
568         if (unlikely((clk->clksel_reg == 0) || (clk->clksel_mask == 0)))
569                 return NULL;
570
571         *field_mask = clk->clksel_mask;
572
573         return clk->clksel_reg;
574 }
575
576 /**
577  * omap2_clksel_get_divisor - get current divider applied to parent clock.
578  * @clk: OMAP struct clk to use.
579  *
580  * Returns the integer divisor upon success or 0 on error.
581  */
582 u32 omap2_clksel_get_divisor(struct clk *clk)
583 {
584         u32 field_mask, field_val;
585         void __iomem *div_addr;
586
587         div_addr = omap2_get_clksel(clk, &field_mask);
588         if (div_addr == 0)
589                 return 0;
590
591         field_val = __raw_readl(div_addr) & field_mask;
592         field_val >>= __ffs(field_mask);
593
594         return omap2_clksel_to_divisor(clk, field_val);
595 }
596
597 int omap2_clksel_set_rate(struct clk *clk, unsigned long rate)
598 {
599         u32 field_mask, field_val, reg_val, validrate, new_div = 0;
600         void __iomem *div_addr;
601
602         validrate = omap2_clksel_round_rate_div(clk, rate, &new_div);
603         if (validrate != rate)
604                 return -EINVAL;
605
606         div_addr = omap2_get_clksel(clk, &field_mask);
607         if (div_addr == 0)
608                 return -EINVAL;
609
610         field_val = omap2_divisor_to_clksel(clk, new_div);
611         if (field_val == ~0)
612                 return -EINVAL;
613
614         reg_val = __raw_readl(div_addr);
615         reg_val &= ~field_mask;
616         reg_val |= (field_val << __ffs(field_mask));
617         __raw_writel(reg_val, div_addr);
618         wmb();
619
620         clk->rate = clk->parent->rate / new_div;
621
622         if (clk->flags & DELAYED_APP && cpu_is_omap24xx()) {
623                 prm_write_mod_reg(OMAP24XX_VALID_CONFIG,
624                         OMAP24XX_GR_MOD, OMAP24XX_PRCM_CLKCFG_CTRL_OFFSET);
625                 wmb();
626         }
627
628         return 0;
629 }
630
631
632 /* Set the clock rate for a clock source */
633 int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
634 {
635         int ret = -EINVAL;
636
637         pr_debug("clock: set_rate for clock %s to rate %ld\n", clk->name, rate);
638
639         /* CONFIG_PARTICIPANT clocks are changed only in sets via the
640            rate table mechanism, driven by mpu_speed  */
641         if (clk->flags & CONFIG_PARTICIPANT)
642                 return -EINVAL;
643
644         /* dpll_ck, core_ck, virt_prcm_set; plus all clksel clocks */
645         if (clk->set_rate != 0)
646                 ret = clk->set_rate(clk, rate);
647
648         if (unlikely(ret == 0 && (clk->flags & RATE_PROPAGATES)))
649                 propagate_rate(clk);
650
651         return ret;
652 }
653
654 /*
655  * Converts encoded control register address into a full address
656  * On error, *src_addr will be returned as 0.
657  */
658 static u32 omap2_clksel_get_src_field(void __iomem **src_addr,
659                                       struct clk *src_clk, u32 *field_mask,
660                                       struct clk *clk, u32 *parent_div)
661 {
662         const struct clksel *clks;
663         const struct clksel_rate *clkr;
664
665         *parent_div = 0;
666         *src_addr = 0;
667
668         clks = omap2_get_clksel_by_parent(clk, src_clk);
669         if (clks == NULL)
670                 return 0;
671
672         for (clkr = clks->rates; clkr->div; clkr++) {
673                 if (clkr->flags & (cpu_mask | DEFAULT_RATE))
674                         break; /* Found the default rate for this platform */
675         }
676
677         if (!clkr->div) {
678                 printk(KERN_ERR "clock: Could not find default rate for "
679                        "clock %s parent %s\n", clk->name,
680                        src_clk->parent->name);
681                 return 0;
682         }
683
684         /* Should never happen.  Add a clksel mask to the struct clk. */
685         WARN_ON(clk->clksel_mask == 0);
686
687         *field_mask = clk->clksel_mask;
688         *src_addr = clk->clksel_reg;
689         *parent_div = clkr->div;
690
691         return clkr->val;
692 }
693
694 int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
695 {
696         void __iomem *src_addr;
697         u32 field_val, field_mask, reg_val, parent_div;
698
699         if (unlikely(clk->flags & CONFIG_PARTICIPANT))
700                 return -EINVAL;
701
702         if (!clk->clksel)
703                 return -EINVAL;
704
705         field_val = omap2_clksel_get_src_field(&src_addr, new_parent,
706                                                &field_mask, clk, &parent_div);
707         if (src_addr == 0)
708                 return -EINVAL;
709
710         if (clk->usecount > 0)
711                 _omap2_clk_disable(clk);
712
713         /* Set new source value (previous dividers if any in effect) */
714         reg_val = __raw_readl(src_addr) & ~field_mask;
715         reg_val |= (field_val << __ffs(field_mask));
716         __raw_writel(reg_val, src_addr);
717         wmb();
718
719         if (clk->flags & DELAYED_APP && cpu_is_omap24xx()) {
720                 __raw_writel(OMAP24XX_VALID_CONFIG, OMAP24XX_PRCM_CLKCFG_CTRL);
721                 wmb();
722         }
723
724         if (clk->usecount > 0)
725                 _omap2_clk_enable(clk);
726
727         clk->parent = new_parent;
728
729         /* CLKSEL clocks follow their parents' rates, divided by a divisor */
730         clk->rate = new_parent->rate;
731
732         if (parent_div > 0)
733                 clk->rate /= parent_div;
734
735         pr_debug("clock: set parent of %s to %s (new rate %ld)\n",
736                  clk->name, clk->parent->name, clk->rate);
737
738         if (unlikely(clk->flags & RATE_PROPAGATES))
739                 propagate_rate(clk);
740
741         return 0;
742 }
743
744 /* DPLL rate rounding code */
745
746 /**
747  * omap2_dpll_set_rate_tolerance: set the error tolerance during rate rounding
748  * @clk: struct clk * of the DPLL
749  * @tolerance: maximum rate error tolerance
750  *
751  * Set the maximum DPLL rate error tolerance for the rate rounding
752  * algorithm.  The rate tolerance is an attempt to balance DPLL power
753  * saving (the least divider value "n") vs. rate fidelity (the least
754  * difference between the desired DPLL target rate and the rounded
755  * rate out of the algorithm).  So, increasing the tolerance is likely
756  * to decrease DPLL power consumption and increase DPLL rate error.
757  * Returns -EINVAL if provided a null clock ptr or a clk that is not a
758  * DPLL; or 0 upon success.
759  */
760 int omap2_dpll_set_rate_tolerance(struct clk *clk, unsigned int tolerance)
761 {
762         if (!clk || !clk->dpll_data)
763                 return -EINVAL;
764
765         clk->dpll_data->rate_tolerance = tolerance;
766
767         return 0;
768 }
769
770 static unsigned long _dpll_compute_new_rate(unsigned long parent_rate, unsigned int m, unsigned int n)
771 {
772         unsigned long long num;
773
774         num = (unsigned long long)parent_rate * m;
775         do_div(num, n);
776         return num;
777 }
778
779 /*
780  * _dpll_test_mult - test a DPLL multiplier value
781  * @m: pointer to the DPLL m (multiplier) value under test
782  * @n: current DPLL n (divider) value under test
783  * @new_rate: pointer to storage for the resulting rounded rate
784  * @target_rate: the desired DPLL rate
785  * @parent_rate: the DPLL's parent clock rate
786  *
787  * This code tests a DPLL multiplier value, ensuring that the
788  * resulting rate will not be higher than the target_rate, and that
789  * the multiplier value itself is valid for the DPLL.  Initially, the
790  * integer pointed to by the m argument should be prescaled by
791  * multiplying by DPLL_SCALE_FACTOR.  The code will replace this with
792  * a non-scaled m upon return.  This non-scaled m will result in a
793  * new_rate as close as possible to target_rate (but not greater than
794  * target_rate) given the current (parent_rate, n, prescaled m)
795  * triple. Returns DPLL_MULT_UNDERFLOW in the event that the
796  * non-scaled m attempted to underflow, which can allow the calling
797  * function to bail out early; or 0 upon success.
798  */
799 static int _dpll_test_mult(int *m, int n, unsigned long *new_rate,
800                            unsigned long target_rate,
801                            unsigned long parent_rate)
802 {
803         int flags = 0, carry = 0;
804
805         /* Unscale m and round if necessary */
806         if (*m % DPLL_SCALE_FACTOR >= DPLL_ROUNDING_VAL)
807                 carry = 1;
808         *m = (*m / DPLL_SCALE_FACTOR) + carry;
809
810         /*
811          * The new rate must be <= the target rate to avoid programming
812          * a rate that is impossible for the hardware to handle
813          */
814         *new_rate = _dpll_compute_new_rate(parent_rate, *m, n);
815         if (*new_rate > target_rate) {
816                 (*m)--;
817                 *new_rate = 0;
818         }
819
820         /* Guard against m underflow */
821         if (*m < DPLL_MIN_MULTIPLIER) {
822                 *m = DPLL_MIN_MULTIPLIER;
823                 *new_rate = 0;
824                 flags = DPLL_MULT_UNDERFLOW;
825         }
826
827         if (*new_rate == 0)
828                 *new_rate = _dpll_compute_new_rate(parent_rate, *m, n);
829
830         return flags;
831 }
832
833 /**
834  * omap2_dpll_round_rate - round a target rate for an OMAP DPLL
835  * @clk: struct clk * for a DPLL
836  * @target_rate: desired DPLL clock rate
837  *
838  * Given a DPLL, a desired target rate, and a rate tolerance, round
839  * the target rate to a possible, programmable rate for this DPLL.
840  * Rate tolerance is assumed to be set by the caller before this
841  * function is called.  Attempts to select the minimum possible n
842  * within the tolerance to reduce power consumption.  Stores the
843  * computed (m, n) in the DPLL's dpll_data structure so set_rate()
844  * will not need to call this (expensive) function again.  Returns ~0
845  * if the target rate cannot be rounded, either because the rate is
846  * too low or because the rate tolerance is set too tightly; or the
847  * rounded rate upon success.
848  */
849 long omap2_dpll_round_rate(struct clk *clk, unsigned long target_rate)
850 {
851         int m, n, r, e, scaled_max_m;
852         unsigned long scaled_rt_rp, new_rate;
853         int min_e = -1, min_e_m = -1, min_e_n = -1;
854
855         if (!clk || !clk->dpll_data)
856                 return ~0;
857
858         pr_debug("clock: starting DPLL round_rate for clock %s, target rate "
859                  "%ld\n", clk->name, target_rate);
860
861         scaled_rt_rp = target_rate / (clk->parent->rate / DPLL_SCALE_FACTOR);
862         scaled_max_m = clk->dpll_data->max_multiplier * DPLL_SCALE_FACTOR;
863
864         clk->dpll_data->last_rounded_rate = 0;
865
866         for (n = clk->dpll_data->max_divider; n >= DPLL_MIN_DIVIDER; n--) {
867
868                 /* Compute the scaled DPLL multiplier, based on the divider */
869                 m = scaled_rt_rp * n;
870
871                 /*
872                  * Since we're counting n down, a m overflow means we can
873                  * can immediately skip to the next n
874                  */
875                 if (m > scaled_max_m)
876                         continue;
877
878                 r = _dpll_test_mult(&m, n, &new_rate, target_rate,
879                                     clk->parent->rate);
880
881                 e = target_rate - new_rate;
882                 pr_debug("clock: n = %d: m = %d: rate error is %d "
883                          "(new_rate = %ld)\n", n, m, e, new_rate);
884
885                 if (min_e == -1 ||
886                     min_e >= (int)(abs(e) - clk->dpll_data->rate_tolerance)) {
887                         min_e = e;
888                         min_e_m = m;
889                         min_e_n = n;
890
891                         pr_debug("clock: found new least error %d\n", min_e);
892                 }
893
894                 /*
895                  * Since we're counting n down, a m underflow means we
896                  * can bail out completely (since as n decreases in
897                  * the next iteration, there's no way that m can
898                  * increase beyond the current m)
899                  */
900                 if (r & DPLL_MULT_UNDERFLOW)
901                         break;
902         }
903
904         if (min_e < 0) {
905                 pr_debug("clock: error: target rate or tolerance too low\n");
906                 return ~0;
907         }
908
909         clk->dpll_data->last_rounded_m = min_e_m;
910         clk->dpll_data->last_rounded_n = min_e_n;
911         clk->dpll_data->last_rounded_rate =
912                 _dpll_compute_new_rate(clk->parent->rate, min_e_m,  min_e_n);
913
914         pr_debug("clock: final least error: e = %d, m = %d, n = %d\n",
915                  min_e, min_e_m, min_e_n);
916         pr_debug("clock: final rate: %ld  (target rate: %ld)\n",
917                  clk->dpll_data->last_rounded_rate, target_rate);
918
919         return clk->dpll_data->last_rounded_rate;
920 }
921
922 /*-------------------------------------------------------------------------
923  * Omap2 clock reset and init functions
924  *-------------------------------------------------------------------------*/
925
926 #ifdef CONFIG_OMAP_RESET_CLOCKS
927 void omap2_clk_disable_unused(struct clk *clk)
928 {
929         u32 regval32, v;
930
931         v = (clk->flags & INVERT_ENABLE) ? (1 << clk->enable_bit) : 0;
932
933         regval32 = __raw_readl(clk->enable_reg);
934         if ((regval32 & (1 << clk->enable_bit)) == v)
935                 return;
936
937         printk(KERN_INFO "Disabling unused clock \"%s\"\n", clk->name);
938         _omap2_clk_disable(clk);
939 }
940 #endif