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