target/linux/mediatek/patches-4.4/0077-cpufreq-mediatek-add-driver.patch

   1 From d1421147c328a7d06d9a6b8330c73e45139b1e48 Mon Sep 17 00:00:00 2001
   2 From: John Crispin <blogic@openwrt.org>
   3 Date: Wed, 30 Mar 2016 23:48:53 +0200
   4 Subject: [PATCH 77/78] cpufreq: mediatek: add driver
   5
   6 Signed-off-by: John Crispin <john@phrozen.org>
   7 ---
   8  arch/arm/boot/dts/mt7623-evb.dts |  160 ++++++++++++----
   9  arch/arm/boot/dts/mt7623.dtsi    |   50 ++++-
  10  drivers/cpufreq/Kconfig.arm      |    9 +
  11  drivers/cpufreq/Makefile         |    1 +
  12  drivers/cpufreq/mt7623-cpufreq.c |  389 ++++++++++++++++++++++++++++++++++++++
  13  5 files changed, 570 insertions(+), 39 deletions(-)
  14  create mode 100644 drivers/cpufreq/mt7623-cpufreq.c
  15
  16 diff --git a/arch/arm/boot/dts/mt7623-evb.dts b/arch/arm/boot/dts/mt7623-evb.dts
  17 index bc2b3f1..4a433f0 100644
  18 --- a/arch/arm/boot/dts/mt7623-evb.dts
  19 +++ b/arch/arm/boot/dts/mt7623-evb.dts
  20 @@ -39,6 +39,22 @@
  21         };
  22  };
  23
  24 +&cpu0 {
  25 +       proc-supply = <&mt6323_vproc_reg>;
  26 +};
  27 +
  28 +&cpu1 {
  29 +       proc-supply = <&mt6323_vproc_reg>;
  30 +};
  31 +
  32 +&cpu2 {
  33 +       proc-supply = <&mt6323_vproc_reg>;
  34 +};
  35 +
  36 +&cpu3 {
  37 +       proc-supply = <&mt6323_vproc_reg>;
  38 +};
  39 +
  40  &pwrap {
  41         pmic: mt6323 {
  42                 compatible = "mediatek,mt6323";
  43 @@ -267,38 +283,36 @@
  44         };
  45  };
  46
  47 -&uart2 {
  48 -       status = "okay";
  49 -};
  50 +&pio {
  51 +       nand_pins_default: nanddefault {
  52 +               pins_dat {
  53 +                       pinmux = <MT7623_PIN_111_MSDC0_DAT7_FUNC_NLD7>,
  54 +                                <MT7623_PIN_112_MSDC0_DAT6_FUNC_NLD6>,
  55 +                                <MT7623_PIN_114_MSDC0_DAT4_FUNC_NLD4>,
  56 +                                <MT7623_PIN_118_MSDC0_DAT3_FUNC_NLD3>,
  57 +                                <MT7623_PIN_121_MSDC0_DAT0_FUNC_NLD0>,
  58 +                                <MT7623_PIN_120_MSDC0_DAT1_FUNC_NLD1>,
  59 +                                <MT7623_PIN_113_MSDC0_DAT5_FUNC_NLD5>,
  60 +                                <MT7623_PIN_115_MSDC0_RSTB_FUNC_NLD8>,
  61 +                                <MT7623_PIN_119_MSDC0_DAT2_FUNC_NLD2>;
  62 +                       input-enable;
  63 +                       drive-strength = <MTK_DRIVE_8mA>;
  64 +                       bias-pull-up;
  65 +               };
  66
  67 -&mmc0 {
  68 -       status = "okay";
  69 -       pinctrl-names = "default", "state_uhs";
  70 -       pinctrl-0 = <&mmc0_pins_default>;
  71 -       pinctrl-1 = <&mmc0_pins_uhs>;
  72 -       bus-width = <8>;
  73 -       max-frequency = <50000000>;
  74 -       cap-mmc-highspeed;
  75 -       vmmc-supply = <&mt6323_vemc3v3_reg>;
  76 -       vqmmc-supply = <&mt6323_vio18_reg>;
  77 -       non-removable;
  78 -};
  79 +               pins_we {
  80 +                       pinmux = <MT7623_PIN_117_MSDC0_CLK_FUNC_NWEB>;
  81 +                       drive-strength = <MTK_DRIVE_8mA>;
  82 +                       bias-pull-up = <MTK_PUPD_SET_R1R0_10>;
  83 +               };
  84
  85 -&mmc1 {
  86 -       status = "okay";
  87 -       pinctrl-names = "default", "state_uhs";
  88 -       pinctrl-0 = <&mmc1_pins_default>;
  89 -       pinctrl-1 = <&mmc1_pins_uhs>;
  90 -       bus-width = <4>;
  91 -       max-frequency = <50000000>;
  92 -       cap-sd-highspeed;
  93 -       sd-uhs-sdr25;
  94 -//     cd-gpios = <&pio 132 0>;
  95 -       vmmc-supply = <&mt6323_vmch_reg>;
  96 -       vqmmc-supply = <&mt6323_vmc_reg>;
  97 -};
  98 +               pins_ale {
  99 +                       pinmux = <MT7623_PIN_116_MSDC0_CMD_FUNC_NALE>;
 100 +                       drive-strength = <MTK_DRIVE_8mA>;
 101 +                       bias-pull-down = <MTK_PUPD_SET_R1R0_10>;
 102 +               };
 103 +       };
 104
 105 -&pio {
 106         mmc0_pins_default: mmc0default {
 107                 pins_cmd_dat {
 108                         pinmux = <MT7623_PIN_111_MSDC0_DAT7_FUNC_MSDC0_DAT7>,
 109 @@ -370,11 +384,6 @@
 110                         bias-pull-down;
 111                         drive-strength = <MTK_DRIVE_4mA>;
 112                 };
 113 -
 114 -//             pins_insert {
 115 -//                     pinmux = <MT8173_PIN_132_I2S0_DATA1_FUNC_GPIO132>;
 116 -//                     bias-pull-up;
 117 -//             };
 118         };
 119
 120         mmc1_pins_uhs: mmc1 {
 121 @@ -422,6 +431,36 @@
 122         };
 123  };
 124
 125 +&uart2 {
 126 +       status = "okay";
 127 +};
 128 +
 129 +&mmc0 {
 130 +       status = "okay";
 131 +       pinctrl-names = "default", "state_uhs";
 132 +       pinctrl-0 = <&mmc0_pins_default>;
 133 +       pinctrl-1 = <&mmc0_pins_uhs>;
 134 +       bus-width = <8>;
 135 +       max-frequency = <50000000>;
 136 +       cap-mmc-highspeed;
 137 +       vmmc-supply = <&mt6323_vemc3v3_reg>;
 138 +       vqmmc-supply = <&mt6323_vio18_reg>;
 139 +       non-removable;
 140 +};
 141 +
 142 +&mmc1 {
 143 +       status = "okay";
 144 +       pinctrl-names = "default", "state_uhs";
 145 +       pinctrl-0 = <&mmc1_pins_default>;
 146 +       pinctrl-1 = <&mmc1_pins_uhs>;
 147 +       bus-width = <4>;
 148 +       max-frequency = <50000000>;
 149 +       cap-sd-highspeed;
 150 +       sd-uhs-sdr25;
 151 +       vmmc-supply = <&mt6323_vmch_reg>;
 152 +       vqmmc-supply = <&mt6323_vmc_reg>;
 153 +};
 154 +
 155  &usb1 {
 156         vusb33-supply = <&mt6323_vusb_reg>;
 157         vbus-supply = <&usb_p1_vbus>;
 158 @@ -456,3 +495,56 @@
 159         mediatek,reset-pin = <&pio 15 0>;
 160         status = "okay";
 161  };
 162 +
 163 +&nand {
 164 +       status = "okay";
 165 +       #address-cells = <1>;
 166 +       #size-cells = <1>;
 167 +
 168 +       pinctrl-names = "default";
 169 +       pinctrl-0 = <&nand_pins_default>;
 170 +
 171 +       partition@0 {
 172 +               label = "preloader";
 173 +               reg = <0x0 0x40000>;
 174 +               read-only;
 175 +       };
 176 +
 177 +       partition@1 {
 178 +               label = "u-boot";
 179 +               reg = <0x40000 0x80000>;
 180 +               read-only;
 181 +       };
 182 +
 183 +       partition@2 {
 184 +               label = "u-boot-env";
 185 +               reg = <0xc0000 0x40000>;
 186 +               read-only;
 187 +       };
 188 +
 189 +       partition@3 {
 190 +               label = "factory";
 191 +               reg = <0x100000 0x40000>;
 192 +               read-only;
 193 +       };
 194 +
 195 +       partition@4 {
 196 +               label = "kernel";
 197 +               reg = <0x140000 0x2000000>;
 198 +       };
 199 +
 200 +       partition@4 {
 201 +               label = "kernel2";
 202 +               reg = <0x2140000 0x2000000>;
 203 +       };
 204 +
 205 +       partition@5 {
 206 +               label = "rootfs";
 207 +               reg = <0x4140000 0x1000000>;
 208 +       };
 209 +
 210 +       partition@6 {
 211 +               label = "usrdata";
 212 +               reg = <0x5140000 0x9f80000>;
 213 +       };
 214 +};
 215 diff --git a/arch/arm/boot/dts/mt7623.dtsi b/arch/arm/boot/dts/mt7623.dtsi
 216 index f405ec7..76d603a 100644
 217 --- a/arch/arm/boot/dts/mt7623.dtsi
 218 +++ b/arch/arm/boot/dts/mt7623.dtsi
 219 @@ -31,25 +31,65 @@
 220                 #size-cells = <0>;
 221                 enable-method = "mediatek,mt6589-smp";
 222
 223 -               cpu@0 {
 224 +               cpu0: cpu@0 {
 225                         device_type = "cpu";
 226                         compatible = "arm,cortex-a7";
 227                         reg = <0x0>;
 228 +                       clocks = <&infracfg CLK_INFRA_CPUSEL>,
 229 +                                <&apmixedsys CLK_APMIXED_MAINPLL>;
 230 +                       clock-names = "cpu", "intermediate";
 231 +                       operating-points = <
 232 +                               598000 1150000
 233 +                               747500 1150000
 234 +                               1040000 1150000
 235 +                               1196000 1200000
 236 +                               1300000 1300000
 237 +                       >;
 238                 };
 239 -               cpu@1 {
 240 +               cpu1: cpu@1 {
 241                         device_type = "cpu";
 242                         compatible = "arm,cortex-a7";
 243                         reg = <0x1>;
 244 +                       clocks = <&infracfg CLK_INFRA_CPUSEL>,
 245 +                                <&apmixedsys CLK_APMIXED_MAINPLL>;
 246 +                       clock-names = "cpu", "intermediate";
 247 +                       operating-points = <
 248 +                               598000 1150000
 249 +                               747500 1150000
 250 +                               1040000 1150000
 251 +                               1196000 1200000
 252 +                               1300000 1300000
 253 +                       >;
 254                 };
 255 -               cpu@2 {
 256 +               cpu2: cpu@2 {
 257                         device_type = "cpu";
 258                         compatible = "arm,cortex-a7";
 259                         reg = <0x2>;
 260 +                       clocks = <&infracfg CLK_INFRA_CPUSEL>,
 261 +                                <&apmixedsys CLK_APMIXED_MAINPLL>;
 262 +                       clock-names = "cpu", "intermediate";
 263 +                       operating-points = <
 264 +                               598000 1150000
 265 +                               747500 1150000
 266 +                               1040000 1150000
 267 +                               1196000 1200000
 268 +                               1300000 1300000
 269 +                       >;
 270                 };
 271 -               cpu@3 {
 272 +               cpu3: cpu@3 {
 273                         device_type = "cpu";
 274                         compatible = "arm,cortex-a7";
 275                         reg = <0x3>;
 276 +                       clocks = <&infracfg CLK_INFRA_CPUSEL>,
 277 +                                <&apmixedsys CLK_APMIXED_MAINPLL>;
 278 +                       clock-names = "cpu", "intermediate";
 279 +                       operating-points = <
 280 +                               598000 1150000
 281 +                               747500 1150000
 282 +                               1040000 1150000
 283 +                               1196000 1200000
 284 +                               1300000 1300000
 285 +                       >;
 286                 };
 287         };
 288
 289 @@ -300,7 +340,7 @@
 290                 clocks = <&pericfg CLK_PERI_NFI>, <&pericfg CLK_PERI_NFI_ECC>,
 291                          <&pericfg CLK_PERI_NFI_PAD>;
 292                 clock-names = "nfi_clk", "nfiecc_clk", "pad_clk";
 293 -               nand-on-flash-bbt;
 294 +       //      nand-on-flash-bbt;
 295                 status = "disabled";
 296         };
 297
 298 diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
 299 index b1f8a73..baf945e 100644
 300 --- a/drivers/cpufreq/Kconfig.arm
 301 +++ b/drivers/cpufreq/Kconfig.arm
 302 @@ -81,6 +81,15 @@ config ARM_KIRKWOOD_CPUFREQ
 303           This adds the CPUFreq driver for Marvell Kirkwood
 304           SoCs.
 305
 306 +config ARM_MT7623_CPUFREQ
 307 +       bool "Mediatek MT7623 CPUFreq support"
 308 +       depends on ARCH_MEDIATEK && REGULATOR
 309 +       depends on ARM || (ARM_CPU_TOPOLOGY && COMPILE_TEST)
 310 +       depends on !CPU_THERMAL || THERMAL=y
 311 +       select PM_OPP
 312 +       help
 313 +         This adds the CPUFreq driver support for Mediatek MT7623 SoC.
 314 +
 315  config ARM_MT8173_CPUFREQ
 316         bool "Mediatek MT8173 CPUFreq support"
 317         depends on ARCH_MEDIATEK && REGULATOR
 318 diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
 319 index c0af1a1..e198752 100644
 320 --- a/drivers/cpufreq/Makefile
 321 +++ b/drivers/cpufreq/Makefile
 322 @@ -57,6 +57,7 @@ obj-$(CONFIG_ARM_HISI_ACPU_CPUFREQ)   += hisi-acpu-cpufreq.o
 323  obj-$(CONFIG_ARM_IMX6Q_CPUFREQ)                += imx6q-cpufreq.o
 324  obj-$(CONFIG_ARM_INTEGRATOR)           += integrator-cpufreq.o
 325  obj-$(CONFIG_ARM_KIRKWOOD_CPUFREQ)     += kirkwood-cpufreq.o
 326 +obj-$(CONFIG_ARM_MT7623_CPUFREQ)       += mt7623-cpufreq.o
 327  obj-$(CONFIG_ARM_MT8173_CPUFREQ)       += mt8173-cpufreq.o
 328  obj-$(CONFIG_ARM_OMAP2PLUS_CPUFREQ)    += omap-cpufreq.o
 329  obj-$(CONFIG_ARM_PXA2xx_CPUFREQ)       += pxa2xx-cpufreq.o
 330 diff --git a/drivers/cpufreq/mt7623-cpufreq.c b/drivers/cpufreq/mt7623-cpufreq.c
 331 new file mode 100644
 332 index 0000000..8d154ce
 333 --- /dev/null
 334 +++ b/drivers/cpufreq/mt7623-cpufreq.c
 335 @@ -0,0 +1,389 @@
 336 +/*
 337 + * Copyright (c) 2015 Linaro Ltd.
 338 + * Author: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
 339 + *
 340 + * This program is free software; you can redistribute it and/or modify
 341 + * it under the terms of the GNU General Public License version 2 as
 342 + * published by the Free Software Foundation.
 343 + *
 344 + * This program is distributed in the hope that it will be useful,
 345 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 346 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 347 + * GNU General Public License for more details.
 348 + */
 349 +
 350 +#include <linux/clk.h>
 351 +#include <linux/cpu.h>
 352 +#include <linux/cpu_cooling.h>
 353 +#include <linux/cpufreq.h>
 354 +#include <linux/cpumask.h>
 355 +#include <linux/of.h>
 356 +#include <linux/platform_device.h>
 357 +#include <linux/pm_opp.h>
 358 +#include <linux/regulator/consumer.h>
 359 +#include <linux/slab.h>
 360 +#include <linux/thermal.h>
 361 +
 362 +#define VOLT_TOL               (10000)
 363 +
 364 +/*
 365 + * When scaling the clock frequency of a CPU clock domain, the clock source
 366 + * needs to be switched to another stable PLL clock temporarily until
 367 + * the original PLL becomes stable at target frequency.
 368 + */
 369 +struct mtk_cpu_dvfs_info {
 370 +       struct device *cpu_dev;
 371 +       struct regulator *proc_reg;
 372 +       struct clk *cpu_clk;
 373 +       struct clk *inter_clk;
 374 +       struct thermal_cooling_device *cdev;
 375 +       int intermediate_voltage;
 376 +};
 377 +
 378 +static int mtk_cpufreq_set_voltage(struct mtk_cpu_dvfs_info *info, int vproc)
 379 +{
 380 +       return regulator_set_voltage(info->proc_reg, vproc,
 381 +                                    vproc + VOLT_TOL);
 382 +}
 383 +
 384 +static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 385 +                                 unsigned int index)
 386 +{
 387 +       struct cpufreq_frequency_table *freq_table = policy->freq_table;
 388 +       struct clk *cpu_clk = policy->clk;
 389 +       struct clk *armpll = clk_get_parent(cpu_clk);
 390 +       struct mtk_cpu_dvfs_info *info = policy->driver_data;
 391 +       struct device *cpu_dev = info->cpu_dev;
 392 +       struct dev_pm_opp *opp;
 393 +       long freq_hz, old_freq_hz;
 394 +       int vproc, old_vproc, inter_vproc, target_vproc, ret;
 395 +
 396 +       inter_vproc = info->intermediate_voltage;
 397 +
 398 +       old_freq_hz = clk_get_rate(cpu_clk);
 399 +       old_vproc = regulator_get_voltage(info->proc_reg);
 400 +
 401 +       freq_hz = freq_table[index].frequency * 1000;
 402 +
 403 +       rcu_read_lock();
 404 +       opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_hz);
 405 +       if (IS_ERR(opp)) {
 406 +               rcu_read_unlock();
 407 +               pr_err("cpu%d: failed to find OPP for %ld\n",
 408 +                      policy->cpu, freq_hz);
 409 +               return PTR_ERR(opp);
 410 +       }
 411 +       vproc = dev_pm_opp_get_voltage(opp);
 412 +       rcu_read_unlock();
 413 +
 414 +       /*
 415 +        * If the new voltage or the intermediate voltage is higher than the
 416 +        * current voltage, scale up voltage first.
 417 +        */
 418 +       target_vproc = (inter_vproc > vproc) ? inter_vproc : vproc;
 419 +       if (old_vproc < target_vproc) {
 420 +               ret = mtk_cpufreq_set_voltage(info, target_vproc);
 421 +               if (ret) {
 422 +                       pr_err("cpu%d: failed to scale up voltage!\n",
 423 +                              policy->cpu);
 424 +                       mtk_cpufreq_set_voltage(info, old_vproc);
 425 +                       return ret;
 426 +               }
 427 +       }
 428 +
 429 +       /* Reparent the CPU clock to intermediate clock. */
 430 +       ret = clk_set_parent(cpu_clk, info->inter_clk);
 431 +       if (ret) {
 432 +               pr_err("cpu%d: failed to re-parent cpu clock!\n",
 433 +                      policy->cpu);
 434 +               mtk_cpufreq_set_voltage(info, old_vproc);
 435 +               WARN_ON(1);
 436 +               return ret;
 437 +       }
 438 +
 439 +       /* Set the original PLL to target rate. */
 440 +       ret = clk_set_rate(armpll, freq_hz);
 441 +       if (ret) {
 442 +               pr_err("cpu%d: failed to scale cpu clock rate!\n",
 443 +                      policy->cpu);
 444 +               clk_set_parent(cpu_clk, armpll);
 445 +               mtk_cpufreq_set_voltage(info, old_vproc);
 446 +               return ret;
 447 +       }
 448 +
 449 +       /* Set parent of CPU clock back to the original PLL. */
 450 +       ret = clk_set_parent(cpu_clk, armpll);
 451 +       if (ret) {
 452 +               pr_err("cpu%d: failed to re-parent cpu clock!\n",
 453 +                      policy->cpu);
 454 +               mtk_cpufreq_set_voltage(info, inter_vproc);
 455 +               WARN_ON(1);
 456 +               return ret;
 457 +       }
 458 +
 459 +       /*
 460 +        * If the new voltage is lower than the intermediate voltage or the
 461 +        * original voltage, scale down to the new voltage.
 462 +        */
 463 +       if (vproc < inter_vproc || vproc < old_vproc) {
 464 +               ret = mtk_cpufreq_set_voltage(info, vproc);
 465 +               if (ret) {
 466 +                       pr_err("cpu%d: failed to scale down voltage!\n",
 467 +                              policy->cpu);
 468 +                       clk_set_parent(cpu_clk, info->inter_clk);
 469 +                       clk_set_rate(armpll, old_freq_hz);
 470 +                       clk_set_parent(cpu_clk, armpll);
 471 +                       return ret;
 472 +               }
 473 +       }
 474 +
 475 +       return 0;
 476 +}
 477 +
 478 +static void mtk_cpufreq_ready(struct cpufreq_policy *policy)
 479 +{
 480 +       struct mtk_cpu_dvfs_info *info = policy->driver_data;
 481 +       struct device_node *np = of_node_get(info->cpu_dev->of_node);
 482 +
 483 +       if (WARN_ON(!np))
 484 +               return;
 485 +
 486 +       if (of_find_property(np, "#cooling-cells", NULL)) {
 487 +               info->cdev = of_cpufreq_cooling_register(np,
 488 +                                                        policy->related_cpus);
 489 +
 490 +               if (IS_ERR(info->cdev)) {
 491 +                       dev_err(info->cpu_dev,
 492 +                               "running cpufreq without cooling device: %ld\n",
 493 +                               PTR_ERR(info->cdev));
 494 +
 495 +                       info->cdev = NULL;
 496 +               }
 497 +       }
 498 +
 499 +       of_node_put(np);
 500 +}
 501 +
 502 +static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
 503 +{
 504 +       struct device *cpu_dev;
 505 +       struct regulator *proc_reg = ERR_PTR(-ENODEV);
 506 +       struct clk *cpu_clk = ERR_PTR(-ENODEV);
 507 +       struct clk *inter_clk = ERR_PTR(-ENODEV);
 508 +       struct dev_pm_opp *opp;
 509 +       unsigned long rate;
 510 +       int ret;
 511 +
 512 +       cpu_dev = get_cpu_device(cpu);
 513 +       if (!cpu_dev) {
 514 +               pr_err("failed to get cpu%d device\n", cpu);
 515 +               return -ENODEV;
 516 +       }
 517 +
 518 +       cpu_clk = clk_get(cpu_dev, "cpu");
 519 +       if (IS_ERR(cpu_clk)) {
 520 +               if (PTR_ERR(cpu_clk) == -EPROBE_DEFER)
 521 +                       pr_warn("cpu clk for cpu%d not ready, retry.\n", cpu);
 522 +               else
 523 +                       pr_err("failed to get cpu clk for cpu%d\n", cpu);
 524 +
 525 +               ret = PTR_ERR(cpu_clk);
 526 +               return ret;
 527 +       }
 528 +
 529 +       inter_clk = clk_get(cpu_dev, "intermediate");
 530 +       if (IS_ERR(inter_clk)) {
 531 +               if (PTR_ERR(inter_clk) == -EPROBE_DEFER)
 532 +                       pr_warn("intermediate clk for cpu%d not ready, retry.\n",
 533 +                               cpu);
 534 +               else
 535 +                       pr_err("failed to get intermediate clk for cpu%d\n",
 536 +                              cpu);
 537 +
 538 +               ret = PTR_ERR(inter_clk);
 539 +               goto out_free_resources;
 540 +       }
 541 +
 542 +       proc_reg = regulator_get_exclusive(cpu_dev, "proc");
 543 +       if (IS_ERR(proc_reg)) {
 544 +               if (PTR_ERR(proc_reg) == -EPROBE_DEFER)
 545 +                       pr_warn("proc regulator for cpu%d not ready, retry.\n",
 546 +                               cpu);
 547 +               else
 548 +                       pr_err("failed to get proc regulator for cpu%d\n",
 549 +                              cpu);
 550 +
 551 +               ret = PTR_ERR(proc_reg);
 552 +               goto out_free_resources;
 553 +       }
 554 +
 555 +       ret = dev_pm_opp_of_add_table(cpu_dev);
 556 +       if (ret) {
 557 +               pr_warn("no OPP table for cpu%d\n", cpu);
 558 +               goto out_free_resources;
 559 +       }
 560 +
 561 +       /* Search a safe voltage for intermediate frequency. */
 562 +       rate = clk_get_rate(inter_clk);
 563 +       rcu_read_lock();
 564 +       opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate);
 565 +       if (IS_ERR(opp)) {
 566 +               rcu_read_unlock();
 567 +               pr_err("failed to get intermediate opp for cpu%d\n", cpu);
 568 +               ret = PTR_ERR(opp);
 569 +               goto out_free_opp_table;
 570 +       }
 571 +       info->intermediate_voltage = dev_pm_opp_get_voltage(opp);
 572 +       rcu_read_unlock();
 573 +
 574 +       info->cpu_dev = cpu_dev;
 575 +       info->proc_reg = proc_reg;
 576 +       info->cpu_clk = cpu_clk;
 577 +       info->inter_clk = inter_clk;
 578 +
 579 +       return 0;
 580 +
 581 +out_free_opp_table:
 582 +       dev_pm_opp_of_remove_table(cpu_dev);
 583 +
 584 +out_free_resources:
 585 +       if (!IS_ERR(proc_reg))
 586 +               regulator_put(proc_reg);
 587 +       if (!IS_ERR(cpu_clk))
 588 +               clk_put(cpu_clk);
 589 +       if (!IS_ERR(inter_clk))
 590 +               clk_put(inter_clk);
 591 +
 592 +       return ret;
 593 +}
 594 +
 595 +static void mtk_cpu_dvfs_info_release(struct mtk_cpu_dvfs_info *info)
 596 +{
 597 +       if (!IS_ERR(info->proc_reg))
 598 +               regulator_put(info->proc_reg);
 599 +       if (!IS_ERR(info->cpu_clk))
 600 +               clk_put(info->cpu_clk);
 601 +       if (!IS_ERR(info->inter_clk))
 602 +               clk_put(info->inter_clk);
 603 +
 604 +       dev_pm_opp_of_remove_table(info->cpu_dev);
 605 +}
 606 +
 607 +static int mtk_cpufreq_init(struct cpufreq_policy *policy)
 608 +{
 609 +       struct mtk_cpu_dvfs_info *info;
 610 +       struct cpufreq_frequency_table *freq_table;
 611 +       int ret;
 612 +
 613 +       info = kzalloc(sizeof(*info), GFP_KERNEL);
 614 +       if (!info)
 615 +               return -ENOMEM;
 616 +
 617 +       ret = mtk_cpu_dvfs_info_init(info, policy->cpu);
 618 +       if (ret) {
 619 +               pr_err("%s failed to initialize dvfs info for cpu%d\n",
 620 +                      __func__, policy->cpu);
 621 +               goto out_free_dvfs_info;
 622 +       }
 623 +
 624 +       ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table);
 625 +       if (ret) {
 626 +               pr_err("failed to init cpufreq table for cpu%d: %d\n",
 627 +                      policy->cpu, ret);
 628 +               goto out_release_dvfs_info;
 629 +       }
 630 +
 631 +       ret = cpufreq_table_validate_and_show(policy, freq_table);
 632 +       if (ret) {
 633 +               pr_err("%s: invalid frequency table: %d\n", __func__, ret);
 634 +               goto out_free_cpufreq_table;
 635 +       }
 636 +
 637 +       /* CPUs in the same cluster share a clock and power domain. */
 638 +       cpumask_setall(policy->cpus);
 639 +       policy->driver_data = info;
 640 +       policy->clk = info->cpu_clk;
 641 +
 642 +       return 0;
 643 +
 644 +out_free_cpufreq_table:
 645 +       dev_pm_opp_free_cpufreq_table(info->cpu_dev, &freq_table);
 646 +
 647 +out_release_dvfs_info:
 648 +       mtk_cpu_dvfs_info_release(info);
 649 +
 650 +out_free_dvfs_info:
 651 +       kfree(info);
 652 +
 653 +       return ret;
 654 +}
 655 +
 656 +static int mtk_cpufreq_exit(struct cpufreq_policy *policy)
 657 +{
 658 +       struct mtk_cpu_dvfs_info *info = policy->driver_data;
 659 +
 660 +       cpufreq_cooling_unregister(info->cdev);
 661 +       dev_pm_opp_free_cpufreq_table(info->cpu_dev, &policy->freq_table);
 662 +       mtk_cpu_dvfs_info_release(info);
 663 +       kfree(info);
 664 +
 665 +       return 0;
 666 +}
 667 +
 668 +static struct cpufreq_driver mt7623_cpufreq_driver = {
 669 +       .flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK,
 670 +       .verify = cpufreq_generic_frequency_table_verify,
 671 +       .target_index = mtk_cpufreq_set_target,
 672 +       .get = cpufreq_generic_get,
 673 +       .init = mtk_cpufreq_init,
 674 +       .exit = mtk_cpufreq_exit,
 675 +       .ready = mtk_cpufreq_ready,
 676 +       .name = "mtk-cpufreq",
 677 +       .attr = cpufreq_generic_attr,
 678 +};
 679 +
 680 +static int mt7623_cpufreq_probe(struct platform_device *pdev)
 681 +{
 682 +       int ret;
 683 +
 684 +       ret = cpufreq_register_driver(&mt7623_cpufreq_driver);
 685 +       if (ret)
 686 +               pr_err("failed to register mtk cpufreq driver\n");
 687 +
 688 +       return ret;
 689 +}
 690 +
 691 +static struct platform_driver mt7623_cpufreq_platdrv = {
 692 +       .driver = {
 693 +               .name   = "mt7623-cpufreq",
 694 +       },
 695 +       .probe          = mt7623_cpufreq_probe,
 696 +};
 697 +
 698 +static int mt7623_cpufreq_driver_init(void)
 699 +{
 700 +       struct platform_device *pdev;
 701 +       int err;
 702 +
 703 +       if (!of_machine_is_compatible("mediatek,mt7623"))
 704 +               return -ENODEV;
 705 +
 706 +       err = platform_driver_register(&mt7623_cpufreq_platdrv);
 707 +       if (err)
 708 +               return err;
 709 +
 710 +       /*
 711 +        * Since there's no place to hold device registration code and no
 712 +        * device tree based way to match cpufreq driver yet, both the driver
 713 +        * and the device registration codes are put here to handle defer
 714 +        * probing.
 715 +        */
 716 +       pdev = platform_device_register_simple("mt7623-cpufreq", -1, NULL, 0);
 717 +       if (IS_ERR(pdev)) {
 718 +               pr_err("failed to register mtk-cpufreq platform device\n");
 719 +               return PTR_ERR(pdev);
 720 +       }
 721 +
 722 +       return 0;
 723 +}
 724 +device_initcall(mt7623_cpufreq_driver_init);
 725 --
 726 1.7.10.4
 727