target/linux/mediatek/patches/0033-cpufreq-mediatek-Add-MT8173-cpufreq-driver.patch

   1 From 2028cb37c941014f6a817d27a867ee1d37ccf2b6 Mon Sep 17 00:00:00 2001
   2 From: "pi-cheng.chen" <pi-cheng.chen@linaro.org>
   3 Date: Mon, 8 Jun 2015 20:29:21 +0800
   4 Subject: [PATCH 33/76] cpufreq: mediatek: Add MT8173 cpufreq driver
   5
   6 This patch implements MT8173 cpufreq driver.
   7
   8 Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
   9 ---
  10  drivers/cpufreq/Kconfig.arm      |    7 +
  11  drivers/cpufreq/Makefile         |    1 +
  12  drivers/cpufreq/mt8173-cpufreq.c |  550 ++++++++++++++++++++++++++++++++++++++
  13  3 files changed, 558 insertions(+)
  14  create mode 100644 drivers/cpufreq/mt8173-cpufreq.c
  15
  16 diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
  17 index 4f3dbc8..350752b 100644
  18 --- a/drivers/cpufreq/Kconfig.arm
  19 +++ b/drivers/cpufreq/Kconfig.arm
  20 @@ -141,6 +141,13 @@ config ARM_KIRKWOOD_CPUFREQ
  21           This adds the CPUFreq driver for Marvell Kirkwood
  22           SoCs.
  23
  24 +config ARM_MT8173_CPUFREQ
  25 +       bool "Mediatek MT8173 CPUFreq support"
  26 +       depends on ARCH_MEDIATEK && REGULATOR
  27 +       select PM_OPP
  28 +       help
  29 +         This adds the CPUFreq driver support for Mediatek MT8173 SoC.
  30 +
  31  config ARM_OMAP2PLUS_CPUFREQ
  32         bool "TI OMAP2+"
  33         depends on ARCH_OMAP2PLUS
  34 diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
  35 index cdce92a..97f9a9b 100644
  36 --- a/drivers/cpufreq/Makefile
  37 +++ b/drivers/cpufreq/Makefile
  38 @@ -63,6 +63,7 @@ obj-$(CONFIG_ARM_HISI_ACPU_CPUFREQ)   += hisi-acpu-cpufreq.o
  39  obj-$(CONFIG_ARM_IMX6Q_CPUFREQ)                += imx6q-cpufreq.o
  40  obj-$(CONFIG_ARM_INTEGRATOR)           += integrator-cpufreq.o
  41  obj-$(CONFIG_ARM_KIRKWOOD_CPUFREQ)     += kirkwood-cpufreq.o
  42 +obj-$(CONFIG_ARM_MT8173_CPUFREQ)       += mt8173-cpufreq.o
  43  obj-$(CONFIG_ARM_OMAP2PLUS_CPUFREQ)    += omap-cpufreq.o
  44  obj-$(CONFIG_ARM_PXA2xx_CPUFREQ)       += pxa2xx-cpufreq.o
  45  obj-$(CONFIG_PXA3xx)                   += pxa3xx-cpufreq.o
  46 diff --git a/drivers/cpufreq/mt8173-cpufreq.c b/drivers/cpufreq/mt8173-cpufreq.c
  47 new file mode 100644
  48 index 0000000..d539e7b
  49 --- /dev/null
  50 +++ b/drivers/cpufreq/mt8173-cpufreq.c
  51 @@ -0,0 +1,550 @@
  52 +/*
  53 + * Copyright (c) 2015 Linaro Ltd.
  54 + * Author: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
  55 + *
  56 + * This program is free software; you can redistribute it and/or modify
  57 + * it under the terms of the GNU General Public License version 2 as
  58 + * published by the Free Software Foundation.
  59 + *
  60 + * This program is distributed in the hope that it will be useful,
  61 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
  62 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  63 + * GNU General Public License for more details.
  64 + */
  65 +
  66 +#include <linux/clk.h>
  67 +#include <linux/cpu.h>
  68 +#include <linux/cpufreq.h>
  69 +#include <linux/cpumask.h>
  70 +#include <linux/module.h>
  71 +#include <linux/of.h>
  72 +#include <linux/platform_device.h>
  73 +#include <linux/pm_opp.h>
  74 +#include <linux/regulator/consumer.h>
  75 +#include <linux/slab.h>
  76 +
  77 +#define MIN_VOLT_SHIFT         (100000)
  78 +#define MAX_VOLT_SHIFT         (200000)
  79 +#define MAX_VOLT_LIMIT         (1150000)
  80 +#define VOLT_TOL               (10000)
  81 +
  82 +/*
  83 + * The struct mtk_cpu_dvfs_info holds necessary information for doing CPU DVFS
  84 + * on each CPU power/clock domain of Mediatek SoCs. Each CPU cluster in
  85 + * Mediatek SoCs has two voltage inputs, Vproc and Vsram. In some cases the two
  86 + * voltage inputs need to be controlled under a hardware limitation:
  87 + * 100mV < Vsram - Vproc < 200mV
  88 + *
  89 + * When scaling the clock frequency of a CPU clock domain, the clock source
  90 + * needs to be switched to another stable PLL clock temporarily until
  91 + * the original PLL becomes stable at target frequency.
  92 + */
  93 +struct mtk_cpu_dvfs_info {
  94 +       struct list_head node;
  95 +       cpumask_var_t cpus;
  96 +       struct cpufreq_frequency_table *freq_table;
  97 +       struct device *cpu_dev;
  98 +       struct regulator *proc_reg;
  99 +       struct regulator *sram_reg;
 100 +       struct clk *cpu_clk;
 101 +       struct clk *inter_clk;
 102 +       int intermediate_voltage;
 103 +       bool need_voltage_trace;
 104 +};
 105 +
 106 +static LIST_HEAD(cpu_dvfs_info_list);
 107 +
 108 +static inline struct mtk_cpu_dvfs_info *to_mtk_cpu_dvfs_info(
 109 +                       struct list_head *list)
 110 +{
 111 +       return list_entry(list, struct mtk_cpu_dvfs_info, node);
 112 +}
 113 +
 114 +static inline void mtk_cpu_dvfs_info_add(struct mtk_cpu_dvfs_info *info)
 115 +{
 116 +       list_add(&info->node, &cpu_dvfs_info_list);
 117 +}
 118 +
 119 +static struct mtk_cpu_dvfs_info *mtk_cpu_dvfs_info_get(int cpu)
 120 +{
 121 +       struct mtk_cpu_dvfs_info *info;
 122 +       struct list_head *list;
 123 +
 124 +       list_for_each(list, &cpu_dvfs_info_list) {
 125 +               info = to_mtk_cpu_dvfs_info(list);
 126 +
 127 +               if (cpumask_test_cpu(cpu, info->cpus))
 128 +                       return info;
 129 +       }
 130 +
 131 +       return NULL;
 132 +}
 133 +
 134 +static void mtk_cpu_dvfs_info_release(void)
 135 +{
 136 +       struct list_head *list, *tmp;
 137 +       struct mtk_cpu_dvfs_info *info;
 138 +
 139 +       list_for_each_safe(list, tmp, &cpu_dvfs_info_list) {
 140 +               info = to_mtk_cpu_dvfs_info(list);
 141 +
 142 +               dev_pm_opp_free_cpufreq_table(info->cpu_dev,
 143 +                                             &info->freq_table);
 144 +
 145 +               if (!IS_ERR(info->proc_reg))
 146 +                       regulator_put(info->proc_reg);
 147 +               if (!IS_ERR(info->sram_reg))
 148 +                       regulator_put(info->sram_reg);
 149 +               if (!IS_ERR(info->cpu_clk))
 150 +                       clk_put(info->cpu_clk);
 151 +               if (!IS_ERR(info->inter_clk))
 152 +                       clk_put(info->inter_clk);
 153 +
 154 +               of_free_opp_table(info->cpu_dev);
 155 +
 156 +               list_del(list);
 157 +               kfree(info);
 158 +       }
 159 +}
 160 +
 161 +#define MIN(a, b) ((a) < (b) ? (a) : (b))
 162 +#define MAX(a, b) ((a) > (b) ? (a) : (b))
 163 +
 164 +static int mtk_cpufreq_voltage_trace(struct mtk_cpu_dvfs_info *info,
 165 +                                    int new_vproc)
 166 +{
 167 +       struct regulator *proc_reg = info->proc_reg;
 168 +       struct regulator *sram_reg = info->sram_reg;
 169 +       int old_vproc, old_vsram, new_vsram, vsram, vproc, ret;
 170 +
 171 +       old_vproc = regulator_get_voltage(proc_reg);
 172 +       old_vsram = regulator_get_voltage(sram_reg);
 173 +       /* Vsram should not exceed the maximum allowed voltage of SoC. */
 174 +       new_vsram = min(new_vproc + MIN_VOLT_SHIFT, MAX_VOLT_LIMIT);
 175 +
 176 +       if (old_vproc < new_vproc) {
 177 +               /*
 178 +                * When scaling up voltages, Vsram and Vproc scale up step
 179 +                * by step. At each step, set Vsram to (Vproc + 200mV) first,
 180 +                * then set Vproc to (Vsram - 100mV).
 181 +                * Keep doing it until Vsram and Vproc hit target voltages.
 182 +                */
 183 +               do {
 184 +                       old_vsram = regulator_get_voltage(sram_reg);
 185 +                       old_vproc = regulator_get_voltage(proc_reg);
 186 +
 187 +                       vsram = MIN(new_vsram, old_vproc + MAX_VOLT_SHIFT);
 188 +
 189 +                       if (vsram + VOLT_TOL >= MAX_VOLT_LIMIT) {
 190 +                               vsram = MAX_VOLT_LIMIT;
 191 +
 192 +                               /*
 193 +                                * If the target Vsram hits the maximum voltage,
 194 +                                * try to set the exact voltage value first.
 195 +                                */
 196 +                               ret = regulator_set_voltage(sram_reg, vsram,
 197 +                                                           vsram);
 198 +                               if (ret)
 199 +                                       ret = regulator_set_voltage(sram_reg,
 200 +                                                       vsram - VOLT_TOL,
 201 +                                                       vsram);
 202 +
 203 +                               vproc = new_vproc;
 204 +                       } else {
 205 +                               ret = regulator_set_voltage(sram_reg, vsram,
 206 +                                                           vsram + VOLT_TOL);
 207 +
 208 +                               vproc = vsram - MIN_VOLT_SHIFT;
 209 +                       }
 210 +                       if (ret)
 211 +                               return ret;
 212 +
 213 +                       ret = regulator_set_voltage(proc_reg, vproc,
 214 +                                                   vproc + VOLT_TOL);
 215 +                       if (ret) {
 216 +                               regulator_set_voltage(sram_reg, old_vsram,
 217 +                                                     old_vsram);
 218 +                               return ret;
 219 +                       }
 220 +               } while (vproc < new_vproc || vsram < new_vsram);
 221 +       } else if (old_vproc > new_vproc) {
 222 +               /*
 223 +                * When scaling down voltages, Vsram and Vproc scale down step
 224 +                * by step. At each step, set Vproc to (Vsram - 200mV) first,
 225 +                * then set Vproc to (Vproc + 100mV).
 226 +                * Keep doing it until Vsram and Vproc hit target voltages.
 227 +                */
 228 +               do {
 229 +                       old_vproc = regulator_get_voltage(proc_reg);
 230 +                       old_vsram = regulator_get_voltage(sram_reg);
 231 +
 232 +                       vproc = MAX(new_vproc, old_vsram - MAX_VOLT_SHIFT);
 233 +                       ret = regulator_set_voltage(proc_reg, vproc,
 234 +                                                   vproc + VOLT_TOL);
 235 +                       if (ret)
 236 +                               return ret;
 237 +
 238 +                       if (vproc == new_vproc)
 239 +                               vsram = new_vsram;
 240 +                       else
 241 +                               vsram = MAX(new_vsram, vproc + MIN_VOLT_SHIFT);
 242 +
 243 +                       if (vsram + VOLT_TOL >= MAX_VOLT_LIMIT) {
 244 +                               vsram = MAX_VOLT_LIMIT;
 245 +
 246 +                               /*
 247 +                                * If the target Vsram hits the maximum voltage,
 248 +                                * try to set the exact voltage value first.
 249 +                                */
 250 +                               ret = regulator_set_voltage(sram_reg, vsram,
 251 +                                                           vsram);
 252 +                               if (ret)
 253 +                                       ret = regulator_set_voltage(sram_reg,
 254 +                                                       vsram - VOLT_TOL,
 255 +                                                       vsram);
 256 +                       } else {
 257 +                               ret = regulator_set_voltage(sram_reg, vsram,
 258 +                                                           vsram + VOLT_TOL);
 259 +                       }
 260 +
 261 +                       if (ret) {
 262 +                               regulator_set_voltage(proc_reg, old_vproc,
 263 +                                                     old_vproc);
 264 +                               return ret;
 265 +                       }
 266 +               } while (vproc > new_vproc + VOLT_TOL ||
 267 +                        vsram > new_vsram + VOLT_TOL);
 268 +       }
 269 +
 270 +       return 0;
 271 +}
 272 +
 273 +static int mtk_cpufreq_set_voltage(struct mtk_cpu_dvfs_info *info, int vproc)
 274 +{
 275 +       if (info->need_voltage_trace)
 276 +               return mtk_cpufreq_voltage_trace(info, vproc);
 277 +       else
 278 +               return regulator_set_voltage(info->proc_reg, vproc,
 279 +                                            vproc + VOLT_TOL);
 280 +}
 281 +
 282 +static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 283 +                                 unsigned int index)
 284 +{
 285 +       struct cpufreq_frequency_table *freq_table = policy->freq_table;
 286 +       struct clk *cpu_clk = policy->clk;
 287 +       struct clk *armpll = clk_get_parent(cpu_clk);
 288 +       struct mtk_cpu_dvfs_info *info = policy->driver_data;
 289 +       struct device *cpu_dev = info->cpu_dev;
 290 +       struct dev_pm_opp *opp;
 291 +       long freq_hz, old_freq_hz;
 292 +       int vproc, old_vproc, inter_vproc, target_vproc, ret;
 293 +
 294 +       inter_vproc = info->intermediate_voltage;
 295 +
 296 +       old_freq_hz = clk_get_rate(cpu_clk);
 297 +       old_vproc = regulator_get_voltage(info->proc_reg);
 298 +
 299 +       freq_hz = freq_table[index].frequency * 1000;
 300 +       rcu_read_lock();
 301 +       opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_hz);
 302 +       if (IS_ERR(opp)) {
 303 +               rcu_read_unlock();
 304 +               pr_err("cpu%d: failed to find OPP for %ld\n",
 305 +                      policy->cpu, freq_hz);
 306 +               return PTR_ERR(opp);
 307 +       }
 308 +       vproc = dev_pm_opp_get_voltage(opp);
 309 +       rcu_read_unlock();
 310 +
 311 +       /*
 312 +        * If the new voltage or the intermediate voltage is higher than the
 313 +        * current voltage, scale up voltage first.
 314 +        */
 315 +       target_vproc = (inter_vproc > vproc) ? inter_vproc : vproc;
 316 +       if (old_vproc < target_vproc) {
 317 +               ret = mtk_cpufreq_set_voltage(info, target_vproc);
 318 +               if (ret) {
 319 +                       pr_err("cpu%d: failed to scale up voltage!\n",
 320 +                              policy->cpu);
 321 +                       mtk_cpufreq_set_voltage(info, old_vproc);
 322 +                       return ret;
 323 +               }
 324 +       }
 325 +
 326 +       /* Reparent the CPU clock to intermediate clock. */
 327 +       ret = clk_set_parent(cpu_clk, info->inter_clk);
 328 +       if (ret) {
 329 +               pr_err("cpu%d: failed to re-parent cpu clock!\n",
 330 +                      policy->cpu);
 331 +               mtk_cpufreq_set_voltage(info, old_vproc);
 332 +               WARN_ON(1);
 333 +               return ret;
 334 +       }
 335 +
 336 +       /* Set the original PLL to target rate. */
 337 +       ret = clk_set_rate(armpll, freq_hz);
 338 +       if (ret) {
 339 +               pr_err("cpu%d: failed to scale cpu clock rate!\n",
 340 +                      policy->cpu);
 341 +               clk_set_parent(cpu_clk, armpll);
 342 +               mtk_cpufreq_set_voltage(info, old_vproc);
 343 +               return ret;
 344 +       }
 345 +
 346 +       /* Set parent of CPU clock back to the original PLL. */
 347 +       ret = clk_set_parent(cpu_clk, armpll);
 348 +       if (ret) {
 349 +               pr_err("cpu%d: failed to re-parent cpu clock!\n",
 350 +                      policy->cpu);
 351 +               mtk_cpufreq_set_voltage(info, inter_vproc);
 352 +               WARN_ON(1);
 353 +               return ret;
 354 +       }
 355 +
 356 +       /*
 357 +        * If the new voltage is lower than the intermediate voltage or the
 358 +        * original voltage, scale down to the new voltage.
 359 +        */
 360 +       if (vproc < inter_vproc || vproc < old_vproc) {
 361 +               ret = mtk_cpufreq_set_voltage(info, vproc);
 362 +               if (ret) {
 363 +                       pr_err("cpu%d: failed to scale down voltage!\n",
 364 +                              policy->cpu);
 365 +                       clk_set_parent(cpu_clk, info->inter_clk);
 366 +                       clk_set_rate(armpll, old_freq_hz);
 367 +                       clk_set_parent(cpu_clk, armpll);
 368 +                       return ret;
 369 +               }
 370 +       }
 371 +
 372 +       return 0;
 373 +}
 374 +
 375 +static int mtk_cpufreq_init(struct cpufreq_policy *policy)
 376 +{
 377 +       struct mtk_cpu_dvfs_info *info;
 378 +       int ret;
 379 +
 380 +       info = mtk_cpu_dvfs_info_get(policy->cpu);
 381 +       if (!info) {
 382 +               pr_err("%s: mtk cpu dvfs info for cpu%d is not initialized\n",
 383 +                      __func__, policy->cpu);
 384 +               return -ENODEV;
 385 +       }
 386 +
 387 +       ret = cpufreq_table_validate_and_show(policy, info->freq_table);
 388 +       if (ret) {
 389 +               pr_err("%s: invalid frequency table: %d\n", __func__, ret);
 390 +               return ret;
 391 +       }
 392 +
 393 +       cpumask_copy(policy->cpus, info->cpus);
 394 +       policy->driver_data = info;
 395 +       policy->clk = info->cpu_clk;
 396 +
 397 +       return 0;
 398 +}
 399 +
 400 +static struct cpufreq_driver mt8173_cpufreq_driver = {
 401 +       .flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK,
 402 +       .verify = cpufreq_generic_frequency_table_verify,
 403 +       .target_index = mtk_cpufreq_set_target,
 404 +       .get = cpufreq_generic_get,
 405 +       .init = mtk_cpufreq_init,
 406 +       .name = "mtk-cpufreq",
 407 +       .attr = cpufreq_generic_attr,
 408 +};
 409 +
 410 +static int mtk_cpu_dvfs_info_init(int cpu)
 411 +{
 412 +       struct device *cpu_dev;
 413 +       struct regulator *proc_reg = ERR_PTR(-ENODEV);
 414 +       struct regulator *sram_reg = ERR_PTR(-ENODEV);
 415 +       struct clk *cpu_clk = ERR_PTR(-ENODEV);
 416 +       struct clk *inter_clk = ERR_PTR(-ENODEV);
 417 +       struct mtk_cpu_dvfs_info *info;
 418 +       struct cpufreq_frequency_table *freq_table;
 419 +       struct dev_pm_opp *opp;
 420 +       unsigned long rate;
 421 +       int ret;
 422 +
 423 +       cpu_dev = get_cpu_device(cpu);
 424 +       if (!cpu_dev) {
 425 +               pr_err("failed to get cpu%d device\n", cpu);
 426 +               return -ENODEV;
 427 +       }
 428 +
 429 +       ret = of_init_opp_table(cpu_dev);
 430 +       if (ret) {
 431 +               pr_warn("no OPP table for cpu%d\n", cpu);
 432 +               return ret;
 433 +       }
 434 +
 435 +       cpu_clk = clk_get(cpu_dev, "cpu");
 436 +       if (IS_ERR(cpu_clk)) {
 437 +               if (PTR_ERR(cpu_clk) == -EPROBE_DEFER)
 438 +                       pr_warn("cpu clk for cpu%d not ready, retry.\n", cpu);
 439 +               else
 440 +                       pr_err("failed to get cpu clk for cpu%d\n", cpu);
 441 +
 442 +               ret = PTR_ERR(cpu_clk);
 443 +               goto out_free_opp_table;
 444 +       }
 445 +
 446 +       inter_clk = clk_get(cpu_dev, "intermediate");
 447 +       if (IS_ERR(inter_clk)) {
 448 +               if (PTR_ERR(inter_clk) == -EPROBE_DEFER)
 449 +                       pr_warn("intermediate clk for cpu%d not ready, retry.\n",
 450 +                               cpu);
 451 +               else
 452 +                       pr_err("failed to get intermediate clk for cpu%d\n",
 453 +                              cpu);
 454 +
 455 +               ret = PTR_ERR(cpu_clk);
 456 +               goto out_free_resources;
 457 +       }
 458 +
 459 +       proc_reg = regulator_get_exclusive(cpu_dev, "proc");
 460 +       if (IS_ERR(proc_reg)) {
 461 +               if (PTR_ERR(proc_reg) == -EPROBE_DEFER)
 462 +                       pr_warn("proc regulator for cpu%d not ready, retry.\n",
 463 +                               cpu);
 464 +               else
 465 +                       pr_err("failed to get proc regulator for cpu%d\n",
 466 +                              cpu);
 467 +
 468 +               ret = PTR_ERR(proc_reg);
 469 +               goto out_free_resources;
 470 +       }
 471 +
 472 +       /* Both presence and absence of sram regulator are valid cases. */
 473 +       sram_reg = regulator_get_exclusive(cpu_dev, "sram");
 474 +
 475 +       info = kzalloc(sizeof(*info), GFP_KERNEL);
 476 +       if (!info) {
 477 +               ret = -ENOMEM;
 478 +               goto out_free_resources;
 479 +       }
 480 +
 481 +       ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table);
 482 +       if (ret) {
 483 +               pr_err("failed to init cpufreq table for cpu%d: %d\n",
 484 +                      cpu, ret);
 485 +               goto out_free_mtk_cpu_dvfs_info;
 486 +       }
 487 +
 488 +       if (!alloc_cpumask_var(&info->cpus, GFP_KERNEL))
 489 +               goto out_free_cpufreq_table;
 490 +
 491 +       /* Search a safe voltage for intermediate frequency. */
 492 +       rate = clk_get_rate(inter_clk);
 493 +       rcu_read_lock();
 494 +       opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate);
 495 +       if (IS_ERR(opp)) {
 496 +               pr_err("failed to get intermediate opp for cpu%d\n", cpu);
 497 +               ret = PTR_ERR(opp);
 498 +               goto out_free_cpumask;
 499 +       }
 500 +       info->intermediate_voltage = dev_pm_opp_get_voltage(opp);
 501 +       rcu_read_unlock();
 502 +
 503 +       /* CPUs in the same cluster share a clock and power domain. */
 504 +       cpumask_copy(info->cpus, &cpu_topology[cpu].core_sibling);
 505 +
 506 +       info->cpu_dev = cpu_dev;
 507 +       info->proc_reg = proc_reg;
 508 +       info->sram_reg = IS_ERR(sram_reg) ? NULL : sram_reg;
 509 +       info->cpu_clk = cpu_clk;
 510 +       info->inter_clk = inter_clk;
 511 +       info->freq_table = freq_table;
 512 +
 513 +       /*
 514 +        * If SRAM regulator is present, software "voltage trace" is needed
 515 +        * for this CPU power domain.
 516 +        */
 517 +       info->need_voltage_trace = !IS_ERR(sram_reg);
 518 +
 519 +       mtk_cpu_dvfs_info_add(info);
 520 +
 521 +       return 0;
 522 +
 523 +out_free_cpumask:
 524 +       free_cpumask_var(info->cpus);
 525 +
 526 +out_free_cpufreq_table:
 527 +       dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
 528 +
 529 +out_free_mtk_cpu_dvfs_info:
 530 +       kfree(info);
 531 +
 532 +out_free_resources:
 533 +       if (!IS_ERR(proc_reg))
 534 +               regulator_put(proc_reg);
 535 +       if (!IS_ERR(sram_reg))
 536 +               regulator_put(sram_reg);
 537 +       if (!IS_ERR(cpu_clk))
 538 +               clk_put(cpu_clk);
 539 +       if (!IS_ERR(inter_clk))
 540 +               clk_put(inter_clk);
 541 +
 542 +out_free_opp_table:
 543 +       of_free_opp_table(cpu_dev);
 544 +
 545 +       return ret;
 546 +}
 547 +
 548 +static int mt8173_cpufreq_probe(struct platform_device *pdev)
 549 +{
 550 +       int cpu, ret;
 551 +
 552 +       for_each_possible_cpu(cpu) {
 553 +               /*
 554 +                * If the struct mtk_cpu_dvfs_info for the cpu power domain
 555 +                * is already initialized, skip this CPU.
 556 +                */
 557 +               if (!mtk_cpu_dvfs_info_get(cpu)) {
 558 +                       ret = mtk_cpu_dvfs_info_init(cpu);
 559 +                       if (ret) {
 560 +                               if (ret != -EPROBE_DEFER)
 561 +                                       pr_err("%s probe fail\n", __func__);
 562 +
 563 +                               mtk_cpu_dvfs_info_release();
 564 +                               return ret;
 565 +                       }
 566 +               }
 567 +       }
 568 +
 569 +       ret = cpufreq_register_driver(&mt8173_cpufreq_driver);
 570 +       if (ret) {
 571 +               pr_err("failed to register mtk cpufreq driver\n");
 572 +               mtk_cpu_dvfs_info_release();
 573 +       }
 574 +
 575 +       return ret;
 576 +}
 577 +
 578 +static struct platform_driver mt8173_cpufreq_platdrv = {
 579 +       .driver = {
 580 +               .name   = "mt8173-cpufreq",
 581 +       },
 582 +       .probe          = mt8173_cpufreq_probe,
 583 +};
 584 +module_platform_driver(mt8173_cpufreq_platdrv);
 585 +
 586 +static int mt8173_cpufreq_driver_init(void)
 587 +{
 588 +       struct platform_device *pdev;
 589 +
 590 +       if (!of_machine_is_compatible("mediatek,mt8173"))
 591 +               return -ENODEV;
 592 +
 593 +       pdev = platform_device_register_simple("mt8173-cpufreq", -1, NULL, 0);
 594 +       if (IS_ERR(pdev)) {
 595 +               pr_err("failed to register mtk-cpufreq platform device\n");
 596 +               return PTR_ERR(pdev);
 597 +       }
 598 +
 599 +       return 0;
 600 +}
 601 +module_init(mt8173_cpufreq_driver_init);
 602 --
 603 1.7.10.4
 604