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 };
  51
  52 struct rockchip_cpu_avs {
  53         unsigned int num_clusters;
  54         struct cluster_info *cluster;
  55         struct notifier_block cpufreq_notify;
  56         unsigned int check_done[MAX_CLUSTERS];
  57 };
  58
  59 #define notifier_to_avs(_n) container_of(_n, struct rockchip_cpu_avs, \
  60         cpufreq_notify)
  61
  62 static void rockchip_leakage_adjust_table(struct device *cpu_dev,
  63                                           struct cpufreq_frequency_table *table,
  64                                           struct cluster_info *cluster)
  65 {
  66         struct cpufreq_frequency_table *pos;
  67         struct dev_pm_opp *opp;
  68         unsigned long adjust_volt;
  69
  70         if (!cluster->offset_volt)
  71                 return;
  72
  73         cpufreq_for_each_valid_entry(pos, table) {
  74                 if (pos->frequency < cluster->min_freq)
  75                         continue;
  76
  77                 rcu_read_lock();
  78
  79                 opp = dev_pm_opp_find_freq_exact(cpu_dev, pos->frequency * 1000,
  80                                                  true);
  81                 if (IS_ERR(opp)) {
  82                         rcu_read_unlock();
  83                         continue;
  84                 }
  85
  86                 adjust_volt = dev_pm_opp_get_voltage(opp) +
  87                         cluster->offset_volt;
  88
  89                 rcu_read_unlock();
  90
  91                 if (adjust_volt < cluster->min_volt)
  92                         continue;
  93
  94                 dev_pm_opp_adjust_voltage(cpu_dev, pos->frequency * 1000,
  95                                           adjust_volt);
  96         }
  97 }
  98
  99 static int rockchip_cpu_avs_notifier(struct notifier_block *nb,
 100                                      unsigned long event, void *data)
 101 {
 102         struct rockchip_cpu_avs *avs = notifier_to_avs(nb);
 103         struct cpufreq_policy *policy = data;
 104         struct cluster_info *cluster;
 105         struct device *cpu_dev;
 106         unsigned long cur_freq;
 107         int i, id, cpu = policy->cpu;
 108         int ret;
 109
 110         if (event != CPUFREQ_START)
 111                 goto out;
 112
 113         id = topology_physical_package_id(cpu);
 114         if (id < 0 || id >= MAX_CLUSTERS) {
 115                 pr_err("cpu%d invalid cluster id\n", cpu);
 116                 goto out;
 117         }
 118
 119         cpu_dev = get_cpu_device(cpu);
 120         if (!cpu_dev) {
 121                 pr_err("cpu%d failed to get device\n", cpu);
 122                 goto out;
 123         }
 124
 125         if (avs->num_clusters == 0)
 126                 goto next;
 127
 128         for (i = 0; i < avs->num_clusters; i++) {
 129                 if (avs->cluster[i].id == id)
 130                         break;
 131         }
 132         if (i == avs->num_clusters)
 133                 goto next;
 134         else
 135                 cluster = &avs->cluster[i];
 136
 137         if (!policy->freq_table) {
 138                 pr_err("cpu%d freq table not found\n", cpu);
 139                 goto next;
 140         }
 141
 142         rockchip_leakage_adjust_table(cpu_dev, policy->freq_table, cluster);
 143
 144 next:
 145         if (avs->check_done[id] == 0) {
 146                 ret = dev_pm_opp_check_initial_rate(cpu_dev, &cur_freq);
 147                 if (!ret)
 148                         policy->cur = cur_freq / 1000;
 149                 avs->check_done[id] = 1;
 150         }
 151
 152 out:
 153
 154         return NOTIFY_OK;
 155 }
 156
 157 static int rockchip_get_leakage_volt_table(struct device_node *np,
 158                                            struct leakage_volt_table **table)
 159 {
 160         struct leakage_volt_table *volt_table;
 161         const struct property *prop;
 162         int count, i;
 163
 164         prop = of_find_property(np, "leakage-adjust-volt", NULL);
 165         if (!prop) {
 166                 pr_err("failed to find property leakage-adjust-volt\n");
 167                 return -EINVAL;
 168         }
 169         if (!prop->value) {
 170                 pr_err("property leakage-adjust-volt is NULL\n");
 171                 return -ENODATA;
 172         }
 173
 174         count = of_property_count_u32_elems(np, "leakage-adjust-volt");
 175         if (count < 0) {
 176                 pr_err("Invalid property (%d)\n", count);
 177                 return -EINVAL;
 178         }
 179         if (count % 3) {
 180                 pr_err("Invalid number of elements in property (%d)\n", count);
 181                 return -EINVAL;
 182         }
 183
 184         volt_table = kzalloc(sizeof(*volt_table) * (count / 3 + 1), GFP_KERNEL);
 185         if (!volt_table)
 186                 return -ENOMEM;
 187
 188         for (i = 0; i < count / 3; i++) {
 189                 of_property_read_u32_index(np, "leakage-adjust-volt", 3 * i,
 190                                            &volt_table[i].min);
 191                 of_property_read_u32_index(np, "leakage-adjust-volt", 3 * i + 1,
 192                                            &volt_table[i].max);
 193                 of_property_read_u32_index(np, "leakage-adjust-volt", 3 * i + 2,
 194                                            (u32 *)&volt_table[i].volt);
 195         }
 196         volt_table[i].min = 0;
 197         volt_table[i].max = 0;
 198         volt_table[i].volt = LEAKAGE_TABLE_END;
 199
 200         *table = volt_table;
 201
 202         return 0;
 203 }
 204
 205 static int rockchip_get_leakage(struct device_node *np, unsigned char *leakage)
 206 {
 207         struct nvmem_cell *cell;
 208         unsigned char *buf;
 209         size_t len;
 210
 211         cell = of_nvmem_cell_get(np, "cpu_leakage");
 212         if (IS_ERR(cell)) {
 213                 pr_err("avs failed to get cpu_leakage cell\n");
 214                 return PTR_ERR(cell);
 215         }
 216
 217         buf = (unsigned char *)nvmem_cell_read(cell, &len);
 218
 219         nvmem_cell_put(cell);
 220
 221         if (IS_ERR(buf))
 222                 return PTR_ERR(buf);
 223
 224         if (buf[0] == INVALID_VALUE)
 225                 return -EINVAL;
 226
 227         *leakage = buf[0];
 228         kfree(buf);
 229
 230         return 0;
 231 }
 232
 233 static int rockchip_get_offset_volt(unsigned char leakage,
 234                                     struct leakage_volt_table *table, int *volt)
 235 {
 236         int i, j = -1;
 237
 238         if (!table)
 239                 return -EINVAL;
 240
 241         for (i = 0; table[i].volt != LEAKAGE_TABLE_END; i++) {
 242                 if (leakage >= (unsigned char)table[i].min)
 243                         j = i;
 244         }
 245
 246         if (j == -1)
 247                 *volt = 0;
 248         else
 249                 *volt = table[j].volt;
 250
 251         return 0;
 252 }
 253
 254 static int rockchip_of_parse_cpu_avs(struct device_node *np,
 255                                      struct cluster_info *cluster)
 256 {
 257         int ret;
 258
 259         ret = of_property_read_u32(np, "cluster-id", &cluster->id);
 260         if (ret < 0) {
 261                 pr_err("prop cluster-id missing\n");
 262                 return -EINVAL;
 263         }
 264         if (cluster->id >= MAX_CLUSTERS) {
 265                 pr_err("prop cluster-id invalid\n");
 266                 return -EINVAL;
 267         }
 268
 269         ret = of_property_read_u32(np, "min-freq", &cluster->min_freq);
 270         if (ret < 0) {
 271                 pr_err("prop min_freq missing\n");
 272                 return -EINVAL;
 273         }
 274
 275         ret = of_property_read_u32(np, "min-volt", &cluster->min_volt);
 276         if (ret < 0) {
 277                 pr_err("prop min_volt missing\n");
 278                 return -EINVAL;
 279         }
 280
 281         ret = rockchip_get_leakage_volt_table(np, &cluster->table);
 282         if (ret) {
 283                 pr_err("prop leakage-adjust-volt invalid\n");
 284                 return -EINVAL;
 285         }
 286
 287         ret = rockchip_get_leakage(np, &cluster->leakage);
 288         if (ret) {
 289                 pr_err("get leakage invalid\n");
 290                 return -EINVAL;
 291         }
 292
 293         ret = rockchip_get_offset_volt(cluster->leakage, cluster->table,
 294                                        &cluster->offset_volt);
 295         if (ret) {
 296                 pr_err("get offset volt err\n");
 297                 return -EINVAL;
 298         }
 299
 300         pr_info("cluster%d leakage=%d adjust_volt=%d\n", cluster->id,
 301                 cluster->leakage, cluster->offset_volt);
 302
 303         return 0;
 304 }
 305
 306 static int rockchip_cpu_avs_probe(struct platform_device *pdev)
 307 {
 308         struct device *dev = &pdev->dev;
 309         struct device_node *np, *node;
 310         struct rockchip_cpu_avs *avs;
 311         int ret, num_clusters = 0, i = 0;
 312
 313         avs = devm_kzalloc(dev, sizeof(*avs), GFP_KERNEL);
 314         if (!avs)
 315                 return -ENOMEM;
 316
 317         np = of_find_node_by_name(NULL, "cpu-avs");
 318         if (!np) {
 319                 pr_info("unable to find cpu-avs node\n");
 320                 goto out;
 321         }
 322
 323         if (!of_device_is_available(np)) {
 324                 pr_info("cpu-avs node disabled\n");
 325                 goto next;
 326         }
 327
 328         for_each_available_child_of_node(np, node)
 329                 num_clusters++;
 330
 331         if (!num_clusters) {
 332                 pr_info("cpu-avs child node disabled\n");
 333                 goto next;
 334         }
 335
 336         avs->cluster = devm_kzalloc(dev, sizeof(*avs->cluster) * num_clusters,
 337                                     GFP_KERNEL);
 338         if (!avs->cluster)
 339                 goto next;
 340
 341         avs->num_clusters = num_clusters;
 342
 343         for_each_available_child_of_node(np, node) {
 344                 ret = rockchip_of_parse_cpu_avs(node, &avs->cluster[i++]);
 345                 if (ret) {
 346                         devm_kfree(dev, avs->cluster);
 347                         avs->num_clusters = 0;
 348                         break;
 349                 }
 350         }
 351
 352 next:
 353         of_node_put(np);
 354
 355 out:
 356         avs->cpufreq_notify.notifier_call = rockchip_cpu_avs_notifier;
 357
 358         return cpufreq_register_notifier(&avs->cpufreq_notify,
 359                 CPUFREQ_POLICY_NOTIFIER);
 360 }
 361
 362 static struct platform_driver rockchip_cpu_avs_platdrv = {
 363         .driver = {
 364                 .name   = "rockchip-cpu-avs",
 365         },
 366         .probe          = rockchip_cpu_avs_probe,
 367 };
 368
 369 static int __init rockchip_cpu_avs_init(void)
 370 {
 371         struct platform_device *pdev;
 372         int ret;
 373
 374         ret = platform_driver_register(&rockchip_cpu_avs_platdrv);
 375         if (ret)
 376                 return ret;
 377
 378         /*
 379          * Since there's no place to hold device registration code and no
 380          * device tree based way to match cpu-avs driver yet, both the driver
 381          * and the device registration codes are put here to handle defer
 382          * probing.
 383          */
 384         pdev = platform_device_register_simple("rockchip-cpu-avs", -1, NULL, 0);
 385         if (IS_ERR(pdev)) {
 386                 pr_err("failed to register rockchip-cpu-avs platform device\n");
 387                 return PTR_ERR(pdev);
 388         }
 389
 390         return 0;
 391 }
 392
 393 module_init(rockchip_cpu_avs_init);
 394
 395 MODULE_DESCRIPTION("Rockchip CPU AVS driver");
 396 MODULE_AUTHOR("Finley Xiao <finley.xiao@rock-chips.com>");
 397 MODULE_LICENSE("GPL v2");