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