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