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/interconnect.h> 10 #include <linux/kernel.h> 11 #include <linux/module.h> 12 #include <linux/of_address.h> 13 #include <linux/of_platform.h> 14 #include <linux/pm_opp.h> 15 #include <linux/slab.h> 16 17 #define LUT_MAX_ENTRIES 40U 18 #define LUT_SRC GENMASK(31, 30) 19 #define LUT_L_VAL GENMASK(7, 0) 20 #define LUT_CORE_COUNT GENMASK(18, 16) 21 #define LUT_VOLT GENMASK(11, 0) 22 #define LUT_ROW_SIZE 32 23 #define CLK_HW_DIV 2 24 #define LUT_TURBO_IND 1 25 26 /* Register offsets */ 27 #define REG_ENABLE 0x0 28 #define REG_FREQ_LUT 0x110 29 #define REG_VOLT_LUT 0x114 30 #define REG_PERF_STATE 0x920 31 32 static unsigned long cpu_hw_rate, xo_rate; 33 static struct platform_device *global_pdev; 34 static bool icc_scaling_enabled; 35 36 static int qcom_cpufreq_set_bw(struct cpufreq_policy *policy, 37 unsigned long freq_khz) 38 { 39 unsigned long freq_hz = freq_khz * 1000; 40 struct dev_pm_opp *opp; 41 struct device *dev; 42 int ret; 43 44 dev = get_cpu_device(policy->cpu); 45 if (!dev) 46 return -ENODEV; 47 48 opp = dev_pm_opp_find_freq_exact(dev, freq_hz, true); 49 if (IS_ERR(opp)) 50 return PTR_ERR(opp); 51 52 ret = dev_pm_opp_set_bw(dev, opp); 53 dev_pm_opp_put(opp); 54 return ret; 55 } 56 57 static int qcom_cpufreq_update_opp(struct device *cpu_dev, 58 unsigned long freq_khz, 59 unsigned long volt) 60 { 61 unsigned long freq_hz = freq_khz * 1000; 62 int ret; 63 64 /* Skip voltage update if the opp table is not available */ 65 if (!icc_scaling_enabled) 66 return dev_pm_opp_add(cpu_dev, freq_hz, volt); 67 68 ret = dev_pm_opp_adjust_voltage(cpu_dev, freq_hz, volt, volt, volt); 69 if (ret) { 70 dev_err(cpu_dev, "Voltage update failed freq=%ld\n", freq_khz); 71 return ret; 72 } 73 74 return dev_pm_opp_enable(cpu_dev, freq_hz); 75 } 76 77 static int qcom_cpufreq_hw_target_index(struct cpufreq_policy *policy, 78 unsigned int index) 79 { 80 void __iomem *perf_state_reg = policy->driver_data; 81 unsigned long freq = policy->freq_table[index].frequency; 82 83 writel_relaxed(index, perf_state_reg); 84 85 if (icc_scaling_enabled) 86 qcom_cpufreq_set_bw(policy, freq); 87 88 arch_set_freq_scale(policy->related_cpus, freq, 89 policy->cpuinfo.max_freq); 90 return 0; 91 } 92 93 static unsigned int qcom_cpufreq_hw_get(unsigned int cpu) 94 { 95 void __iomem *perf_state_reg; 96 struct cpufreq_policy *policy; 97 unsigned int index; 98 99 policy = cpufreq_cpu_get_raw(cpu); 100 if (!policy) 101 return 0; 102 103 perf_state_reg = policy->driver_data; 104 105 index = readl_relaxed(perf_state_reg); 106 index = min(index, LUT_MAX_ENTRIES - 1); 107 108 return policy->freq_table[index].frequency; 109 } 110 111 static unsigned int qcom_cpufreq_hw_fast_switch(struct cpufreq_policy *policy, 112 unsigned int target_freq) 113 { 114 void __iomem *perf_state_reg = policy->driver_data; 115 unsigned int index; 116 unsigned long freq; 117 118 index = policy->cached_resolved_idx; 119 writel_relaxed(index, perf_state_reg); 120 121 freq = policy->freq_table[index].frequency; 122 arch_set_freq_scale(policy->related_cpus, freq, 123 policy->cpuinfo.max_freq); 124 125 return freq; 126 } 127 128 static int qcom_cpufreq_hw_read_lut(struct device *cpu_dev, 129 struct cpufreq_policy *policy, 130 void __iomem *base) 131 { 132 u32 data, src, lval, i, core_count, prev_freq = 0, freq; 133 u32 volt; 134 struct cpufreq_frequency_table *table; 135 struct dev_pm_opp *opp; 136 unsigned long rate; 137 int ret; 138 139 table = kcalloc(LUT_MAX_ENTRIES + 1, sizeof(*table), GFP_KERNEL); 140 if (!table) 141 return -ENOMEM; 142 143 ret = dev_pm_opp_of_add_table(cpu_dev); 144 if (!ret) { 145 /* Disable all opps and cross-validate against LUT later */ 146 icc_scaling_enabled = true; 147 for (rate = 0; ; rate++) { 148 opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate); 149 if (IS_ERR(opp)) 150 break; 151 152 dev_pm_opp_put(opp); 153 dev_pm_opp_disable(cpu_dev, rate); 154 } 155 } else if (ret != -ENODEV) { 156 dev_err(cpu_dev, "Invalid opp table in device tree\n"); 157 return ret; 158 } else { 159 policy->fast_switch_possible = true; 160 icc_scaling_enabled = false; 161 } 162 163 for (i = 0; i < LUT_MAX_ENTRIES; i++) { 164 data = readl_relaxed(base + REG_FREQ_LUT + 165 i * LUT_ROW_SIZE); 166 src = FIELD_GET(LUT_SRC, data); 167 lval = FIELD_GET(LUT_L_VAL, data); 168 core_count = FIELD_GET(LUT_CORE_COUNT, data); 169 170 data = readl_relaxed(base + REG_VOLT_LUT + 171 i * LUT_ROW_SIZE); 172 volt = FIELD_GET(LUT_VOLT, data) * 1000; 173 174 if (src) 175 freq = xo_rate * lval / 1000; 176 else 177 freq = cpu_hw_rate / 1000; 178 179 if (freq != prev_freq && core_count != LUT_TURBO_IND) { 180 table[i].frequency = freq; 181 qcom_cpufreq_update_opp(cpu_dev, freq, volt); 182 dev_dbg(cpu_dev, "index=%d freq=%d, core_count %d\n", i, 183 freq, core_count); 184 } else if (core_count == LUT_TURBO_IND) { 185 table[i].frequency = CPUFREQ_ENTRY_INVALID; 186 } 187 188 /* 189 * Two of the same frequencies with the same core counts means 190 * end of table 191 */ 192 if (i > 0 && prev_freq == freq) { 193 struct cpufreq_frequency_table *prev = &table[i - 1]; 194 195 /* 196 * Only treat the last frequency that might be a boost 197 * as the boost frequency 198 */ 199 if (prev->frequency == CPUFREQ_ENTRY_INVALID) { 200 prev->frequency = prev_freq; 201 prev->flags = CPUFREQ_BOOST_FREQ; 202 qcom_cpufreq_update_opp(cpu_dev, prev_freq, volt); 203 } 204 205 break; 206 } 207 208 prev_freq = freq; 209 } 210 211 table[i].frequency = CPUFREQ_TABLE_END; 212 policy->freq_table = table; 213 dev_pm_opp_set_sharing_cpus(cpu_dev, policy->cpus); 214 215 return 0; 216 } 217 218 static void qcom_get_related_cpus(int index, struct cpumask *m) 219 { 220 struct device_node *cpu_np; 221 struct of_phandle_args args; 222 int cpu, ret; 223 224 for_each_possible_cpu(cpu) { 225 cpu_np = of_cpu_device_node_get(cpu); 226 if (!cpu_np) 227 continue; 228 229 ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain", 230 "#freq-domain-cells", 0, 231 &args); 232 of_node_put(cpu_np); 233 if (ret < 0) 234 continue; 235 236 if (index == args.args[0]) 237 cpumask_set_cpu(cpu, m); 238 } 239 } 240 241 static int qcom_cpufreq_hw_cpu_init(struct cpufreq_policy *policy) 242 { 243 struct device *dev = &global_pdev->dev; 244 struct of_phandle_args args; 245 struct device_node *cpu_np; 246 struct device *cpu_dev; 247 struct resource *res; 248 void __iomem *base; 249 int ret, index; 250 251 cpu_dev = get_cpu_device(policy->cpu); 252 if (!cpu_dev) { 253 pr_err("%s: failed to get cpu%d device\n", __func__, 254 policy->cpu); 255 return -ENODEV; 256 } 257 258 cpu_np = of_cpu_device_node_get(policy->cpu); 259 if (!cpu_np) 260 return -EINVAL; 261 262 ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain", 263 "#freq-domain-cells", 0, &args); 264 of_node_put(cpu_np); 265 if (ret) 266 return ret; 267 268 index = args.args[0]; 269 270 res = platform_get_resource(global_pdev, IORESOURCE_MEM, index); 271 if (!res) 272 return -ENODEV; 273 274 base = devm_ioremap(dev, res->start, resource_size(res)); 275 if (!base) 276 return -ENOMEM; 277 278 /* HW should be in enabled state to proceed */ 279 if (!(readl_relaxed(base + REG_ENABLE) & 0x1)) { 280 dev_err(dev, "Domain-%d cpufreq hardware not enabled\n", index); 281 ret = -ENODEV; 282 goto error; 283 } 284 285 qcom_get_related_cpus(index, policy->cpus); 286 if (!cpumask_weight(policy->cpus)) { 287 dev_err(dev, "Domain-%d failed to get related CPUs\n", index); 288 ret = -ENOENT; 289 goto error; 290 } 291 292 policy->driver_data = base + REG_PERF_STATE; 293 294 ret = qcom_cpufreq_hw_read_lut(cpu_dev, policy, base); 295 if (ret) { 296 dev_err(dev, "Domain-%d failed to read LUT\n", index); 297 goto error; 298 } 299 300 ret = dev_pm_opp_get_opp_count(cpu_dev); 301 if (ret <= 0) { 302 dev_err(cpu_dev, "Failed to add OPPs\n"); 303 ret = -ENODEV; 304 goto error; 305 } 306 307 dev_pm_opp_of_register_em(cpu_dev, policy->cpus); 308 309 return 0; 310 error: 311 devm_iounmap(dev, base); 312 return ret; 313 } 314 315 static int qcom_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy) 316 { 317 struct device *cpu_dev = get_cpu_device(policy->cpu); 318 void __iomem *base = policy->driver_data - REG_PERF_STATE; 319 320 dev_pm_opp_remove_all_dynamic(cpu_dev); 321 dev_pm_opp_of_cpumask_remove_table(policy->related_cpus); 322 kfree(policy->freq_table); 323 devm_iounmap(&global_pdev->dev, base); 324 325 return 0; 326 } 327 328 static struct freq_attr *qcom_cpufreq_hw_attr[] = { 329 &cpufreq_freq_attr_scaling_available_freqs, 330 &cpufreq_freq_attr_scaling_boost_freqs, 331 NULL 332 }; 333 334 static struct cpufreq_driver cpufreq_qcom_hw_driver = { 335 .flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK | 336 CPUFREQ_HAVE_GOVERNOR_PER_POLICY | 337 CPUFREQ_IS_COOLING_DEV, 338 .verify = cpufreq_generic_frequency_table_verify, 339 .target_index = qcom_cpufreq_hw_target_index, 340 .get = qcom_cpufreq_hw_get, 341 .init = qcom_cpufreq_hw_cpu_init, 342 .exit = qcom_cpufreq_hw_cpu_exit, 343 .fast_switch = qcom_cpufreq_hw_fast_switch, 344 .name = "qcom-cpufreq-hw", 345 .attr = qcom_cpufreq_hw_attr, 346 }; 347 348 static int qcom_cpufreq_hw_driver_probe(struct platform_device *pdev) 349 { 350 struct device *cpu_dev; 351 struct clk *clk; 352 int ret; 353 354 clk = clk_get(&pdev->dev, "xo"); 355 if (IS_ERR(clk)) 356 return PTR_ERR(clk); 357 358 xo_rate = clk_get_rate(clk); 359 clk_put(clk); 360 361 clk = clk_get(&pdev->dev, "alternate"); 362 if (IS_ERR(clk)) 363 return PTR_ERR(clk); 364 365 cpu_hw_rate = clk_get_rate(clk) / CLK_HW_DIV; 366 clk_put(clk); 367 368 global_pdev = pdev; 369 370 /* Check for optional interconnect paths on CPU0 */ 371 cpu_dev = get_cpu_device(0); 372 if (!cpu_dev) 373 return -EPROBE_DEFER; 374 375 ret = dev_pm_opp_of_find_icc_paths(cpu_dev, NULL); 376 if (ret) 377 return ret; 378 379 ret = cpufreq_register_driver(&cpufreq_qcom_hw_driver); 380 if (ret) 381 dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n"); 382 else 383 dev_dbg(&pdev->dev, "QCOM CPUFreq HW driver initialized\n"); 384 385 return ret; 386 } 387 388 static int qcom_cpufreq_hw_driver_remove(struct platform_device *pdev) 389 { 390 return cpufreq_unregister_driver(&cpufreq_qcom_hw_driver); 391 } 392 393 static const struct of_device_id qcom_cpufreq_hw_match[] = { 394 { .compatible = "qcom,cpufreq-hw" }, 395 {} 396 }; 397 MODULE_DEVICE_TABLE(of, qcom_cpufreq_hw_match); 398 399 static struct platform_driver qcom_cpufreq_hw_driver = { 400 .probe = qcom_cpufreq_hw_driver_probe, 401 .remove = qcom_cpufreq_hw_driver_remove, 402 .driver = { 403 .name = "qcom-cpufreq-hw", 404 .of_match_table = qcom_cpufreq_hw_match, 405 }, 406 }; 407 408 static int __init qcom_cpufreq_hw_init(void) 409 { 410 return platform_driver_register(&qcom_cpufreq_hw_driver); 411 } 412 postcore_initcall(qcom_cpufreq_hw_init); 413 414 static void __exit qcom_cpufreq_hw_exit(void) 415 { 416 platform_driver_unregister(&qcom_cpufreq_hw_driver); 417 } 418 module_exit(qcom_cpufreq_hw_exit); 419 420 MODULE_DESCRIPTION("QCOM CPUFREQ HW Driver"); 421 MODULE_LICENSE("GPL v2"); 422