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