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