arch/arm/mach-omap2/clock.c

   1 /*
   2  *  linux/arch/arm/mach-omap2/clock.c
   3  *
   4  *  Copyright (C) 2005 Texas Instruments Inc.
   5  *  Richard Woodruff <r-woodruff2@ti.com>
   6  *  Created for OMAP2.
   7  *
   8  *  Cleaned up and modified to use omap shared clock framework by
   9  *  Tony Lindgren <tony@atomide.com>
  10  *
  11  *  Based on omap1 clock.c, Copyright (C) 2004 - 2005 Nokia corporation
  12  *  Written by Tuukka Tikkanen <tuukka.tikkanen@elektrobit.com>
  13  *
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License version 2 as
  16  * published by the Free Software Foundation.
  17  */
  18 #include <linux/module.h>
  19 #include <linux/kernel.h>
  20 #include <linux/device.h>
  21 #include <linux/list.h>
  22 #include <linux/errno.h>
  23 #include <linux/delay.h>
  24 #include <linux/clk.h>
  25
  26 #include <asm/io.h>
  27
  28 #include <asm/arch/clock.h>
  29 #include <asm/arch/sram.h>
  30 #include <asm/div64.h>
  31
  32 #include "memory.h"
  33 #include "clock.h"
  34 #include "prm.h"
  35 #include "prm_regbits_24xx.h"
  36 #include "cm.h"
  37 #include "cm_regbits_24xx.h"
  38
  39 #undef DEBUG
  40
  41 /* CM_CLKSEL1_CORE.CLKSEL_VLYNQ options (2420) */
  42 #define CLKSEL_VLYNQ_96MHZ              0
  43 #define CLKSEL_VLYNQ_CORECLK_16         0x10
  44
  45 /* CM_CLKEN_PLL.EN_{54,96}M_PLL options (24XX) */
  46 #define EN_APLL_STOPPED                 0
  47 #define EN_APLL_LOCKED                  3
  48
  49 /* CM_CLKSEL1_PLL.APLLS_CLKIN options (24XX) */
  50 #define APLLS_CLKIN_19_2MHZ             0
  51 #define APLLS_CLKIN_13MHZ               2
  52 #define APLLS_CLKIN_12MHZ               3
  53
  54 #define MAX_PLL_LOCK_WAIT               100000
  55
  56 //#define DOWN_VARIABLE_DPLL 1                  /* Experimental */
  57
  58 static struct prcm_config *curr_prcm_set;
  59 static struct clk *vclk;
  60 static struct clk *sclk;
  61 static u8 cpu_mask;
  62
  63 /*-------------------------------------------------------------------------
  64  * Omap2 specific clock functions
  65  *-------------------------------------------------------------------------*/
  66
  67 static inline u8 mask_to_shift(u32 mask)
  68 {
  69         return ffs(mask) - 1;
  70 }
  71
  72 /**
  73  * omap2_init_clksel_parent - set a clksel clk's parent field from the hardware
  74  * @clk: OMAP clock struct ptr to use
  75  *
  76  * Given a pointer to a source-selectable struct clk, read the hardware
  77  * register and determine what its parent is currently set to.  Update the
  78  * clk->parent field with the appropriate clk ptr.
  79  */
  80 static void omap2_init_clksel_parent(struct clk *clk)
  81 {
  82         const struct clksel *clks;
  83         const struct clksel_rate *clkr;
  84         u32 r, found = 0;
  85
  86         if (!clk->clksel)
  87                 return;
  88
  89         /* XXX Should be __raw_readl for non-CM 3430 clocks ? */
  90         r = cm_read_reg(clk->clksel_reg) & clk->clksel_mask;
  91         r >>= mask_to_shift(clk->clksel_mask);
  92
  93         for (clks = clk->clksel; clks->parent && !found; clks++) {
  94                 for (clkr = clks->rates; clkr->div && !found; clkr++) {
  95                         if ((clkr->flags & cpu_mask) && (clkr->val == r)) {
  96                                 if (clk->parent != clks->parent) {
  97                                         pr_debug("clock: inited %s parent "
  98                                                  "to %s (was %s)\n",
  99                                                  clk->name, clks->parent->name,
 100                                                  ((clk->parent->name) ?
 101                                                   clk->parent->name : "NULL"));
 102                                         clk->parent = clks->parent;
 103                                 };
 104                                 found = 1;
 105                         }
 106                 }
 107         }
 108
 109         if (!found)
 110                 printk(KERN_ERR "clock: init parent: could not find "
 111                        "regval %0x for clock %s\n", r,  clk->name);
 112
 113         return;
 114 }
 115
 116 /* Recalculate SYST_CLK */
 117 static void omap2_sys_clk_recalc(struct clk * clk)
 118 {
 119         u32 div;
 120
 121         if (!cpu_is_omap34xx()) {
 122                 div = prm_read_reg(OMAP24XX_PRCM_CLKSRC_CTRL);
 123                 /* Test if ext clk divided by 1 or 2 */
 124                 div &= OMAP_SYSCLKDIV_MASK;
 125                 div >>= clk->rate_offset;
 126                 clk->rate = (clk->parent->rate / div);
 127         }
 128         propagate_rate(clk);
 129 }
 130
 131 static u32 omap2_get_dpll_rate(struct clk * tclk)
 132 {
 133         long long dpll_clk;
 134         int dpll_mult, dpll_div, amult;
 135         u32 dpll;
 136
 137         dpll = cm_read_mod_reg(PLL_MOD, CM_CLKSEL1);
 138
 139         dpll_mult = dpll & OMAP24XX_DPLL_MULT_MASK;
 140         dpll_mult >>= OMAP24XX_DPLL_MULT_SHIFT;         /* 10 bits */
 141         dpll_div = dpll & OMAP24XX_DPLL_DIV_MASK;
 142         dpll_div >>= OMAP24XX_DPLL_DIV_SHIFT;           /* 4 bits */
 143         dpll_clk = (long long)tclk->parent->rate * dpll_mult;
 144         do_div(dpll_clk, dpll_div + 1);
 145         amult = cm_read_mod_reg(PLL_MOD, CM_CLKSEL2);
 146         amult &= OMAP24XX_CORE_CLK_SRC_MASK;
 147         dpll_clk *= amult;
 148
 149         return dpll_clk;
 150 }
 151
 152 /*
 153  * Used for clocks that have the same value as the parent clock,
 154  * divided by some factor
 155  */
 156 static void omap2_fixed_divisor_recalc(struct clk *clk)
 157 {
 158         WARN_ON(!clk->fixed_div);
 159
 160         clk->rate = clk->parent->rate / clk->fixed_div;
 161
 162         if (clk->flags & RATE_PROPAGATES)
 163                 propagate_rate(clk);
 164 }
 165
 166 static void omap2_set_osc_ck(int enable)
 167 {
 168         u32 pcc;
 169
 170         pcc = prm_read_reg(OMAP24XX_PRCM_CLKSRC_CTRL);
 171
 172         if (enable)
 173                 prm_write_reg(pcc & ~OMAP_AUTOEXTCLKMODE_MASK,
 174                               OMAP24XX_PRCM_CLKSRC_CTRL);
 175         else
 176                 prm_write_reg(pcc | OMAP_AUTOEXTCLKMODE_MASK,
 177                               OMAP24XX_PRCM_CLKSRC_CTRL);
 178 }
 179
 180 /*
 181  * omap2_wait_clock_ready - wait for PLL to lock
 182  *
 183  * Returns 1 if the PLL locked, 0 if it failed to lock.
 184  */
 185 static int omap2_wait_clock_ready(void __iomem *reg, u32 cval, const char *name)
 186 {
 187         int i = 0;
 188
 189         /* Wait for lock */
 190         while (!(cm_read_reg(reg) & cval)) {
 191                 ++i;
 192                 udelay(1);
 193                 if (i == MAX_PLL_LOCK_WAIT) {
 194                         printk(KERN_ERR "Clock %s didn't lock in %d tries\n",
 195                                name, MAX_PLL_LOCK_WAIT);
 196                         break;
 197                 }
 198         }
 199
 200         if (i)
 201                 pr_debug("Clock %s stable after %d loops\n", name, i);
 202
 203         return (i < MAX_PLL_LOCK_WAIT) ? 1 : 0;
 204 };
 205
 206
 207 /* Enable an APLL if off */
 208 static void omap2_clk_fixed_enable(struct clk *clk)
 209 {
 210         u32 cval, apll_mask;
 211
 212         apll_mask = EN_APLL_LOCKED << clk->enable_bit;
 213
 214         cval = cm_read_mod_reg(PLL_MOD, CM_CLKEN);
 215
 216         if ((cval & apll_mask) == apll_mask)
 217                 return;   /* apll already enabled */
 218
 219         cval &= ~apll_mask;
 220         cval |= apll_mask;
 221         cm_write_mod_reg(cval, PLL_MOD, CM_CLKEN);
 222
 223         if (clk == &apll96_ck)
 224                 cval = OMAP24XX_ST_96M_APLL;
 225         else if (clk == &apll54_ck)
 226                 cval = OMAP24XX_ST_54M_CLK;
 227
 228         omap2_wait_clock_ready(OMAP_CM_REGADDR(PLL_MOD, CM_IDLEST), cval,
 229                             clk->name);
 230 }
 231
 232 static void omap2_clk_wait_ready(struct clk *clk)
 233 {
 234         void __iomem *reg, *other_reg, *st_reg;
 235         u32 bit;
 236
 237         reg = clk->enable_reg;
 238         if (reg == OMAP_CM_REGADDR(CORE_MOD, CM_FCLKEN1) ||
 239             reg == OMAP_CM_REGADDR(CORE_MOD, OMAP24XX_CM_FCLKEN2))
 240                 other_reg = (void __iomem *)(((u32)reg & ~0xf0) | 0x10); /* CM_ICLKEN* */
 241         else if (reg == OMAP_CM_REGADDR(CORE_MOD, CM_ICLKEN1) ||
 242                  reg == OMAP_CM_REGADDR(CORE_MOD, CM_ICLKEN2))
 243                 other_reg = (void __iomem *)(((u32)reg & ~0xf0) | 0x00); /* CM_FCLKEN* */
 244         else
 245                 return;
 246
 247         /* No check for DSS or cam clocks */
 248         if (((u32)reg & 0x0f) == 0) { /* CM_{F,I}CLKEN1 */
 249                 if (clk->enable_bit == OMAP24XX_EN_DSS2_SHIFT ||
 250                     clk->enable_bit == OMAP24XX_EN_DSS1_SHIFT ||
 251                     clk->enable_bit == OMAP24XX_EN_CAM_SHIFT)
 252                         return;
 253         }
 254
 255         /* Check if both functional and interface clocks
 256          * are running. */
 257         bit = 1 << clk->enable_bit;
 258         if (!(cm_read_reg(other_reg) & bit))
 259                 return;
 260         st_reg = (void __iomem *)(((u32)other_reg & ~0xf0) | 0x20); /* CM_IDLEST* */
 261
 262         omap2_wait_clock_ready(st_reg, bit, clk->name);
 263 }
 264
 265 /* Enables clock without considering parent dependencies or use count
 266  * REVISIT: Maybe change this to use clk->enable like on omap1?
 267  */
 268 static int _omap2_clk_enable(struct clk * clk)
 269 {
 270         u32 regval32;
 271
 272         if (clk->flags & (ALWAYS_ENABLED | PARENT_CONTROLS_CLOCK))
 273                 return 0;
 274
 275         if (unlikely(clk == &osc_ck)) {
 276                 omap2_set_osc_ck(1);
 277                 return 0;
 278         }
 279
 280         if (unlikely(clk->enable_reg == 0)) {
 281                 printk(KERN_ERR "clock.c: Enable for %s without enable code\n",
 282                        clk->name);
 283                 return -EINVAL;
 284         }
 285
 286         if (clk->enable_reg == OMAP_CM_REGADDR(PLL_MOD, CM_CLKEN)) {
 287                 omap2_clk_fixed_enable(clk);
 288                 return 0;
 289         }
 290
 291         regval32 = cm_read_reg(clk->enable_reg);
 292         regval32 |= (1 << clk->enable_bit);
 293         cm_write_reg(regval32, clk->enable_reg);
 294         wmb();
 295
 296         omap2_clk_wait_ready(clk);
 297
 298         return 0;
 299 }
 300
 301 /* Stop APLL */
 302 static void omap2_clk_fixed_disable(struct clk *clk)
 303 {
 304         u32 cval;
 305
 306         cval = cm_read_mod_reg(PLL_MOD, CM_CLKEN);
 307         cval &= ~(EN_APLL_LOCKED << clk->enable_bit);
 308         cm_write_mod_reg(cval, PLL_MOD, CM_CLKEN);
 309 }
 310
 311 /* Disables clock without considering parent dependencies or use count */
 312 static void _omap2_clk_disable(struct clk *clk)
 313 {
 314         u32 regval32;
 315
 316         if (clk->flags & (ALWAYS_ENABLED | PARENT_CONTROLS_CLOCK))
 317                 return;
 318
 319         if (unlikely(clk == &osc_ck)) {
 320                 omap2_set_osc_ck(0);
 321                 return;
 322         }
 323
 324         if (clk->enable_reg == 0) {
 325                 /*
 326                  * 'Independent' here refers to a clock which is not
 327                  * controlled by its parent.
 328                  */
 329                 printk(KERN_ERR "clock: clk_disable called on independent "
 330                        "clock %s which has no enable_reg\n", clk->name);
 331                 return;
 332         }
 333
 334         if (clk->enable_reg == OMAP_CM_REGADDR(PLL_MOD, CM_CLKEN)) {
 335                 omap2_clk_fixed_disable(clk);
 336                 return;
 337         }
 338
 339         regval32 = cm_read_reg(clk->enable_reg);
 340         regval32 &= ~(1 << clk->enable_bit);
 341         cm_write_reg(regval32, clk->enable_reg);
 342         wmb();
 343 }
 344
 345 static int omap2_clk_enable(struct clk *clk)
 346 {
 347         int ret = 0;
 348
 349         if (clk->usecount++ == 0) {
 350                 if (likely((u32)clk->parent))
 351                         ret = omap2_clk_enable(clk->parent);
 352
 353                 if (unlikely(ret != 0)) {
 354                         clk->usecount--;
 355                         return ret;
 356                 }
 357
 358                 ret = _omap2_clk_enable(clk);
 359
 360                 if (unlikely(ret != 0) && clk->parent) {
 361                         omap2_clk_disable(clk->parent);
 362                         clk->usecount--;
 363                 }
 364         }
 365
 366         return ret;
 367 }
 368
 369 static void omap2_clk_disable(struct clk *clk)
 370 {
 371         if (clk->usecount > 0 && !(--clk->usecount)) {
 372                 _omap2_clk_disable(clk);
 373                 if (likely((u32)clk->parent))
 374                         omap2_clk_disable(clk->parent);
 375         }
 376 }
 377
 378 /*
 379  * Uses the current prcm set to tell if a rate is valid.
 380  * You can go slower, but not faster within a given rate set.
 381  */
 382 static u32 omap2_dpll_round_rate(unsigned long target_rate)
 383 {
 384         u32 high, low, core_clk_src;
 385
 386         core_clk_src = cm_read_mod_reg(PLL_MOD, CM_CLKSEL2);
 387         core_clk_src &= OMAP24XX_CORE_CLK_SRC_MASK;
 388
 389         if (core_clk_src == CORE_CLK_SRC_DPLL) {        /* DPLL clockout */
 390                 high = curr_prcm_set->dpll_speed * 2;
 391                 low = curr_prcm_set->dpll_speed;
 392         } else {                                /* DPLL clockout x 2 */
 393                 high = curr_prcm_set->dpll_speed;
 394                 low = curr_prcm_set->dpll_speed / 2;
 395         }
 396
 397 #ifdef DOWN_VARIABLE_DPLL
 398         if (target_rate > high)
 399                 return high;
 400         else
 401                 return target_rate;
 402 #else
 403         if (target_rate > low)
 404                 return high;
 405         else
 406                 return low;
 407 #endif
 408
 409 }
 410
 411 static void omap2_dpll_recalc(struct clk *clk)
 412 {
 413         clk->rate = omap2_get_dpll_rate(clk);
 414
 415         propagate_rate(clk);
 416 }
 417
 418 /*
 419  * Used for clocks that are part of CLKSEL_xyz governed clocks.
 420  * REVISIT: Maybe change to use clk->enable() functions like on omap1?
 421  */
 422 static void omap2_clksel_recalc(struct clk * clk)
 423 {
 424         u32 clksel1_core, div = 0;
 425
 426         clksel1_core = cm_read_mod_reg(CORE_MOD, CM_CLKSEL1);
 427
 428         if ((clk == &dss1_fck) &&
 429             (clksel1_core & OMAP24XX_CLKSEL_DSS1_MASK) == 0) {
 430                 div = 1;
 431         }
 432
 433         if ((clk == &vlynq_fck) && cpu_is_omap2420() &&
 434             (clksel1_core & OMAP2420_CLKSEL_VLYNQ_MASK) == CLKSEL_VLYNQ_96MHZ) {
 435                 div = 1;
 436         }
 437
 438         div = omap2_clksel_get_divisor(clk);
 439         if (div == 0)
 440                 return;
 441
 442         if (unlikely(clk->rate == clk->parent->rate / div))
 443                 return;
 444         clk->rate = clk->parent->rate / div;
 445
 446         if (unlikely(clk->flags & RATE_PROPAGATES))
 447                 propagate_rate(clk);
 448 }
 449
 450 /**
 451  * omap2_get_clksel_by_parent - return clksel struct for a given clk & parent
 452  * @clk: OMAP struct clk ptr to inspect
 453  * @src_clk: OMAP struct clk ptr of the parent clk to search for
 454  *
 455  * Scan the struct clksel array associated with the clock to find
 456  * the element associated with the supplied parent clock address.
 457  * Returns a pointer to the struct clksel on success or NULL on error.
 458  */
 459 const static struct clksel *omap2_get_clksel_by_parent(struct clk *clk,
 460                                                        struct clk *src_clk)
 461 {
 462         const struct clksel *clks;
 463
 464         if (!clk->clksel)
 465                 return NULL;
 466
 467         for (clks = clk->clksel; clks->parent; clks++) {
 468                 if (clks->parent == src_clk)
 469                         break; /* Found the requested parent */
 470         }
 471
 472         if (!clks->parent) {
 473                 printk(KERN_ERR "clock: Could not find parent clock %s in "
 474                        "clksel array of clock %s\n", src_clk->name,
 475                        clk->name);
 476                 return NULL;
 477         }
 478
 479         return clks;
 480 }
 481
 482 /**
 483  * omap2_clksel_round_rate - find divisor for the given clock and target rate.
 484  * @clk: OMAP struct clk to use
 485  * @target_rate: desired clock rate
 486  * @new_div: ptr to where we should store the divisor
 487  *
 488  * Finds 'best' divider value in an array based on the source and target
 489  * rates.  The divider array must be sorted with smallest divider first.
 490  * Note that this will not work for clocks which are part of CONFIG_PARTICIPANT,
 491  * they are only settable as part of virtual_prcm set.
 492  *
 493  * Returns the rounded clock rate or returns 0xffffffff on error.
 494  */
 495 static u32 omap2_clksel_round_rate(struct clk *clk, unsigned long target_rate,
 496                                    u32 *new_div)
 497 {
 498         unsigned long test_rate;
 499         const struct clksel *clks;
 500         const struct clksel_rate *clkr;
 501         u32 last_div = 0;
 502
 503         printk(KERN_INFO "clock: clksel_round_rate for %s target_rate %ld\n",
 504                clk->name, target_rate);
 505
 506         *new_div = 1;
 507
 508         clks = omap2_get_clksel_by_parent(clk, clk->parent);
 509         if (clks == NULL)
 510                 return ~0;
 511
 512         for (clkr = clks->rates; clkr->div; clkr++) {
 513                 if (!(clkr->flags & cpu_mask))
 514                     continue;
 515
 516                 /* Sanity check */
 517                 if (clkr->div <= last_div)
 518                         printk(KERN_ERR "clock: clksel_rate table not sorted "
 519                                "for clock %s", clk->name);
 520
 521                 last_div = clkr->div;
 522
 523                 test_rate = clk->parent->rate / clkr->div;
 524
 525                 if (test_rate <= target_rate)
 526                         break; /* found it */
 527         }
 528
 529         if (!clkr->div) {
 530                 printk(KERN_ERR "clock: Could not find divisor for target "
 531                        "rate %ld for clock %s parent %s\n", target_rate,
 532                        clk->name, clk->parent->name);
 533                 return ~0;
 534         }
 535
 536         *new_div = clkr->div;
 537
 538         printk(KERN_INFO "clock: new_div = %d, new_rate = %ld\n", *new_div,
 539                (clk->parent->rate / clkr->div));
 540
 541         return (clk->parent->rate / clkr->div);
 542 }
 543
 544 /* Given a clock and a rate apply a clock specific rounding function */
 545 static long omap2_clk_round_rate(struct clk *clk, unsigned long rate)
 546 {
 547         u32 new_div = 0;
 548
 549         if (clk->flags & RATE_FIXED)
 550                 return clk->rate;
 551
 552         if (clk->flags & RATE_CKCTL)
 553                 return omap2_clksel_round_rate(clk, rate, &new_div);
 554
 555         if (clk->round_rate != 0)
 556                 return clk->round_rate(clk, rate);
 557
 558         return clk->rate;
 559 }
 560
 561 static int omap2_reprogram_dpll(struct clk * clk, unsigned long rate)
 562 {
 563         u32 flags, cur_rate, low, mult, div, valid_rate, done_rate;
 564         u32 bypass = 0;
 565         struct prcm_config tmpset;
 566         int ret = -EINVAL;
 567
 568         local_irq_save(flags);
 569         cur_rate = omap2_get_dpll_rate(&dpll_ck);
 570         mult = cm_read_mod_reg(PLL_MOD, CM_CLKSEL2);
 571         mult &= OMAP24XX_CORE_CLK_SRC_MASK;
 572
 573         if ((rate == (cur_rate / 2)) && (mult == 2)) {
 574                 omap2_reprogram_sdrc(CORE_CLK_SRC_DPLL, 1);
 575         } else if ((rate == (cur_rate * 2)) && (mult == 1)) {
 576                 omap2_reprogram_sdrc(CORE_CLK_SRC_DPLL_X2, 1);
 577         } else if (rate != cur_rate) {
 578                 valid_rate = omap2_dpll_round_rate(rate);
 579                 if (valid_rate != rate)
 580                         goto dpll_exit;
 581
 582                 if (mult == 1)
 583                         low = curr_prcm_set->dpll_speed;
 584                 else
 585                         low = curr_prcm_set->dpll_speed / 2;
 586
 587                 tmpset.cm_clksel1_pll = cm_read_mod_reg(PLL_MOD, CM_CLKSEL1);
 588                 tmpset.cm_clksel1_pll &= ~(OMAP24XX_DPLL_MULT_MASK |
 589                                            OMAP24XX_DPLL_DIV_MASK);
 590                 div = ((curr_prcm_set->xtal_speed / 1000000) - 1);
 591                 tmpset.cm_clksel2_pll = cm_read_mod_reg(PLL_MOD, CM_CLKSEL2);
 592                 tmpset.cm_clksel2_pll &= ~OMAP24XX_CORE_CLK_SRC_MASK;
 593                 if (rate > low) {
 594                         tmpset.cm_clksel2_pll |= CORE_CLK_SRC_DPLL_X2;
 595                         mult = ((rate / 2) / 1000000);
 596                         done_rate = CORE_CLK_SRC_DPLL_X2;
 597                 } else {
 598                         tmpset.cm_clksel2_pll |= CORE_CLK_SRC_DPLL;
 599                         mult = (rate / 1000000);
 600                         done_rate = CORE_CLK_SRC_DPLL;
 601                 }
 602                 tmpset.cm_clksel1_pll |= (div << OMAP24XX_DPLL_DIV_SHIFT);
 603                 tmpset.cm_clksel1_pll |= (mult << OMAP24XX_DPLL_MULT_SHIFT);
 604
 605                 /* Worst case */
 606                 tmpset.base_sdrc_rfr = V24XX_SDRC_RFR_CTRL_BYPASS;
 607
 608                 if (rate == curr_prcm_set->xtal_speed)  /* If asking for 1-1 */
 609                         bypass = 1;
 610
 611                 omap2_reprogram_sdrc(CORE_CLK_SRC_DPLL_X2, 1); /* For init_mem */
 612
 613                 /* Force dll lock mode */
 614                 omap2_set_prcm(tmpset.cm_clksel1_pll, tmpset.base_sdrc_rfr,
 615                                bypass);
 616
 617                 /* Errata: ret dll entry state */
 618                 omap2_init_memory_params(omap2_dll_force_needed());
 619                 omap2_reprogram_sdrc(done_rate, 0);
 620         }
 621         omap2_dpll_recalc(&dpll_ck);
 622         ret = 0;
 623
 624 dpll_exit:
 625         local_irq_restore(flags);
 626         return(ret);
 627 }
 628
 629 /**
 630  * omap2_table_mpu_recalc - just return the MPU speed
 631  * @clk: virt_prcm_set struct clk
 632  *
 633  * Set virt_prcm_set's rate to the mpu_speed field of the current PRCM set.
 634  */
 635 static void omap2_table_mpu_recalc(struct clk *clk)
 636 {
 637         clk->rate = curr_prcm_set->mpu_speed;
 638 }
 639
 640 /*
 641  * Look for a rate equal or less than the target rate given a configuration set.
 642  *
 643  * What's not entirely clear is "which" field represents the key field.
 644  * Some might argue L3-DDR, others ARM, others IVA. This code is simple and
 645  * just uses the ARM rates.
 646  */
 647 static long omap2_round_to_table_rate(struct clk * clk, unsigned long rate)
 648 {
 649         struct prcm_config * ptr;
 650         long highest_rate;
 651
 652         if (clk != &virt_prcm_set)
 653                 return -EINVAL;
 654
 655         highest_rate = -EINVAL;
 656
 657         for (ptr = rate_table; ptr->mpu_speed; ptr++) {
 658                 if (!(ptr->flags & cpu_mask))
 659                         continue;
 660                 if (ptr->xtal_speed != sys_ck.rate)
 661                         continue;
 662
 663                 highest_rate = ptr->mpu_speed;
 664
 665                 /* Can check only after xtal frequency check */
 666                 if (ptr->mpu_speed <= rate)
 667                         break;
 668         }
 669         return highest_rate;
 670 }
 671
 672 /**
 673  * omap2_clksel_to_divisor() - turn clksel field value into integer divider
 674  * @clk: OMAP struct clk to use
 675  * @field_val: register field value to find
 676  *
 677  * Given a struct clk of a rate-selectable clksel clock, and a register field
 678  * value to search for, find the corresponding clock divisor.  The register
 679  * field value should be pre-masked and shifted down so the LSB is at bit 0
 680  * before calling.  Returns 0 on error
 681  */
 682 static u32 omap2_clksel_to_divisor(struct clk *clk, u32 field_val)
 683 {
 684         const struct clksel *clks;
 685         const struct clksel_rate *clkr;
 686
 687         clks = omap2_get_clksel_by_parent(clk, clk->parent);
 688         if (clks == NULL)
 689                 return 0;
 690
 691         for (clkr = clks->rates; clkr->div; clkr++) {
 692                 if ((clkr->flags & cpu_mask) && (clkr->val == field_val))
 693                         break;
 694         }
 695
 696         if (!clkr->div) {
 697                 printk(KERN_ERR "clock: Could not find fieldval %d for "
 698                        "clock %s parent %s\n", field_val, clk->name,
 699                        clk->parent->name);
 700                 return 0;
 701         }
 702
 703         return clkr->div;
 704 }
 705
 706 /**
 707  * omap2_divisor_to_clksel() - turn clksel integer divisor into a field value
 708  * @clk: OMAP struct clk to use
 709  * @div: integer divisor to search for
 710  *
 711  * Given a struct clk of a rate-selectable clksel clock, and a clock divisor,
 712  * find the corresponding register field value.  The return register value is
 713  * the value before left-shifting.  Returns 0xffffffff on error
 714  */
 715 static u32 omap2_divisor_to_clksel(struct clk *clk, u32 div)
 716 {
 717         const struct clksel *clks;
 718         const struct clksel_rate *clkr;
 719
 720         /* should never happen */
 721         WARN_ON(div == 0);
 722
 723         clks = omap2_get_clksel_by_parent(clk, clk->parent);
 724         if (clks == NULL)
 725                 return 0;
 726
 727         for (clkr = clks->rates; clkr->div; clkr++) {
 728                 if ((clkr->flags & cpu_mask) && (clkr->div == div))
 729                         break;
 730         }
 731
 732         if (!clkr->div) {
 733                 printk(KERN_ERR "clock: Could not find divisor %d for "
 734                        "clock %s parent %s\n", div, clk->name,
 735                        clk->parent->name);
 736                 return 0;
 737         }
 738
 739         return clkr->val;
 740 }
 741
 742 /**
 743  * omap2_get_clksel - find clksel register addr & field mask for a clk
 744  * @clk: struct clk to use
 745  * @field_mask: ptr to u32 to store the register field mask
 746  *
 747  * Returns the address of the clksel register upon success or NULL on error.
 748  */
 749 static void __iomem *omap2_get_clksel(struct clk *clk, u32 *field_mask)
 750 {
 751         if (unlikely((clk->clksel_reg == 0) || (clk->clksel_mask == 0)))
 752                 return NULL;
 753
 754         *field_mask = clk->clksel_mask;
 755
 756         return clk->clksel_reg;
 757 }
 758
 759 /**
 760  * omap2_clksel_get_divisor - get current divider applied to parent clock.
 761  * @clk: OMAP struct clk to use.
 762  *
 763  * Returns the integer divisor upon success or 0 on error.
 764  */
 765 static u32 omap2_clksel_get_divisor(struct clk *clk)
 766 {
 767         u32 field_mask, field_val;
 768         void __iomem *div_addr;
 769
 770         div_addr = omap2_get_clksel(clk, &field_mask);
 771         if (div_addr == 0)
 772                 return 0;
 773
 774         field_val = cm_read_reg(div_addr) & field_mask;
 775         field_val >>= mask_to_shift(field_mask);
 776
 777         return omap2_clksel_to_divisor(clk, field_val);
 778 }
 779
 780 /* Set the clock rate for a clock source */
 781 static int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 782 {
 783         int ret = -EINVAL;
 784         u32 field_mask, field_val, reg_val, new_div = 0;
 785         unsigned long validrate;
 786         void __iomem *div_addr;
 787
 788         if (!(clk->flags & CONFIG_PARTICIPANT) && (clk->flags & RATE_CKCTL)) {
 789                 if (clk == &dpll_ck)
 790                         return omap2_reprogram_dpll(clk, rate);
 791
 792                 validrate = omap2_clksel_round_rate(clk, rate, &new_div);
 793                 if (validrate != rate)
 794                         return ret;
 795
 796                 div_addr = omap2_get_clksel(clk, &field_mask);
 797                 if (div_addr == 0)
 798                         return ret;
 799
 800                 field_val = omap2_divisor_to_clksel(clk, new_div);
 801                 if (field_val == ~0)
 802                         return ret;
 803
 804                 reg_val = cm_read_reg(div_addr);
 805                 reg_val &= ~field_mask;
 806                 reg_val |= (field_val << mask_to_shift(field_mask));
 807                 cm_write_reg(reg_val, div_addr);
 808                 wmb();
 809                 clk->rate = clk->parent->rate / new_div;
 810
 811                 if (clk->flags & DELAYED_APP) {
 812                         prm_write_reg(OMAP24XX_VALID_CONFIG,
 813                                       OMAP24XX_PRCM_CLKCFG_CTRL);
 814                         wmb();
 815                 }
 816                 ret = 0;
 817         } else if (clk->set_rate != 0) {
 818                 ret = clk->set_rate(clk, rate);
 819         }
 820
 821         if (unlikely(ret == 0 && (clk->flags & RATE_PROPAGATES)))
 822                 propagate_rate(clk);
 823
 824         return ret;
 825 }
 826
 827 /*
 828  * Converts encoded control register address into a full address
 829  * On error, *src_addr will be returned as 0.
 830  */
 831 static u32 omap2_clksel_get_src_field(void __iomem **src_addr,
 832                                       struct clk *src_clk, u32 *field_mask,
 833                                       struct clk *clk, u32 *parent_div)
 834 {
 835         const struct clksel *clks;
 836         const struct clksel_rate *clkr;
 837
 838         *parent_div = 0;
 839         *src_addr = 0;
 840
 841         clks = omap2_get_clksel_by_parent(clk, src_clk);
 842         if (clks == NULL)
 843                 return 0;
 844
 845         for (clkr = clks->rates; clkr->div; clkr++) {
 846                 if (clkr->flags & (cpu_mask | DEFAULT_RATE))
 847                         break; /* Found the default rate for this platform */
 848         }
 849
 850         if (!clkr->div) {
 851                 printk(KERN_ERR "clock: Could not find default rate for "
 852                        "clock %s parent %s\n", clk->name,
 853                        src_clk->parent->name);
 854                 return 0;
 855         }
 856
 857         /* Should never happen.  Add a clksel mask to the struct clk. */
 858         WARN_ON(clk->clksel_mask == 0);
 859
 860         *field_mask = clk->clksel_mask;
 861         *src_addr = clk->clksel_reg;
 862         *parent_div = clkr->div;
 863
 864         return clkr->val;
 865 }
 866
 867 static int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 868 {
 869         void __iomem *src_addr;
 870         u32 field_val, field_mask, reg_val, parent_div;
 871
 872         if (unlikely(clk->flags & CONFIG_PARTICIPANT))
 873                 return -EINVAL;
 874
 875         if (unlikely(!(clk->flags & SRC_SEL_MASK)))
 876                 return -EINVAL;
 877
 878         field_val = omap2_clksel_get_src_field(&src_addr, new_parent,
 879                                                &field_mask, clk, &parent_div);
 880         if (src_addr == 0)
 881                 return -EINVAL;
 882
 883         if (clk->usecount > 0)
 884                 _omap2_clk_disable(clk);
 885
 886         /* Set new source value (previous dividers if any in effect) */
 887         reg_val = __raw_readl(src_addr) & ~field_mask;
 888         reg_val |= (field_val << clk->src_offset);
 889         __raw_writel(reg_val, src_addr);
 890         wmb();
 891
 892         if (clk->flags & DELAYED_APP) {
 893                 prm_write_reg(OMAP24XX_VALID_CONFIG,
 894                               OMAP24XX_PRCM_CLKCFG_CTRL);
 895                 wmb();
 896         }
 897
 898         if (clk->usecount > 0)
 899                 _omap2_clk_enable(clk);
 900
 901         clk->parent = new_parent;
 902
 903         /* SRC_RATE_SEL_MASK clocks follow their parents rates.*/
 904         clk->rate = new_parent->rate;
 905
 906         if (parent_div > 0)
 907                 clk->rate /= parent_div;
 908
 909         if (unlikely(clk->flags & RATE_PROPAGATES))
 910                 propagate_rate(clk);
 911
 912         return 0;
 913 }
 914
 915 /* Sets basic clocks based on the specified rate */
 916 static int omap2_select_table_rate(struct clk * clk, unsigned long rate)
 917 {
 918         u32 flags, cur_rate, done_rate, bypass = 0, tmp;
 919         struct prcm_config *prcm;
 920         unsigned long found_speed = 0;
 921
 922         if (clk != &virt_prcm_set)
 923                 return -EINVAL;
 924
 925         for (prcm = rate_table; prcm->mpu_speed; prcm++) {
 926                 if (!(prcm->flags & cpu_mask))
 927                         continue;
 928
 929                 if (prcm->xtal_speed != sys_ck.rate)
 930                         continue;
 931
 932                 if (prcm->mpu_speed <= rate) {
 933                         found_speed = prcm->mpu_speed;
 934                         break;
 935                 }
 936         }
 937
 938         if (!found_speed) {
 939                 printk(KERN_INFO "Could not set MPU rate to %luMHz\n",
 940                        rate / 1000000);
 941                 return -EINVAL;
 942         }
 943
 944         curr_prcm_set = prcm;
 945         cur_rate = omap2_get_dpll_rate(&dpll_ck);
 946
 947         if (prcm->dpll_speed == cur_rate / 2) {
 948                 omap2_reprogram_sdrc(CORE_CLK_SRC_DPLL, 1);
 949         } else if (prcm->dpll_speed == cur_rate * 2) {
 950                 omap2_reprogram_sdrc(CORE_CLK_SRC_DPLL_X2, 1);
 951         } else if (prcm->dpll_speed != cur_rate) {
 952                 local_irq_save(flags);
 953
 954                 if (prcm->dpll_speed == prcm->xtal_speed)
 955                         bypass = 1;
 956
 957                 if ((prcm->cm_clksel2_pll & OMAP24XX_CORE_CLK_SRC_MASK) ==
 958                     CORE_CLK_SRC_DPLL_X2)
 959                         done_rate = CORE_CLK_SRC_DPLL_X2;
 960                 else
 961                         done_rate = CORE_CLK_SRC_DPLL;
 962
 963                 /* MPU divider */
 964                 cm_write_mod_reg(prcm->cm_clksel_mpu, MPU_MOD, CM_CLKSEL);
 965
 966                 /* dsp + iva1 div(2420), iva2.1(2430) */
 967                 cm_write_mod_reg(prcm->cm_clksel_dsp,
 968                                  OMAP24XX_DSP_MOD, CM_CLKSEL);
 969
 970                 cm_write_mod_reg(prcm->cm_clksel_gfx, GFX_MOD, CM_CLKSEL);
 971
 972                 /* Major subsystem dividers */
 973                 tmp = cm_read_mod_reg(CORE_MOD, CM_CLKSEL1) & OMAP24XX_CLKSEL_DSS2_MASK;
 974                 cm_write_mod_reg(prcm->cm_clksel1_core | tmp, CORE_MOD, CM_CLKSEL1);
 975                 if (cpu_is_omap2430())
 976                         cm_write_mod_reg(prcm->cm_clksel_mdm,
 977                                          OMAP2430_MDM_MOD, CM_CLKSEL);
 978
 979                 /* x2 to enter init_mem */
 980                 omap2_reprogram_sdrc(CORE_CLK_SRC_DPLL_X2, 1);
 981
 982                 omap2_set_prcm(prcm->cm_clksel1_pll, prcm->base_sdrc_rfr,
 983                                bypass);
 984
 985                 omap2_init_memory_params(omap2_dll_force_needed());
 986                 omap2_reprogram_sdrc(done_rate, 0);
 987
 988                 local_irq_restore(flags);
 989         }
 990         omap2_dpll_recalc(&dpll_ck);
 991
 992         return 0;
 993 }
 994
 995 /*-------------------------------------------------------------------------
 996  * Omap2 clock reset and init functions
 997  *-------------------------------------------------------------------------*/
 998
 999 #ifdef CONFIG_OMAP_RESET_CLOCKS
1000 static void __init omap2_clk_disable_unused(struct clk *clk)
1001 {
1002         u32 regval32;
1003
1004         regval32 = cm_read_reg(clk->enable_reg);
1005         if ((regval32 & (1 << clk->enable_bit)) == 0)
1006                 return;
1007
1008         printk(KERN_INFO "Disabling unused clock \"%s\"\n", clk->name);
1009         _omap2_clk_disable(clk);
1010 }
1011 #else
1012 #define omap2_clk_disable_unused        NULL
1013 #endif
1014
1015 static struct clk_functions omap2_clk_functions = {
1016         .clk_enable             = omap2_clk_enable,
1017         .clk_disable            = omap2_clk_disable,
1018         .clk_round_rate         = omap2_clk_round_rate,
1019         .clk_set_rate           = omap2_clk_set_rate,
1020         .clk_set_parent         = omap2_clk_set_parent,
1021         .clk_disable_unused     = omap2_clk_disable_unused,
1022 };
1023
1024 static void __init omap2_get_crystal_rate(struct clk *osc, struct clk *sys)
1025 {
1026         u32 div, aplls, sclk = 13000000;
1027
1028         aplls = cm_read_mod_reg(PLL_MOD, CM_CLKSEL1);
1029         aplls &= OMAP24XX_APLLS_CLKIN_MASK;
1030         aplls >>= OMAP24XX_APLLS_CLKIN_SHIFT;
1031
1032         if (aplls == APLLS_CLKIN_19_2MHZ)
1033                 sclk = 19200000;
1034         else if (aplls == APLLS_CLKIN_13MHZ)
1035                 sclk = 13000000;
1036         else if (aplls == APLLS_CLKIN_12MHZ)
1037                 sclk = 12000000;
1038
1039         div = prm_read_reg(OMAP24XX_PRCM_CLKSRC_CTRL);
1040         div &= OMAP_SYSCLKDIV_MASK;
1041         div >>= sys->rate_offset;
1042
1043         osc->rate = sclk * div;
1044         sys->rate = sclk;
1045 }
1046
1047 /*
1048  * Set clocks for bypass mode for reboot to work.
1049  */
1050 void omap2_clk_prepare_for_reboot(void)
1051 {
1052         u32 rate;
1053
1054         if (vclk == NULL || sclk == NULL)
1055                 return;
1056
1057         rate = clk_get_rate(sclk);
1058         clk_set_rate(vclk, rate);
1059 }
1060
1061 /*
1062  * Switch the MPU rate if specified on cmdline.
1063  * We cannot do this early until cmdline is parsed.
1064  */
1065 static int __init omap2_clk_arch_init(void)
1066 {
1067         if (!mpurate)
1068                 return -EINVAL;
1069
1070         if (omap2_select_table_rate(&virt_prcm_set, mpurate))
1071                 printk(KERN_ERR "Could not find matching MPU rate\n");
1072
1073         recalculate_root_clocks();
1074
1075         printk(KERN_INFO "Switched to new clocking rate (Crystal/DPLL/MPU): "
1076                "%ld.%01ld/%ld/%ld MHz\n",
1077                (sys_ck.rate / 1000000), (sys_ck.rate / 100000) % 10,
1078                (dpll_ck.rate / 1000000), (mpu_ck.rate / 1000000)) ;
1079
1080         return 0;
1081 }
1082 arch_initcall(omap2_clk_arch_init);
1083
1084 int __init omap2_clk_init(void)
1085 {
1086         struct prcm_config *prcm;
1087         struct clk ** clkp;
1088         u32 clkrate;
1089
1090         if (cpu_is_omap242x())
1091                 cpu_mask = RATE_IN_242X;
1092         else if (cpu_is_omap2430())
1093                 cpu_mask = RATE_IN_243X;
1094
1095         clk_init(&omap2_clk_functions);
1096         omap2_get_crystal_rate(&osc_ck, &sys_ck);
1097
1098         for (clkp = onchip_clks; clkp < onchip_clks + ARRAY_SIZE(onchip_clks);
1099              clkp++) {
1100
1101                 if ((*clkp)->flags & CLOCK_IN_OMAP242X && cpu_is_omap2420()) {
1102                         clk_register(*clkp);
1103                         continue;
1104                 }
1105
1106                 if ((*clkp)->flags & CLOCK_IN_OMAP243X && (cpu_is_omap2430() || cpu_is_omap34xx())) {
1107                         clk_register(*clkp);
1108                         continue;
1109                 }
1110         }
1111
1112         /* Check the MPU rate set by bootloader */
1113         clkrate = omap2_get_dpll_rate(&dpll_ck);
1114         for (prcm = rate_table; prcm->mpu_speed; prcm++) {
1115                 if (!(prcm->flags & cpu_mask))
1116                         continue;
1117                 if (prcm->xtal_speed != sys_ck.rate)
1118                         continue;
1119                 if (prcm->dpll_speed <= clkrate)
1120                          break;
1121         }
1122         curr_prcm_set = prcm;
1123
1124         recalculate_root_clocks();
1125
1126         printk(KERN_INFO "Clocking rate (Crystal/DPLL/MPU): "
1127                "%ld.%01ld/%ld/%ld MHz\n",
1128                (sys_ck.rate / 1000000), (sys_ck.rate / 100000) % 10,
1129                (dpll_ck.rate / 1000000), (mpu_ck.rate / 1000000)) ;
1130
1131         /*
1132          * Only enable those clocks we will need, let the drivers
1133          * enable other clocks as necessary
1134          */
1135         clk_enable(&sync_32k_ick);
1136         clk_enable(&omapctrl_ick);
1137
1138         /* Force the APLLs always active. The clocks are idled
1139          * automatically by hardware. */
1140         clk_enable(&apll96_ck);
1141         clk_enable(&apll54_ck);
1142
1143         if (cpu_is_omap2430())
1144                 clk_enable(&sdrc_ick);
1145
1146         /* Avoid sleeping sleeping during omap2_clk_prepare_for_reboot() */
1147         vclk = clk_get(NULL, "virt_prcm_set");
1148         sclk = clk_get(NULL, "sys_ck");
1149
1150         return 0;
1151 }