arch/arm/mach-rk30/dvfs.c

   1 /* arch/arm/mach-rk30/rk30_dvfs.c\r
   2  *\r
   3  * Copyright (C) 2012 ROCKCHIP, Inc.\r
   4  *\r
   5  * This software is licensed under the terms of the GNU General Public\r
   6  * License version 2, as published by the Free Software Foundation, and\r
   7  * may be copied, distributed, and modified under those terms.\r
   8  *\r
   9  * This program is distributed in the hope that it will be useful,\r
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  12  * GNU General Public License for more details.\r
  13  *\r
  14  */\r
  15 \r
  16 #include <linux/kernel.h>\r
  17 #include <linux/err.h>\r
  18 #include <linux/spinlock.h>\r
  19 #include <linux/list.h>\r
  20 #include <linux/slab.h>\r
  21 #include <linux/clk.h>\r
  22 #include <linux/cpufreq.h>\r
  23 \r
  24 #include "clock.h"\r
  25 #include <mach/dvfs.h>\r
  26 #include <mach/clock.h>\r
  27 \r
  28 #if 0\r
  29 #define DVFS_DBG(fmt, args...) pr_debug(fmt, ##args)\r
  30 #define DVFS_ERR(fmt, args...) pr_err(fmt, ##args)\r
  31 #else\r
  32 #define DEBUG_RK30_DVFS\r
  33 #define DVFS_DBG(fmt, args...) printk(fmt, ##args)\r
  34 #define DVFS_ERR(fmt, args...) printk(fmt, ##args)\r
  35 #endif\r
  36 \r
  37 #ifndef CONFIG_ARCH_RK30\r
  38 #define DVFS_TEST_OFF_BOARD\r
  39 #endif\r
  40 \r
  41 \r
  42 \r
  43 #ifdef DVFS_TEST_OFF_BOARD\r
  44 /* Just for simulation */\r
  45 \r
  46 struct regulator {\r
  47     int min_uV;\r
  48 };\r
  49 #if 0\r
  50 static void test_regulator_put(struct regulator *regulator)\r
  51 {\r
  52     kfree(regulator);\r
  53 }\r
  54 #endif\r
  55 struct regulator regulators[100];\r
  56 static struct regulator *test_regulator_get(struct device *dev, const char *id) {\r
  57     static int ret_cnt = 0;\r
  58     return &regulators[ret_cnt++];\r
  59 }\r
  60 \r
  61 static int test_regulator_set_voltage(struct regulator *regulator, int min_uV, int max_uV)\r
  62 {\r
  63     regulator->min_uV = min_uV;\r
  64     return 0;\r
  65 }\r
  66 \r
  67 static int test_regulator_get_voltage(struct regulator *regulator)\r
  68 {\r
  69     return regulator->min_uV;\r
  70 }\r
  71 \r
  72 int rk30_clk_set_rate(struct clk *clk, unsigned long rate);\r
  73 static void dump_dbg_map(void);\r
  74 int rk30_dvfs_init_test(void);\r
  75 int rk30_clk_enable(struct clk *clk);\r
  76 int rk30_clk_disable(struct clk *clk);\r
  77 \r
  78 #define dvfs_regulator_get(dev,id) test_regulator_get((dev),(id))\r
  79 #define dvfs_regulator_put(regu) test_regulator_get((regu))\r
  80 #define dvfs_regulator_set_voltage(regu,min_uV,max_uV) test_regulator_set_voltage((regu),(min_uV),(max_uV))\r
  81 #define dvfs_regulator_get_voltage(regu) test_regulator_get_voltage((regu))\r
  82 \r
  83 /* clock */\r
  84 #define dvfs_clk_get(a,b) rk30_clk_get((a),(b))\r
  85 #define dvfs_clk_set_rate(a,b) rk30_clk_set_rate((a),(b))\r
  86 #define dvfs_clk_enable(a) rk30_clk_enable((a))\r
  87 #define dvfs_clk_disable(a) rk30_clk_disable((a))\r
  88 \r
  89 #else\r
  90 /* board runing */\r
  91 #include <linux/regulator/consumer.h>\r
  92 \r
  93 #define dvfs_regulator_get(dev,id) regulator_get((dev),(id))\r
  94 #define dvfs_regulator_put(regu) regulator_get((regu))\r
  95 #define dvfs_regulator_set_voltage(regu,min_uV,max_uV) regulator_set_voltage((regu),(min_uV),(max_uV))\r
  96 #define dvfs_regulator_get_voltage(regu) regulator_get_voltage((regu))\r
  97 \r
  98 #define dvfs_clk_get(a,b) clk_get((a),(b))\r
  99 #define dvfs_clk_set_rate(a,b) clk_set_rate((a),(b))\r
 100 #define dvfs_clk_enable(a) clk_enable((a))\r
 101 #define dvfs_clk_disable(a) clk_disable((a))\r
 102 #endif\r
 103 \r
 104 \r
 105 static LIST_HEAD(rk_dvfs_tree);\r
 106 static DEFINE_MUTEX(mutex);\r
 107 /*\r
 108 int dvfs_target_core(struct clk *clk, unsigned int rate);\r
 109 int dvfs_target(struct clk *clk, unsigned int rate);\r
 110 int dvfs_clk_set_rate(struct clk *clk, unsigned long rate);\r
 111 */\r
 112 extern int rk30_clk_notifier_register(struct clk *clk, struct notifier_block *nb);\r
 113 extern int rk30_clk_notifier_unregister(struct clk *clk, struct notifier_block *nb);\r
 114 \r
 115 #define FV_TABLE_END 0\r
 116 #define PD_ON   1\r
 117 #define PD_OFF  0\r
 118 \r
 119 \r
 120 static void dvfs_clk_scale_volt(struct clk_node *dvfs_clk, unsigned int volt);\r
 121 static int dvfs_clk_get_volt(struct clk_node *dvfs_clk, unsigned long rate,\r
 122                              struct cpufreq_frequency_table *clk_fv);\r
 123 \r
 124 /**\r
 125  * **************************FUNCTIONS***********************************\r
 126  */\r
 127 \r
 128 #ifdef DEBUG_RK30_DVFS\r
 129 /**\r
 130  * dump_dbg_map() : Draw all informations of dvfs while debug\r
 131  */\r
 132 static void dump_dbg_map(void)\r
 133 {\r
 134     int i;\r
 135     struct vd_node      *vd;\r
 136     struct pd_node      *pd, *clkparent;\r
 137     struct clk_list     *child;\r
 138     struct clk_node     *dvfs_clk;\r
 139 \r
 140     DVFS_DBG("-------------DVFS DEBUG-----------\n\n\n");\r
 141     DVFS_DBG("RK30 DVFS TREE:\n");\r
 142     list_for_each_entry(vd, &rk_dvfs_tree, node) {\r
 143         DVFS_DBG("|\n|- voltage domain:%s\n", vd->name);\r
 144         DVFS_DBG("|- current voltage:%d\n", vd->cur_volt);\r
 145 \r
 146         list_for_each_entry(pd, &vd->pd_list, node) {\r
 147             DVFS_DBG("|  |\n|  |- power domain:%s, status = %s, current volt = %d\n",\r
 148                      pd->name, (pd->pd_status == PD_ON) ? "ON" : "OFF", pd->cur_volt);\r
 149 \r
 150             list_for_each_entry(child, &pd->clk_list, node) {\r
 151                 dvfs_clk = child->dvfs_clk;\r
 152                 DVFS_DBG("|  |  |\n|  |  |- clock: %s current: rate %d, volt = %d, enable_dvfs = %s\n",\r
 153                          dvfs_clk->name, dvfs_clk->cur_freq, dvfs_clk->cur_volt, dvfs_clk->enable_dvfs == 0 ? "DISABLE" : "ENABLE");\r
 154                 for (i = 0; dvfs_clk->pds[i].pd != NULL; i++) {\r
 155                     clkparent = dvfs_clk->pds[i].pd;\r
 156                     DVFS_DBG("|  |  |  |- clock parents: %s, vd_parent = %s\n", clkparent->name, clkparent->vd->name);\r
 157                 }\r
 158 \r
 159                 for (i = 0; (dvfs_clk->dvfs_table[i].frequency != FV_TABLE_END); i++) {\r
 160                     DVFS_DBG("|  |  |  |- freq = %d, volt = %d\n", dvfs_clk->dvfs_table[i].frequency, dvfs_clk->dvfs_table[i].index);\r
 161 \r
 162                 }\r
 163             }\r
 164         }\r
 165     }\r
 166     DVFS_DBG("-------------DVFS DEBUG END------------\n");\r
 167 }\r
 168 #endif\r
 169 \r
 170 int is_support_dvfs(struct clk_node *dvfs_info)\r
 171 {\r
 172     return (dvfs_info->vd && dvfs_info->vd->vd_dvfs_target && dvfs_info->enable_dvfs);\r
 173 }\r
 174 static int rk_dvfs_clk_notifier_event(struct notifier_block *this,\r
 175                                       unsigned long event, void *ptr)\r
 176 {\r
 177     struct clk_notifier_data *noti_info;\r
 178     struct clk *clk;\r
 179     struct clk_node *dvfs_clk;\r
 180     noti_info = (struct clk_notifier_data *)ptr;\r
 181     clk = noti_info->clk;\r
 182     dvfs_clk = clk->dvfs_info;\r
 183 \r
 184     switch (event) {\r
 185     case CLK_PRE_RATE_CHANGE:\r
 186         DVFS_DBG("%s CLK_PRE_RATE_CHANGE\n", __func__);\r
 187         break;\r
 188     case CLK_POST_RATE_CHANGE:\r
 189         DVFS_DBG("%s CLK_POST_RATE_CHANGE\n", __func__);\r
 190         break;\r
 191     case CLK_ABORT_RATE_CHANGE:\r
 192         DVFS_DBG("%s CLK_ABORT_RATE_CHANGE\n", __func__);\r
 193         break;\r
 194     case CLK_PRE_ENABLE:\r
 195         DVFS_DBG("%s CLK_PRE_ENABLE\n", __func__);\r
 196         break;\r
 197     case CLK_POST_ENABLE:\r
 198         DVFS_DBG("%s CLK_POST_ENABLE\n", __func__);\r
 199         break;\r
 200     case CLK_ABORT_ENABLE:\r
 201         DVFS_DBG("%s CLK_ABORT_ENABLE\n", __func__);\r
 202         break;\r
 203     case CLK_PRE_DISABLE:\r
 204         DVFS_DBG("%s CLK_PRE_DISABLE\n", __func__);\r
 205         break;\r
 206     case CLK_POST_DISABLE:\r
 207         DVFS_DBG("%s CLK_POST_DISABLE\n", __func__);\r
 208         dvfs_clk->cur_freq = 0;\r
 209         dvfs_clk_scale_volt(dvfs_clk, 0);\r
 210         break;\r
 211     case CLK_ABORT_DISABLE:\r
 212         DVFS_DBG("%s CLK_ABORT_DISABLE\n", __func__);\r
 213 \r
 214         break;\r
 215     default:\r
 216         break;\r
 217     }\r
 218     return 0;\r
 219 }\r
 220 static struct notifier_block rk_dvfs_clk_notifier = {\r
 221     .notifier_call = rk_dvfs_clk_notifier_event,\r
 222 };\r
 223 int clk_disable_dvfs(struct clk *clk)\r
 224 {\r
 225     struct clk_node *dvfs_clk;\r
 226     dvfs_clk = clk->dvfs_info;\r
 227     if(dvfs_clk->enable_dvfs - 1 < 0) {\r
 228         DVFS_ERR("clk is already closed!\n");\r
 229         return -1;\r
 230     } else {\r
 231         DVFS_ERR("clk is disable now!\n");\r
 232         dvfs_clk->enable_dvfs--;\r
 233         if(0 == dvfs_clk->enable_dvfs) {\r
 234             DVFS_ERR("clk closed!\n");\r
 235             rk30_clk_notifier_unregister(clk, dvfs_clk->dvfs_nb);\r
 236             DVFS_ERR("clk unregister nb!\n");\r
 237             dvfs_clk_scale_volt(dvfs_clk, 0);\r
 238         }\r
 239     }\r
 240     dump_dbg_map();\r
 241     return 0;\r
 242 }\r
 243 \r
 244 int clk_enable_dvfs(struct clk *clk)\r
 245 {\r
 246     struct regulator *regulator;\r
 247     struct clk_node *dvfs_clk;\r
 248     struct cpufreq_frequency_table clk_fv;\r
 249 \r
 250     if(!clk->dvfs_info) {\r
 251         DVFS_ERR("This clk(%s) not support dvfs!\n", clk->name);\r
 252         return -1;\r
 253     }\r
 254 \r
 255     dvfs_clk = clk->dvfs_info;\r
 256     DVFS_ERR("dvfs clk enable dvfs %s\n", dvfs_clk->name);\r
 257     if(0 == dvfs_clk->enable_dvfs) {\r
 258         dvfs_clk->enable_dvfs++;\r
 259         if(!dvfs_clk->vd->regulator) {\r
 260             regulator = dvfs_regulator_get(NULL, dvfs_clk->vd->regulator_name);\r
 261             if(regulator)\r
 262                 dvfs_clk->vd->regulator = regulator;\r
 263             else\r
 264                 dvfs_clk->vd->regulator = NULL;\r
 265         }\r
 266         if(dvfs_clk->dvfs_nb) {\r
 267             // must unregister when clk disable\r
 268             rk30_clk_notifier_register(clk, dvfs_clk->dvfs_nb);\r
 269         }\r
 270 \r
 271         if(!clk || IS_ERR(clk)) {\r
 272             DVFS_ERR("%s get clk %s error\n", __func__, dvfs_clk->name);\r
 273             return -1;\r
 274         }\r
 275         //DVFS_DBG("%s get clk %s rate = %lu\n", __func__, clk->name, clk->rate);\r
 276                 if(dvfs_clk->cur_freq == 0)\r
 277                 dvfs_clk_get_volt(dvfs_clk, clk->rate, &clk_fv);\r
 278                 else\r
 279                         dvfs_clk_get_volt(dvfs_clk, dvfs_clk->cur_freq, &clk_fv);\r
 280         dvfs_clk->cur_volt = clk_fv.index;\r
 281         dvfs_clk->cur_freq = clk_fv.frequency;\r
 282         dvfs_clk_scale_volt(dvfs_clk, dvfs_clk->cur_volt);\r
 283                 dump_dbg_map();\r
 284 \r
 285     } else {\r
 286         DVFS_ERR("dvfs already enable clk enable = %d!\n", dvfs_clk->enable_dvfs);\r
 287         dvfs_clk->enable_dvfs++;\r
 288     }\r
 289     return 0;\r
 290 }\r
 291 \r
 292 int dvfs_set_rate(struct clk *clk, unsigned long rate)\r
 293 {\r
 294     int ret = 0;\r
 295     struct vd_node *vd;\r
 296     DVFS_DBG("%s dvfs start\n", clk->name);\r
 297     if(!clk->dvfs_info) {\r
 298         DVFS_ERR("%s :This clk do not support dvfs!\n", __func__);\r
 299         ret = -1;\r
 300     } else {\r
 301         vd = clk->dvfs_info->vd;\r
 302         mutex_lock(&vd->dvfs_mutex);\r
 303         ret = vd->vd_dvfs_target(clk, rate);\r
 304         mutex_unlock(&vd->dvfs_mutex);\r
 305     }\r
 306     return ret;\r
 307 }\r
 308 \r
 309 /**\r
 310  * get correspond voltage khz\r
 311  */\r
 312 static int dvfs_clk_get_volt(struct clk_node *dvfs_clk, unsigned long rate,\r
 313                              struct cpufreq_frequency_table *clk_fv)\r
 314 {\r
 315     int i = 0;\r
 316     if (rate == 0) {\r
 317         /* since no need*/\r
 318         return -1;\r
 319     }\r
 320     clk_fv->frequency = rate;\r
 321     clk_fv->index = 0;\r
 322     for(i = 0; (dvfs_clk->dvfs_table[i].frequency != FV_TABLE_END); i++) {\r
 323         if(dvfs_clk->dvfs_table[i].frequency >= rate) {\r
 324             clk_fv->frequency = dvfs_clk->dvfs_table[i].frequency;\r
 325             clk_fv->index = dvfs_clk->dvfs_table[i].index;\r
 326             DVFS_DBG("%s dvfs_clk_get_volt rate=%u hz ref vol=%d uV\n", dvfs_clk->name, clk_fv->frequency, clk_fv->index);\r
 327             return 0;\r
 328         }\r
 329     }\r
 330     clk_fv->frequency = 0;\r
 331     clk_fv->index = 0;\r
 332     DVFS_ERR("%s get corresponding voltage error! out of bound\n", dvfs_clk->name);\r
 333     return -1;\r
 334 }\r
 335 \r
 336 static int dvfs_clk_round_volt(struct clk_node *dvfs_clk, int volt)\r
 337 {\r
 338     struct pd_node      *pd;\r
 339     struct clk_node     *dvfs_clk_tmp;\r
 340     int volt_max = 0;\r
 341     int i;\r
 342 \r
 343     for(i = 0; (dvfs_clk->pds[i].pd != NULL); i++) {\r
 344         pd = dvfs_clk->pds[i].pd;\r
 345         if(volt > pd->cur_volt) {\r
 346             /**\r
 347              * if dvfs_clk parent power domain's voltage is smaller then\r
 348              * this dvfs_clk's voltage ignore this power domain\r
 349              */\r
 350             volt_max = max(volt_max, volt);\r
 351             continue;\r
 352         }\r
 353         list_for_each_entry(dvfs_clk_tmp, &pd->clk_list, node) {\r
 354             /**\r
 355              * found the max voltage uninclude dvfs_clk\r
 356              */\r
 357             if(dvfs_clk_tmp != dvfs_clk) {\r
 358                 volt_max = max(volt_max, dvfs_clk_tmp->cur_volt);\r
 359             }\r
 360         }\r
 361     }\r
 362 \r
 363     volt_max = max(volt_max, volt);\r
 364     return volt_max;\r
 365 }\r
 366 \r
 367 static void dvfs_clk_scale_volt(struct clk_node *dvfs_clk, unsigned int volt)\r
 368 {\r
 369     struct vd_node *vd;\r
 370     struct pd_node *pd;\r
 371     struct clk_list     *child;\r
 372     struct clk_node     *dvfs_clk_tmp;\r
 373     int volt_max_vd = 0, volt_max_pd = 0, i;\r
 374 \r
 375     dvfs_clk->cur_volt = volt;//set  clk node volt\r
 376     vd = dvfs_clk->vd;// vd\r
 377     for(i = 0; (dvfs_clk->pds[i].pd != NULL); i++) {\r
 378         pd = dvfs_clk->pds[i].pd;\r
 379         volt_max_pd = 0;\r
 380         /**\r
 381          * set corresponding voltage, clk do not need to set voltage,just for\r
 382          * powerdomain\r
 383          */\r
 384 \r
 385         if(volt > pd->cur_volt) {\r
 386             pd->cur_volt = volt;\r
 387             pd->pd_status = (pd->cur_volt == 0) ? PD_OFF : PD_ON;\r
 388             continue;\r
 389         }\r
 390 \r
 391         /* set power domain voltage */\r
 392         list_for_each_entry(child, &pd->clk_list, node) {\r
 393             dvfs_clk_tmp = child->dvfs_clk;\r
 394                         if(dvfs_clk_tmp->enable_dvfs){\r
 395                 volt_max_pd = max(volt_max_pd, dvfs_clk_tmp->cur_volt);\r
 396                         }\r
 397         }\r
 398         pd->cur_volt = volt_max_pd;\r
 399 \r
 400         pd->pd_status = (volt_max_pd == 0) ? PD_OFF : PD_ON;\r
 401     }\r
 402 \r
 403     /* set voltage domain voltage */\r
 404     volt_max_vd = 0;\r
 405     list_for_each_entry(pd, &vd->pd_list, node) {\r
 406         volt_max_vd = max(volt_max_vd, pd->cur_volt);\r
 407     }\r
 408     vd->cur_volt = volt_max_vd;\r
 409 }\r
 410 \r
 411 int dvfs_target_set_rate_core(struct clk *clk, unsigned long rate)\r
 412 {\r
 413     struct clk_node *dvfs_clk;\r
 414     int volt_new = 0, volt_old = 0;\r
 415     struct cpufreq_frequency_table clk_fv;\r
 416     int ret = 0;\r
 417     dvfs_clk = clk_get_dvfs_info(clk);\r
 418 \r
 419     DVFS_ERR("%s get clk %s\n", __func__, clk->name);\r
 420     if(dvfs_clk->vd->regulator == NULL) {\r
 421         DVFS_ERR("%s can't get dvfs regulater\n", clk->name);\r
 422         return -1;\r
 423     }\r
 424 \r
 425     /* If power domain off do scale in the notify function */\r
 426     /*\r
 427        if (rate == 0) {\r
 428        dvfs_clk->cur_freq = 0;\r
 429        dvfs_clk_scale_volt(dvfs_clk, 0);\r
 430        return 0;\r
 431        }\r
 432        */\r
 433     /* need round rate */\r
 434     DVFS_ERR("%s going to round rate = %lu\n", clk->name, rate);\r
 435     rate = clk_round_rate_nolock(clk, rate);\r
 436     DVFS_ERR("%s round get rate = %lu\n", clk->name, rate);\r
 437     /* find the clk corresponding voltage */\r
 438     if (0 != dvfs_clk_get_volt(dvfs_clk, rate, &clk_fv)) {\r
 439         DVFS_ERR("%s rate %lukhz is larger,not support\n", clk->name, rate);\r
 440         return -1;\r
 441     }\r
 442     volt_old = dvfs_clk->vd->cur_volt;\r
 443     volt_new = clk_fv.index;\r
 444 \r
 445     DVFS_DBG("vol_new = %d mV(was %d mV)\n", volt_new, volt_old);\r\r
 446 \r
 447     /* if up the voltage*/\r
 448     if (volt_old < volt_new) {\r
 449         if(dvfs_regulator_set_voltage(dvfs_clk->vd->regulator, volt_new, volt_new) < 0) {\r
 450             DVFS_ERR("set voltage err\n");\r
 451 \r
 452             return -1;\r
 453         }\r
 454         dvfs_clk->vd->cur_volt = volt_new;\r
 455         /* CPU do not use power domain, so save scale times */\r
 456         //dvfs_clk_scale_volt(dvfs_clk, clk_fv.index);\r
 457     }\r
 458 \r
 459     if(dvfs_clk->clk_dvfs_target) {\r
 460         ret = dvfs_clk->clk_dvfs_target(clk, rate, clk_set_rate_locked);\r
 461     } else {\r
 462         ret = clk_set_rate_locked(clk, rate);\r
 463     }\r
 464     if (ret < 0) {\r
 465         DVFS_ERR("set rate err\n");\r
 466         return -1;\r
 467     }\r
 468     dvfs_clk->cur_freq  = rate;\r
 469     dvfs_clk->cur_volt  = volt_new;\r
 470 \r
 471     /* if down the voltage */\r
 472     if (volt_old > volt_new) {\r
 473         if(dvfs_regulator_set_voltage(dvfs_clk->vd->regulator, volt_new, volt_new) < 0) {\r
 474             DVFS_ERR("set voltage err\n");\r
 475 \r
 476             return -1;\r
 477         }\r
 478         dvfs_clk->vd->cur_volt = volt_new;\r
 479         /* CPU do not use power domain, so save scale times */\r
 480         //dvfs_clk_scale_volt(dvfs_clk, clk_fv.index);\r
 481     }\r
 482 \r
 483     return ret;\r
 484 }\r
 485 \r
 486 int dvfs_target_set_rate_normal(struct clk *clk, unsigned long rate)\r
 487 {\r
 488     struct clk_node *dvfs_clk;\r
 489     unsigned int volt_new = 0, volt_old = 0;\r
 490     struct cpufreq_frequency_table clk_fv = {0, 0};\r
 491     int ret = 0;\r
 492 \r
 493     dvfs_clk = clk_get_dvfs_info(clk);\r
 494     DVFS_ERR("%s get clk %s\n", __func__, clk->name);\r
 495     if(dvfs_clk->vd->regulator == NULL) {\r
 496         DVFS_DBG("%s can't get dvfs regulater\n", clk->name);\r
 497         return -1;\r
 498     }\r
 499 \r
 500     /* need round rate */\r
 501     DVFS_ERR("%s going to round rate = %lu\n", clk->name, rate);\r
 502     rate = clk_round_rate_nolock(clk, rate);\r
 503     DVFS_ERR("%s round get rate = %lu\n", clk->name, rate);\r
 504     /* find the clk corresponding voltage */\r
 505     if (dvfs_clk_get_volt(dvfs_clk, rate, &clk_fv)) {\r
 506         DVFS_DBG("dvfs_clk_get_volt:rate = Get corresponding voltage error!\n");\r
 507         return -1;\r
 508     }\r
 509 \r
 510     volt_old = dvfs_clk->vd->cur_volt;\r
 511     volt_new = dvfs_clk_round_volt(dvfs_clk, clk_fv.index);\r
 512 \r
 513     // if up the voltage\r
 514     if (volt_old < volt_new) {\r
 515         if(dvfs_regulator_set_voltage(dvfs_clk->vd->regulator, volt_new, volt_new) < 0) {\r
 516             DVFS_DBG("set voltage err\n");\r\r
 517             return -1;\r
 518         }\r
 519         dvfs_clk_scale_volt(dvfs_clk, clk_fv.index);\r
 520     }\r
 521 \r
 522     if(dvfs_clk->clk_dvfs_target) {\r
 523         ret = dvfs_clk->clk_dvfs_target(clk, rate, clk_set_rate_locked);\r
 524     } else {\r
 525         ret = clk_set_rate_locked(clk, rate);\r\r
 526     }\r
 527     if (ret < 0) {\r
 528         DVFS_ERR("set rate err\n");\r
 529         return -1;\r
 530     }\r
 531     dvfs_clk->cur_freq  = rate;\r
 532     dvfs_clk->cur_volt  = volt_new;\r
 533 \r
 534     // if down the voltage\r
 535     if (volt_old > volt_new) {\r
 536         if(dvfs_regulator_set_voltage(dvfs_clk->vd->regulator, volt_new, volt_new) < 0) {\r
 537             DVFS_DBG("set voltage err\n");\r
 538                         return -1;\r
 539 \r
 540         }\r
 541         dvfs_clk_scale_volt(dvfs_clk, clk_fv.index);\r
 542     }\r
 543 \r
 544     return 0;\r
 545 }\r
 546 \r
 547 \r
 548 /*****************************init**************************/\r
 549 /**\r
 550  * rate must be raising sequence\r
 551  */\r
 552 \r
 553 struct cpufreq_frequency_table cpu_dvfs_table[] = {\r
 554     {.frequency = 126000000, .index     = 800000},\r
 555     {.frequency = 252000000, .index     = 850000},\r
 556     {.frequency = 504000000, .index     = 900000},\r
 557     {.frequency = 816000000, .index     = 1050000},\r
 558     {.frequency = 1008000000, .index    = 1100000},\r
 559     {.frequency = 1200000000, .index    = 1200000},\r
 560     {.frequency = FV_TABLE_END},\r
 561 };\r
 562 struct cpufreq_frequency_table ddr_dvfs_table[] = {\r
 563     {.frequency = 24000000, .index              = 600000},\r
 564     {.frequency = 64000000, .index              = 700000},\r
 565     {.frequency = 126000000, .index     = 800000},\r
 566     {.frequency = 252000000, .index     = 850000},\r
 567     {.frequency = 504000000, .index     = 900000},\r
 568     {.frequency = FV_TABLE_END},\r
 569 };\r
 570 struct cpufreq_frequency_table gpu_dvfs_table[] = {\r
 571     {.frequency = 64000000, .index              = 700000},\r
 572     {.frequency = 126000000, .index     = 800000},\r
 573     {.frequency = 360000000, .index     = 850000},\r
 574     {.frequency = FV_TABLE_END},\r
 575 };\r
 576 \r
 577 static struct vd_node vd_cpu = {\r
 578     .name                       = "vd_cpu",\r
 579     .vd_dvfs_target     = dvfs_target_set_rate_core,\r
 580 };\r
 581 \r
 582 static struct vd_node vd_core = {\r
 583     .name                       = "vd_core",\r
 584     .vd_dvfs_target     = dvfs_target_set_rate_normal,\r
 585 };\r
 586 \r
 587 static struct vd_node vd_rtc = {\r
 588     .name                       = "vd_rtc",\r
 589     .vd_dvfs_target     = NULL,\r
 590 };\r
 591 \r
 592 #define LOOKUP_VD(_pvd, _regulator_name)        \\r
 593 {       \\r
 594         .vd                             = _pvd, \\r
 595         .regulator_name = _regulator_name,      \\r
 596 }\r
 597 static struct vd_node_lookup rk30_vds[] = {\r
 598     LOOKUP_VD(&vd_cpu, "cpu"),\r
 599     LOOKUP_VD(&vd_core, "core"),\r
 600     LOOKUP_VD(&vd_rtc, "rtc"),\r
 601 };\r
 602 \r
 603 static struct pd_node pd_a9_0 = {\r
 604     .name                       = "pd_a9_0",\r
 605     .vd                         = &vd_cpu,\r
 606 };\r
 607 static struct pd_node pd_a9_1 = {\r
 608     .name                       = "pd_a9_1",\r
 609     .vd                         = &vd_cpu,\r
 610 };\r
 611 static struct pd_node pd_debug = {\r
 612     .name                       = "pd_debug",\r
 613     .vd                         = &vd_cpu,\r
 614 };\r
 615 static struct pd_node pd_scu = {\r
 616     .name                       = "pd_scu",\r
 617     .vd                         = &vd_cpu,\r
 618 };\r
 619 static struct pd_node pd_video = {\r
 620     .name                       = "pd_video",\r
 621     .vd                         = &vd_core,\r
 622 };\r
 623 static struct pd_node pd_vio = {\r
 624     .name                       = "pd_vio",\r
 625     .vd                         = &vd_core,\r
 626 };\r
 627 static struct pd_node pd_gpu = {\r
 628     .name                       = "pd_gpu",\r
 629     .vd                         = &vd_core,\r
 630 };\r
 631 static struct pd_node pd_peri = {\r
 632     .name                       = "pd_peri",\r
 633     .vd                         = &vd_core,\r
 634 };\r
 635 static struct pd_node pd_cpu = {\r
 636     .name                       = "pd_cpu",\r
 637     .vd                         = &vd_core,\r
 638 };\r
 639 static struct pd_node pd_alive = {\r
 640     .name                       = "pd_alive",\r
 641     .vd                         = &vd_core,\r
 642 };\r
 643 static struct pd_node pd_rtc = {\r
 644     .name                       = "pd_rtc",\r
 645     .vd                         = &vd_rtc,\r
 646 };\r
 647 #define LOOKUP_PD(_ppd) \\r
 648 {       \\r
 649         .pd     = _ppd, \\r
 650 }\r
 651 static struct pd_node_lookup rk30_pds[] = {\r
 652     LOOKUP_PD(&pd_a9_0),\r
 653     LOOKUP_PD(&pd_a9_1),\r
 654     LOOKUP_PD(&pd_debug),\r
 655     LOOKUP_PD(&pd_scu),\r
 656     LOOKUP_PD(&pd_video),\r
 657     LOOKUP_PD(&pd_vio),\r
 658     LOOKUP_PD(&pd_gpu),\r
 659     LOOKUP_PD(&pd_peri),\r
 660     LOOKUP_PD(&pd_cpu),\r
 661     LOOKUP_PD(&pd_alive),\r
 662     LOOKUP_PD(&pd_rtc),\r
 663 };\r
 664 \r
 665 #define CLK_PDS(_ppd) \\r
 666 {       \\r
 667         .pd     = _ppd, \\r
 668 }\r
 669 \r
 670 static struct pds_list cpu_pds[] = {\r
 671     CLK_PDS(&pd_a9_0),\r
 672     CLK_PDS(&pd_a9_1),\r
 673     CLK_PDS(NULL),\r
 674 };\r
 675 static struct pds_list ddr_pds[] = {\r
 676     CLK_PDS(&pd_cpu),\r
 677     CLK_PDS(NULL),\r
 678 };\r
 679 static struct pds_list gpu_pds[] = {\r
 680     CLK_PDS(&pd_gpu),\r
 681     CLK_PDS(NULL),\r
 682 };\r
 683 \r
 684 #define RK_CLKS(_clk_name, _ppds, _dvfs_table, _dvfs_nb)        \\r
 685 {       \\r
 686         .name   = _clk_name,    \\r
 687         .pds            = _ppds,        \\r
 688         .dvfs_table = _dvfs_table,      \\r
 689         .dvfs_nb        = _dvfs_nb,     \\r
 690 }\r
 691 static struct clk_node rk30_clks[] = {\r
 692     RK_CLKS("cpu", cpu_pds, cpu_dvfs_table, &rk_dvfs_clk_notifier),\r
 693     RK_CLKS("ddr", ddr_pds, ddr_dvfs_table, &rk_dvfs_clk_notifier),\r
 694     RK_CLKS("gpu", gpu_pds, gpu_dvfs_table, &rk_dvfs_clk_notifier),\r
 695 };\r
 696 /**\r
 697  * first scale regulator volt\r
 698  */\r
 699 static int rk_dvfs_check_regulator_volt(void)\r
 700 {\r
 701     struct vd_node      *vd;\r
 702     struct pd_node      *pd;\r
 703     struct clk_list     *child;\r
 704     struct clk_node     *dvfs_clk;\r
 705     struct clk          *clk;\r
 706     struct cpufreq_frequency_table clk_fv;\r
 707     unsigned int vmax_pd = 0, vmax_vd = 0;\r
 708 \r
 709     list_for_each_entry(vd, &rk_dvfs_tree, node) {\r
 710         vmax_vd = 0;\r
 711         list_for_each_entry(pd, &vd->pd_list, node) {\r
 712             vmax_pd = 0;\r
 713             list_for_each_entry(child, &pd->clk_list, node) {\r
 714 \r
 715                 dvfs_clk = child->dvfs_clk;\r
 716                 clk = dvfs_clk_get(NULL, dvfs_clk->name);\r
 717                 if(!clk || IS_ERR(clk)) {\r
 718                     DVFS_ERR("%s get clk %s error\n", __func__, dvfs_clk->name);\r
 719                     continue;\r
 720                 }\r
 721                 //DVFS_DBG("%s get clk %s rate = %lu\n", __func__, clk->name, clk->rate);\r
 722                 dvfs_clk_get_volt(dvfs_clk, clk->rate, &clk_fv);\r
 723                 dvfs_clk->cur_volt = clk_fv.index;\r
 724                 dvfs_clk->cur_freq = clk_fv.frequency;\r
 725                 vmax_pd = max(vmax_pd, clk_fv.index);\r
 726                 pd->pd_status = (vmax_pd == 0) ? PD_OFF : PD_ON;\r
 727             }\r
 728             pd->cur_volt = vmax_pd;\r
 729             vmax_vd = max(vmax_vd, vmax_pd);\r
 730         }\r
 731 \r
 732         vd->cur_volt = vmax_vd;\r
 733         //DVFS_DBG("%s check error: %d, %d\n", vd->name, vd->cur_volt, dvfs_regulator_get_voltage(vd->regulator));\r
 734         //if (vd->cur_volt != dvfs_regulator_get_voltage(vd->regulator)) {\r
 735         //      DVFS_ERR("%s default voltage domain value error!\n", vd->name);\r
 736         //}\r
 737     }\r
 738     return 0;\r
 739 }\r
 740 \r
 741 static int rk_regist_vd(struct vd_node_lookup *vd_lookup)\r
 742 {\r
 743     struct vd_node *vd;\r
 744     if(!vd_lookup)\r
 745         return -1;\r
 746     vd = vd_lookup->vd;\r
 747     vd->regulator_name = vd_lookup->regulator_name;\r
 748 \r
 749     mutex_lock(&mutex);\r
 750 \r
 751     mutex_init(&vd->dvfs_mutex);\r
 752     list_add(&vd->node, &rk_dvfs_tree);\r
 753     INIT_LIST_HEAD(&vd->pd_list);\r
 754 \r
 755     mutex_unlock(&mutex);\r
 756 \r
 757     return 0;\r
 758 }\r
 759 static int rk_regist_pd(struct pd_node_lookup *pd_lookup)\r
 760 {\r
 761     struct vd_node      *vd;\r
 762     struct pd_node      *pd;\r
 763 \r
 764     mutex_lock(&mutex);\r
 765     pd = pd_lookup->pd;\r
 766 \r
 767     list_for_each_entry(vd, &rk_dvfs_tree, node) {\r
 768         if (vd == pd->vd) {\r
 769             list_add(&pd->node, &vd->pd_list);\r
 770             INIT_LIST_HEAD(&pd->clk_list);\r
 771             break;\r
 772         }\r
 773     }\r
 774     mutex_unlock(&mutex);\r
 775     return 0;\r
 776 }\r
 777 //extern int rk30_clk_notifier_register(struct clk *clk, struct notifier_block *nb);\r
 778 \r
 779 static int rk_regist_clk(struct clk_node *dvfs_clk)\r
 780 {\r
 781     struct pd_node      *pd;\r
 782     struct clk_list     *child;\r
 783     struct clk  *clk;\r
 784     int i = 0;\r
 785 \r
 786     if(!dvfs_clk)\r
 787         return -1;\r
 788 \r
 789     if(!dvfs_clk->pds)\r
 790         return -1;\r
 791 \r
 792     mutex_lock(&mutex);\r
 793     // set clk unsupport dvfs\r
 794     dvfs_clk->enable_dvfs = 0;\r
 795     dvfs_clk->vd = dvfs_clk->pds[0].pd->vd;\r
 796     for (i = 0; dvfs_clk->pds[i].pd != NULL; i++) {\r
 797         child = &(dvfs_clk->pds[i].clk_list);\r
 798         child->dvfs_clk = dvfs_clk;\r
 799         pd = dvfs_clk->pds[i].pd;\r
 800         list_add(&child->node, &pd->clk_list);\r
 801     }\r
 802     clk = dvfs_clk_get(NULL, dvfs_clk->name);\r
 803     clk_register_dvfs(dvfs_clk, clk);\r
 804     mutex_unlock(&mutex);\r
 805     return 0;\r
 806 }\r
 807 int rk30_dvfs_init(void)\r
 808 {\r
 809     int i = 0;\r
 810     for (i = 0; i < ARRAY_SIZE(rk30_vds); i++) {\r
 811         rk_regist_vd(&rk30_vds[i]);\r
 812     }\r
 813     for (i = 0; i < ARRAY_SIZE(rk30_pds); i++) {\r
 814         rk_regist_pd(&rk30_pds[i]);\r
 815     }\r
 816     for (i = 0; i < ARRAY_SIZE(rk30_clks); i++) {\r
 817         rk_regist_clk(&rk30_clks[i]);\r
 818     }\r
 819     dump_dbg_map();\r
 820     //DVFS_DBG("%s dvfs tree create finish!\n", __func__);\r
 821     //rk_dvfs_check_regulator_volt();\r
 822     return 0;\r
 823 }\r
 824 \r
 825 void dvfs_clk_set_rate_callback(struct clk *clk, clk_dvfs_target_callback clk_dvfs_target)\r
 826 {\r
 827     struct clk_node *dvfs_clk = clk_get_dvfs_info(clk);\r
 828     dvfs_clk->clk_dvfs_target = clk_dvfs_target;\r
 829 }\r
 830 //\r
 831 \r
 832 /*\r
 833  *cpufreq_frequency_table->index for cpufreq is index\r
 834  *cpufreq_frequency_table->index for dvfstable is volt\r
 835  */\r
 836 int cpufreq_dvfs_init(struct clk *clk, struct cpufreq_frequency_table **table, clk_dvfs_target_callback clk_dvfs_target)\r
 837 {\r
 838 \r
 839     struct cpufreq_frequency_table *freq_table;\r
 840     struct clk_node *info = clk_get_dvfs_info(clk);\r
 841     struct cpufreq_frequency_table *dvfs_table;//dvfs volt freq table\r
 842     int i = 0;\r
 843     DVFS_DBG("%s clk name %s\n", __func__, clk->name);\r
 844     if(!info) {\r
 845         return -1;\r
 846     }\r
 847     dvfs_table = info->dvfs_table;\r
 848 \r
 849     if(!dvfs_table) {\r
 850         return -1;\r
 851     }\r
 852 \r
 853     /********************************count table num****************************/\r
 854     i = 0;\r
 855     while(dvfs_table[i].frequency != FV_TABLE_END) {\r
 856         //DVFS_DBG("dvfs_table1 %lu\n",dvfs_table[i].frequency);\r
 857         i++;\r
 858     }\r
 859 \r
 860     freq_table = kzalloc(sizeof(struct cpufreq_frequency_table) * (i + 1), GFP_KERNEL);\r
 861     //last freq is end tab\r
 862     freq_table[i].index = i;\r
 863     freq_table[i].frequency = CPUFREQ_TABLE_END;\r
 864 \r
 865     //set freq table\r
 866     i = 0;\r
 867     while(dvfs_table[i].frequency != FV_TABLE_END) {\r
 868         freq_table[i].index = i;\r
 869         freq_table[i].frequency = dvfs_table[i].frequency;\r
 870         //DVFS_DBG("dvfs_table %d %lu\n",i,dvfs_table[i].frequency);\r
 871         i++;\r
 872     }\r
 873     *table = &freq_table[0];\r
 874     dvfs_clk_set_rate_callback(clk, clk_dvfs_target);\r
 875     return 0;\r
 876 }\r
 877 \r
 878 int clk_dvfs_set_dvfs_table(struct clk *clk, struct cpufreq_frequency_table *table)\r
 879 {\r
 880     struct clk_node *info = clk_get_dvfs_info(clk);\r
 881     if(!table || !info)\r
 882         return -1;\r
 883     info->dvfs_table = table;\r
 884     return 0;\r
 885 }\r
 886 /********************************simulation cases****************************/\r
 887 \r
 888 #ifdef DVFS_TEST_OFF_BOARD\r
 889 int rk30_dvfs_init_test(void)\r
 890 {\r
 891     struct clk *clk1;\r
 892     DVFS_DBG("********************************simulation cases****************************\n");\r
 893 #ifdef DEBUG_RK30_DVFS\r
 894     DVFS_DBG("\n\n");\r
 895     dump_dbg_map();\r
 896 #endif\r
 897     clk1 = dvfs_clk_get(NULL, "cpu");\r
 898     if (clk1) {\r
 899         dvfs_clk_set_rate(clk1, 1008000000);\r
 900         dump_dbg_map();\r
 901         dvfs_clk_set_rate(clk1, 816000000);\r
 902         dump_dbg_map();\r
 903         dvfs_clk_set_rate(clk1, 0);\r
 904         dump_dbg_map();\r
 905         dvfs_clk_set_rate(clk1, 1200000000);\r
 906         dump_dbg_map();\r
 907         dvfs_clk_set_rate(clk1, 1009000000);\r
 908         dump_dbg_map();\r
 909         dvfs_clk_set_rate(clk1, 1416000000);\r
 910         dump_dbg_map();\r
 911 \r
 912     } else {\r
 913         DVFS_DBG("\t\t%s:\t can not find clk cpu\n", __func__);\r
 914     }\r
 915 \r
 916     clk1 = dvfs_clk_get(NULL, "gpu");\r
 917     if (clk1) {\r
 918         dvfs_clk_set_rate(clk1, 120000000);\r
 919         dump_dbg_map();\r
 920         dvfs_clk_enable(clk1);\r
 921         dvfs_clk_disable(clk1);\r
 922         dump_dbg_map();\r
 923     } else {\r
 924         DVFS_DBG("\t\t%s:\t can not find clk gpu\n", __func__);\r
 925         dump_dbg_map();\r
 926     }\r
 927 \r
 928     clk1 = dvfs_clk_get(NULL, "arm_pll");\r
 929     if (clk1) {\r
 930         dvfs_clk_set_rate(clk1, 24000000);\r
 931         dump_dbg_map();\r
 932     } else {\r
 933         DVFS_DBG("\t\t%s:\t can not find clk arm_pll\n", __func__);\r
 934     }\r
 935 \r
 936     DVFS_DBG("********************************simulation cases end***************************\n");\r
 937 \r
 938     return 0;\r
 939 \r
 940 }\r
 941 #endif\r
 942 \r