arch/arm/mach-rockchip/dvfs.c

   1 /* arch/arm/mach-rk30/rk30_dvfs.c
   2  *
   3  * Copyright (C) 2012 ROCKCHIP, Inc.
   4  *
   5  * This software is licensed under the terms of the GNU General Public
   6  * License version 2, as published by the Free Software Foundation, and
   7  * may be copied, distributed, and modified under those terms.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  */
  15 #include <linux/slab.h>
  16 #include <linux/clk.h>
  17 #include <linux/regulator/consumer.h>
  18 #include <linux/delay.h>
  19 #include <linux/stat.h>
  20 #include <linux/of.h>
  21 #include <linux/opp.h>
  22 #include <linux/rockchip/dvfs.h>
  23 #include <linux/rockchip/common.h>
  24
  25 extern int rockchip_tsadc_get_temp(int chn);
  26
  27 #define MHz     (1000 * 1000)
  28 static LIST_HEAD(rk_dvfs_tree);
  29 static DEFINE_MUTEX(rk_dvfs_mutex);
  30 static struct workqueue_struct *dvfs_wq;
  31 static struct dvfs_node *clk_cpu_dvfs_node;
  32 static unsigned int target_temp = 80;
  33 static int temp_limit_enable = 1;
  34
  35 static void dvfs_volt_up_delay(struct vd_node *vd, int new_volt, int old_volt)
  36 {
  37         int u_time;
  38
  39         if(new_volt <= old_volt)
  40                 return;
  41         if(vd->volt_time_flag > 0)
  42                 u_time = regulator_set_voltage_time(vd->regulator, old_volt, new_volt);
  43         else
  44                 u_time = -1;
  45         if(u_time < 0) {// regulator is not suported time,useing default time
  46                 DVFS_DBG("%s:vd %s is not suported getting delay time,so we use default\n",
  47                                 __func__, vd->name);
  48                 u_time = ((new_volt) - (old_volt)) >> 9;
  49         }
  50
  51         DVFS_DBG("%s: vd %s volt %d to %d delay %d us\n",
  52                 __func__, vd->name, old_volt, new_volt, u_time);
  53
  54         if (u_time >= 1000) {
  55                 mdelay(u_time / 1000);
  56                 udelay(u_time % 1000);
  57                 DVFS_WARNING("%s: regulator set vol delay is larger 1ms,old is %d,new is %d\n",
  58                         __func__, old_volt, new_volt);
  59         } else if (u_time) {
  60                 udelay(u_time);
  61         }
  62 }
  63
  64 static int dvfs_regulator_set_voltage_readback(struct regulator *regulator, int min_uV, int max_uV)
  65 {
  66         int ret = 0, read_back = 0;
  67
  68         ret = dvfs_regulator_set_voltage(regulator, max_uV, max_uV);
  69         if (ret < 0) {
  70                 DVFS_ERR("%s: now read back to check voltage\n", __func__);
  71
  72                 /* read back to judge if it is already effect */
  73                 mdelay(2);
  74                 read_back = dvfs_regulator_get_voltage(regulator);
  75                 if (read_back == max_uV) {
  76                         DVFS_ERR("%s: set ERROR but already effected, volt=%d\n", __func__, read_back);
  77                         ret = 0;
  78                 } else {
  79                         DVFS_ERR("%s: set ERROR AND NOT effected, volt=%d\n", __func__, read_back);
  80                 }
  81         }
  82
  83         return ret;
  84 }
  85
  86 static int dvfs_scale_volt_direct(struct vd_node *vd_clk, int volt_new)
  87 {
  88         int ret = 0;
  89
  90         DVFS_DBG("%s: volt=%d(old=%d)\n", __func__, volt_new, vd_clk->cur_volt);
  91
  92         if (IS_ERR_OR_NULL(vd_clk)) {
  93                 DVFS_ERR("%s: vd_node error\n", __func__);
  94                 return -EINVAL;
  95         }
  96
  97         if (!IS_ERR_OR_NULL(vd_clk->regulator)) {
  98                 ret = dvfs_regulator_set_voltage_readback(vd_clk->regulator, volt_new, volt_new);
  99                 dvfs_volt_up_delay(vd_clk,volt_new, vd_clk->cur_volt);
 100                 if (ret < 0) {
 101                         vd_clk->volt_set_flag = DVFS_SET_VOLT_FAILURE;
 102                         DVFS_ERR("%s: %s set voltage up err ret = %d, Vnew = %d(was %d)mV\n",
 103                                         __func__, vd_clk->name, ret, volt_new, vd_clk->cur_volt);
 104                         return -EAGAIN;
 105                 }
 106
 107         } else {
 108                 DVFS_ERR("%s: invalid regulator\n", __func__);
 109                 return -EINVAL;
 110         }
 111
 112         vd_clk->volt_set_flag = DVFS_SET_VOLT_SUCCESS;
 113         vd_clk->cur_volt = volt_new;
 114
 115         return 0;
 116
 117 }
 118
 119 static int dvfs_reset_volt(struct vd_node *dvfs_vd)
 120 {
 121         int flag_set_volt_correct = 0;
 122         if (!IS_ERR_OR_NULL(dvfs_vd->regulator))
 123                 flag_set_volt_correct = dvfs_regulator_get_voltage(dvfs_vd->regulator);
 124         else {
 125                 DVFS_ERR("%s: invalid regulator\n", __func__);
 126                 return -EINVAL;
 127         }
 128         if (flag_set_volt_correct <= 0) {
 129                 DVFS_ERR("%s (vd:%s), try to reload volt ,by it is error again(%d)!!! stop scaling\n",
 130                                 __func__, dvfs_vd->name, flag_set_volt_correct);
 131                 return -EAGAIN;
 132         }
 133         dvfs_vd->volt_set_flag = DVFS_SET_VOLT_SUCCESS;
 134         DVFS_WARNING("%s:vd(%s) try to reload volt = %d\n",
 135                         __func__, dvfs_vd->name, flag_set_volt_correct);
 136
 137         /* Reset vd's voltage */
 138         dvfs_vd->cur_volt = flag_set_volt_correct;
 139
 140         return dvfs_vd->cur_volt;
 141 }
 142
 143
 144 // for clk enable case to get vd regulator info
 145 static void clk_enable_dvfs_regulator_check(struct vd_node *vd)
 146 {
 147         vd->cur_volt = dvfs_regulator_get_voltage(vd->regulator);
 148         if(vd->cur_volt <= 0){
 149                 vd->volt_set_flag = DVFS_SET_VOLT_FAILURE;
 150         }
 151         vd->volt_set_flag = DVFS_SET_VOLT_SUCCESS;
 152 }
 153
 154 static void dvfs_get_vd_regulator_volt_list(struct vd_node *vd)
 155 {
 156         unsigned int i, selector = dvfs_regulator_count_voltages(vd->regulator);
 157         int n = 0, sel_volt = 0;
 158
 159         if(selector > VD_VOL_LIST_CNT)
 160                 selector = VD_VOL_LIST_CNT;
 161
 162         for (i = 0; i < selector; i++) {
 163                 sel_volt = dvfs_regulator_list_voltage(vd->regulator, i);
 164                 if(sel_volt <= 0){
 165                         //DVFS_WARNING("%s: vd(%s) list volt selector=%u, but volt(%d) <=0\n",
 166                         //      __func__, vd->name, i, sel_volt);
 167                         continue;
 168                 }
 169                 vd->volt_list[n++] = sel_volt;
 170                 DVFS_DBG("%s: vd(%s) list volt selector=%u, n=%d, volt=%d\n",
 171                         __func__, vd->name, i, n, sel_volt);
 172         }
 173
 174         vd->n_voltages = n;
 175 }
 176
 177 // >= volt
 178 static int vd_regulator_round_volt_max(struct vd_node *vd, int volt)
 179 {
 180         int sel_volt;
 181         int i;
 182
 183         for (i = 0; i < vd->n_voltages; i++) {
 184                 sel_volt = vd->volt_list[i];
 185                 if(sel_volt <= 0){
 186                         DVFS_WARNING("%s: selector=%u, but volt <=0\n",
 187                                 __func__, i);
 188                         continue;
 189                 }
 190                 if(sel_volt >= volt)
 191                         return sel_volt;
 192         }
 193         return -EINVAL;
 194 }
 195
 196 // >=volt
 197 static int vd_regulator_round_volt_min(struct vd_node *vd, int volt)
 198 {
 199         int sel_volt;
 200         int i;
 201
 202         for (i = 0; i < vd->n_voltages; i++) {
 203                 sel_volt = vd->volt_list[i];
 204                 if(sel_volt <= 0){
 205                         DVFS_WARNING("%s: selector=%u, but volt <=0\n",
 206                                 __func__, i);
 207                         continue;
 208                 }
 209                 if(sel_volt > volt){
 210                         if(i > 0)
 211                                 return vd->volt_list[i-1];
 212                         else
 213                                 return -EINVAL;
 214                 }
 215         }
 216
 217         return -EINVAL;
 218 }
 219
 220 // >=volt
 221 static int vd_regulator_round_volt(struct vd_node *vd, int volt, int flags)
 222 {
 223         if(!vd->n_voltages)
 224                 return -EINVAL;
 225         if(flags == VD_LIST_RELATION_L)
 226                 return vd_regulator_round_volt_min(vd, volt);
 227         else
 228                 return vd_regulator_round_volt_max(vd, volt);
 229 }
 230
 231 static void dvfs_table_round_volt(struct dvfs_node *clk_dvfs_node)
 232 {
 233         int i, test_volt;
 234
 235         if(!clk_dvfs_node->dvfs_table || !clk_dvfs_node->vd ||
 236                 IS_ERR_OR_NULL(clk_dvfs_node->vd->regulator))
 237                 return;
 238
 239         for (i = 0; (clk_dvfs_node->dvfs_table[i].frequency != CPUFREQ_TABLE_END); i++) {
 240
 241                 test_volt = vd_regulator_round_volt(clk_dvfs_node->vd, clk_dvfs_node->dvfs_table[i].index, VD_LIST_RELATION_H);
 242                 if(test_volt <= 0)
 243                 {
 244                         DVFS_WARNING("%s: clk(%s) round volt(%d) but list <=0\n",
 245                                 __func__, clk_dvfs_node->name, clk_dvfs_node->dvfs_table[i].index);
 246                         break;
 247                 }
 248                 DVFS_DBG("clk %s:round_volt %d to %d\n",
 249                         clk_dvfs_node->name, clk_dvfs_node->dvfs_table[i].index, test_volt);
 250
 251                 clk_dvfs_node->dvfs_table[i].index=test_volt;
 252         }
 253 }
 254
 255 static void dvfs_vd_get_regulator_volt_time_info(struct vd_node *vd)
 256 {
 257         if(vd->volt_time_flag <= 0){// check regulator support get uping vol timer
 258                 vd->volt_time_flag = dvfs_regulator_set_voltage_time(vd->regulator, vd->cur_volt, vd->cur_volt+200*1000);
 259                 if(vd->volt_time_flag < 0){
 260                         DVFS_DBG("%s,vd %s volt_time is no support\n",
 261                                 __func__, vd->name);
 262                 }
 263                 else{
 264                         DVFS_DBG("%s,vd %s volt_time is support,up 200mv need delay %d us\n",
 265                                 __func__, vd->name, vd->volt_time_flag);
 266                 }
 267         }
 268 }
 269 #if 0
 270 static void dvfs_vd_get_regulator_mode_info(struct vd_node *vd)
 271 {
 272         //REGULATOR_MODE_FAST
 273         if(vd->mode_flag <= 0){// check regulator support get uping vol timer{
 274                 vd->mode_flag = dvfs_regulator_get_mode(vd->regulator);
 275                 if(vd->mode_flag==REGULATOR_MODE_FAST || vd->mode_flag==REGULATOR_MODE_NORMAL
 276                         || vd->mode_flag == REGULATOR_MODE_IDLE || vd->mode_flag==REGULATOR_MODE_STANDBY){
 277
 278                         if(dvfs_regulator_set_mode(vd->regulator, vd->mode_flag) < 0){
 279                                 vd->mode_flag = 0;// check again
 280                         }
 281                 }
 282                 if(vd->mode_flag > 0){
 283                         DVFS_DBG("%s,vd %s mode(now is %d) support\n",
 284                                 __func__, vd->name, vd->mode_flag);
 285                 }
 286                 else{
 287                         DVFS_DBG("%s,vd %s mode is not support now check\n",
 288                                 __func__, vd->name);
 289                 }
 290         }
 291 }
 292 #endif
 293
 294 struct regulator *dvfs_get_regulator(char *regulator_name)
 295 {
 296         struct vd_node *vd;
 297
 298         mutex_lock(&rk_dvfs_mutex);
 299         list_for_each_entry(vd, &rk_dvfs_tree, node) {
 300                 if (strcmp(regulator_name, vd->regulator_name) == 0) {
 301                         mutex_unlock(&rk_dvfs_mutex);
 302                         return vd->regulator;
 303                 }
 304         }
 305         mutex_unlock(&rk_dvfs_mutex);
 306         return NULL;
 307 }
 308
 309 static int dvfs_get_rate_range(struct dvfs_node *clk_dvfs_node)
 310 {
 311         struct cpufreq_frequency_table *table;
 312         int i = 0;
 313
 314         if (!clk_dvfs_node)
 315                 return -EINVAL;
 316
 317         clk_dvfs_node->min_rate = 0;
 318         clk_dvfs_node->max_rate = 0;
 319
 320         table = clk_dvfs_node->dvfs_table;
 321         for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
 322                 clk_dvfs_node->max_rate = table[i].frequency / 1000 * 1000 * 1000;
 323                 if (i == 0)
 324                         clk_dvfs_node->min_rate = table[i].frequency / 1000 * 1000 * 1000;
 325         }
 326
 327         DVFS_DBG("%s: clk %s, limit rate [min, max] = [%u, %u]\n",
 328                         __func__, clk_dvfs_node->name, clk_dvfs_node->min_rate, clk_dvfs_node->max_rate);
 329
 330         return 0;
 331 }
 332
 333 static void dvfs_table_round_clk_rate(struct dvfs_node  *clk_dvfs_node)
 334 {
 335         int i, rate, temp_rate, flags;
 336
 337         if(!clk_dvfs_node || !clk_dvfs_node->dvfs_table || !clk_dvfs_node->clk)
 338                 return;
 339
 340         for (i = 0; (clk_dvfs_node->dvfs_table[i].frequency != CPUFREQ_TABLE_END); i++) {
 341                 //ddr rate = real rate+flags
 342                 flags = clk_dvfs_node->dvfs_table[i].frequency%1000;
 343                 rate = (clk_dvfs_node->dvfs_table[i].frequency/1000)*1000;
 344                 temp_rate = __clk_round_rate(clk_dvfs_node->clk, rate*1000);
 345                 if(temp_rate <= 0){
 346                         DVFS_WARNING("%s: clk(%s) rate %d round return %d\n",
 347                                 __func__, clk_dvfs_node->name, clk_dvfs_node->dvfs_table[i].frequency, temp_rate);
 348                         continue;
 349                 }
 350
 351                 /* Set rate unit as MHZ */
 352                 if (temp_rate % MHz != 0)
 353                         temp_rate = (temp_rate / MHz + 1) * MHz;
 354
 355                 temp_rate = (temp_rate / 1000) + flags;
 356
 357                 DVFS_DBG("clk %s round_clk_rate %d to %d\n",
 358                         clk_dvfs_node->name,clk_dvfs_node->dvfs_table[i].frequency, temp_rate);
 359
 360                 clk_dvfs_node->dvfs_table[i].frequency = temp_rate;
 361         }
 362 }
 363
 364 static int clk_dvfs_node_get_ref_volt(struct dvfs_node *clk_dvfs_node, int rate_khz,
 365                 struct cpufreq_frequency_table *clk_fv)
 366 {
 367         int i = 0;
 368
 369         if (rate_khz == 0 || !clk_dvfs_node || !clk_dvfs_node->dvfs_table) {
 370                 /* since no need */
 371                 return -EINVAL;
 372         }
 373         clk_fv->frequency = rate_khz;
 374         clk_fv->index = 0;
 375
 376         for (i = 0; (clk_dvfs_node->dvfs_table[i].frequency != CPUFREQ_TABLE_END); i++) {
 377                 if (clk_dvfs_node->dvfs_table[i].frequency >= rate_khz) {
 378                         clk_fv->frequency = clk_dvfs_node->dvfs_table[i].frequency;
 379                         clk_fv->index = clk_dvfs_node->dvfs_table[i].index;
 380                          //printk("%s,%s rate=%ukhz(vol=%d)\n",__func__,clk_dvfs_node->name,
 381                          //clk_fv->frequency, clk_fv->index);
 382                         return 0;
 383                 }
 384         }
 385         clk_fv->frequency = 0;
 386         clk_fv->index = 0;
 387         //DVFS_DBG("%s get corresponding voltage error! out of bound\n", clk_dvfs_node->name);
 388         return -EINVAL;
 389 }
 390
 391 static int dvfs_pd_get_newvolt_byclk(struct pd_node *pd, struct dvfs_node *clk_dvfs_node)
 392 {
 393         int volt_max = 0;
 394
 395         if (clk_dvfs_node->enable_count && (clk_dvfs_node->set_volt >= pd->cur_volt)) {
 396                 return clk_dvfs_node->set_volt;
 397         }
 398
 399         list_for_each_entry(clk_dvfs_node, &pd->clk_list, node) {
 400                 if (clk_dvfs_node->enable_count)
 401                         volt_max = max(volt_max, clk_dvfs_node->set_volt);
 402         }
 403         return volt_max;
 404 }
 405
 406 static void dvfs_update_clk_pds_volt(struct dvfs_node *clk_dvfs_node)
 407 {
 408         struct pd_node *pd;
 409
 410         if (!clk_dvfs_node)
 411                 return;
 412
 413         pd = clk_dvfs_node->pd;
 414         if (!pd)
 415                 return;
 416
 417         pd->cur_volt = dvfs_pd_get_newvolt_byclk(pd, clk_dvfs_node);
 418 }
 419
 420 static int dvfs_vd_get_newvolt_bypd(struct vd_node *vd)
 421 {
 422         int volt_max_vd = 0;
 423         struct pd_node *pd;
 424
 425         if (!vd)
 426                 return -EINVAL;
 427
 428         list_for_each_entry(pd, &vd->pd_list, node) {
 429                 volt_max_vd = max(volt_max_vd, pd->cur_volt);
 430         }
 431
 432         return volt_max_vd;
 433 }
 434
 435 static int dvfs_vd_get_newvolt_byclk(struct dvfs_node *clk_dvfs_node)
 436 {
 437         if (!clk_dvfs_node)
 438                 return -EINVAL;
 439
 440         dvfs_update_clk_pds_volt(clk_dvfs_node);
 441         return  dvfs_vd_get_newvolt_bypd(clk_dvfs_node->vd);
 442 }
 443 #if 0
 444 static void dvfs_temp_limit_work_func(struct work_struct *work)
 445 {
 446         unsigned long delay = HZ / 10; // 100ms
 447         struct vd_node *vd;
 448         struct pd_node *pd;
 449         struct dvfs_node *clk_dvfs_node;
 450
 451         queue_delayed_work_on(0, dvfs_wq, to_delayed_work(work), delay);
 452
 453         mutex_lock(&rk_dvfs_mutex);
 454         list_for_each_entry(vd, &rk_dvfs_tree, node) {
 455                 mutex_lock(&vd->mutex);
 456                 list_for_each_entry(pd, &vd->pd_list, node) {
 457                         list_for_each_entry(clk_dvfs_node, &pd->clk_list, node) {
 458                                 if (clk_dvfs_node->temp_limit_table) {
 459                                         clk_dvfs_node->temp = rockchip_tsadc_get_temp(clk_dvfs_node->temp_channel);
 460                                         clk_dvfs_node->vd->vd_dvfs_target(clk_dvfs_node, clk_dvfs_node->last_set_rate);
 461                                 }
 462                         }
 463                 }
 464                 mutex_unlock(&vd->mutex);
 465         }
 466         mutex_unlock(&rk_dvfs_mutex);
 467 }
 468 #endif
 469
 470 static void dvfs_temp_limit_work_func(struct work_struct *work)
 471 {
 472         int temp=0, delta_temp=0;
 473         unsigned long delay = HZ/10;
 474         unsigned long arm_rate_step=0;
 475         static int old_temp=0;
 476         int i;
 477
 478         queue_delayed_work_on(0, dvfs_wq, to_delayed_work(work), delay);
 479
 480         temp = rockchip_tsadc_get_temp(1);
 481
 482         //debounce
 483         delta_temp = (old_temp>temp) ? (old_temp-temp) : (temp-old_temp);
 484         if (delta_temp <= 1)
 485                 return;
 486
 487         if (ROCKCHIP_PM_POLICY_PERFORMANCE == rockchip_pm_get_policy()) {
 488                 if (!clk_cpu_dvfs_node->per_temp_limit_table) {
 489                         return;
 490                 }
 491
 492                 clk_cpu_dvfs_node->temp_limit_rate = clk_cpu_dvfs_node->max_rate;
 493                 for (i=0; clk_cpu_dvfs_node->per_temp_limit_table[i].frequency != CPUFREQ_TABLE_END; i++) {
 494                         if (temp > clk_cpu_dvfs_node->per_temp_limit_table[i].index) {
 495                                 clk_cpu_dvfs_node->temp_limit_rate = clk_cpu_dvfs_node->per_temp_limit_table[i].frequency;
 496                         }
 497                 }
 498                 dvfs_clk_set_rate(clk_cpu_dvfs_node, clk_cpu_dvfs_node->last_set_rate);
 499         } else if (ROCKCHIP_PM_POLICY_NORMAL == rockchip_pm_get_policy()){
 500                 if (!clk_cpu_dvfs_node->nor_temp_limit_table) {
 501                         return;
 502                 }
 503
 504                 if (temp > target_temp) {
 505                         if (temp > old_temp) {
 506                                 delta_temp = temp - target_temp;
 507                                 for (i=0; clk_cpu_dvfs_node->nor_temp_limit_table[i].frequency != CPUFREQ_TABLE_END; i++) {
 508                                         if (delta_temp > clk_cpu_dvfs_node->nor_temp_limit_table[i].index) {
 509                                                 arm_rate_step = clk_cpu_dvfs_node->nor_temp_limit_table[i].frequency;
 510                                         }
 511                                 }
 512                                 if (arm_rate_step && (clk_cpu_dvfs_node->temp_limit_rate > arm_rate_step)) {
 513                                         clk_cpu_dvfs_node->temp_limit_rate -= arm_rate_step;
 514                                         dvfs_clk_set_rate(clk_cpu_dvfs_node, clk_cpu_dvfs_node->last_set_rate);
 515                                 }
 516                         }
 517                 } else {
 518                         if (clk_cpu_dvfs_node->temp_limit_rate < clk_cpu_dvfs_node->max_rate) {
 519                                 delta_temp = target_temp - temp;
 520                                 for (i=0; clk_cpu_dvfs_node->nor_temp_limit_table[i].frequency != CPUFREQ_TABLE_END; i++) {
 521                                         if (delta_temp > clk_cpu_dvfs_node->nor_temp_limit_table[i].index) {
 522                                                 arm_rate_step = clk_cpu_dvfs_node->nor_temp_limit_table[i].frequency;
 523                                         }
 524                                 }
 525
 526                                 if (arm_rate_step) {
 527                                         clk_cpu_dvfs_node->temp_limit_rate += arm_rate_step;
 528                                         if (clk_cpu_dvfs_node->temp_limit_rate > clk_cpu_dvfs_node->max_rate) {
 529                                                 clk_cpu_dvfs_node->temp_limit_rate = clk_cpu_dvfs_node->max_rate;
 530                                         }
 531                                         dvfs_clk_set_rate(clk_cpu_dvfs_node, clk_cpu_dvfs_node->last_set_rate);
 532                                 }
 533                         }
 534                 }
 535         }
 536
 537         DVFS_DBG("cur temp: %d, temp_limit_core_rate: %lu\n", temp, clk_cpu_dvfs_node->temp_limit_rate);
 538
 539         old_temp = temp;
 540 }
 541 static DECLARE_DELAYED_WORK(dvfs_temp_limit_work, dvfs_temp_limit_work_func);
 542
 543
 544 int dvfs_clk_enable_limit(struct dvfs_node *clk_dvfs_node, unsigned int min_rate, unsigned int max_rate)
 545 {
 546         u32 rate = 0, ret = 0;
 547
 548         if (!clk_dvfs_node || (min_rate > max_rate))
 549                 return -EINVAL;
 550
 551         if (clk_dvfs_node->vd && clk_dvfs_node->vd->vd_dvfs_target){
 552                 mutex_lock(&clk_dvfs_node->vd->mutex);
 553
 554                 /* To reset clk_dvfs_node->min_rate/max_rate */
 555                 dvfs_get_rate_range(clk_dvfs_node);
 556                 clk_dvfs_node->freq_limit_en = 1;
 557
 558                 if ((min_rate >= clk_dvfs_node->min_rate) && (min_rate <= clk_dvfs_node->max_rate)) {
 559                         clk_dvfs_node->min_rate = min_rate;
 560                 }
 561
 562                 if ((max_rate >= clk_dvfs_node->min_rate) && (max_rate <= clk_dvfs_node->max_rate)) {
 563                         clk_dvfs_node->max_rate = max_rate;
 564                 }
 565
 566                 if (clk_dvfs_node->last_set_rate == 0)
 567                         rate = __clk_get_rate(clk_dvfs_node->clk);
 568                 else
 569                         rate = clk_dvfs_node->last_set_rate;
 570                 ret = clk_dvfs_node->vd->vd_dvfs_target(clk_dvfs_node, rate);
 571
 572                 mutex_unlock(&clk_dvfs_node->vd->mutex);
 573
 574         }
 575
 576         DVFS_DBG("%s:clk(%s) last_set_rate=%u; [min_rate, max_rate]=[%u, %u]\n",
 577                         __func__, __clk_get_name(clk_dvfs_node->clk), clk_dvfs_node->last_set_rate,
 578                         clk_dvfs_node->min_rate, clk_dvfs_node->max_rate);
 579
 580         return 0;
 581 }
 582 EXPORT_SYMBOL(dvfs_clk_enable_limit);
 583
 584 int dvfs_clk_disable_limit(struct dvfs_node *clk_dvfs_node)
 585 {
 586         u32 ret = 0;
 587
 588         if (!clk_dvfs_node)
 589                 return -EINVAL;
 590
 591         if (clk_dvfs_node->vd && clk_dvfs_node->vd->vd_dvfs_target){
 592                 mutex_lock(&clk_dvfs_node->vd->mutex);
 593
 594                 /* To reset clk_dvfs_node->min_rate/max_rate */
 595                 dvfs_get_rate_range(clk_dvfs_node);
 596                 clk_dvfs_node->freq_limit_en = 0;
 597                 ret = clk_dvfs_node->vd->vd_dvfs_target(clk_dvfs_node, clk_dvfs_node->last_set_rate);
 598
 599                 mutex_unlock(&clk_dvfs_node->vd->mutex);
 600         }
 601
 602         DVFS_DBG("%s: clk(%s) last_set_rate=%u; [min_rate, max_rate]=[%u, %u]\n",
 603                         __func__, __clk_get_name(clk_dvfs_node->clk), clk_dvfs_node->last_set_rate, clk_dvfs_node->min_rate, clk_dvfs_node->max_rate);
 604         return 0;
 605 }
 606 EXPORT_SYMBOL(dvfs_clk_disable_limit);
 607
 608 void dvfs_disable_temp_limit(void) {
 609         temp_limit_enable = 0;
 610         cancel_delayed_work_sync(&dvfs_temp_limit_work);
 611 }
 612
 613 int dvfs_clk_get_limit(struct dvfs_node *clk_dvfs_node, unsigned int *min_rate, unsigned int *max_rate)
 614 {
 615         int freq_limit_en;
 616
 617         if (!clk_dvfs_node)
 618                 return -EINVAL;
 619
 620         mutex_lock(&clk_dvfs_node->vd->mutex);
 621
 622         *min_rate = clk_dvfs_node->min_rate;
 623         *max_rate = clk_dvfs_node->max_rate;
 624         freq_limit_en = clk_dvfs_node->freq_limit_en;
 625
 626         mutex_unlock(&clk_dvfs_node->vd->mutex);
 627
 628         return freq_limit_en;
 629 }
 630 EXPORT_SYMBOL(dvfs_clk_get_limit);
 631
 632 int dvfs_clk_register_set_rate_callback(struct dvfs_node *clk_dvfs_node, clk_set_rate_callback clk_dvfs_target)
 633 {
 634         if (!clk_dvfs_node)
 635                 return -EINVAL;
 636
 637         mutex_lock(&clk_dvfs_node->vd->mutex);
 638         clk_dvfs_node->clk_dvfs_target = clk_dvfs_target;
 639         mutex_unlock(&clk_dvfs_node->vd->mutex);
 640
 641         return 0;
 642 }
 643 EXPORT_SYMBOL(dvfs_clk_register_set_rate_callback);
 644
 645 struct cpufreq_frequency_table *dvfs_get_freq_volt_table(struct dvfs_node *clk_dvfs_node)
 646 {
 647         struct cpufreq_frequency_table *table;
 648
 649         if (!clk_dvfs_node)
 650                 return NULL;
 651
 652         mutex_lock(&clk_dvfs_node->vd->mutex);
 653         table = clk_dvfs_node->dvfs_table;
 654         mutex_unlock(&clk_dvfs_node->vd->mutex);
 655
 656         return table;
 657 }
 658 EXPORT_SYMBOL(dvfs_get_freq_volt_table);
 659
 660 int dvfs_set_freq_volt_table(struct dvfs_node *clk_dvfs_node, struct cpufreq_frequency_table *table)
 661 {
 662         if (!clk_dvfs_node)
 663                 return -EINVAL;
 664
 665         if (IS_ERR_OR_NULL(table)){
 666                 DVFS_ERR("%s:invalid table!\n", __func__);
 667                 return -EINVAL;
 668         }
 669
 670         mutex_lock(&clk_dvfs_node->vd->mutex);
 671         clk_dvfs_node->dvfs_table = table;
 672         dvfs_get_rate_range(clk_dvfs_node);
 673         dvfs_table_round_clk_rate(clk_dvfs_node);
 674         dvfs_table_round_volt(clk_dvfs_node);
 675         mutex_unlock(&clk_dvfs_node->vd->mutex);
 676
 677         return 0;
 678 }
 679 EXPORT_SYMBOL(dvfs_set_freq_volt_table);
 680
 681 int clk_enable_dvfs(struct dvfs_node *clk_dvfs_node)
 682 {
 683         struct cpufreq_frequency_table clk_fv;
 684         int volt_new;
 685
 686         if (!clk_dvfs_node)
 687                 return -EINVAL;
 688
 689         DVFS_DBG("%s: dvfs clk(%s) enable dvfs!\n",
 690                 __func__, __clk_get_name(clk_dvfs_node->clk));
 691
 692         if (!clk_dvfs_node->vd) {
 693                 DVFS_ERR("%s: dvfs node(%s) has no vd node!\n",
 694                         __func__, clk_dvfs_node->name);
 695                 return -EINVAL;
 696         }
 697         mutex_lock(&clk_dvfs_node->vd->mutex);
 698         if (clk_dvfs_node->enable_count == 0) {
 699                 if (IS_ERR_OR_NULL(clk_dvfs_node->vd->regulator)) {
 700                         if (clk_dvfs_node->vd->regulator_name)
 701                                 clk_dvfs_node->vd->regulator = dvfs_regulator_get(NULL, clk_dvfs_node->vd->regulator_name);
 702                         if (!IS_ERR_OR_NULL(clk_dvfs_node->vd->regulator)) {
 703                                 DVFS_DBG("%s: vd(%s) get regulator(%s) ok\n",
 704                                         __func__, clk_dvfs_node->vd->name, clk_dvfs_node->vd->regulator_name);
 705                                 clk_enable_dvfs_regulator_check(clk_dvfs_node->vd);
 706                                 dvfs_get_vd_regulator_volt_list(clk_dvfs_node->vd);
 707                                 dvfs_vd_get_regulator_volt_time_info(clk_dvfs_node->vd);
 708                         } else {
 709                                 clk_dvfs_node->enable_count = 0;
 710                                 DVFS_ERR("%s: vd(%s) can't get regulator(%s)!\n",
 711                                         __func__, clk_dvfs_node->vd->name, clk_dvfs_node->vd->regulator_name);
 712                                 mutex_unlock(&clk_dvfs_node->vd->mutex);
 713                                 return -ENXIO;
 714                         }
 715                 } else {
 716                         clk_enable_dvfs_regulator_check(clk_dvfs_node->vd);
 717                 }
 718
 719                 DVFS_DBG("%s: vd(%s) cur volt=%d\n",
 720                         __func__, clk_dvfs_node->name, clk_dvfs_node->vd->cur_volt);
 721
 722                 dvfs_table_round_clk_rate(clk_dvfs_node);
 723                 dvfs_get_rate_range(clk_dvfs_node);
 724                 clk_dvfs_node->freq_limit_en = 1;
 725                 dvfs_table_round_volt(clk_dvfs_node);
 726                 clk_dvfs_node->set_freq = clk_dvfs_node_get_rate_kz(clk_dvfs_node->clk);
 727                 clk_dvfs_node->last_set_rate = clk_dvfs_node->set_freq*1000;
 728
 729                 DVFS_DBG("%s: %s get freq %u!\n",
 730                         __func__, clk_dvfs_node->name, clk_dvfs_node->set_freq);
 731
 732                 if (clk_dvfs_node_get_ref_volt(clk_dvfs_node, clk_dvfs_node->set_freq, &clk_fv)) {
 733                         if (clk_dvfs_node->dvfs_table[0].frequency == CPUFREQ_TABLE_END) {
 734                                 DVFS_ERR("%s: table empty\n", __func__);
 735                                 clk_dvfs_node->enable_count = 0;
 736                                 mutex_unlock(&clk_dvfs_node->vd->mutex);
 737                                 return -EINVAL;
 738                         } else {
 739                                 DVFS_WARNING("%s: clk(%s) freq table all value are smaller than default(%d), use default, just enable dvfs\n",
 740                                         __func__, clk_dvfs_node->name, clk_dvfs_node->set_freq);
 741                                 clk_dvfs_node->enable_count++;
 742                                 mutex_unlock(&clk_dvfs_node->vd->mutex);
 743                                 return 0;
 744                         }
 745                 }
 746                 clk_dvfs_node->enable_count++;
 747                 clk_dvfs_node->set_volt = clk_fv.index;
 748                 volt_new = dvfs_vd_get_newvolt_byclk(clk_dvfs_node);
 749                 DVFS_DBG("%s: %s, freq %u(ref vol %u)\n",
 750                         __func__, clk_dvfs_node->name, clk_dvfs_node->set_freq, clk_dvfs_node->set_volt);
 751 #if 0
 752                 if (clk_dvfs_node->dvfs_nb) {
 753                         // must unregister when clk disable
 754                         clk_notifier_register(clk, clk_dvfs_node->dvfs_nb);
 755                 }
 756 #endif
 757                 if(clk_dvfs_node->vd->cur_volt != volt_new) {
 758                         int ret;
 759                         ret = dvfs_regulator_set_voltage_readback(clk_dvfs_node->vd->regulator, volt_new, volt_new);
 760                         dvfs_volt_up_delay(clk_dvfs_node->vd,volt_new, clk_dvfs_node->vd->cur_volt);
 761                         if (ret < 0) {
 762                                 clk_dvfs_node->vd->volt_set_flag = DVFS_SET_VOLT_FAILURE;
 763                                 clk_dvfs_node->enable_count = 0;
 764                                 DVFS_ERR("dvfs enable clk %s,set volt error \n", clk_dvfs_node->name);
 765                                 mutex_unlock(&clk_dvfs_node->vd->mutex);
 766                                 return -EAGAIN;
 767                         }
 768                         clk_dvfs_node->vd->cur_volt = volt_new;
 769                         clk_dvfs_node->vd->volt_set_flag = DVFS_SET_VOLT_SUCCESS;
 770                 }
 771
 772         } else {
 773                 DVFS_DBG("%s: dvfs already enable clk enable = %d!\n",
 774                         __func__, clk_dvfs_node->enable_count);
 775                 clk_dvfs_node->enable_count++;
 776         }
 777
 778         mutex_unlock(&clk_dvfs_node->vd->mutex);
 779
 780         return 0;
 781 }
 782 EXPORT_SYMBOL(clk_enable_dvfs);
 783
 784 int clk_disable_dvfs(struct dvfs_node *clk_dvfs_node)
 785 {
 786         int volt_new;
 787
 788         if (!clk_dvfs_node)
 789                 return -EINVAL;
 790
 791         DVFS_DBG("%s:dvfs clk(%s) disable dvfs!\n",
 792                 __func__, __clk_get_name(clk_dvfs_node->clk));
 793
 794         mutex_lock(&clk_dvfs_node->vd->mutex);
 795         if (!clk_dvfs_node->enable_count) {
 796                 DVFS_WARNING("%s:clk(%s) is already closed!\n",
 797                         __func__, __clk_get_name(clk_dvfs_node->clk));
 798                 mutex_unlock(&clk_dvfs_node->vd->mutex);
 799                 return 0;
 800         } else {
 801                 clk_dvfs_node->enable_count--;
 802                 if (0 == clk_dvfs_node->enable_count) {
 803                         DVFS_DBG("%s:dvfs clk(%s) disable dvfs ok!\n",
 804                                 __func__, __clk_get_name(clk_dvfs_node->clk));
 805                         volt_new = dvfs_vd_get_newvolt_byclk(clk_dvfs_node);
 806                         dvfs_scale_volt_direct(clk_dvfs_node->vd, volt_new);
 807
 808 #if 0
 809                         clk_notifier_unregister(clk, clk_dvfs_node->dvfs_nb);
 810                         DVFS_DBG("clk unregister nb!\n");
 811 #endif
 812                 }
 813         }
 814         mutex_unlock(&clk_dvfs_node->vd->mutex);
 815         return 0;
 816 }
 817 EXPORT_SYMBOL(clk_disable_dvfs);
 818
 819 static unsigned long dvfs_get_limit_rate(struct dvfs_node *clk_dvfs_node, unsigned long rate)
 820 {
 821         unsigned long limit_rate;
 822
 823         limit_rate = rate;
 824         if (clk_dvfs_node->freq_limit_en) {
 825                 //dvfs table limit
 826                 if (rate < clk_dvfs_node->min_rate) {
 827                         limit_rate = clk_dvfs_node->min_rate;
 828                 } else if (rate > clk_dvfs_node->max_rate) {
 829                         limit_rate = clk_dvfs_node->max_rate;
 830                 }
 831                 if (temp_limit_enable) {
 832                         if (limit_rate > clk_dvfs_node->temp_limit_rate) {
 833                                 limit_rate = clk_dvfs_node->temp_limit_rate;
 834                         }
 835                 }
 836         }
 837
 838         DVFS_DBG("%s: rate:%ld, limit_rate:%ld,\n", __func__, rate, limit_rate);
 839
 840         return limit_rate;
 841 }
 842
 843 static int dvfs_target(struct dvfs_node *clk_dvfs_node, unsigned long rate)
 844 {
 845         struct cpufreq_frequency_table clk_fv;
 846         unsigned long old_rate = 0, new_rate = 0, volt_new = 0, clk_volt_store = 0;
 847         struct clk *clk = clk_dvfs_node->clk;
 848         int ret;
 849
 850         if (!clk)
 851                 return -EINVAL;
 852
 853         if (!clk_dvfs_node->enable_count){
 854                 DVFS_WARNING("%s:dvfs(%s) is disable\n",
 855                         __func__, clk_dvfs_node->name);
 856                 return 0;
 857         }
 858
 859         if (clk_dvfs_node->vd->volt_set_flag == DVFS_SET_VOLT_FAILURE) {
 860                 /* It means the last time set voltage error */
 861                 ret = dvfs_reset_volt(clk_dvfs_node->vd);
 862                 if (ret < 0) {
 863                         return -EAGAIN;
 864                 }
 865         }
 866
 867         rate = dvfs_get_limit_rate(clk_dvfs_node, rate);
 868         new_rate = __clk_round_rate(clk, rate);
 869         old_rate = __clk_get_rate(clk);
 870         if (new_rate == old_rate)
 871                 return 0;
 872
 873         DVFS_DBG("enter %s: clk(%s) new_rate = %lu Hz, old_rate =  %lu Hz\n",
 874                 __func__, clk_dvfs_node->name, rate, old_rate);
 875
 876         /* find the clk corresponding voltage */
 877         ret = clk_dvfs_node_get_ref_volt(clk_dvfs_node, new_rate / 1000, &clk_fv);
 878         if (ret) {
 879                 DVFS_ERR("%s:dvfs clk(%s) rate %luhz is not support\n",
 880                         __func__, clk_dvfs_node->name, new_rate);
 881                 return ret;
 882         }
 883         clk_volt_store = clk_dvfs_node->set_volt;
 884         clk_dvfs_node->set_volt = clk_fv.index;
 885         volt_new = dvfs_vd_get_newvolt_byclk(clk_dvfs_node);
 886         DVFS_DBG("%s:%s new rate=%lu(was=%lu),new volt=%lu,(was=%d)\n",
 887                 __func__, clk_dvfs_node->name, new_rate, old_rate, volt_new,clk_dvfs_node->vd->cur_volt);
 888
 889         /* if up the rate */
 890         if (new_rate > old_rate) {
 891                 ret = dvfs_scale_volt_direct(clk_dvfs_node->vd, volt_new);
 892                 if (ret)
 893                         goto fail_roll_back;
 894         }
 895
 896         /* scale rate */
 897         if (clk_dvfs_node->clk_dvfs_target) {
 898                 ret = clk_dvfs_node->clk_dvfs_target(clk, rate);
 899         } else {
 900                 ret = clk_set_rate(clk, rate);
 901         }
 902
 903         if (ret) {
 904                 DVFS_ERR("%s:clk(%s) set rate err\n",
 905                         __func__, __clk_get_name(clk));
 906                 goto fail_roll_back;
 907         }
 908         clk_dvfs_node->set_freq = new_rate / 1000;
 909
 910         DVFS_DBG("%s:dvfs clk(%s) set rate %lu ok\n",
 911                 __func__, clk_dvfs_node->name, __clk_get_rate(clk));
 912
 913         /* if down the rate */
 914         if (new_rate < old_rate) {
 915                 ret = dvfs_scale_volt_direct(clk_dvfs_node->vd, volt_new);
 916                 if (ret)
 917                         goto out;
 918         }
 919
 920         return 0;
 921 fail_roll_back:
 922         clk_dvfs_node->set_volt = clk_volt_store;
 923 out:
 924         return ret;
 925 }
 926
 927 unsigned long dvfs_clk_round_rate(struct dvfs_node *clk_dvfs_node, unsigned long rate)
 928 {
 929         return __clk_round_rate(clk_dvfs_node->clk, rate);
 930 }
 931 EXPORT_SYMBOL_GPL(dvfs_clk_round_rate);
 932
 933 unsigned long dvfs_clk_get_rate(struct dvfs_node *clk_dvfs_node)
 934 {
 935         return __clk_get_rate(clk_dvfs_node->clk);
 936 }
 937 EXPORT_SYMBOL_GPL(dvfs_clk_get_rate);
 938
 939 unsigned long dvfs_clk_get_last_set_rate(struct dvfs_node *clk_dvfs_node)
 940 {
 941         unsigned long last_set_rate;
 942
 943         mutex_lock(&clk_dvfs_node->vd->mutex);
 944         last_set_rate = clk_dvfs_node->last_set_rate;
 945         mutex_unlock(&clk_dvfs_node->vd->mutex);
 946
 947         return last_set_rate;
 948 }
 949 EXPORT_SYMBOL_GPL(dvfs_clk_get_last_set_rate);
 950
 951
 952 int dvfs_clk_enable(struct dvfs_node *clk_dvfs_node)
 953 {
 954         return clk_enable(clk_dvfs_node->clk);
 955 }
 956 EXPORT_SYMBOL_GPL(dvfs_clk_enable);
 957
 958 void dvfs_clk_disable(struct dvfs_node *clk_dvfs_node)
 959 {
 960         return clk_disable(clk_dvfs_node->clk);
 961 }
 962 EXPORT_SYMBOL_GPL(dvfs_clk_disable);
 963
 964 struct dvfs_node *clk_get_dvfs_node(char *clk_name)
 965 {
 966         struct vd_node *vd;
 967         struct pd_node *pd;
 968         struct dvfs_node *clk_dvfs_node;
 969
 970         mutex_lock(&rk_dvfs_mutex);
 971         list_for_each_entry(vd, &rk_dvfs_tree, node) {
 972                 mutex_lock(&vd->mutex);
 973                 list_for_each_entry(pd, &vd->pd_list, node) {
 974                         list_for_each_entry(clk_dvfs_node, &pd->clk_list, node) {
 975                                 if (0 == strcmp(clk_dvfs_node->name, clk_name)) {
 976                                         mutex_unlock(&vd->mutex);
 977                                         mutex_unlock(&rk_dvfs_mutex);
 978                                         return clk_dvfs_node;
 979                                 }
 980                         }
 981                 }
 982                 mutex_unlock(&vd->mutex);
 983         }
 984         mutex_unlock(&rk_dvfs_mutex);
 985
 986         return NULL;
 987 }
 988 EXPORT_SYMBOL_GPL(clk_get_dvfs_node);
 989
 990 void clk_put_dvfs_node(struct dvfs_node *clk_dvfs_node)
 991 {
 992         return;
 993 }
 994 EXPORT_SYMBOL_GPL(clk_put_dvfs_node);
 995
 996 int dvfs_clk_prepare_enable(struct dvfs_node *clk_dvfs_node)
 997 {
 998         return clk_prepare_enable(clk_dvfs_node->clk);
 999 }
1000 EXPORT_SYMBOL_GPL(dvfs_clk_prepare_enable);
1001
1002
1003 void dvfs_clk_disable_unprepare(struct dvfs_node *clk_dvfs_node)
1004 {
1005         clk_disable_unprepare(clk_dvfs_node->clk);
1006 }
1007 EXPORT_SYMBOL_GPL(dvfs_clk_disable_unprepare);
1008
1009 int dvfs_clk_set_rate(struct dvfs_node *clk_dvfs_node, unsigned long rate)
1010 {
1011         int ret = -EINVAL;
1012
1013         if (!clk_dvfs_node)
1014                 return -EINVAL;
1015
1016         DVFS_DBG("%s:dvfs node(%s) set rate(%lu)\n",
1017                 __func__, clk_dvfs_node->name, rate);
1018
1019         #if 0 // judge by reference func in rk
1020         if (dvfs_support_clk_set_rate(dvfs_info)==false) {
1021                 DVFS_ERR("dvfs func:%s is not support!\n", __func__);
1022                 return ret;
1023         }
1024         #endif
1025
1026         if (clk_dvfs_node->vd && clk_dvfs_node->vd->vd_dvfs_target) {
1027                 mutex_lock(&clk_dvfs_node->vd->mutex);
1028                 ret = clk_dvfs_node->vd->vd_dvfs_target(clk_dvfs_node, rate);
1029                 clk_dvfs_node->last_set_rate = rate;
1030                 mutex_unlock(&clk_dvfs_node->vd->mutex);
1031         } else {
1032                 DVFS_ERR("%s:dvfs node(%s) has no vd node or target callback!\n",
1033                         __func__, clk_dvfs_node->name);
1034         }
1035
1036         return ret;
1037 }
1038 EXPORT_SYMBOL_GPL(dvfs_clk_set_rate);
1039
1040
1041 int rk_regist_vd(struct vd_node *vd)
1042 {
1043         if (!vd)
1044                 return -EINVAL;
1045
1046         vd->mode_flag=0;
1047         vd->volt_time_flag=0;
1048         vd->n_voltages=0;
1049         INIT_LIST_HEAD(&vd->pd_list);
1050         mutex_lock(&rk_dvfs_mutex);
1051         list_add(&vd->node, &rk_dvfs_tree);
1052         mutex_unlock(&rk_dvfs_mutex);
1053
1054         return 0;
1055 }
1056 EXPORT_SYMBOL_GPL(rk_regist_vd);
1057
1058 int rk_regist_pd(struct pd_node *pd)
1059 {
1060         struct vd_node  *vd;
1061
1062         if (!pd)
1063                 return -EINVAL;
1064
1065         vd = pd->vd;
1066         if (!vd)
1067                 return -EINVAL;
1068
1069         INIT_LIST_HEAD(&pd->clk_list);
1070         mutex_lock(&vd->mutex);
1071         list_add(&pd->node, &vd->pd_list);
1072         mutex_unlock(&vd->mutex);
1073
1074         return 0;
1075 }
1076 EXPORT_SYMBOL_GPL(rk_regist_pd);
1077
1078 int rk_regist_clk(struct dvfs_node *clk_dvfs_node)
1079 {
1080         struct vd_node  *vd;
1081         struct pd_node  *pd;
1082
1083         if (!clk_dvfs_node)
1084                 return -EINVAL;
1085
1086         vd = clk_dvfs_node->vd;
1087         pd = clk_dvfs_node->pd;
1088         if (!vd || !pd)
1089                 return -EINVAL;
1090
1091         mutex_lock(&vd->mutex);
1092         list_add(&clk_dvfs_node->node, &pd->clk_list);
1093         mutex_unlock(&vd->mutex);
1094
1095         return 0;
1096 }
1097 EXPORT_SYMBOL_GPL(rk_regist_clk);
1098
1099 static int rk_convert_cpufreq_table(struct dvfs_node *dvfs_node)
1100 {
1101         struct opp *opp;
1102         struct device *dev;
1103         struct cpufreq_frequency_table *table;
1104         int i;
1105
1106         table = dvfs_node->dvfs_table;
1107         dev = &dvfs_node->dev;
1108
1109         for (i = 0; table[i].frequency!= CPUFREQ_TABLE_END; i++){
1110                 opp = opp_find_freq_exact(dev, table[i].frequency * 1000, true);
1111                 if (IS_ERR(opp))
1112                         return PTR_ERR(opp);
1113                 table[i].index = opp_get_voltage(opp);
1114         }
1115         return 0;
1116 }
1117
1118 static struct cpufreq_frequency_table *of_get_temp_limit_table(struct device_node *dev_node, const char *propname)
1119 {
1120         struct cpufreq_frequency_table *temp_limt_table = NULL;
1121         const struct property *prop;
1122         const __be32 *val;
1123         int nr, i;
1124
1125         prop = of_find_property(dev_node, propname, NULL);
1126         if (!prop)
1127                 return NULL;
1128         if (!prop->value)
1129                 return NULL;
1130
1131         nr = prop->length / sizeof(u32);
1132         if (nr % 2) {
1133                 pr_err("%s: Invalid freq list\n", __func__);
1134                 return NULL;
1135         }
1136
1137         temp_limt_table = kzalloc(sizeof(struct cpufreq_frequency_table) *
1138                              (nr/2 + 1), GFP_KERNEL);
1139
1140         val = prop->value;
1141
1142         for (i=0; i<nr/2; i++){
1143                 temp_limt_table[i].index = be32_to_cpup(val++);
1144                 temp_limt_table[i].frequency = be32_to_cpup(val++) * 1000;
1145         }
1146
1147         temp_limt_table[i].index = 0;
1148         temp_limt_table[i].frequency = CPUFREQ_TABLE_END;
1149
1150         return temp_limt_table;
1151
1152 }
1153
1154 int of_dvfs_init(void)
1155 {
1156         struct vd_node *vd;
1157         struct pd_node *pd;
1158         struct device_node *dvfs_dev_node, *clk_dev_node, *vd_dev_node, *pd_dev_node;
1159         struct dvfs_node *dvfs_node;
1160         struct clk *clk;
1161         const __be32 *val;
1162         int ret;
1163
1164         DVFS_DBG("%s\n", __func__);
1165
1166         dvfs_dev_node = of_find_node_by_name(NULL, "dvfs");
1167         if (IS_ERR_OR_NULL(dvfs_dev_node)) {
1168                 DVFS_ERR("%s get dvfs dev node err\n", __func__);
1169                 return PTR_ERR(dvfs_dev_node);
1170         }
1171
1172         val = of_get_property(dvfs_dev_node, "target-temp", NULL);
1173         if (val) {
1174                 target_temp = be32_to_cpup(val);
1175         }
1176
1177         val = of_get_property(dvfs_dev_node, "temp-limit-enable", NULL);
1178         if (val) {
1179                 temp_limit_enable = be32_to_cpup(val);
1180         }
1181
1182         for_each_available_child_of_node(dvfs_dev_node, vd_dev_node) {
1183                 vd = kzalloc(sizeof(struct vd_node), GFP_KERNEL);
1184                 if (!vd)
1185                         return -ENOMEM;
1186
1187                 mutex_init(&vd->mutex);
1188                 vd->name = vd_dev_node->name;
1189                 ret = of_property_read_string(vd_dev_node, "regulator_name", &vd->regulator_name);
1190                 if (ret) {
1191                         DVFS_ERR("%s:vd(%s) get regulator_name err, ret:%d\n",
1192                                 __func__, vd_dev_node->name, ret);
1193                         kfree(vd);
1194                         continue;
1195                 }
1196
1197                 vd->suspend_volt = 0;
1198
1199                 vd->volt_set_flag = DVFS_SET_VOLT_FAILURE;
1200                 vd->vd_dvfs_target = dvfs_target;
1201                 ret = rk_regist_vd(vd);
1202                 if (ret){
1203                         DVFS_ERR("%s:vd(%s) register err:%d\n", __func__, vd->name, ret);
1204                         kfree(vd);
1205                         continue;
1206                 }
1207
1208                 DVFS_DBG("%s:vd(%s) register ok, regulator name:%s,suspend volt:%d\n",
1209                         __func__, vd->name, vd->regulator_name, vd->suspend_volt);
1210
1211                 for_each_available_child_of_node(vd_dev_node, pd_dev_node) {
1212                         pd = kzalloc(sizeof(struct pd_node), GFP_KERNEL);
1213                         if (!pd)
1214                                 return -ENOMEM;
1215
1216                         pd->vd = vd;
1217                         pd->name = pd_dev_node->name;
1218
1219                         ret = rk_regist_pd(pd);
1220                         if (ret){
1221                                 DVFS_ERR("%s:pd(%s) register err:%d\n", __func__, pd->name, ret);
1222                                 kfree(pd);
1223                                 continue;
1224                         }
1225                         DVFS_DBG("%s:pd(%s) register ok, parent vd:%s\n",
1226                                 __func__, pd->name, vd->name);
1227                         for_each_available_child_of_node(pd_dev_node, clk_dev_node) {
1228                                 if (!of_device_is_available(clk_dev_node))
1229                                         continue;
1230
1231                                 dvfs_node = kzalloc(sizeof(struct dvfs_node), GFP_KERNEL);
1232                                 if (!dvfs_node)
1233                                         return -ENOMEM;
1234
1235                                 dvfs_node->name = clk_dev_node->name;
1236                                 dvfs_node->pd = pd;
1237                                 dvfs_node->vd = vd;
1238                                 if (temp_limit_enable) {
1239                                         val = of_get_property(clk_dev_node, "temp-channel", NULL);
1240                                         if (val) {
1241                                                 dvfs_node->temp_channel = be32_to_cpup(val);
1242                                         }
1243                                         dvfs_node->nor_temp_limit_table = of_get_temp_limit_table(clk_dev_node, "normal-temp-limit");
1244                                         dvfs_node->per_temp_limit_table = of_get_temp_limit_table(clk_dev_node, "performance-temp-limit");
1245                                 }
1246                                 dvfs_node->temp_limit_rate = -1;
1247                                 dvfs_node->dev.of_node = clk_dev_node;
1248                                 ret = of_init_opp_table(&dvfs_node->dev);
1249                                 if (ret) {
1250                                         DVFS_ERR("%s:clk(%s) get opp table err:%d\n", __func__, dvfs_node->name, ret);
1251                                         kfree(dvfs_node);
1252                                         continue;
1253                                 }
1254
1255                                 ret = opp_init_cpufreq_table(&dvfs_node->dev, &dvfs_node->dvfs_table);
1256                                 if (ret) {
1257                                         DVFS_ERR("%s:clk(%s) get cpufreq table err:%d\n", __func__, dvfs_node->name, ret);
1258                                         kfree(dvfs_node);
1259                                         continue;
1260                                 }
1261                                 ret = rk_convert_cpufreq_table(dvfs_node);
1262                                 if (ret) {
1263                                         kfree(dvfs_node);
1264                                         continue;
1265                                 }
1266
1267                                 clk = clk_get(NULL, clk_dev_node->name);
1268                                 if (IS_ERR(clk)){
1269                                         DVFS_ERR("%s:get clk(%s) err:%ld\n", __func__, dvfs_node->name, PTR_ERR(clk));
1270                                         kfree(dvfs_node);
1271                                         continue;
1272
1273                                 }
1274
1275                                 dvfs_node->clk = clk;
1276                                 ret = rk_regist_clk(dvfs_node);
1277                                 if (ret){
1278                                         DVFS_ERR("%s:dvfs_node(%s) register err:%d\n", __func__, dvfs_node->name, ret);
1279                                         return ret;
1280                                 }
1281
1282                                 DVFS_DBG("%s:dvfs_node(%s) register ok, parent pd:%s\n",
1283                                         __func__, clk_dev_node->name, pd->name);
1284
1285                         }
1286                 }
1287         }
1288         return 0;
1289 }
1290
1291 /*********************************************************************************/
1292 /**
1293  * dump_dbg_map() : Draw all informations of dvfs while debug
1294  */
1295 static int dump_dbg_map(char *buf)
1296 {
1297         int i;
1298         struct vd_node  *vd;
1299         struct pd_node  *pd;
1300         struct dvfs_node        *clk_dvfs_node;
1301         char *s = buf;
1302
1303         mutex_lock(&rk_dvfs_mutex);
1304         printk( "-------------DVFS TREE-----------\n\n\n");
1305         printk( "DVFS TREE:\n");
1306
1307         list_for_each_entry(vd, &rk_dvfs_tree, node) {
1308                 mutex_lock(&vd->mutex);
1309                 printk( "|\n|- voltage domain:%s\n", vd->name);
1310                 printk( "|- current voltage:%d\n", vd->cur_volt);
1311
1312                 list_for_each_entry(pd, &vd->pd_list, node) {
1313                         printk( "|  |\n|  |- power domain:%s, status = %s, current volt = %d\n",
1314                                         pd->name, (pd->pd_status == 1) ? "ON" : "OFF", pd->cur_volt);
1315
1316                         list_for_each_entry(clk_dvfs_node, &pd->clk_list, node) {
1317                                 printk( "|  |  |\n|  |  |- clock: %s current: rate %d, volt = %d,"
1318                                                 " enable_dvfs = %s\n",
1319                                                 clk_dvfs_node->name, clk_dvfs_node->set_freq, clk_dvfs_node->set_volt,
1320                                                 clk_dvfs_node->enable_count == 0 ? "DISABLE" : "ENABLE");
1321                                 printk( "|  |  |- clk limit(%s):[%u, %u]; last set rate = %u\n",
1322                                                 clk_dvfs_node->freq_limit_en ? "enable" : "disable",
1323                                                 clk_dvfs_node->min_rate, clk_dvfs_node->max_rate,
1324                                                 clk_dvfs_node->last_set_rate/1000);
1325
1326                                 for (i = 0; (clk_dvfs_node->dvfs_table[i].frequency != CPUFREQ_TABLE_END); i++) {
1327                                         printk( "|  |  |  |- freq = %d, volt = %d\n",
1328                                                         clk_dvfs_node->dvfs_table[i].frequency,
1329                                                         clk_dvfs_node->dvfs_table[i].index);
1330
1331                                 }
1332                         }
1333                 }
1334                 mutex_unlock(&vd->mutex);
1335         }
1336
1337         printk( "-------------DVFS TREE END------------\n");
1338         mutex_unlock(&rk_dvfs_mutex);
1339
1340         return s - buf;
1341 }
1342
1343 /*********************************************************************************/
1344 static struct kobject *dvfs_kobj;
1345 struct dvfs_attribute {
1346         struct attribute        attr;
1347         ssize_t (*show)(struct kobject *kobj, struct kobj_attribute *attr,
1348                         char *buf);
1349         ssize_t (*store)(struct kobject *kobj, struct kobj_attribute *attr,
1350                         const char *buf, size_t n);
1351 };
1352
1353 static ssize_t dvfs_tree_store(struct kobject *kobj, struct kobj_attribute *attr,
1354                const char *buf, size_t n)
1355 {
1356        return n;
1357 }
1358 static ssize_t dvfs_tree_show(struct kobject *kobj, struct kobj_attribute *attr,
1359                char *buf)
1360 {
1361        return dump_dbg_map(buf);
1362 }
1363
1364
1365 static struct dvfs_attribute dvfs_attrs[] = {
1366         /*     node_name        permision               show_func       store_func */
1367 //#ifdef CONFIG_RK_CLOCK_PROC
1368         __ATTR(dvfs_tree,       S_IRUSR | S_IRGRP | S_IWUSR,    dvfs_tree_show, dvfs_tree_store),
1369 //#endif
1370 };
1371
1372 static int __init dvfs_init(void)
1373 {
1374         int i, ret = 0;
1375
1376         dvfs_kobj = kobject_create_and_add("dvfs", NULL);
1377         if (!dvfs_kobj)
1378                 return -ENOMEM;
1379         for (i = 0; i < ARRAY_SIZE(dvfs_attrs); i++) {
1380                 ret = sysfs_create_file(dvfs_kobj, &dvfs_attrs[i].attr);
1381                 if (ret != 0) {
1382                         DVFS_ERR("create index %d error\n", i);
1383                         return ret;
1384                 }
1385         }
1386
1387         if (temp_limit_enable) {
1388                 clk_cpu_dvfs_node = clk_get_dvfs_node("clk_core");
1389                 if (!clk_cpu_dvfs_node){
1390                         return -EINVAL;
1391                 }
1392
1393                 clk_cpu_dvfs_node->temp_limit_rate = clk_cpu_dvfs_node->max_rate;
1394                 dvfs_wq = alloc_workqueue("dvfs", WQ_NON_REENTRANT | WQ_MEM_RECLAIM | WQ_HIGHPRI | WQ_FREEZABLE, 1);
1395                 queue_delayed_work_on(0, dvfs_wq, &dvfs_temp_limit_work, 0*HZ);
1396         }
1397
1398         return ret;
1399 }
1400
1401 late_initcall(dvfs_init);