arch/arm/mach-s5pv310/cpufreq.c

   1 /* linux/arch/arm/mach-s5pv310/cpufreq.c
   2  *
   3  * Copyright (c) 2010 Samsung Electronics Co., Ltd.
   4  *              http://www.samsung.com/
   5  *
   6  * S5PV310 - CPU frequency scaling support
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License version 2 as
  10  * published by the Free Software Foundation.
  11  *
  12  * This is for S5PC210 EVT1. EVT0 support is removed.
  13 */
  14
  15 #include <linux/types.h>
  16 #include <linux/kernel.h>
  17 #include <linux/err.h>
  18 #include <linux/clk.h>
  19 #include <linux/io.h>
  20 #include <linux/suspend.h>
  21 #include <linux/slab.h>
  22 #include <linux/delay.h>
  23 #include <linux/regulator/consumer.h>
  24 #include <linux/cpufreq.h>
  25 #include <linux/suspend.h>
  26 #include <linux/reboot.h>
  27 #include <linux/pm_qos_params.h>
  28
  29 #include <plat/pm.h>
  30 #include <plat/s5pv310.h>
  31
  32 #include <mach/regs-iem.h>
  33 #include <mach/regs-clock.h>
  34 #include <mach/cpu_revision.h>
  35 #include <mach/map.h>
  36 #include <mach/pm-core.h>
  37 #include <mach/cpufreq.h>
  38 #include <mach/asv.h>
  39
  40 #define CPUMON 0
  41
  42 static struct clk *arm_clk;
  43 static struct clk *moutcore;
  44 static struct clk *mout_mpll;
  45 static struct clk *mout_apll;
  46
  47 #ifdef CONFIG_REGULATOR
  48 static struct regulator *arm_regulator;
  49 #endif
  50
  51 static struct cpufreq_freqs freqs;
  52 static int s5pv310_dvs_locking;
  53 static bool s5pv310_cpufreq_init_done;
  54 static DEFINE_MUTEX(set_cpu_freq_change);
  55 static DEFINE_MUTEX(set_cpu_freq_lock);
  56
  57 #define LOCK_CPUFREQ(x)         (x | 0x8000)
  58 #define UNLOCK_CPUFREQ(x)       (x | 0x4000)
  59 #define SLEEP_FREQ              800000 /* L1. The frequency set by IPL */
  60
  61 /* temperary define for additional symantics for relation */
  62 #define DISABLE_FURTHER_CPUFREQ         0x10
  63 #define ENABLE_FURTHER_CPUFREQ          0x20
  64 #define MASK_FURTHER_CPUFREQ            0x30
  65 #define MASK_ONLY_SET_CPUFREQ           0x40
  66 #define SET_CPU_FREQ_SAMPLING_RATE      100000
  67
  68 #define ARMCLOCK_1200MHZ                1200000
  69 #define ARMCLOCK_1000MHZ                1000000
  70 #define ARMCLOCK_500MHZ                 500000
  71
  72 static int s5pv310_max_armclk;
  73
  74 enum s5pv310_memory_type{
  75         DDR2 = 0x4,
  76         LPDDR2 = 0x5,
  77         DDR3 = 0x6,
  78 };
  79
  80 struct cpufreq_clkdiv {
  81         unsigned int    index;
  82         unsigned int    clkdiv;
  83 };
  84
  85 enum cpufreq_level_index{
  86 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
  87         L0,
  88 #endif
  89         L1, L2, L3, L4, CPUFREQ_LEVEL_END,
  90 };
  91
  92 /* Using lookup table to support 1200MHz/1000MHz by reading chip id */
  93 static struct cpufreq_frequency_table s5pv310_lookup_freq_table[]= {
  94 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
  95         {L0, 1200*1000},
  96 #endif
  97         {L1, 1000*1000},
  98         {L2, 800*1000},
  99         {L3, 500*1000},
 100         {L4, 200*1000},
 101         {0, CPUFREQ_TABLE_END},
 102 };
 103
 104 static struct cpufreq_frequency_table s5pv310_freq_table[] = {
 105 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 106         {L0, 1200*1000},
 107 #endif
 108         {L1, 1000*1000},
 109         {L2, 800*1000},
 110         {L3, 500*1000},
 111         {L4, 200*1000},
 112         {0, CPUFREQ_TABLE_END},
 113 };
 114
 115 static unsigned long cpufreq_lpj_table[CPUFREQ_LEVEL_END] = {};
 116
 117 static struct cpufreq_clkdiv s5pv310_clkdiv_table[] = {
 118 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 119         {L0, 0},
 120 #endif
 121         {L1, 0},
 122         {L2, 0},
 123         {L3, 0},
 124         {L4, 0},
 125 };
 126
 127 /* --- Implementation for PM QOS --- */
 128 static enum cpufreq_level_index pm_qos_min_guarantee = UINT_MAX;
 129 static enum cpufreq_level_index pm_qos_filter(enum cpufreq_level_index index)
 130 {
 131         /* Copy the value to avoid R/W locking for pm_qos_min_guarantee. */
 132         enum cpufreq_level_index ret = pm_qos_min_guarantee;
 133
 134         ret = (index > ret) ? ret : index;
 135
 136         return ret;
 137 }
 138
 139 struct pm_qos_dma2cpufreq {
 140         unsigned int dma_latency;
 141         unsigned int minimum_frequency;
 142         enum cpufreq_level_index level;
 143 };
 144
 145 /*
 146  * Sorted from the slowest frequency / greatest latency.
 147  * Based on DW_MMC Unmap time (512kiB)
 148  * Unmapping 512kiB with DW_MMC takes
 149  * 15ms @ 200MHz
 150  * 5ms @ 500MHz
 151  * 3ms @ 800MHz
 152  * 2ms @ 1000MHz
 153  * Unmapping 512kiB takes up to 128 times of unmap tasks.
 154  * As DW_MMC driver has been showing that unmap taks more time than
 155  * other DMA-related operations and we want to use the worse-case
 156  * scenario, we assume that the values represent the latency with
 157  * 128 operations. Thus, we device the latency values with 128.
 158  * Note that the metric in the following table is us, not ms.
 159  * Also, the values are rounded-up.
 160  */
 161 static struct pm_qos_dma2cpufreq pm_qos_requirement[] = {
 162         { 118,  200000, UINT_MAX }, /* UINT_MAX = to be defined in runtime */
 163         { 40,   500000, UINT_MAX },
 164         { 24,   800000, UINT_MAX },
 165         { 16,   1000000, UINT_MAX },
 166         { 0,    0,      0},
 167 };
 168 static int pm_qos_notify(struct notifier_block *nb, unsigned long value,
 169                          void *data)
 170 {
 171         int index = 0, i = 0, chosen = -EINVAL;
 172
 173         while (pm_qos_requirement[index].dma_latency) {
 174                 if (value >= pm_qos_requirement[index].dma_latency)
 175                         break;
 176                 index++;
 177         };
 178
 179         if (index)
 180                 index--;
 181
 182         if (pm_qos_requirement[index].level >= CPUFREQ_LEVEL_END) {
 183                 for (i = 0;
 184                      s5pv310_freq_table[i].frequency != CPUFREQ_TABLE_END &&
 185                      s5pv310_freq_table[i].frequency >=
 186                                 pm_qos_requirement[index].minimum_frequency;
 187                      i++)
 188                         chosen = i;
 189                 if (chosen >= 0)
 190                         pm_qos_requirement[index].level = chosen;
 191         }
 192
 193         if (WARN(pm_qos_requirement[index].level >= CPUFREQ_LEVEL_END,
 194                  "Cannot find the proper frequency.\n"))
 195                 return -EINVAL;
 196
 197         pm_qos_min_guarantee = pm_qos_requirement[index].level;
 198
 199         return NOTIFY_OK;
 200 }
 201
 202 static struct notifier_block pm_qos_notifier = {
 203         .notifier_call = pm_qos_notify,
 204 };
 205
 206
 207
 208 /* This defines are for cpufreq lock */
 209 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 210 #define CPUFREQ_LIMIT_LEVEL     L0
 211 #else
 212 #define CPUFREQ_LIMIT_LEVEL     L1
 213 #endif
 214
 215 unsigned int g_cpufreq_limit_id;
 216 unsigned int g_cpufreq_limit_val[DVFS_LOCK_ID_END];
 217 unsigned int g_cpufreq_limit_level = CPUFREQ_LIMIT_LEVEL;
 218
 219 #define CPUFREQ_MIN_LEVEL       (CPUFREQ_LEVEL_END - 1)
 220
 221 unsigned int g_cpufreq_lock_id;
 222 unsigned int g_cpufreq_lock_val[DVFS_LOCK_ID_END];
 223 unsigned int g_cpufreq_lock_level = CPUFREQ_MIN_LEVEL;
 224
 225 static unsigned int clkdiv_cpu0[CPUFREQ_LEVEL_END][7] = {
 226         /*
 227          * Clock divider value for following
 228          * { DIVCORE, DIVCOREM0, DIVCOREM1, DIVPERIPH,
 229          *              DIVATB, DIVPCLK_DBG, DIVAPLL }
 230          *         CORE    / COREM0 / COREM1 / PERI/ ATB / PDBG / SCLK_APLL
 231          */
 232 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 233         /* ARM L0: 1200MHz / 300MHz / 150MHz / 300 / 200 / 100 / 100 */
 234         { 0, 3, 7, 3, 4, 1, 7 },
 235 #endif
 236         /* ARM L1: 1000MHz / 250MHz / 125MHz / 250 / 200 / 100 / 100 */
 237         { 0, 3, 7, 3, 4, 1, 7 },
 238         /* ARM L2: 800MHz  / 200MHz / 100MHz / 200 / 200 / 100 / 100 */
 239         { 0, 3, 7, 3, 3, 1, 7 },
 240         /* ARM L3: 500MHz  / 125MHz / 63MHz / 125 / 125 / 63 / 100 */
 241         { 0, 3, 7, 3, 3, 1, 7 },
 242         /* ARM L4: 200MHz  / 100MHz / 50MHz  / 100 / 50 / 25 / 100 */
 243         { 0, 1, 3, 1, 3, 1, 7 },
 244 };
 245
 246 static unsigned int clkdiv_cpu1[CPUFREQ_LEVEL_END][2] = {
 247         /* Clock divider value for following
 248          * { DIVCOPY, DIVHPM }
 249          */
 250 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 251         /* ARM L0: 1200MHz */
 252         { 5, 0 },
 253 #endif
 254         /* ARM L1: 1000MHz */
 255         { 4, 0 },
 256         /* ARM L2: 800MHz */
 257         { 3, 0 },
 258         /* ARM L3: 500MHz */
 259         { 3, 0 },
 260         /* ARM L4: 200MHz */
 261         { 3, 0 },
 262 };
 263
 264 static unsigned int clkdiv_cpu0_lookup[][7] = {
 265         /*
 266          * Clock divider value for following
 267          * { DIVCORE, DIVCOREM0, DIVCOREM1, DIVPERIPH,
 268          *              DIVATB, DIVPCLK_DBG, DIVAPLL }
 269          */
 270 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 271         /* ARM L0: 1200MHz */
 272         { 0, 3, 7, 3, 4, 1, 7 },
 273 #endif
 274         /* ARM L1: 1000MHz */
 275         { 0, 3, 7, 3, 4, 1, 7 },
 276
 277         /* ARM L2: 800MHz */
 278         { 0, 3, 7, 3, 3, 1, 7 },
 279
 280         /* ARM L3: 500MHz */
 281         { 0, 3, 7, 3, 3, 1, 7 },
 282
 283         /* ARM L4: 200MHz */
 284         { 0, 1, 3, 1, 3, 1, 7 },
 285 };
 286
 287 static unsigned int clkdiv_cpu1_lookup[][2] = {
 288         /* Clock divider value for following
 289          * { DIVCOPY, DIVHPM }
 290          */
 291 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 292         /* ARM L0: 1200MHz */
 293         { 5, 0 },
 294 #endif
 295         /* ARM L1: 1000MHz */
 296         { 4, 0 },
 297
 298         /* ARM L1: 800MHz */
 299         { 3, 0 },
 300
 301         /* ARM L2: 500MHz */
 302         { 3, 0 },
 303
 304         /* ARM L3: 200MHz */
 305         { 3, 0 },
 306 };
 307
 308 struct cpufreq_voltage_table {
 309         unsigned int    index;          /* any */
 310         unsigned int    arm_volt;       /* uV */
 311 };
 312
 313 /* Using lookup table to support 1200MHz/1000MHz by reading chip id */
 314 static struct cpufreq_voltage_table s5pv310_lookup_volt_table[] = {
 315         {
 316 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 317                 .index          = L0,
 318                 .arm_volt       = 1300000,
 319         }, {
 320 #endif
 321                 .index          = L1,
 322                 .arm_volt       = 1200000,
 323         }, {
 324                 .index          = L2,
 325                 .arm_volt       = 1100000,
 326         }, {
 327                 .index          = L3,
 328                 .arm_volt       = 1000000,
 329         }, {
 330                 .index          = L4,
 331                 .arm_volt       = 975000,
 332         },
 333 };
 334
 335 static const struct cpufreq_voltage_table s5pv310_volt_table_SS[CPUFREQ_LEVEL_END] = {
 336         {
 337 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 338                 .index          = L0,
 339                 .arm_volt       = 1350000,
 340         }, {
 341 #endif
 342                 .index          = L1,
 343                 .arm_volt       = 1300000,
 344         }, {
 345                 .index          = L2,
 346                 .arm_volt       = 1200000,
 347         }, {
 348                 .index          = L3,
 349                 .arm_volt       = 1100000,
 350         }, {
 351                 .index          = L4,
 352                 .arm_volt       = 1050000,
 353         },
 354 };
 355
 356 static const struct cpufreq_voltage_table s5pv310_volt_table_A1[CPUFREQ_LEVEL_END] = {
 357         {
 358 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 359                 .index          = L0,
 360                 .arm_volt       = 1350000,
 361         }, {
 362 #endif
 363                 .index          = L1,
 364                 .arm_volt       = 1250000,
 365         }, {
 366                 .index          = L2,
 367                 .arm_volt       = 1150000,
 368         }, {
 369                 .index          = L3,
 370                 .arm_volt       = 1050000,
 371         }, {
 372                 .index          = L4,
 373                 .arm_volt       = 1000000,
 374         },
 375 };
 376
 377 static const struct cpufreq_voltage_table s5pv310_volt_table_A2[CPUFREQ_LEVEL_END] = {
 378         {
 379 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 380                 .index          = L0,
 381                 .arm_volt       = 1300000,
 382         }, {
 383 #endif
 384                 .index          = L1,
 385                 .arm_volt       = 1200000,
 386         }, {
 387                 .index          = L2,
 388                 .arm_volt       = 1100000,
 389         }, {
 390                 .index          = L3,
 391                 .arm_volt       = 1000000,
 392         }, {
 393                 .index          = L4,
 394                 .arm_volt       = 975000,
 395         },
 396 };
 397
 398 static const struct cpufreq_voltage_table s5pv310_volt_table_B1[CPUFREQ_LEVEL_END] = {
 399         {
 400 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 401                 .index          = L0,
 402                 .arm_volt       = 1275000,
 403         }, {
 404 #endif
 405                 .index          = L1,
 406                 .arm_volt       = 1175000,
 407         }, {
 408                 .index          = L2,
 409                 .arm_volt       = 1075000,
 410         }, {
 411                 .index          = L3,
 412                 .arm_volt       = 975000,
 413         }, {
 414                 .index          = L4,
 415                 .arm_volt       = 950000,
 416         },
 417 };
 418
 419 static const struct cpufreq_voltage_table s5pv310_volt_table_B2[CPUFREQ_LEVEL_END] = {
 420         {
 421 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 422                 .index          = L0,
 423                 .arm_volt       = 1250000,
 424         }, {
 425 #endif
 426                 .index          = L1,
 427                 .arm_volt       = 1150000,
 428         }, {
 429                 .index          = L2,
 430                 .arm_volt       = 1050000,
 431         }, {
 432                 .index          = L3,
 433                 .arm_volt       = 975000,
 434         }, {
 435                 .index          = L4,
 436                 .arm_volt       = 950000,
 437         },
 438 };
 439
 440 static const struct cpufreq_voltage_table s5pv310_volt_table_C1[CPUFREQ_LEVEL_END] = {
 441         {
 442 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 443                 .index          = L0,
 444                 .arm_volt       = 1225000,
 445         }, {
 446 #endif
 447                 .index          = L1,
 448                 .arm_volt       = 1125000,
 449         }, {
 450                 .index          = L2,
 451                 .arm_volt       = 1025000,
 452         }, {
 453                 .index          = L3,
 454                 .arm_volt       = 950000,
 455         }, {
 456                 .index          = L4,
 457                 .arm_volt       = 925000,
 458         },
 459 };
 460
 461 static const struct cpufreq_voltage_table s5pv310_volt_table_C2[CPUFREQ_LEVEL_END] = {
 462         {
 463 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 464                 .index          = L0,
 465                 .arm_volt       = 1200000,
 466         }, {
 467 #endif
 468                 .index          = L1,
 469                 .arm_volt       = 1100000,
 470         }, {
 471                 .index          = L2,
 472                 .arm_volt       = 1000000,
 473         }, {
 474                 .index          = L3,
 475                 .arm_volt       = 925000,
 476         }, {
 477                 .index          = L4,
 478                 .arm_volt       = 925000,
 479         },
 480 };
 481
 482 static const struct cpufreq_voltage_table s5pv310_volt_table_D[CPUFREQ_LEVEL_END] = {
 483         {
 484 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 485                 .index          = L0,
 486                 .arm_volt       = 1175000,
 487         }, {
 488 #endif
 489                 .index          = L1,
 490                 .arm_volt       = 1075000,
 491         }, {
 492                 .index          = L2,
 493                 .arm_volt       = 975000,
 494         }, {
 495                 .index          = L3,
 496                 .arm_volt       = 925000,
 497         }, {
 498                 .index          = L4,
 499                 .arm_volt       = 925000,
 500         },
 501 };
 502
 503 static struct cpufreq_voltage_table s5pv310_volt_table[CPUFREQ_LEVEL_END];
 504
 505 static unsigned int s5pv310_lookup_apll_pms_table[CPUFREQ_LEVEL_END] = {
 506 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 507         /* APLL FOUT L0: 1200MHz */
 508         ((150<<16)|(3<<8)|(0x1)),
 509 #endif
 510         /* APLL FOUT L1: 1000MHz */
 511         ((250<<16)|(6<<8)|(0x1)),
 512
 513         /* APLL FOUT L2: 800MHz */
 514         ((200<<16)|(6<<8)|(0x1)),
 515
 516         /* APLL FOUT L3: 500MHz */
 517         ((250<<16)|(6<<8)|(0x2)),
 518
 519         /* APLL FOUT L4: 200MHz */
 520         ((200<<16)|(6<<8)|(0x3)),
 521 };
 522
 523 static unsigned int s5pv310_apll_pms_table[CPUFREQ_LEVEL_END] = {
 524 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 525         /* APLL FOUT L0: 1200MHz */
 526         ((150<<16)|(3<<8)|(0x1)),
 527 #endif
 528         /* APLL FOUT L1: 1000MHz */
 529         ((250<<16)|(6<<8)|(0x1)),
 530
 531         /* APLL FOUT L2: 800MHz */
 532         ((200<<16)|(6<<8)|(0x1)),
 533
 534         /* APLL FOUT L3: 500MHz */
 535         ((250<<16)|(6<<8)|(0x2)),
 536
 537         /* APLL FOUT L4: 200MHz */
 538         ((200<<16)|(6<<8)|(0x3)),
 539 };
 540
 541 static int s5pv310_verify_policy(struct cpufreq_policy *policy)
 542 {
 543         return cpufreq_frequency_table_verify(policy, s5pv310_freq_table);
 544 }
 545
 546 static unsigned int s5pv310_getspeed(unsigned int cpu)
 547 {
 548         unsigned long rate;
 549
 550         rate = clk_get_rate(arm_clk) / 1000;
 551
 552         return rate;
 553 }
 554
 555 static void s5pv310_set_clkdiv(unsigned int div_index)
 556 {
 557         unsigned int tmp;
 558         /* Change Divider - CPU0 */
 559         tmp = __raw_readl(S5P_CLKDIV_CPU);
 560
 561         tmp &= ~(S5P_CLKDIV_CPU0_CORE_MASK | S5P_CLKDIV_CPU0_COREM0_MASK |
 562                 S5P_CLKDIV_CPU0_COREM1_MASK | S5P_CLKDIV_CPU0_PERIPH_MASK |
 563                 S5P_CLKDIV_CPU0_ATB_MASK | S5P_CLKDIV_CPU0_PCLKDBG_MASK |
 564                 S5P_CLKDIV_CPU0_APLL_MASK);
 565
 566         tmp |= ((clkdiv_cpu0[div_index][0] << S5P_CLKDIV_CPU0_CORE_SHIFT) |
 567                 (clkdiv_cpu0[div_index][1] << S5P_CLKDIV_CPU0_COREM0_SHIFT) |
 568                 (clkdiv_cpu0[div_index][2] << S5P_CLKDIV_CPU0_COREM1_SHIFT) |
 569                 (clkdiv_cpu0[div_index][3] << S5P_CLKDIV_CPU0_PERIPH_SHIFT) |
 570                 (clkdiv_cpu0[div_index][4] << S5P_CLKDIV_CPU0_ATB_SHIFT) |
 571                 (clkdiv_cpu0[div_index][5] << S5P_CLKDIV_CPU0_PCLKDBG_SHIFT) |
 572                 (clkdiv_cpu0[div_index][6] << S5P_CLKDIV_CPU0_APLL_SHIFT));
 573
 574         __raw_writel(tmp, S5P_CLKDIV_CPU);
 575
 576         do {
 577                 tmp = __raw_readl(S5P_CLKDIV_STATCPU);
 578         } while (tmp & 0x1111111);
 579
 580         /* Change Divider - CPU1 */
 581         tmp = __raw_readl(S5P_CLKDIV_CPU1);
 582
 583         tmp &= ~((0x7 << 4) | (0x7));
 584
 585         tmp |= ((clkdiv_cpu1[div_index][0] << 4) |
 586                 (clkdiv_cpu1[div_index][1] << 0));
 587
 588         __raw_writel(tmp, S5P_CLKDIV_CPU1);
 589
 590         do {
 591                 tmp = __raw_readl(S5P_CLKDIV_STATCPU1);
 592         } while (tmp & 0x11);
 593 }
 594
 595 static void s5pv310_set_apll(unsigned int index)
 596 {
 597         unsigned int tmp;
 598         unsigned int save_val;
 599
 600         /* 1. MUX_CORE_SEL = MPLL,
 601          * Reduce the CLKDIVCPU value for using MPLL */
 602         save_val = __raw_readl(S5P_CLKDIV_CPU);
 603         tmp = save_val;
 604         tmp &= ~S5P_CLKDIV_CPU0_CORE_MASK;
 605         tmp |= (((save_val & 0xf) + 1) << S5P_CLKDIV_CPU0_CORE_SHIFT);
 606         __raw_writel(tmp, S5P_CLKDIV_CPU);
 607         do {
 608                 tmp = __raw_readl(S5P_CLKDIV_STATCPU);
 609         } while (tmp & 0x1);
 610
 611         /* ARMCLK uses MPLL for lock time */
 612         clk_set_parent(moutcore, mout_mpll);
 613
 614         do {
 615                 tmp = (__raw_readl(S5P_CLKMUX_STATCPU)
 616                         >> S5P_CLKSRC_CPU_MUXCORE_SHIFT);
 617                 tmp &= 0x7;
 618         } while (tmp != 0x2);
 619
 620         /* 2. Set APLL Lock time */
 621         __raw_writel(S5P_APLL_LOCKTIME, S5P_APLL_LOCK);
 622
 623         /* 3. Change PLL PMS values */
 624         tmp = __raw_readl(S5P_APLL_CON0);
 625         tmp &= ~((0x3ff << 16) | (0x3f << 8) | (0x7 << 0));
 626         tmp |= s5pv310_apll_pms_table[index];
 627         __raw_writel(tmp, S5P_APLL_CON0);
 628
 629         /* 4. wait_lock_time */
 630         do {
 631                 tmp = __raw_readl(S5P_APLL_CON0);
 632         } while (!(tmp & (0x1 << S5P_APLLCON0_LOCKED_SHIFT)));
 633
 634         /* 5. MUX_CORE_SEL = APLL */
 635         clk_set_parent(moutcore, mout_apll);
 636
 637         do {
 638                 tmp = __raw_readl(S5P_CLKMUX_STATCPU);
 639                 tmp &= S5P_CLKMUX_STATCPU_MUXCORE_MASK;
 640         } while (tmp != (0x1 << S5P_CLKSRC_CPU_MUXCORE_SHIFT));
 641
 642         /* Restore the CLKDIVCPU value for APLL */
 643         __raw_writel(save_val, S5P_CLKDIV_CPU);
 644         do {
 645                 tmp = __raw_readl(S5P_CLKDIV_STATCPU);
 646         } while (tmp & 0x1);
 647
 648 }
 649
 650 static void s5pv310_set_frequency(unsigned int old_index,
 651                 unsigned int new_index)
 652 {
 653         unsigned int tmp;
 654         unsigned int is_curfreq_table = 0;
 655         unsigned int change_s_value = 0;
 656
 657         if (freqs.old == s5pv310_freq_table[old_index].frequency)
 658                 is_curfreq_table = 1;
 659
 660         if (freqs.old < freqs.new) {
 661                 if (s5pv310_max_armclk == ARMCLOCK_1200MHZ) {
 662                         /* L1/L3, L2/L4 Level change require to only change s value */
 663                         if (is_curfreq_table &&
 664                                 (((old_index == L3) && (new_index == L1)) ||
 665                                 ((old_index == L4) && (new_index == L2))))
 666                                         change_s_value = 1;
 667
 668                 } else {
 669                         /* L0/L2, L1/L3 Level change require to only change s value */
 670                         if (is_curfreq_table &&
 671                                 (
 672 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 673                                 ((old_index == L2) && (new_index == L0)) ||
 674 #endif
 675                                  ((old_index == L3) && (new_index == L1))))
 676                                         change_s_value = 1;
 677                 }
 678
 679                 if (change_s_value) {
 680                         /* 1. Change the system clock divider values */
 681                         s5pv310_set_clkdiv(new_index);
 682
 683                         /* 2. Change just s value in apll m,p,s value */
 684                         tmp = __raw_readl(S5P_APLL_CON0);
 685                         tmp &= ~(0x7 << 0);
 686                         tmp |= (s5pv310_apll_pms_table[new_index] & 0x7);
 687                         __raw_writel(tmp, S5P_APLL_CON0);
 688
 689                 } else {
 690                         /* Clock Configuration Procedure */
 691
 692                         /* 1. Change the system clock divider values */
 693                         s5pv310_set_clkdiv(new_index);
 694
 695                         /* 2. Change the apll m,p,s value */
 696                         if (freqs.new == ARMCLOCK_500MHZ) {
 697                                 regulator_set_voltage(arm_regulator,
 698                                         s5pv310_volt_table[new_index - 1].arm_volt,
 699                                         s5pv310_volt_table[new_index - 1].arm_volt);
 700                         }
 701                         s5pv310_set_apll(new_index);
 702                 }
 703         } else if (freqs.old > freqs.new) {
 704                 if (s5pv310_max_armclk == ARMCLOCK_1200MHZ) {
 705                         /* L1/L3, L2/L4 Level change require to only change s value */
 706                         if (is_curfreq_table &&
 707                                 (((old_index == L1) && (new_index == L3)) ||
 708                                 ((old_index == L2) && (new_index == L4))))
 709                                         change_s_value = 1;
 710                 } else {
 711                         /* L0/L2, L1/L3 Level change require to only change s value */
 712                         if (is_curfreq_table &&
 713                                 (
 714 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 715                                 ((old_index == L0) && (new_index == L2)) ||
 716 #endif
 717                                 ((old_index == L1) && (new_index == L3))))
 718                                         change_s_value = 1;
 719                 }
 720
 721                 if (change_s_value) {
 722                         /* 1. Change just s value in apll m,p,s value */
 723                         tmp = __raw_readl(S5P_APLL_CON0);
 724                         tmp &= ~(0x7 << 0);
 725                         tmp |= (s5pv310_apll_pms_table[new_index] & 0x7);
 726                         __raw_writel(tmp, S5P_APLL_CON0);
 727
 728                         /* 2. Change the system clock divider values */
 729                         s5pv310_set_clkdiv(new_index);
 730                 } else {
 731                         /* Clock Configuration Procedure */
 732
 733                         /* 1. Change the apll m,p,s value */
 734                         s5pv310_set_apll(new_index);
 735
 736                         /* 2. Change the system clock divider values */
 737                         s5pv310_set_clkdiv(new_index);
 738                 }
 739         }
 740 }
 741
 742 /*
 743  * In S5PC210/S5PV310, every core has the same frequency so, we
 744  * don't need to distinguish cores when we set jiffies or frequencies.
 745  */
 746 static void adjust_jiffies(unsigned int index)
 747 {
 748 #ifdef CONFIG_SMP
 749         int cpu;
 750 #endif
 751
 752         loops_per_jiffy = cpufreq_lpj_table[index];
 753
 754 #ifdef CONFIG_SMP
 755         for_each_online_cpu(cpu)
 756                 smp_store_cpu_info(cpu);
 757 #endif
 758 }
 759
 760 static int s5pv310_target(struct cpufreq_policy *policy,
 761                 unsigned int target_freq, unsigned int relation)
 762 {
 763
 764         int ret = 0;
 765         unsigned int index, old_index;
 766         unsigned int arm_volt;
 767
 768         unsigned int check_gov = 0;
 769
 770         mutex_lock(&set_cpu_freq_change);
 771
 772         if ((relation & ENABLE_FURTHER_CPUFREQ) &&
 773                 (relation & DISABLE_FURTHER_CPUFREQ)) {
 774                 /* Invalidate both if both marked */
 775                 relation &= ~ENABLE_FURTHER_CPUFREQ;
 776                 relation &= ~DISABLE_FURTHER_CPUFREQ;
 777                 printk(KERN_ERR "%s:%d denied marking \"FURTHER_CPUFREQ\""
 778                                 " as both marked.\n",
 779                                 __FILE__, __LINE__);
 780         }
 781
 782         if (!strncmp(policy->governor->name, "ondemand", CPUFREQ_NAME_LEN)
 783         || !strncmp(policy->governor->name, "conservative", CPUFREQ_NAME_LEN)) {
 784                 check_gov = 1;
 785                 if (relation & ENABLE_FURTHER_CPUFREQ)
 786                         s5pv310_dvs_locking = 0;
 787
 788                 if (s5pv310_dvs_locking == 1)
 789                         goto cpufreq_out;
 790
 791                 if (relation & DISABLE_FURTHER_CPUFREQ)
 792                         s5pv310_dvs_locking = 1;
 793
 794                 relation &= ~(MASK_FURTHER_CPUFREQ | MASK_ONLY_SET_CPUFREQ);
 795         } else {
 796                 if ((relation & ENABLE_FURTHER_CPUFREQ) ||
 797                         (relation & DISABLE_FURTHER_CPUFREQ) ||
 798                         (relation & MASK_ONLY_SET_CPUFREQ))
 799                         goto cpufreq_out;
 800         }
 801
 802         freqs.old = s5pv310_getspeed(policy->cpu);
 803
 804         if (cpufreq_frequency_table_target(policy, s5pv310_freq_table,
 805                                 freqs.old, relation, &old_index)) {
 806                 ret = -EINVAL;
 807                 goto cpufreq_out;
 808         }
 809
 810         if (cpufreq_frequency_table_target(policy, s5pv310_freq_table,
 811                                 target_freq, relation, &index)) {
 812                 ret = -EINVAL;
 813                 goto cpufreq_out;
 814         }
 815
 816         if (s5pv310_max_armclk == ARMCLOCK_1200MHZ) {
 817 #ifdef CONFIG_FREQ_STEP_UP_L2_L0
 818 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 819                 /* change L2 -> L0 */
 820                 if ((index == L0) && (old_index > L2)) {
 821                         index = L2;
 822                 }
 823 #endif
 824 #endif
 825         } else {
 826                 /* Prevent from jumping to 1GHz directly */
 827 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 828                 if((index == L0) && (old_index > L1))
 829                         index = L1;
 830 #endif
 831                 if (index > L3)
 832                         index = L3;
 833
 834                 if (old_index > L3)
 835                         old_index = L3;
 836         }
 837
 838         if ((index > g_cpufreq_lock_level) && check_gov)
 839                 index = g_cpufreq_lock_level;
 840         if ((index < g_cpufreq_limit_level) && check_gov)
 841                 index = g_cpufreq_limit_level;
 842
 843         /* The filter with highest priority: PM QOS */
 844         index = pm_qos_filter(index);
 845
 846         freqs.new = s5pv310_freq_table[index].frequency;
 847         freqs.cpu = policy->cpu;
 848
 849         /* If the new frequency is same with previous frequency, skip */
 850         if (freqs.new == freqs.old)
 851                 goto bus_freq;
 852
 853 #if CPUMON
 854         printk(KERN_ERR "CPUMON F %d\n", freqs.new);
 855 #endif
 856
 857         /* get the voltage value */
 858         arm_volt = s5pv310_volt_table[index].arm_volt;
 859         cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
 860
 861         /* When the new frequency is higher than current frequency */
 862         if (freqs.new > freqs.old) {
 863                 /* Firstly, voltage up to increase frequency */
 864 #if defined(CONFIG_REGULATOR)
 865                 regulator_set_voltage(arm_regulator, arm_volt, arm_volt + 150000);
 866 #endif
 867         }
 868
 869         s5pv310_set_frequency(old_index, index);
 870
 871         adjust_jiffies(index);
 872         cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
 873
 874         /* When the new frequency is lower than current frequency */
 875         if (freqs.new < freqs.old) {
 876                 /* down the voltage after frequency change */
 877 #if defined(CONFIG_REGULATOR)
 878                 regulator_set_voltage(arm_regulator, arm_volt, arm_volt + 150000);
 879 #endif
 880         }
 881
 882 cpufreq_out:
 883         mutex_unlock(&set_cpu_freq_change);
 884         return ret;
 885
 886 bus_freq:
 887         mutex_unlock(&set_cpu_freq_change);
 888         s5pv310_busfreq_force_notify(&freqs);
 889         return ret;
 890
 891 }
 892
 893 atomic_t s5pv310_cpufreq_lock_count;
 894
 895 int s5pv310_cpufreq_lock(unsigned int nId,
 896                          enum cpufreq_level_request cpufreq_level)
 897 {
 898         int ret = 0, cpu = 0;
 899         unsigned int cur_freq;
 900
 901         if (!s5pv310_cpufreq_init_done)
 902                 return 0;
 903
 904         if (s5pv310_max_armclk != ARMCLOCK_1200MHZ) {
 905 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 906                 if (cpufreq_level != CPU_L0) {
 907 #endif
 908                         cpufreq_level -= 1;
 909 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 910                 } else {
 911                         printk(KERN_WARNING
 912                                 "[CPUFREQ]cpufreq lock to 1GHz in place of 1.2GHz\n");
 913                 }
 914 #endif
 915         }
 916
 917         if (g_cpufreq_lock_id & (1 << nId)) {
 918                 printk(KERN_ERR
 919                 "[CPUFREQ]This device [%d] already locked cpufreq\n", nId);
 920                 return 0;
 921         }
 922         mutex_lock(&set_cpu_freq_lock);
 923         g_cpufreq_lock_id |= (1 << nId);
 924         g_cpufreq_lock_val[nId] = cpufreq_level;
 925
 926         /* If the requested cpufreq is higher than current min frequency */
 927         if (cpufreq_level < g_cpufreq_lock_level)
 928                 g_cpufreq_lock_level = cpufreq_level;
 929         mutex_unlock(&set_cpu_freq_lock);
 930
 931         /* If current frequency is lower than requested freq, need to update */
 932         cur_freq = s5pv310_getspeed(cpu);
 933         if (cur_freq < s5pv310_freq_table[cpufreq_level].frequency) {
 934                 ret = cpufreq_driver_target(cpufreq_cpu_get(cpu),
 935                                 s5pv310_freq_table[cpufreq_level].frequency,
 936                                 MASK_ONLY_SET_CPUFREQ);
 937         }
 938
 939         return ret;
 940 }
 941 EXPORT_SYMBOL_GPL(s5pv310_cpufreq_lock);
 942
 943 void s5pv310_cpufreq_lock_free(unsigned int nId)
 944 {
 945         unsigned int i;
 946
 947         if (!s5pv310_cpufreq_init_done)
 948                 return;
 949
 950         mutex_lock(&set_cpu_freq_lock);
 951
 952         g_cpufreq_lock_id &= ~(1 << nId);
 953         g_cpufreq_lock_val[nId] = CPUFREQ_MIN_LEVEL;
 954         g_cpufreq_lock_level = CPUFREQ_MIN_LEVEL;
 955         if (g_cpufreq_lock_id) {
 956                 for (i = 0; i < DVFS_LOCK_ID_END; i++) {
 957                         if (g_cpufreq_lock_val[i] < g_cpufreq_lock_level)
 958                                 g_cpufreq_lock_level = g_cpufreq_lock_val[i];
 959                 }
 960         }
 961         mutex_unlock(&set_cpu_freq_lock);
 962 }
 963 EXPORT_SYMBOL_GPL(s5pv310_cpufreq_lock_free);
 964
 965 int s5pv310_cpufreq_upper_limit(unsigned int nId, enum cpufreq_level_request cpufreq_level)
 966 {
 967         int ret = 0, cpu = 0;
 968         unsigned int cur_freq;
 969
 970         if (!s5pv310_cpufreq_init_done)
 971                 return 0;
 972
 973         if (s5pv310_max_armclk != ARMCLOCK_1200MHZ) {
 974 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 975                 if (cpufreq_level != CPU_L0) {
 976 #endif
 977                         cpufreq_level -= 1;
 978 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
 979                 } else {
 980                         printk(KERN_DEBUG
 981                                 "[CPUFREQ]cpufreq lock to 1GHz in place of 1.2GHz\n");
 982                 }
 983 #endif
 984         }
 985
 986         if (g_cpufreq_limit_id & (1 << nId)) {
 987                 printk(KERN_ERR "[CPUFREQ]This device [%d] already limited cpufreq\n", nId);
 988                 return 0;
 989         }
 990
 991         mutex_lock(&set_cpu_freq_lock);
 992         g_cpufreq_limit_id |= (1 << nId);
 993         g_cpufreq_limit_val[nId] = cpufreq_level;
 994
 995         /* If the requested limit level is lower than current value */
 996         if (cpufreq_level > g_cpufreq_limit_level)
 997                 g_cpufreq_limit_level = cpufreq_level;
 998
 999         mutex_unlock(&set_cpu_freq_lock);
1000
1001         /* If cur frequency is higher than limit freq, it needs to update */
1002         cur_freq = s5pv310_getspeed(cpu);
1003         if (cur_freq > s5pv310_freq_table[cpufreq_level].frequency) {
1004                 ret = cpufreq_driver_target(cpufreq_cpu_get(cpu),
1005                                 s5pv310_freq_table[cpufreq_level].frequency,
1006                                 MASK_ONLY_SET_CPUFREQ);
1007         }
1008
1009         return ret;
1010 }
1011
1012 void s5pv310_cpufreq_upper_limit_free(unsigned int nId)
1013 {
1014         unsigned int i;
1015
1016         if (!s5pv310_cpufreq_init_done)
1017                 return;
1018
1019         mutex_lock(&set_cpu_freq_lock);
1020         g_cpufreq_limit_id &= ~(1 << nId);
1021         g_cpufreq_limit_val[nId] = CPUFREQ_LIMIT_LEVEL;
1022         g_cpufreq_limit_level = CPUFREQ_LIMIT_LEVEL;
1023         if (g_cpufreq_limit_id) {
1024                 for (i = 0; i < DVFS_LOCK_ID_END; i++) {
1025                         if (g_cpufreq_limit_val[i] > g_cpufreq_limit_level)
1026                                 g_cpufreq_limit_level = g_cpufreq_limit_val[i];
1027                 }
1028         }
1029         mutex_unlock(&set_cpu_freq_lock);
1030 }
1031
1032 #ifdef CONFIG_PM
1033 static int s5pv310_cpufreq_suspend(struct cpufreq_policy *policy,
1034                         pm_message_t pmsg)
1035 {
1036         return 0;
1037 }
1038
1039 static int s5pv310_cpufreq_resume(struct cpufreq_policy *policy)
1040 {
1041         return 0;
1042 }
1043 #endif
1044
1045 static int s5pv310_cpufreq_notifier_event(struct notifier_block *this,
1046                 unsigned long event, void *ptr)
1047 {
1048         unsigned int cpu = 0;
1049         int ret = 0;
1050
1051         switch (event) {
1052         case PM_RESTORE_PREPARE:
1053         case PM_HIBERNATION_PREPARE:
1054         case PM_SUSPEND_PREPARE:
1055                 ret = cpufreq_driver_target(cpufreq_cpu_get(cpu),
1056                 s5pv310_freq_table[L1].frequency, DISABLE_FURTHER_CPUFREQ);
1057                 if (WARN_ON(ret < 0))
1058                         return NOTIFY_BAD;
1059                 printk(KERN_DEBUG "PM_SUSPEND_PREPARE for CPUFREQ\n");
1060                 return NOTIFY_OK;
1061         case PM_POST_HIBERNATION:
1062         case PM_POST_RESTORE:
1063         case PM_POST_SUSPEND:
1064                 printk(KERN_DEBUG "PM_POST_SUSPEND for CPUFREQ: %d\n", ret);
1065                 ret = cpufreq_driver_target(cpufreq_cpu_get(cpu),
1066                 s5pv310_freq_table[L1].frequency, ENABLE_FURTHER_CPUFREQ);
1067                 return NOTIFY_OK;
1068         }
1069         return NOTIFY_DONE;
1070 }
1071
1072 static struct notifier_block s5pv310_cpufreq_notifier = {
1073         .notifier_call = s5pv310_cpufreq_notifier_event,
1074 };
1075
1076 static int s5pv310_cpufreq_reboot_notifier_call(struct notifier_block *this,
1077                                    unsigned long code, void *_cmd)
1078 {
1079         unsigned int cpu = 0;
1080         int ret = 0;
1081         struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
1082
1083         if (strncmp(policy->governor->name, "powersave", CPUFREQ_NAME_LEN)) {
1084                 ret = cpufreq_driver_target(policy,
1085 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
1086                         s5pv310_freq_table[L0].frequency, DISABLE_FURTHER_CPUFREQ);
1087 #else
1088                         s5pv310_freq_table[L1].frequency, DISABLE_FURTHER_CPUFREQ);
1089 #endif
1090                 if (ret < 0)
1091                         return NOTIFY_BAD;
1092                 s5pv310_busfreq_lock(DVFS_LOCK_ID_PM, BUS_L0);
1093         } else {
1094 #if defined(CONFIG_REGULATOR)
1095 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
1096                 regulator_set_voltage(arm_regulator, s5pv310_volt_table[L0].arm_volt,
1097                         s5pv310_volt_table[L0].arm_volt);
1098 #else
1099                 regulator_set_voltage(arm_regulator, s5pv310_volt_table[L1].arm_volt,
1100                         s5pv310_volt_table[L1].arm_volt);
1101 #endif
1102 #endif
1103         }
1104
1105         printk(KERN_INFO "U1 REBOOT Notifier for CPUFREQ\n");
1106
1107         return NOTIFY_DONE;
1108
1109 }
1110
1111 static struct notifier_block s5pv310_cpufreq_reboot_notifier = {
1112         .notifier_call = s5pv310_cpufreq_reboot_notifier_call,
1113 };
1114
1115 static int s5pv310_cpufreq_cpu_init(struct cpufreq_policy *policy)
1116 {
1117         policy->cur = policy->min = policy->max = s5pv310_getspeed(policy->cpu);
1118
1119         cpufreq_frequency_table_get_attr(s5pv310_freq_table, policy->cpu);
1120
1121         policy->cpuinfo.transition_latency = 100000;
1122
1123         if (!cpu_online(1)) {
1124                 cpumask_copy(policy->related_cpus, cpu_possible_mask);
1125                 cpumask_copy(policy->cpus, cpu_online_mask);
1126         } else {
1127                 cpumask_setall(policy->cpus);
1128         }
1129
1130         return cpufreq_frequency_table_cpuinfo(policy, s5pv310_freq_table);
1131 }
1132
1133 static struct cpufreq_driver s5pv310_driver = {
1134         .flags = CPUFREQ_STICKY,
1135         .verify = s5pv310_verify_policy,
1136         .target = s5pv310_target,
1137         .get = s5pv310_getspeed,
1138         .init = s5pv310_cpufreq_cpu_init,
1139         .name = "s5pv310_cpufreq",
1140 #ifdef CONFIG_PM
1141         .suspend = s5pv310_cpufreq_suspend,
1142         .resume = s5pv310_cpufreq_resume,
1143 #endif
1144 };
1145
1146 #define HPM_OFFSET      12
1147 #define HPM_MASK        0x1F
1148 #define IDS_OFFSET      24
1149 #define IDS_MASK        0xFF
1150
1151 #define HPM_MINIMUM_B   7
1152 #define HPM_MINIMUM_C   14
1153 #define HPM_MINIMUM_D   21
1154
1155 #define IDS_MINIMUM_B   8
1156 #define IDS_MINIMUM_C   20
1157 #define IDS_MINIMUM_D   60
1158
1159 static void __init s5pv310_set_voltage(void)
1160 {
1161         unsigned int asv_group;
1162
1163         asv_group = exynos4_asv_group;
1164
1165         printk(KERN_INFO "DVFS : VDD_ARM Voltage table set with %d Group\n",asv_group);
1166
1167         switch (asv_group) {
1168         case 0:
1169                 memcpy(s5pv310_volt_table, s5pv310_volt_table_SS,
1170                         sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1171                 break;
1172         case 1:
1173                 memcpy(s5pv310_volt_table, s5pv310_volt_table_A1,
1174                         sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1175                 break;
1176         case 2:
1177                 memcpy(s5pv310_volt_table, s5pv310_volt_table_A2,
1178                         sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1179                 break;
1180         case 3:
1181                 memcpy(s5pv310_volt_table, s5pv310_volt_table_B1,
1182                         sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1183                 break;
1184         case 4:
1185                 memcpy(s5pv310_volt_table, s5pv310_volt_table_B2,
1186                         sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1187                 break;
1188         case 5:
1189                 memcpy(s5pv310_volt_table, s5pv310_volt_table_C1,
1190                         sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1191                 break;
1192         case 6:
1193                 memcpy(s5pv310_volt_table, s5pv310_volt_table_C2,
1194                         sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1195                 break;
1196         case 7:
1197                 memcpy(s5pv310_volt_table, s5pv310_volt_table_D,
1198                         sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1199                 break;
1200         default:
1201                 WARN(true, "Incorrect ASV Value.\n");
1202         }
1203
1204         memcpy(s5pv310_lookup_volt_table, s5pv310_volt_table,
1205                sizeof(struct cpufreq_voltage_table) * CPUFREQ_LEVEL_END);
1206 }
1207
1208 static int s5pv310_update_dvfs_table(void)
1209 {
1210         unsigned int i, j;
1211         int ret = 0;
1212
1213 #if CPUMON
1214         printk(KERN_INFO "@@@@@ original dvfs table data @@@@@\n");
1215         for (i = 0; i < CPUFREQ_LEVEL_END; i++)
1216                 printk(KERN_ERR "index = %d, frequecy = %d\n",
1217                         s5pv310_freq_table[i].index, s5pv310_freq_table[i].frequency);
1218
1219         for (i = 0; i < CPUFREQ_LEVEL_END; i++)
1220                 printk(KERN_ERR "index = %d, arm_volt = %d\n",
1221                         s5pv310_volt_table[i].index, s5pv310_volt_table[i].arm_volt);
1222
1223         for (i = 0; i < CPUFREQ_LEVEL_END; i++)
1224                 printk(KERN_ERR "apll pms_table = 0x%08x\n", s5pv310_apll_pms_table[i]);
1225
1226         for (i = 0; i < CPUFREQ_LEVEL_END; i++) {
1227                 for (j = 0; j < 7; j++)
1228                         printk("%d, ", clkdiv_cpu0[i][j]);
1229                 printk("\n");
1230         }
1231
1232         for (i = 0; i < CPUFREQ_LEVEL_END; i++) {
1233                 for (j = 0; j < 2; j++)
1234                         printk("%d, ", clkdiv_cpu1[i][j]);
1235                 printk("\n");
1236         }
1237 #endif
1238
1239         // Get the maximum arm clock */
1240         s5pv310_max_armclk = s5pv310_get_max_speed();
1241         printk(KERN_INFO "armclk set max %d \n", s5pv310_max_armclk);
1242
1243         if (s5pv310_max_armclk < 0) {
1244                 printk(KERN_ERR "Fail to get max armclk infomatioin.\n");
1245                 s5pv310_max_armclk  = 0; /* 1000MHz as default value */
1246                 ret = -EINVAL;
1247         }
1248
1249         switch (s5pv310_max_armclk) {
1250         case 1200000:
1251                 printk(KERN_INFO "armclk set max 1200MHz as default@@@@@\n");
1252                 break;
1253
1254         case 0:
1255         case 1000000:
1256         default:
1257                 /* TODO: This could be buggy. Fortunately we have 1.2G for now */
1258                 s5pv310_max_armclk = ARMCLOCK_1000MHZ;
1259                 printk(KERN_INFO "@@@@@ armclk set max 1000MHz @@@@@\n");
1260                 /*
1261                  *  Prepare to dvfs table to work maximum 1000MHz
1262                  *
1263                  *  Copy freq_table, volt_table, apll_pms_table, clk_div0_table,
1264                  * and clk_div1_table from lists of lookup table.
1265                 */
1266                 for (i = 1; i < CPUFREQ_LEVEL_END; i++) {
1267                         s5pv310_freq_table[i-1].index = s5pv310_lookup_freq_table[i].index - 1;
1268                         s5pv310_freq_table[i-1].frequency = s5pv310_lookup_freq_table[i].frequency;
1269                         printk(KERN_INFO "index = %d, frequency = %d\n",
1270                                 s5pv310_freq_table[i-1].index, s5pv310_freq_table[i-1].frequency);
1271                 }
1272
1273                 for (i = 1; i < CPUFREQ_LEVEL_END; i++) {
1274                         s5pv310_volt_table[i-1].index = s5pv310_lookup_volt_table[i].index - 1;
1275                         s5pv310_volt_table[i-1].arm_volt = s5pv310_lookup_volt_table[i].arm_volt;
1276                         printk(KERN_INFO "index = %d, arm_volt = %d\n",
1277                                  s5pv310_volt_table[i-1].index, s5pv310_volt_table[i-1].arm_volt);
1278                 }
1279
1280                 for (i = 1; i < CPUFREQ_LEVEL_END; i++) {
1281                         s5pv310_apll_pms_table[i-1] = s5pv310_lookup_apll_pms_table[i];
1282                         printk(KERN_INFO "apll pms_table = 0x%08x\n", s5pv310_apll_pms_table[i-1]);
1283                 }
1284
1285                 for (i = 1; i < CPUFREQ_LEVEL_END; i++) {
1286                         for (j = 0; j < 7; j++) {
1287                                 clkdiv_cpu0[i-1][j] = clkdiv_cpu0_lookup[i][j];
1288                                 printk("%d, ", clkdiv_cpu0[i-1][j]);
1289                         }
1290                         printk("\n");
1291                 }
1292
1293                 for (i = 1; i < CPUFREQ_LEVEL_END; i++) {
1294                         for (j = 0; j < 2; j++) {
1295                                 clkdiv_cpu1[i-1][j] = clkdiv_cpu1_lookup[i][j];
1296                                 printk("%d, ", clkdiv_cpu1[i-1][j]);
1297                         }
1298                         printk("\n");
1299                 }
1300                 printk(KERN_INFO "@@@@@ updated dvfs table @@@@@@\n");
1301                 break;
1302         }
1303         return ret;
1304 }
1305
1306 static int __init s5pv310_cpufreq_init(void)
1307 {
1308         int i;
1309         unsigned int tmp;
1310         unsigned long def_freq;
1311
1312         if (cpu_revision < 1) {
1313                 pr_emerg("This driver supports S5PC210 EVT1 or later only.\n");
1314                 return -EINVAL;
1315         }
1316
1317         s5pv310_set_voltage();
1318
1319         arm_clk = clk_get(NULL, "armclk");
1320         if (IS_ERR(arm_clk))
1321                 return PTR_ERR(arm_clk);
1322
1323         moutcore = clk_get(NULL, "moutcore");
1324         if (IS_ERR(moutcore))
1325                 goto err_moutcore;
1326
1327         mout_mpll = clk_get(NULL, "mout_mpll");
1328         if (IS_ERR(mout_mpll))
1329                 goto err_mout_mpll;
1330
1331         mout_apll = clk_get(NULL, "mout_apll");
1332         if (IS_ERR(mout_apll))
1333                 goto err_mout_apll;
1334
1335 #if defined(CONFIG_REGULATOR)
1336         arm_regulator = regulator_get(NULL, "vdd_arm");
1337         if (IS_ERR(arm_regulator)) {
1338                 pr_err("failed to get resource %s\n", "vdd_arm");
1339                 goto err_vdd_arm;
1340         }
1341         s5pv310_dvs_locking = 0;
1342 #endif
1343
1344         /*
1345          * By getting chip information from pkg_id & pro_id register,
1346          * adujst dvfs level, update clk divider, voltage table,
1347          * apll pms value of dvfs table.
1348         */
1349         if (s5pv310_update_dvfs_table() < 0)
1350                 printk(KERN_INFO "arm clock limited to maximum 1000MHz.\n");
1351
1352         register_pm_notifier(&s5pv310_cpufreq_notifier);
1353         register_reboot_notifier(&s5pv310_cpufreq_reboot_notifier);
1354
1355         tmp = __raw_readl(S5P_CLKDIV_CPU);
1356
1357 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
1358         for (i = L0; i <  CPUFREQ_LEVEL_END; i++) {
1359 #else
1360         for (i = L1; i <  CPUFREQ_LEVEL_END; i++) {
1361 #endif
1362                 tmp &= ~(S5P_CLKDIV_CPU0_CORE_MASK |
1363                         S5P_CLKDIV_CPU0_COREM0_MASK |
1364                         S5P_CLKDIV_CPU0_COREM1_MASK |
1365                         S5P_CLKDIV_CPU0_PERIPH_MASK |
1366                         S5P_CLKDIV_CPU0_ATB_MASK |
1367                         S5P_CLKDIV_CPU0_PCLKDBG_MASK |
1368                         S5P_CLKDIV_CPU0_APLL_MASK);
1369
1370                 tmp |= ((clkdiv_cpu0[i][0] << S5P_CLKDIV_CPU0_CORE_SHIFT) |
1371                         (clkdiv_cpu0[i][1] << S5P_CLKDIV_CPU0_COREM0_SHIFT) |
1372                         (clkdiv_cpu0[i][2] << S5P_CLKDIV_CPU0_COREM1_SHIFT) |
1373                         (clkdiv_cpu0[i][3] << S5P_CLKDIV_CPU0_PERIPH_SHIFT) |
1374                         (clkdiv_cpu0[i][4] << S5P_CLKDIV_CPU0_ATB_SHIFT) |
1375                         (clkdiv_cpu0[i][5] << S5P_CLKDIV_CPU0_PCLKDBG_SHIFT) |
1376                         (clkdiv_cpu0[i][6] << S5P_CLKDIV_CPU0_APLL_SHIFT));
1377
1378                 s5pv310_clkdiv_table[i].clkdiv = tmp;
1379         }
1380
1381         def_freq = s5pv310_getspeed(0) / 100000; /* by 100MHz */
1382 #ifdef CONFIG_S5PV310_ENABLE_1200MHZ
1383         for (i = L0; i <  CPUFREQ_LEVEL_END; i++) {
1384 #else
1385         for (i = L1; i <  CPUFREQ_LEVEL_END; i++) {
1386 #endif
1387                 cpufreq_lpj_table[i] = loops_per_jiffy *
1388                                 (s5pv310_freq_table[i].frequency /
1389                                  100000) /* by 100MHz */
1390                                 / def_freq;
1391         }
1392
1393         if (cpufreq_register_driver(&s5pv310_driver)) {
1394                 pr_err("failed to register cpufreq driver\n");
1395                 goto err_cpufreq;
1396         }
1397
1398         atomic_set(&s5pv310_cpufreq_lock_count, 0);
1399         s5pv310_cpufreq_init_done = true;
1400
1401         pm_qos_add_notifier(PM_QOS_CPU_DMA_LATENCY, &pm_qos_notifier);
1402
1403         return 0;
1404 err_cpufreq:
1405 #ifdef CONFIG_REGULATOR
1406         if (!IS_ERR(arm_regulator))
1407                 regulator_put(arm_regulator);
1408 #endif
1409 err_vdd_arm:
1410         if (!IS_ERR(mout_apll))
1411                 clk_put(mout_apll);
1412 err_mout_apll:
1413         if (!IS_ERR(mout_mpll))
1414                 clk_put(mout_mpll);
1415 err_mout_mpll:
1416         if (!IS_ERR(moutcore))
1417                 clk_put(moutcore);
1418 err_moutcore:
1419         if (!IS_ERR(arm_clk))
1420                 clk_put(arm_clk);
1421
1422         pr_debug("%s: failed initialization\n", __func__);
1423         return -EINVAL;
1424 }
1425
1426 late_initcall(s5pv310_cpufreq_init);