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