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