drivers/cpufreq/qcom-cpufreq-hw.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2018, The Linux Foundation. All rights reserved.
   4  */
   5
   6 #include <linux/bitfield.h>
   7 #include <linux/cpufreq.h>
   8 #include <linux/init.h>
   9 #include <linux/kernel.h>
  10 #include <linux/module.h>
  11 #include <linux/of_address.h>
  12 #include <linux/of_platform.h>
  13 #include <linux/slab.h>
  14
  15 #define LUT_MAX_ENTRIES                 40U
  16 #define LUT_SRC                         GENMASK(31, 30)
  17 #define LUT_L_VAL                       GENMASK(7, 0)
  18 #define LUT_CORE_COUNT                  GENMASK(18, 16)
  19 #define LUT_ROW_SIZE                    32
  20 #define CLK_HW_DIV                      2
  21
  22 /* Register offsets */
  23 #define REG_ENABLE                      0x0
  24 #define REG_LUT_TABLE                   0x110
  25 #define REG_PERF_STATE                  0x920
  26
  27 static unsigned long cpu_hw_rate, xo_rate;
  28 static struct platform_device *global_pdev;
  29
  30 static int qcom_cpufreq_hw_target_index(struct cpufreq_policy *policy,
  31                                         unsigned int index)
  32 {
  33         void __iomem *perf_state_reg = policy->driver_data;
  34
  35         writel_relaxed(index, perf_state_reg);
  36
  37         return 0;
  38 }
  39
  40 static unsigned int qcom_cpufreq_hw_get(unsigned int cpu)
  41 {
  42         void __iomem *perf_state_reg;
  43         struct cpufreq_policy *policy;
  44         unsigned int index;
  45
  46         policy = cpufreq_cpu_get_raw(cpu);
  47         if (!policy)
  48                 return 0;
  49
  50         perf_state_reg = policy->driver_data;
  51
  52         index = readl_relaxed(perf_state_reg);
  53         index = min(index, LUT_MAX_ENTRIES - 1);
  54
  55         return policy->freq_table[index].frequency;
  56 }
  57
  58 static unsigned int qcom_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,
  59                                                 unsigned int target_freq)
  60 {
  61         void __iomem *perf_state_reg = policy->driver_data;
  62         int index;
  63
  64         index = policy->cached_resolved_idx;
  65         if (index < 0)
  66                 return 0;
  67
  68         writel_relaxed(index, perf_state_reg);
  69
  70         return policy->freq_table[index].frequency;
  71 }
  72
  73 static int qcom_cpufreq_hw_read_lut(struct device *dev,
  74                                     struct cpufreq_policy *policy,
  75                                     void __iomem *base)
  76 {
  77         u32 data, src, lval, i, core_count, prev_cc = 0, prev_freq = 0, freq;
  78         unsigned int max_cores = cpumask_weight(policy->cpus);
  79         struct cpufreq_frequency_table  *table;
  80
  81         table = kcalloc(LUT_MAX_ENTRIES + 1, sizeof(*table), GFP_KERNEL);
  82         if (!table)
  83                 return -ENOMEM;
  84
  85         for (i = 0; i < LUT_MAX_ENTRIES; i++) {
  86                 data = readl_relaxed(base + REG_LUT_TABLE + i * LUT_ROW_SIZE);
  87                 src = FIELD_GET(LUT_SRC, data);
  88                 lval = FIELD_GET(LUT_L_VAL, data);
  89                 core_count = FIELD_GET(LUT_CORE_COUNT, data);
  90
  91                 if (src)
  92                         freq = xo_rate * lval / 1000;
  93                 else
  94                         freq = cpu_hw_rate / 1000;
  95
  96                 /* Ignore boosts in the middle of the table */
  97                 if (core_count != max_cores) {
  98                         table[i].frequency = CPUFREQ_ENTRY_INVALID;
  99                 } else {
 100                         table[i].frequency = freq;
 101                         dev_dbg(dev, "index=%d freq=%d, core_count %d\n", i,
 102                                 freq, core_count);
 103                 }
 104
 105                 /*
 106                  * Two of the same frequencies with the same core counts means
 107                  * end of table
 108                  */
 109                 if (i > 0 && prev_freq == freq && prev_cc == core_count) {
 110                         struct cpufreq_frequency_table *prev = &table[i - 1];
 111
 112                         /*
 113                          * Only treat the last frequency that might be a boost
 114                          * as the boost frequency
 115                          */
 116                         if (prev_cc != max_cores) {
 117                                 prev->frequency = prev_freq;
 118                                 prev->flags = CPUFREQ_BOOST_FREQ;
 119                         }
 120
 121                         break;
 122                 }
 123
 124                 prev_cc = core_count;
 125                 prev_freq = freq;
 126         }
 127
 128         table[i].frequency = CPUFREQ_TABLE_END;
 129         policy->freq_table = table;
 130
 131         return 0;
 132 }
 133
 134 static void qcom_get_related_cpus(int index, struct cpumask *m)
 135 {
 136         struct device_node *cpu_np;
 137         struct of_phandle_args args;
 138         int cpu, ret;
 139
 140         for_each_possible_cpu(cpu) {
 141                 cpu_np = of_cpu_device_node_get(cpu);
 142                 if (!cpu_np)
 143                         continue;
 144
 145                 ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain",
 146                                                  "#freq-domain-cells", 0,
 147                                                  &args);
 148                 of_node_put(cpu_np);
 149                 if (ret < 0)
 150                         continue;
 151
 152                 if (index == args.args[0])
 153                         cpumask_set_cpu(cpu, m);
 154         }
 155 }
 156
 157 static int qcom_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
 158 {
 159         struct device *dev = &global_pdev->dev;
 160         struct of_phandle_args args;
 161         struct device_node *cpu_np;
 162         struct resource *res;
 163         void __iomem *base;
 164         int ret, index;
 165
 166         cpu_np = of_cpu_device_node_get(policy->cpu);
 167         if (!cpu_np)
 168                 return -EINVAL;
 169
 170         ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain",
 171                                          "#freq-domain-cells", 0, &args);
 172         of_node_put(cpu_np);
 173         if (ret)
 174                 return ret;
 175
 176         index = args.args[0];
 177
 178         res = platform_get_resource(global_pdev, IORESOURCE_MEM, index);
 179         if (!res)
 180                 return -ENODEV;
 181
 182         base = devm_ioremap(dev, res->start, resource_size(res));
 183         if (!base)
 184                 return -ENOMEM;
 185
 186         /* HW should be in enabled state to proceed */
 187         if (!(readl_relaxed(base + REG_ENABLE) & 0x1)) {
 188                 dev_err(dev, "Domain-%d cpufreq hardware not enabled\n", index);
 189                 ret = -ENODEV;
 190                 goto error;
 191         }
 192
 193         qcom_get_related_cpus(index, policy->cpus);
 194         if (!cpumask_weight(policy->cpus)) {
 195                 dev_err(dev, "Domain-%d failed to get related CPUs\n", index);
 196                 ret = -ENOENT;
 197                 goto error;
 198         }
 199
 200         policy->driver_data = base + REG_PERF_STATE;
 201
 202         ret = qcom_cpufreq_hw_read_lut(dev, policy, base);
 203         if (ret) {
 204                 dev_err(dev, "Domain-%d failed to read LUT\n", index);
 205                 goto error;
 206         }
 207
 208         policy->fast_switch_possible = true;
 209
 210         return 0;
 211 error:
 212         devm_iounmap(dev, base);
 213         return ret;
 214 }
 215
 216 static int qcom_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)
 217 {
 218         void __iomem *base = policy->driver_data - REG_PERF_STATE;
 219
 220         kfree(policy->freq_table);
 221         devm_iounmap(&global_pdev->dev, base);
 222
 223         return 0;
 224 }
 225
 226 static struct freq_attr *qcom_cpufreq_hw_attr[] = {
 227         &cpufreq_freq_attr_scaling_available_freqs,
 228         &cpufreq_freq_attr_scaling_boost_freqs,
 229         NULL
 230 };
 231
 232 static struct cpufreq_driver cpufreq_qcom_hw_driver = {
 233         .flags          = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK |
 234                           CPUFREQ_HAVE_GOVERNOR_PER_POLICY,
 235         .verify         = cpufreq_generic_frequency_table_verify,
 236         .target_index   = qcom_cpufreq_hw_target_index,
 237         .get            = qcom_cpufreq_hw_get,
 238         .init           = qcom_cpufreq_hw_cpu_init,
 239         .exit           = qcom_cpufreq_hw_cpu_exit,
 240         .fast_switch    = qcom_cpufreq_hw_fast_switch,
 241         .name           = "qcom-cpufreq-hw",
 242         .attr           = qcom_cpufreq_hw_attr,
 243 };
 244
 245 static int qcom_cpufreq_hw_driver_probe(struct platform_device *pdev)
 246 {
 247         struct clk *clk;
 248         int ret;
 249
 250         clk = clk_get(&pdev->dev, "xo");
 251         if (IS_ERR(clk))
 252                 return PTR_ERR(clk);
 253
 254         xo_rate = clk_get_rate(clk);
 255         clk_put(clk);
 256
 257         clk = clk_get(&pdev->dev, "alternate");
 258         if (IS_ERR(clk))
 259                 return PTR_ERR(clk);
 260
 261         cpu_hw_rate = clk_get_rate(clk) / CLK_HW_DIV;
 262         clk_put(clk);
 263
 264         global_pdev = pdev;
 265
 266         ret = cpufreq_register_driver(&cpufreq_qcom_hw_driver);
 267         if (ret)
 268                 dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n");
 269         else
 270                 dev_dbg(&pdev->dev, "QCOM CPUFreq HW driver initialized\n");
 271
 272         return ret;
 273 }
 274
 275 static int qcom_cpufreq_hw_driver_remove(struct platform_device *pdev)
 276 {
 277         return cpufreq_unregister_driver(&cpufreq_qcom_hw_driver);
 278 }
 279
 280 static const struct of_device_id qcom_cpufreq_hw_match[] = {
 281         { .compatible = "qcom,cpufreq-hw" },
 282         {}
 283 };
 284 MODULE_DEVICE_TABLE(of, qcom_cpufreq_hw_match);
 285
 286 static struct platform_driver qcom_cpufreq_hw_driver = {
 287         .probe = qcom_cpufreq_hw_driver_probe,
 288         .remove = qcom_cpufreq_hw_driver_remove,
 289         .driver = {
 290                 .name = "qcom-cpufreq-hw",
 291                 .of_match_table = qcom_cpufreq_hw_match,
 292         },
 293 };
 294
 295 static int __init qcom_cpufreq_hw_init(void)
 296 {
 297         return platform_driver_register(&qcom_cpufreq_hw_driver);
 298 }
 299 subsys_initcall(qcom_cpufreq_hw_init);
 300
 301 static void __exit qcom_cpufreq_hw_exit(void)
 302 {
 303         platform_driver_unregister(&qcom_cpufreq_hw_driver);
 304 }
 305 module_exit(qcom_cpufreq_hw_exit);
 306
 307 MODULE_DESCRIPTION("QCOM CPUFREQ HW Driver");
 308 MODULE_LICENSE("GPL v2");