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

   1 From 60f4e41b367bdb29530468c91c1e613b17a37755 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 055/102] cpufreq: mediatek: add driver
   5
   6 Signed-off-by: John Crispin <john@phrozen.org>
   7 ---
   8  drivers/cpufreq/Kconfig.arm      |    9 +
   9  drivers/cpufreq/Makefile         |    1 +
  10  drivers/cpufreq/mt7623-cpufreq.c |  389 ++++++++++++++++++++++++++++++++++++++
  11  3 files changed, 399 insertions(+)
  12  create mode 100644 drivers/cpufreq/mt7623-cpufreq.c
  13
  14 diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
  15 index b1f8a73..baf945e 100644
  16 --- a/drivers/cpufreq/Kconfig.arm
  17 +++ b/drivers/cpufreq/Kconfig.arm
  18 @@ -81,6 +81,15 @@ config ARM_KIRKWOOD_CPUFREQ
  19           This adds the CPUFreq driver for Marvell Kirkwood
  20           SoCs.
  21
  22 +config ARM_MT7623_CPUFREQ
  23 +       bool "Mediatek MT7623 CPUFreq support"
  24 +       depends on ARCH_MEDIATEK && REGULATOR
  25 +       depends on ARM || (ARM_CPU_TOPOLOGY && COMPILE_TEST)
  26 +       depends on !CPU_THERMAL || THERMAL=y
  27 +       select PM_OPP
  28 +       help
  29 +         This adds the CPUFreq driver support for Mediatek MT7623 SoC.
  30 +
  31  config ARM_MT8173_CPUFREQ
  32         bool "Mediatek MT8173 CPUFreq support"
  33         depends on ARCH_MEDIATEK && REGULATOR
  34 diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
  35 index c0af1a1..e198752 100644
  36 --- a/drivers/cpufreq/Makefile
  37 +++ b/drivers/cpufreq/Makefile
  38 @@ -57,6 +57,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_MT7623_CPUFREQ)       += mt7623-cpufreq.o
  43  obj-$(CONFIG_ARM_MT8173_CPUFREQ)       += mt8173-cpufreq.o
  44  obj-$(CONFIG_ARM_OMAP2PLUS_CPUFREQ)    += omap-cpufreq.o
  45  obj-$(CONFIG_ARM_PXA2xx_CPUFREQ)       += pxa2xx-cpufreq.o
  46 diff --git a/drivers/cpufreq/mt7623-cpufreq.c b/drivers/cpufreq/mt7623-cpufreq.c
  47 new file mode 100644
  48 index 0000000..8d154ce
  49 --- /dev/null
  50 +++ b/drivers/cpufreq/mt7623-cpufreq.c
  51 @@ -0,0 +1,389 @@
  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/cpu_cooling.h>
  69 +#include <linux/cpufreq.h>
  70 +#include <linux/cpumask.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 +#include <linux/thermal.h>
  77 +
  78 +#define VOLT_TOL               (10000)
  79 +
  80 +/*
  81 + * When scaling the clock frequency of a CPU clock domain, the clock source
  82 + * needs to be switched to another stable PLL clock temporarily until
  83 + * the original PLL becomes stable at target frequency.
  84 + */
  85 +struct mtk_cpu_dvfs_info {
  86 +       struct device *cpu_dev;
  87 +       struct regulator *proc_reg;
  88 +       struct clk *cpu_clk;
  89 +       struct clk *inter_clk;
  90 +       struct thermal_cooling_device *cdev;
  91 +       int intermediate_voltage;
  92 +};
  93 +
  94 +static int mtk_cpufreq_set_voltage(struct mtk_cpu_dvfs_info *info, int vproc)
  95 +{
  96 +       return regulator_set_voltage(info->proc_reg, vproc,
  97 +                                    vproc + VOLT_TOL);
  98 +}
  99 +
 100 +static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 101 +                                 unsigned int index)
 102 +{
 103 +       struct cpufreq_frequency_table *freq_table = policy->freq_table;
 104 +       struct clk *cpu_clk = policy->clk;
 105 +       struct clk *armpll = clk_get_parent(cpu_clk);
 106 +       struct mtk_cpu_dvfs_info *info = policy->driver_data;
 107 +       struct device *cpu_dev = info->cpu_dev;
 108 +       struct dev_pm_opp *opp;
 109 +       long freq_hz, old_freq_hz;
 110 +       int vproc, old_vproc, inter_vproc, target_vproc, ret;
 111 +
 112 +       inter_vproc = info->intermediate_voltage;
 113 +
 114 +       old_freq_hz = clk_get_rate(cpu_clk);
 115 +       old_vproc = regulator_get_voltage(info->proc_reg);
 116 +
 117 +       freq_hz = freq_table[index].frequency * 1000;
 118 +
 119 +       rcu_read_lock();
 120 +       opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_hz);
 121 +       if (IS_ERR(opp)) {
 122 +               rcu_read_unlock();
 123 +               pr_err("cpu%d: failed to find OPP for %ld\n",
 124 +                      policy->cpu, freq_hz);
 125 +               return PTR_ERR(opp);
 126 +       }
 127 +       vproc = dev_pm_opp_get_voltage(opp);
 128 +       rcu_read_unlock();
 129 +
 130 +       /*
 131 +        * If the new voltage or the intermediate voltage is higher than the
 132 +        * current voltage, scale up voltage first.
 133 +        */
 134 +       target_vproc = (inter_vproc > vproc) ? inter_vproc : vproc;
 135 +       if (old_vproc < target_vproc) {
 136 +               ret = mtk_cpufreq_set_voltage(info, target_vproc);
 137 +               if (ret) {
 138 +                       pr_err("cpu%d: failed to scale up voltage!\n",
 139 +                              policy->cpu);
 140 +                       mtk_cpufreq_set_voltage(info, old_vproc);
 141 +                       return ret;
 142 +               }
 143 +       }
 144 +
 145 +       /* Reparent the CPU clock to intermediate clock. */
 146 +       ret = clk_set_parent(cpu_clk, info->inter_clk);
 147 +       if (ret) {
 148 +               pr_err("cpu%d: failed to re-parent cpu clock!\n",
 149 +                      policy->cpu);
 150 +               mtk_cpufreq_set_voltage(info, old_vproc);
 151 +               WARN_ON(1);
 152 +               return ret;
 153 +       }
 154 +
 155 +       /* Set the original PLL to target rate. */
 156 +       ret = clk_set_rate(armpll, freq_hz);
 157 +       if (ret) {
 158 +               pr_err("cpu%d: failed to scale cpu clock rate!\n",
 159 +                      policy->cpu);
 160 +               clk_set_parent(cpu_clk, armpll);
 161 +               mtk_cpufreq_set_voltage(info, old_vproc);
 162 +               return ret;
 163 +       }
 164 +
 165 +       /* Set parent of CPU clock back to the original PLL. */
 166 +       ret = clk_set_parent(cpu_clk, armpll);
 167 +       if (ret) {
 168 +               pr_err("cpu%d: failed to re-parent cpu clock!\n",
 169 +                      policy->cpu);
 170 +               mtk_cpufreq_set_voltage(info, inter_vproc);
 171 +               WARN_ON(1);
 172 +               return ret;
 173 +       }
 174 +
 175 +       /*
 176 +        * If the new voltage is lower than the intermediate voltage or the
 177 +        * original voltage, scale down to the new voltage.
 178 +        */
 179 +       if (vproc < inter_vproc || vproc < old_vproc) {
 180 +               ret = mtk_cpufreq_set_voltage(info, vproc);
 181 +               if (ret) {
 182 +                       pr_err("cpu%d: failed to scale down voltage!\n",
 183 +                              policy->cpu);
 184 +                       clk_set_parent(cpu_clk, info->inter_clk);
 185 +                       clk_set_rate(armpll, old_freq_hz);
 186 +                       clk_set_parent(cpu_clk, armpll);
 187 +                       return ret;
 188 +               }
 189 +       }
 190 +
 191 +       return 0;
 192 +}
 193 +
 194 +static void mtk_cpufreq_ready(struct cpufreq_policy *policy)
 195 +{
 196 +       struct mtk_cpu_dvfs_info *info = policy->driver_data;
 197 +       struct device_node *np = of_node_get(info->cpu_dev->of_node);
 198 +
 199 +       if (WARN_ON(!np))
 200 +               return;
 201 +
 202 +       if (of_find_property(np, "#cooling-cells", NULL)) {
 203 +               info->cdev = of_cpufreq_cooling_register(np,
 204 +                                                        policy->related_cpus);
 205 +
 206 +               if (IS_ERR(info->cdev)) {
 207 +                       dev_err(info->cpu_dev,
 208 +                               "running cpufreq without cooling device: %ld\n",
 209 +                               PTR_ERR(info->cdev));
 210 +
 211 +                       info->cdev = NULL;
 212 +               }
 213 +       }
 214 +
 215 +       of_node_put(np);
 216 +}
 217 +
 218 +static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
 219 +{
 220 +       struct device *cpu_dev;
 221 +       struct regulator *proc_reg = ERR_PTR(-ENODEV);
 222 +       struct clk *cpu_clk = ERR_PTR(-ENODEV);
 223 +       struct clk *inter_clk = ERR_PTR(-ENODEV);
 224 +       struct dev_pm_opp *opp;
 225 +       unsigned long rate;
 226 +       int ret;
 227 +
 228 +       cpu_dev = get_cpu_device(cpu);
 229 +       if (!cpu_dev) {
 230 +               pr_err("failed to get cpu%d device\n", cpu);
 231 +               return -ENODEV;
 232 +       }
 233 +
 234 +       cpu_clk = clk_get(cpu_dev, "cpu");
 235 +       if (IS_ERR(cpu_clk)) {
 236 +               if (PTR_ERR(cpu_clk) == -EPROBE_DEFER)
 237 +                       pr_warn("cpu clk for cpu%d not ready, retry.\n", cpu);
 238 +               else
 239 +                       pr_err("failed to get cpu clk for cpu%d\n", cpu);
 240 +
 241 +               ret = PTR_ERR(cpu_clk);
 242 +               return ret;
 243 +       }
 244 +
 245 +       inter_clk = clk_get(cpu_dev, "intermediate");
 246 +       if (IS_ERR(inter_clk)) {
 247 +               if (PTR_ERR(inter_clk) == -EPROBE_DEFER)
 248 +                       pr_warn("intermediate clk for cpu%d not ready, retry.\n",
 249 +                               cpu);
 250 +               else
 251 +                       pr_err("failed to get intermediate clk for cpu%d\n",
 252 +                              cpu);
 253 +
 254 +               ret = PTR_ERR(inter_clk);
 255 +               goto out_free_resources;
 256 +       }
 257 +
 258 +       proc_reg = regulator_get_exclusive(cpu_dev, "proc");
 259 +       if (IS_ERR(proc_reg)) {
 260 +               if (PTR_ERR(proc_reg) == -EPROBE_DEFER)
 261 +                       pr_warn("proc regulator for cpu%d not ready, retry.\n",
 262 +                               cpu);
 263 +               else
 264 +                       pr_err("failed to get proc regulator for cpu%d\n",
 265 +                              cpu);
 266 +
 267 +               ret = PTR_ERR(proc_reg);
 268 +               goto out_free_resources;
 269 +       }
 270 +
 271 +       ret = dev_pm_opp_of_add_table(cpu_dev);
 272 +       if (ret) {
 273 +               pr_warn("no OPP table for cpu%d\n", cpu);
 274 +               goto out_free_resources;
 275 +       }
 276 +
 277 +       /* Search a safe voltage for intermediate frequency. */
 278 +       rate = clk_get_rate(inter_clk);
 279 +       rcu_read_lock();
 280 +       opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate);
 281 +       if (IS_ERR(opp)) {
 282 +               rcu_read_unlock();
 283 +               pr_err("failed to get intermediate opp for cpu%d\n", cpu);
 284 +               ret = PTR_ERR(opp);
 285 +               goto out_free_opp_table;
 286 +       }
 287 +       info->intermediate_voltage = dev_pm_opp_get_voltage(opp);
 288 +       rcu_read_unlock();
 289 +
 290 +       info->cpu_dev = cpu_dev;
 291 +       info->proc_reg = proc_reg;
 292 +       info->cpu_clk = cpu_clk;
 293 +       info->inter_clk = inter_clk;
 294 +
 295 +       return 0;
 296 +
 297 +out_free_opp_table:
 298 +       dev_pm_opp_of_remove_table(cpu_dev);
 299 +
 300 +out_free_resources:
 301 +       if (!IS_ERR(proc_reg))
 302 +               regulator_put(proc_reg);
 303 +       if (!IS_ERR(cpu_clk))
 304 +               clk_put(cpu_clk);
 305 +       if (!IS_ERR(inter_clk))
 306 +               clk_put(inter_clk);
 307 +
 308 +       return ret;
 309 +}
 310 +
 311 +static void mtk_cpu_dvfs_info_release(struct mtk_cpu_dvfs_info *info)
 312 +{
 313 +       if (!IS_ERR(info->proc_reg))
 314 +               regulator_put(info->proc_reg);
 315 +       if (!IS_ERR(info->cpu_clk))
 316 +               clk_put(info->cpu_clk);
 317 +       if (!IS_ERR(info->inter_clk))
 318 +               clk_put(info->inter_clk);
 319 +
 320 +       dev_pm_opp_of_remove_table(info->cpu_dev);
 321 +}
 322 +
 323 +static int mtk_cpufreq_init(struct cpufreq_policy *policy)
 324 +{
 325 +       struct mtk_cpu_dvfs_info *info;
 326 +       struct cpufreq_frequency_table *freq_table;
 327 +       int ret;
 328 +
 329 +       info = kzalloc(sizeof(*info), GFP_KERNEL);
 330 +       if (!info)
 331 +               return -ENOMEM;
 332 +
 333 +       ret = mtk_cpu_dvfs_info_init(info, policy->cpu);
 334 +       if (ret) {
 335 +               pr_err("%s failed to initialize dvfs info for cpu%d\n",
 336 +                      __func__, policy->cpu);
 337 +               goto out_free_dvfs_info;
 338 +       }
 339 +
 340 +       ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table);
 341 +       if (ret) {
 342 +               pr_err("failed to init cpufreq table for cpu%d: %d\n",
 343 +                      policy->cpu, ret);
 344 +               goto out_release_dvfs_info;
 345 +       }
 346 +
 347 +       ret = cpufreq_table_validate_and_show(policy, freq_table);
 348 +       if (ret) {
 349 +               pr_err("%s: invalid frequency table: %d\n", __func__, ret);
 350 +               goto out_free_cpufreq_table;
 351 +       }
 352 +
 353 +       /* CPUs in the same cluster share a clock and power domain. */
 354 +       cpumask_setall(policy->cpus);
 355 +       policy->driver_data = info;
 356 +       policy->clk = info->cpu_clk;
 357 +
 358 +       return 0;
 359 +
 360 +out_free_cpufreq_table:
 361 +       dev_pm_opp_free_cpufreq_table(info->cpu_dev, &freq_table);
 362 +
 363 +out_release_dvfs_info:
 364 +       mtk_cpu_dvfs_info_release(info);
 365 +
 366 +out_free_dvfs_info:
 367 +       kfree(info);
 368 +
 369 +       return ret;
 370 +}
 371 +
 372 +static int mtk_cpufreq_exit(struct cpufreq_policy *policy)
 373 +{
 374 +       struct mtk_cpu_dvfs_info *info = policy->driver_data;
 375 +
 376 +       cpufreq_cooling_unregister(info->cdev);
 377 +       dev_pm_opp_free_cpufreq_table(info->cpu_dev, &policy->freq_table);
 378 +       mtk_cpu_dvfs_info_release(info);
 379 +       kfree(info);
 380 +
 381 +       return 0;
 382 +}
 383 +
 384 +static struct cpufreq_driver mt7623_cpufreq_driver = {
 385 +       .flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK,
 386 +       .verify = cpufreq_generic_frequency_table_verify,
 387 +       .target_index = mtk_cpufreq_set_target,
 388 +       .get = cpufreq_generic_get,
 389 +       .init = mtk_cpufreq_init,
 390 +       .exit = mtk_cpufreq_exit,
 391 +       .ready = mtk_cpufreq_ready,
 392 +       .name = "mtk-cpufreq",
 393 +       .attr = cpufreq_generic_attr,
 394 +};
 395 +
 396 +static int mt7623_cpufreq_probe(struct platform_device *pdev)
 397 +{
 398 +       int ret;
 399 +
 400 +       ret = cpufreq_register_driver(&mt7623_cpufreq_driver);
 401 +       if (ret)
 402 +               pr_err("failed to register mtk cpufreq driver\n");
 403 +
 404 +       return ret;
 405 +}
 406 +
 407 +static struct platform_driver mt7623_cpufreq_platdrv = {
 408 +       .driver = {
 409 +               .name   = "mt7623-cpufreq",
 410 +       },
 411 +       .probe          = mt7623_cpufreq_probe,
 412 +};
 413 +
 414 +static int mt7623_cpufreq_driver_init(void)
 415 +{
 416 +       struct platform_device *pdev;
 417 +       int err;
 418 +
 419 +       if (!of_machine_is_compatible("mediatek,mt7623"))
 420 +               return -ENODEV;
 421 +
 422 +       err = platform_driver_register(&mt7623_cpufreq_platdrv);
 423 +       if (err)
 424 +               return err;
 425 +
 426 +       /*
 427 +        * Since there's no place to hold device registration code and no
 428 +        * device tree based way to match cpufreq driver yet, both the driver
 429 +        * and the device registration codes are put here to handle defer
 430 +        * probing.
 431 +        */
 432 +       pdev = platform_device_register_simple("mt7623-cpufreq", -1, NULL, 0);
 433 +       if (IS_ERR(pdev)) {
 434 +               pr_err("failed to register mtk-cpufreq platform device\n");
 435 +               return PTR_ERR(pdev);
 436 +       }
 437 +
 438 +       return 0;
 439 +}
 440 +device_initcall(mt7623_cpufreq_driver_init);
 441 --
 442 1.7.10.4
 443