drivers/power/avs/rockchip-cpu-avs.c

   1 /*
   2  * Rockchip CPU AVS support.
   3  *
   4  * Copyright (c) 2016 Rockchip Electronics Co. Ltd.
   5  * Author: Finley Xiao <finley.xiao@rock-chips.com>
   6  *
   7  * This program is free software; you can redistribute it and/or modify it
   8  * under the terms of version 2 of the GNU General Public License as
   9  * published by the Free Software Foundation.
  10  *
  11  * This program is distributed in the hope that it will be useful, but WITHOUT
  12  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  13  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  14  * more details.
  15  */
  16
  17 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  18
  19 #include <linux/cpu.h>
  20 #include <linux/cpufreq.h>
  21 #include <linux/cpumask.h>
  22 #include <linux/err.h>
  23 #include <linux/init.h>
  24 #include <linux/kernel.h>
  25 #include <linux/module.h>
  26 #include <linux/nvmem-consumer.h>
  27 #include <linux/of.h>
  28 #include <linux/platform_device.h>
  29 #include <linux/pm_opp.h>
  30 #include <linux/slab.h>
  31 #include <linux/types.h>
  32
  33 #define MAX_CLUSTERS            2
  34 #define LEAKAGE_TABLE_END       ~1
  35 #define INVALID_VALUE           0xff
  36
  37 struct leakage_volt_table {
  38         unsigned int min;
  39         unsigned int max;
  40         int volt;
  41 };
  42
  43 struct cluster_info {
  44         unsigned int id;
  45         int offset_volt;
  46         unsigned char leakage;
  47         unsigned int min_freq;
  48         unsigned int min_volt;
  49         struct leakage_volt_table *table;
  50         unsigned int adjust_done;
  51 };
  52
  53 struct rockchip_cpu_avs {
  54         unsigned int num_clusters;
  55         struct cluster_info *cluster;
  56         struct notifier_block cpufreq_notify;
  57         unsigned int check_done[MAX_CLUSTERS];
  58         struct cpumask allowed_cpus[MAX_CLUSTERS];
  59 };
  60
  61 #define notifier_to_avs(_n) container_of(_n, struct rockchip_cpu_avs, \
  62         cpufreq_notify)
  63
  64 static void rockchip_leakage_adjust_table(struct device *cpu_dev,
  65                                           struct cpufreq_frequency_table *table,
  66                                           struct cluster_info *cluster)
  67 {
  68         struct cpufreq_frequency_table *pos;
  69         struct dev_pm_opp *opp;
  70         unsigned long adjust_volt;
  71
  72         if (!cluster->offset_volt)
  73                 return;
  74
  75         cpufreq_for_each_valid_entry(pos, table) {
  76                 if (pos->frequency < cluster->min_freq)
  77                         continue;
  78
  79                 rcu_read_lock();
  80
  81                 opp = dev_pm_opp_find_freq_exact(cpu_dev, pos->frequency * 1000,
  82                                                  true);
  83                 if (IS_ERR(opp)) {
  84                         rcu_read_unlock();
  85                         continue;
  86                 }
  87
  88                 adjust_volt = dev_pm_opp_get_voltage(opp) +
  89                         cluster->offset_volt;
  90
  91                 rcu_read_unlock();
  92
  93                 if (adjust_volt < cluster->min_volt)
  94                         continue;
  95
  96                 dev_pm_opp_adjust_voltage(cpu_dev, pos->frequency * 1000,
  97                                           adjust_volt);
  98         }
  99 }
 100
 101 static int rockchip_cpu_avs_notifier(struct notifier_block *nb,
 102                                      unsigned long event, void *data)
 103 {
 104         struct rockchip_cpu_avs *avs = notifier_to_avs(nb);
 105         struct cpufreq_policy *policy = data;
 106         struct cluster_info *cluster;
 107         struct device *cpu_dev;
 108         unsigned long cur_freq;
 109         int i, id, cpu = policy->cpu;
 110         int ret;
 111
 112         if (event != CPUFREQ_CREATE_POLICY && event  != CPUFREQ_START)
 113                 goto out;
 114
 115         for (id = 0; id < MAX_CLUSTERS; id++) {
 116                 if (cpumask_test_cpu(cpu, &avs->allowed_cpus[id]))
 117                         break;
 118                 if (cpumask_empty(&avs->allowed_cpus[id])) {
 119                         cpumask_copy(&avs->allowed_cpus[id],
 120                                      policy->related_cpus);
 121                         break;
 122                 }
 123         }
 124
 125         if (id == MAX_CLUSTERS) {
 126                 pr_err("cpu%d invalid cluster id\n", cpu);
 127                 goto out;
 128         }
 129
 130         cpu_dev = get_cpu_device(cpu);
 131         if (!cpu_dev) {
 132                 pr_err("cpu%d failed to get device\n", cpu);
 133                 goto out;
 134         }
 135
 136         if (avs->num_clusters == 0)
 137                 goto next;
 138
 139         for (i = 0; i < avs->num_clusters; i++) {
 140                 if (avs->cluster[i].id == id)
 141                         break;
 142         }
 143         if (i == avs->num_clusters)
 144                 goto next;
 145         else
 146                 cluster = &avs->cluster[i];
 147
 148         if (!policy->freq_table) {
 149                 pr_err("cpu%d freq table not found\n", cpu);
 150                 goto next;
 151         }
 152
 153         /*
 154          * First time, CPUFREQ_CREATE_POLICY, adjust talbe
 155          * Second time, CPUFREQ_START, no need to adjust again
 156          * Later, cpu down and up, CPUFREQ_START, adjust table
 157          */
 158         if (event == CPUFREQ_CREATE_POLICY) {
 159                 rockchip_leakage_adjust_table(cpu_dev, policy->freq_table,
 160                                               cluster);
 161                 cluster->adjust_done = 1;
 162         } else if (event == CPUFREQ_START) {
 163                 if (cluster->adjust_done == 1)
 164                         cluster->adjust_done = 0;
 165                 else
 166                         rockchip_leakage_adjust_table(cpu_dev,
 167                                                       policy->freq_table,
 168                                                       cluster);
 169         }
 170
 171 next:
 172         if (avs->check_done[id] == 0) {
 173                 ret = dev_pm_opp_check_initial_rate(cpu_dev, &cur_freq);
 174                 if (!ret)
 175                         policy->cur = cur_freq / 1000;
 176                 avs->check_done[id] = 1;
 177         }
 178
 179 out:
 180
 181         return NOTIFY_OK;
 182 }
 183
 184 static int rockchip_get_leakage_volt_table(struct device_node *np,
 185                                            struct leakage_volt_table **table)
 186 {
 187         struct leakage_volt_table *volt_table;
 188         const struct property *prop;
 189         int count, i;
 190
 191         prop = of_find_property(np, "leakage-adjust-volt", NULL);
 192         if (!prop) {
 193                 pr_err("failed to find property leakage-adjust-volt\n");
 194                 return -EINVAL;
 195         }
 196         if (!prop->value) {
 197                 pr_err("property leakage-adjust-volt is NULL\n");
 198                 return -ENODATA;
 199         }
 200
 201         count = of_property_count_u32_elems(np, "leakage-adjust-volt");
 202         if (count < 0) {
 203                 pr_err("Invalid property (%d)\n", count);
 204                 return -EINVAL;
 205         }
 206         if (count % 3) {
 207                 pr_err("Invalid number of elements in property (%d)\n", count);
 208                 return -EINVAL;
 209         }
 210
 211         volt_table = kzalloc(sizeof(*volt_table) * (count / 3 + 1), GFP_KERNEL);
 212         if (!volt_table)
 213                 return -ENOMEM;
 214
 215         for (i = 0; i < count / 3; i++) {
 216                 of_property_read_u32_index(np, "leakage-adjust-volt", 3 * i,
 217                                            &volt_table[i].min);
 218                 of_property_read_u32_index(np, "leakage-adjust-volt", 3 * i + 1,
 219                                            &volt_table[i].max);
 220                 of_property_read_u32_index(np, "leakage-adjust-volt", 3 * i + 2,
 221                                            (u32 *)&volt_table[i].volt);
 222         }
 223         volt_table[i].min = 0;
 224         volt_table[i].max = 0;
 225         volt_table[i].volt = LEAKAGE_TABLE_END;
 226
 227         *table = volt_table;
 228
 229         return 0;
 230 }
 231
 232 static int rockchip_get_leakage(struct device_node *np, unsigned char *leakage)
 233 {
 234         struct nvmem_cell *cell;
 235         unsigned char *buf;
 236         size_t len;
 237
 238         cell = of_nvmem_cell_get(np, "cpu_leakage");
 239         if (IS_ERR(cell)) {
 240                 pr_err("avs failed to get cpu_leakage cell\n");
 241                 return PTR_ERR(cell);
 242         }
 243
 244         buf = (unsigned char *)nvmem_cell_read(cell, &len);
 245
 246         nvmem_cell_put(cell);
 247
 248         if (IS_ERR(buf))
 249                 return PTR_ERR(buf);
 250
 251         if (buf[0] == INVALID_VALUE)
 252                 return -EINVAL;
 253
 254         *leakage = buf[0];
 255         kfree(buf);
 256
 257         return 0;
 258 }
 259
 260 static int rockchip_get_offset_volt(unsigned char leakage,
 261                                     struct leakage_volt_table *table, int *volt)
 262 {
 263         int i, j = -1;
 264
 265         if (!table)
 266                 return -EINVAL;
 267
 268         for (i = 0; table[i].volt != LEAKAGE_TABLE_END; i++) {
 269                 if (leakage >= (unsigned char)table[i].min)
 270                         j = i;
 271         }
 272
 273         if (j == -1)
 274                 *volt = 0;
 275         else
 276                 *volt = table[j].volt;
 277
 278         return 0;
 279 }
 280
 281 static int rockchip_of_parse_cpu_avs(struct device_node *np,
 282                                      struct cluster_info *cluster)
 283 {
 284         int ret;
 285
 286         ret = of_property_read_u32(np, "cluster-id", &cluster->id);
 287         if (ret < 0) {
 288                 pr_err("prop cluster-id missing\n");
 289                 return -EINVAL;
 290         }
 291         if (cluster->id >= MAX_CLUSTERS) {
 292                 pr_err("prop cluster-id invalid\n");
 293                 return -EINVAL;
 294         }
 295
 296         ret = of_property_read_u32(np, "min-freq", &cluster->min_freq);
 297         if (ret < 0) {
 298                 pr_err("prop min_freq missing\n");
 299                 return -EINVAL;
 300         }
 301
 302         ret = of_property_read_u32(np, "min-volt", &cluster->min_volt);
 303         if (ret < 0) {
 304                 pr_err("prop min_volt missing\n");
 305                 return -EINVAL;
 306         }
 307
 308         ret = rockchip_get_leakage_volt_table(np, &cluster->table);
 309         if (ret) {
 310                 pr_err("prop leakage-adjust-volt invalid\n");
 311                 return -EINVAL;
 312         }
 313
 314         ret = rockchip_get_leakage(np, &cluster->leakage);
 315         if (ret) {
 316                 pr_err("get leakage invalid\n");
 317                 return -EINVAL;
 318         }
 319
 320         ret = rockchip_get_offset_volt(cluster->leakage, cluster->table,
 321                                        &cluster->offset_volt);
 322         if (ret) {
 323                 pr_err("get offset volt err\n");
 324                 return -EINVAL;
 325         }
 326
 327         pr_info("cluster%d leakage=%d adjust_volt=%d\n", cluster->id,
 328                 cluster->leakage, cluster->offset_volt);
 329
 330         return 0;
 331 }
 332
 333 static int rockchip_cpu_avs_probe(struct platform_device *pdev)
 334 {
 335         struct device *dev = &pdev->dev;
 336         struct device_node *np, *node;
 337         struct rockchip_cpu_avs *avs;
 338         int ret, num_clusters = 0, i = 0;
 339
 340         avs = devm_kzalloc(dev, sizeof(*avs), GFP_KERNEL);
 341         if (!avs)
 342                 return -ENOMEM;
 343
 344         np = of_find_node_by_name(NULL, "cpu-avs");
 345         if (!np) {
 346                 pr_info("unable to find cpu-avs node\n");
 347                 goto out;
 348         }
 349
 350         if (!of_device_is_available(np)) {
 351                 pr_info("cpu-avs node disabled\n");
 352                 goto next;
 353         }
 354
 355         for_each_available_child_of_node(np, node)
 356                 num_clusters++;
 357
 358         if (!num_clusters) {
 359                 pr_info("cpu-avs child node disabled\n");
 360                 goto next;
 361         }
 362
 363         avs->cluster = devm_kzalloc(dev, sizeof(*avs->cluster) * num_clusters,
 364                                     GFP_KERNEL);
 365         if (!avs->cluster)
 366                 goto next;
 367
 368         avs->num_clusters = num_clusters;
 369
 370         for_each_available_child_of_node(np, node) {
 371                 ret = rockchip_of_parse_cpu_avs(node, &avs->cluster[i++]);
 372                 if (ret) {
 373                         devm_kfree(dev, avs->cluster);
 374                         avs->num_clusters = 0;
 375                         break;
 376                 }
 377         }
 378
 379 next:
 380         of_node_put(np);
 381
 382 out:
 383         avs->cpufreq_notify.notifier_call = rockchip_cpu_avs_notifier;
 384
 385         return cpufreq_register_notifier(&avs->cpufreq_notify,
 386                 CPUFREQ_POLICY_NOTIFIER);
 387 }
 388
 389 static struct platform_driver rockchip_cpu_avs_platdrv = {
 390         .driver = {
 391                 .name   = "rockchip-cpu-avs",
 392         },
 393         .probe          = rockchip_cpu_avs_probe,
 394 };
 395
 396 static int __init rockchip_cpu_avs_init(void)
 397 {
 398         struct platform_device *pdev;
 399         int ret;
 400
 401         ret = platform_driver_register(&rockchip_cpu_avs_platdrv);
 402         if (ret)
 403                 return ret;
 404
 405         /*
 406          * Since there's no place to hold device registration code and no
 407          * device tree based way to match cpu-avs driver yet, both the driver
 408          * and the device registration codes are put here to handle defer
 409          * probing.
 410          */
 411         pdev = platform_device_register_simple("rockchip-cpu-avs", -1, NULL, 0);
 412         if (IS_ERR(pdev)) {
 413                 pr_err("failed to register rockchip-cpu-avs platform device\n");
 414                 return PTR_ERR(pdev);
 415         }
 416
 417         return 0;
 418 }
 419
 420 module_init(rockchip_cpu_avs_init);
 421
 422 MODULE_DESCRIPTION("Rockchip CPU AVS driver");
 423 MODULE_AUTHOR("Finley Xiao <finley.xiao@rock-chips.com>");
 424 MODULE_LICENSE("GPL v2");