1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (c) 2018, The Linux Foundation. All rights reserved. 4 * Copyright (c) Qualcomm Technologies, Inc. and/or its subsidiaries. 5 */ 6 7 #include <linux/bitfield.h> 8 #include <linux/clk-provider.h> 9 #include <linux/cpufreq.h> 10 #include <linux/init.h> 11 #include <linux/interconnect.h> 12 #include <linux/interrupt.h> 13 #include <linux/io.h> 14 #include <linux/kernel.h> 15 #include <linux/module.h> 16 #include <linux/of.h> 17 #include <linux/platform_device.h> 18 #include <linux/pm_opp.h> 19 #include <linux/slab.h> 20 #include <linux/spinlock.h> 21 #include <linux/units.h> 22 23 #define LUT_MAX_ENTRIES 40U 24 #define LUT_SRC GENMASK(31, 30) 25 #define LUT_L_VAL GENMASK(7, 0) 26 #define LUT_CORE_COUNT GENMASK(18, 16) 27 #define LUT_VOLT GENMASK(11, 0) 28 #define CLK_HW_DIV 2 29 #define LUT_TURBO_IND 1 30 31 #define GT_IRQ_STATUS BIT(2) 32 33 #define MAX_FREQ_DOMAINS 4 34 35 struct qcom_cpufreq_soc_data { 36 u32 reg_enable; 37 u32 reg_domain_state; 38 u32 reg_dcvs_ctrl; 39 u32 reg_freq_lut; 40 u32 reg_volt_lut; 41 u32 reg_intr_clr; 42 u32 reg_current_vote; 43 u32 reg_perf_state; 44 u32 lut_max_entries; 45 u8 lut_row_size; 46 }; 47 48 struct qcom_cpufreq_data { 49 void __iomem *base; 50 51 /* 52 * Mutex to synchronize between de-init sequence and re-starting LMh 53 * polling/interrupts 54 */ 55 struct mutex throttle_lock; 56 int throttle_irq; 57 char irq_name[15]; 58 bool cancel_throttle; 59 struct delayed_work throttle_work; 60 struct cpufreq_policy *policy; 61 struct clk_hw cpu_clk; 62 63 bool per_core_dcvs; 64 }; 65 66 static struct { 67 struct qcom_cpufreq_data *data; 68 const struct qcom_cpufreq_soc_data *soc_data; 69 } qcom_cpufreq; 70 71 static unsigned long cpu_hw_rate, xo_rate; 72 static bool icc_scaling_enabled; 73 74 static int qcom_cpufreq_set_bw(struct cpufreq_policy *policy, 75 unsigned long freq_khz) 76 { 77 unsigned long freq_hz = freq_khz * 1000; 78 struct dev_pm_opp *opp; 79 struct device *dev; 80 int ret; 81 82 dev = get_cpu_device(policy->cpu); 83 if (!dev) 84 return -ENODEV; 85 86 opp = dev_pm_opp_find_freq_exact(dev, freq_hz, true); 87 if (IS_ERR(opp)) 88 return PTR_ERR(opp); 89 90 ret = dev_pm_opp_set_opp(dev, opp); 91 dev_pm_opp_put(opp); 92 return ret; 93 } 94 95 static int qcom_cpufreq_update_opp(struct device *cpu_dev, 96 unsigned long freq_khz, 97 unsigned long volt) 98 { 99 unsigned long freq_hz = freq_khz * 1000; 100 int ret; 101 102 /* Skip voltage update if the opp table is not available */ 103 if (!icc_scaling_enabled) 104 return dev_pm_opp_add(cpu_dev, freq_hz, volt); 105 106 ret = dev_pm_opp_adjust_voltage(cpu_dev, freq_hz, volt, volt, volt); 107 if (ret) { 108 dev_err(cpu_dev, "Voltage update failed freq=%ld\n", freq_khz); 109 return ret; 110 } 111 112 return dev_pm_opp_enable(cpu_dev, freq_hz); 113 } 114 115 static int qcom_cpufreq_hw_target_index(struct cpufreq_policy *policy, 116 unsigned int index) 117 { 118 struct qcom_cpufreq_data *data = policy->driver_data; 119 const struct qcom_cpufreq_soc_data *soc_data = qcom_cpufreq.soc_data; 120 unsigned long freq = policy->freq_table[index].frequency; 121 unsigned int i; 122 123 writel_relaxed(index, data->base + soc_data->reg_perf_state); 124 125 if (data->per_core_dcvs) 126 for (i = 1; i < cpumask_weight(policy->related_cpus); i++) 127 writel_relaxed(index, data->base + soc_data->reg_perf_state + i * 4); 128 129 if (icc_scaling_enabled) 130 qcom_cpufreq_set_bw(policy, freq); 131 132 return 0; 133 } 134 135 static unsigned long qcom_lmh_get_throttle_freq(struct qcom_cpufreq_data *data) 136 { 137 unsigned int lval; 138 139 if (qcom_cpufreq.soc_data->reg_current_vote) 140 lval = readl_relaxed(data->base + qcom_cpufreq.soc_data->reg_current_vote) & 0x3ff; 141 else 142 lval = readl_relaxed(data->base + qcom_cpufreq.soc_data->reg_domain_state) & 0xff; 143 144 return lval * xo_rate; 145 } 146 147 /* Get the frequency requested by the cpufreq core for the CPU */ 148 static unsigned int qcom_cpufreq_get_freq(struct cpufreq_policy *policy) 149 { 150 struct qcom_cpufreq_data *data; 151 const struct qcom_cpufreq_soc_data *soc_data; 152 unsigned int index; 153 154 if (!policy) 155 return 0; 156 157 data = policy->driver_data; 158 soc_data = qcom_cpufreq.soc_data; 159 160 index = readl_relaxed(data->base + soc_data->reg_perf_state); 161 index = min(index, soc_data->lut_max_entries - 1); 162 163 return policy->freq_table[index].frequency; 164 } 165 166 static unsigned int __qcom_cpufreq_hw_get(struct cpufreq_policy *policy) 167 { 168 struct qcom_cpufreq_data *data; 169 170 if (!policy) 171 return 0; 172 173 data = policy->driver_data; 174 175 if (data->throttle_irq >= 0) 176 return qcom_lmh_get_throttle_freq(data) / HZ_PER_KHZ; 177 178 return qcom_cpufreq_get_freq(policy); 179 } 180 181 static unsigned int qcom_cpufreq_hw_get(unsigned int cpu) 182 { 183 return __qcom_cpufreq_hw_get(cpufreq_cpu_get_raw(cpu)); 184 } 185 186 static unsigned int qcom_cpufreq_hw_fast_switch(struct cpufreq_policy *policy, 187 unsigned int target_freq) 188 { 189 struct qcom_cpufreq_data *data = policy->driver_data; 190 const struct qcom_cpufreq_soc_data *soc_data = qcom_cpufreq.soc_data; 191 unsigned int index; 192 unsigned int i; 193 194 index = policy->cached_resolved_idx; 195 writel_relaxed(index, data->base + soc_data->reg_perf_state); 196 197 if (data->per_core_dcvs) 198 for (i = 1; i < cpumask_weight(policy->related_cpus); i++) 199 writel_relaxed(index, data->base + soc_data->reg_perf_state + i * 4); 200 201 return policy->freq_table[index].frequency; 202 } 203 204 static int qcom_cpufreq_hw_read_lut(struct device *cpu_dev, 205 struct cpufreq_policy *policy) 206 { 207 u32 data, src, lval, i, core_count, prev_freq = 0, freq; 208 u32 volt; 209 struct cpufreq_frequency_table *table; 210 struct dev_pm_opp *opp; 211 unsigned long rate; 212 int ret; 213 struct qcom_cpufreq_data *drv_data = policy->driver_data; 214 const struct qcom_cpufreq_soc_data *soc_data = qcom_cpufreq.soc_data; 215 216 table = kzalloc_objs(*table, soc_data->lut_max_entries + 1); 217 if (!table) 218 return -ENOMEM; 219 220 ret = dev_pm_opp_of_add_table(cpu_dev); 221 if (!ret) { 222 /* Disable all opps and cross-validate against LUT later */ 223 icc_scaling_enabled = true; 224 for (rate = 0; ; rate++) { 225 opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate); 226 if (IS_ERR(opp)) 227 break; 228 229 dev_pm_opp_put(opp); 230 dev_pm_opp_disable(cpu_dev, rate); 231 } 232 } else if (ret != -ENODEV) { 233 dev_err(cpu_dev, "Invalid opp table in device tree\n"); 234 kfree(table); 235 return ret; 236 } else { 237 policy->fast_switch_possible = true; 238 icc_scaling_enabled = false; 239 } 240 241 for (i = 0; i < soc_data->lut_max_entries; i++) { 242 data = readl_relaxed(drv_data->base + soc_data->reg_freq_lut + 243 i * soc_data->lut_row_size); 244 src = FIELD_GET(LUT_SRC, data); 245 lval = FIELD_GET(LUT_L_VAL, data); 246 core_count = FIELD_GET(LUT_CORE_COUNT, data); 247 248 data = readl_relaxed(drv_data->base + soc_data->reg_volt_lut + 249 i * soc_data->lut_row_size); 250 volt = FIELD_GET(LUT_VOLT, data) * 1000; 251 252 if (src) 253 freq = xo_rate * lval / 1000; 254 else 255 freq = cpu_hw_rate / 1000; 256 257 if (freq != prev_freq && core_count != LUT_TURBO_IND) { 258 if (!qcom_cpufreq_update_opp(cpu_dev, freq, volt)) { 259 table[i].frequency = freq; 260 dev_dbg(cpu_dev, "index=%d freq=%d, core_count %d\n", i, 261 freq, core_count); 262 } else { 263 dev_warn(cpu_dev, "failed to update OPP for freq=%d\n", freq); 264 table[i].frequency = CPUFREQ_ENTRY_INVALID; 265 } 266 267 } else if (core_count == LUT_TURBO_IND) { 268 table[i].frequency = CPUFREQ_ENTRY_INVALID; 269 } 270 271 /* 272 * Two of the same frequencies with the same core counts means 273 * end of table 274 */ 275 if (i > 0 && prev_freq == freq) { 276 struct cpufreq_frequency_table *prev = &table[i - 1]; 277 278 /* 279 * Only treat the last frequency that might be a boost 280 * as the boost frequency 281 */ 282 if (prev->frequency == CPUFREQ_ENTRY_INVALID) { 283 if (!qcom_cpufreq_update_opp(cpu_dev, prev_freq, volt)) { 284 prev->frequency = prev_freq; 285 prev->flags = CPUFREQ_BOOST_FREQ; 286 } else { 287 dev_warn(cpu_dev, "failed to update OPP for freq=%d\n", 288 freq); 289 } 290 } 291 292 break; 293 } 294 295 prev_freq = freq; 296 } 297 298 table[i].frequency = CPUFREQ_TABLE_END; 299 policy->freq_table = table; 300 dev_pm_opp_set_sharing_cpus(cpu_dev, policy->cpus); 301 302 return 0; 303 } 304 305 static void qcom_get_related_cpus(int index, struct cpumask *m) 306 { 307 struct device_node *cpu_np; 308 struct of_phandle_args args; 309 int cpu, ret; 310 311 for_each_present_cpu(cpu) { 312 cpu_np = of_cpu_device_node_get(cpu); 313 if (!cpu_np) 314 continue; 315 316 ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain", 317 "#freq-domain-cells", 0, 318 &args); 319 of_node_put(cpu_np); 320 if (ret < 0) 321 continue; 322 323 if (index == args.args[0]) 324 cpumask_set_cpu(cpu, m); 325 } 326 } 327 328 static void qcom_lmh_dcvs_notify(struct qcom_cpufreq_data *data) 329 { 330 struct cpufreq_policy *policy = data->policy; 331 int cpu = cpumask_first(policy->related_cpus); 332 struct device *dev = get_cpu_device(cpu); 333 unsigned long freq_hz, throttled_freq; 334 struct dev_pm_opp *opp; 335 336 /* 337 * Get the h/w throttled frequency, normalize it using the 338 * registered opp table and use it to calculate thermal pressure. 339 */ 340 freq_hz = qcom_lmh_get_throttle_freq(data); 341 342 opp = dev_pm_opp_find_freq_floor(dev, &freq_hz); 343 if (IS_ERR(opp) && PTR_ERR(opp) == -ERANGE) 344 opp = dev_pm_opp_find_freq_ceil(dev, &freq_hz); 345 346 if (IS_ERR(opp)) { 347 dev_warn(dev, "Can't find the OPP for throttling: %pe!\n", opp); 348 } else { 349 dev_pm_opp_put(opp); 350 } 351 352 throttled_freq = freq_hz / HZ_PER_KHZ; 353 354 /* Update HW pressure (the boost frequencies are accepted) */ 355 arch_update_hw_pressure(policy->related_cpus, throttled_freq); 356 357 /* 358 * In the unlikely case policy is unregistered do not enable 359 * polling or h/w interrupt 360 */ 361 mutex_lock(&data->throttle_lock); 362 if (data->cancel_throttle) 363 goto out; 364 365 /* 366 * If h/w throttled frequency is higher than what cpufreq has requested 367 * for, then stop polling and switch back to interrupt mechanism. 368 */ 369 if (throttled_freq >= qcom_cpufreq_get_freq(cpufreq_cpu_get_raw(cpu))) 370 enable_irq(data->throttle_irq); 371 else 372 mod_delayed_work(system_highpri_wq, &data->throttle_work, 373 msecs_to_jiffies(10)); 374 375 out: 376 mutex_unlock(&data->throttle_lock); 377 } 378 379 static void qcom_lmh_dcvs_poll(struct work_struct *work) 380 { 381 struct qcom_cpufreq_data *data; 382 383 data = container_of(work, struct qcom_cpufreq_data, throttle_work.work); 384 qcom_lmh_dcvs_notify(data); 385 } 386 387 static irqreturn_t qcom_lmh_dcvs_handle_irq(int irq, void *data) 388 { 389 struct qcom_cpufreq_data *c_data = data; 390 391 /* Disable interrupt and enable polling */ 392 disable_irq_nosync(c_data->throttle_irq); 393 schedule_delayed_work(&c_data->throttle_work, 0); 394 395 if (qcom_cpufreq.soc_data->reg_intr_clr) 396 writel_relaxed(GT_IRQ_STATUS, 397 c_data->base + qcom_cpufreq.soc_data->reg_intr_clr); 398 399 return IRQ_HANDLED; 400 } 401 402 static const struct qcom_cpufreq_soc_data qcom_soc_data = { 403 .reg_enable = 0x0, 404 .reg_dcvs_ctrl = 0xbc, 405 .reg_freq_lut = 0x110, 406 .reg_volt_lut = 0x114, 407 .reg_current_vote = 0x704, 408 .reg_perf_state = 0x920, 409 .lut_row_size = 32, 410 .lut_max_entries = LUT_MAX_ENTRIES, 411 }; 412 413 static const struct qcom_cpufreq_soc_data epss_soc_data = { 414 .reg_enable = 0x0, 415 .reg_domain_state = 0x20, 416 .reg_dcvs_ctrl = 0xb0, 417 .reg_freq_lut = 0x100, 418 .reg_volt_lut = 0x200, 419 .reg_intr_clr = 0x308, 420 .reg_perf_state = 0x320, 421 .lut_row_size = 4, 422 .lut_max_entries = LUT_MAX_ENTRIES, 423 }; 424 425 static const struct qcom_cpufreq_soc_data shikra_epss_soc_data = { 426 .reg_enable = 0x0, 427 .reg_domain_state = 0x20, 428 .reg_dcvs_ctrl = 0xb0, 429 .reg_freq_lut = 0x100, 430 .reg_volt_lut = 0x200, 431 .reg_intr_clr = 0x308, 432 .reg_perf_state = 0x320, 433 .lut_row_size = 4, 434 .lut_max_entries = 12, 435 }; 436 437 static const struct of_device_id qcom_cpufreq_hw_match[] = { 438 { .compatible = "qcom,cpufreq-hw", .data = &qcom_soc_data }, 439 { .compatible = "qcom,cpufreq-epss", .data = &epss_soc_data }, 440 { .compatible = "qcom,shikra-epss", .data = &shikra_epss_soc_data }, 441 {} 442 }; 443 MODULE_DEVICE_TABLE(of, qcom_cpufreq_hw_match); 444 445 static int qcom_cpufreq_hw_lmh_init(struct cpufreq_policy *policy, int index) 446 { 447 struct qcom_cpufreq_data *data = policy->driver_data; 448 struct platform_device *pdev = cpufreq_get_driver_data(); 449 int ret; 450 451 /* 452 * Look for LMh interrupt. If no interrupt line is specified / 453 * if there is an error, allow cpufreq to be enabled as usual. 454 */ 455 data->throttle_irq = platform_get_irq_optional(pdev, index); 456 if (data->throttle_irq == -ENXIO) 457 return 0; 458 if (data->throttle_irq < 0) 459 return data->throttle_irq; 460 461 data->cancel_throttle = false; 462 463 mutex_init(&data->throttle_lock); 464 INIT_DEFERRABLE_WORK(&data->throttle_work, qcom_lmh_dcvs_poll); 465 466 snprintf(data->irq_name, sizeof(data->irq_name), "dcvsh-irq-%u", policy->cpu); 467 ret = request_threaded_irq(data->throttle_irq, NULL, qcom_lmh_dcvs_handle_irq, 468 IRQF_ONESHOT | IRQF_NO_AUTOEN, data->irq_name, data); 469 if (ret) { 470 dev_err(&pdev->dev, "Error registering %s: %d\n", data->irq_name, ret); 471 return 0; 472 } 473 474 ret = irq_set_affinity_and_hint(data->throttle_irq, policy->cpus); 475 if (ret) 476 dev_err(&pdev->dev, "Failed to set CPU affinity of %s[%d]\n", 477 data->irq_name, data->throttle_irq); 478 479 return 0; 480 } 481 482 static int qcom_cpufreq_hw_cpu_online(struct cpufreq_policy *policy) 483 { 484 struct qcom_cpufreq_data *data = policy->driver_data; 485 struct platform_device *pdev = cpufreq_get_driver_data(); 486 int ret; 487 488 if (data->throttle_irq <= 0) 489 return 0; 490 491 mutex_lock(&data->throttle_lock); 492 data->cancel_throttle = false; 493 mutex_unlock(&data->throttle_lock); 494 495 ret = irq_set_affinity_and_hint(data->throttle_irq, policy->cpus); 496 if (ret) 497 dev_err(&pdev->dev, "Failed to set CPU affinity of %s[%d]\n", 498 data->irq_name, data->throttle_irq); 499 500 return ret; 501 } 502 503 static int qcom_cpufreq_hw_cpu_offline(struct cpufreq_policy *policy) 504 { 505 struct qcom_cpufreq_data *data = policy->driver_data; 506 507 if (data->throttle_irq <= 0) 508 return 0; 509 510 mutex_lock(&data->throttle_lock); 511 data->cancel_throttle = true; 512 mutex_unlock(&data->throttle_lock); 513 514 cancel_delayed_work_sync(&data->throttle_work); 515 irq_set_affinity_and_hint(data->throttle_irq, NULL); 516 disable_irq_nosync(data->throttle_irq); 517 518 return 0; 519 } 520 521 static void qcom_cpufreq_hw_lmh_exit(struct qcom_cpufreq_data *data) 522 { 523 if (data->throttle_irq <= 0) 524 return; 525 526 free_irq(data->throttle_irq, data); 527 } 528 529 static int qcom_cpufreq_hw_cpu_init(struct cpufreq_policy *policy) 530 { 531 struct platform_device *pdev = cpufreq_get_driver_data(); 532 struct device *dev = &pdev->dev; 533 struct of_phandle_args args; 534 struct device_node *cpu_np; 535 struct device *cpu_dev; 536 struct qcom_cpufreq_data *data; 537 int ret, index; 538 539 cpu_dev = get_cpu_device(policy->cpu); 540 if (!cpu_dev) { 541 pr_err("%s: failed to get cpu%d device\n", __func__, 542 policy->cpu); 543 return -ENODEV; 544 } 545 546 cpu_np = of_cpu_device_node_get(policy->cpu); 547 if (!cpu_np) 548 return -EINVAL; 549 550 ret = of_parse_phandle_with_args(cpu_np, "qcom,freq-domain", 551 "#freq-domain-cells", 0, &args); 552 of_node_put(cpu_np); 553 if (ret) 554 return ret; 555 556 index = args.args[0]; 557 data = &qcom_cpufreq.data[index]; 558 559 /* HW should be in enabled state to proceed */ 560 if (!(readl_relaxed(data->base + qcom_cpufreq.soc_data->reg_enable) & 0x1)) { 561 dev_err(dev, "Domain-%d cpufreq hardware not enabled\n", index); 562 return -ENODEV; 563 } 564 565 if (readl_relaxed(data->base + qcom_cpufreq.soc_data->reg_dcvs_ctrl) & 0x1) 566 data->per_core_dcvs = true; 567 568 qcom_get_related_cpus(index, policy->cpus); 569 570 policy->driver_data = data; 571 policy->dvfs_possible_from_any_cpu = true; 572 data->policy = policy; 573 574 ret = qcom_cpufreq_hw_read_lut(cpu_dev, policy); 575 if (ret) { 576 dev_err(dev, "Domain-%d failed to read LUT\n", index); 577 return ret; 578 } 579 580 ret = dev_pm_opp_get_opp_count(cpu_dev); 581 if (ret <= 0) { 582 dev_err(cpu_dev, "Failed to add OPPs\n"); 583 return -ENODEV; 584 } 585 586 return qcom_cpufreq_hw_lmh_init(policy, index); 587 } 588 589 static void qcom_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy) 590 { 591 struct device *cpu_dev = get_cpu_device(policy->cpu); 592 struct qcom_cpufreq_data *data = policy->driver_data; 593 594 dev_pm_opp_remove_all_dynamic(cpu_dev); 595 dev_pm_opp_of_cpumask_remove_table(policy->related_cpus); 596 qcom_cpufreq_hw_lmh_exit(data); 597 kfree(policy->freq_table); 598 } 599 600 static void qcom_cpufreq_ready(struct cpufreq_policy *policy) 601 { 602 struct qcom_cpufreq_data *data = policy->driver_data; 603 604 if (data->throttle_irq >= 0) 605 enable_irq(data->throttle_irq); 606 } 607 608 static struct cpufreq_driver cpufreq_qcom_hw_driver = { 609 .flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK | 610 CPUFREQ_HAVE_GOVERNOR_PER_POLICY | 611 CPUFREQ_IS_COOLING_DEV, 612 .verify = cpufreq_generic_frequency_table_verify, 613 .target_index = qcom_cpufreq_hw_target_index, 614 .get = qcom_cpufreq_hw_get, 615 .init = qcom_cpufreq_hw_cpu_init, 616 .exit = qcom_cpufreq_hw_cpu_exit, 617 .online = qcom_cpufreq_hw_cpu_online, 618 .offline = qcom_cpufreq_hw_cpu_offline, 619 .register_em = cpufreq_register_em_with_opp, 620 .fast_switch = qcom_cpufreq_hw_fast_switch, 621 .name = "qcom-cpufreq-hw", 622 .ready = qcom_cpufreq_ready, 623 .set_boost = cpufreq_boost_set_sw, 624 }; 625 626 static unsigned long qcom_cpufreq_hw_recalc_rate(struct clk_hw *hw, unsigned long parent_rate) 627 { 628 struct qcom_cpufreq_data *data = container_of(hw, struct qcom_cpufreq_data, cpu_clk); 629 630 return __qcom_cpufreq_hw_get(data->policy) * HZ_PER_KHZ; 631 } 632 633 /* 634 * Since we cannot determine the closest rate of the target rate, let's just 635 * return the actual rate at which the clock is running at. This is needed to 636 * make clk_set_rate() API work properly. 637 */ 638 static int qcom_cpufreq_hw_determine_rate(struct clk_hw *hw, struct clk_rate_request *req) 639 { 640 req->rate = qcom_cpufreq_hw_recalc_rate(hw, 0); 641 642 return 0; 643 } 644 645 static const struct clk_ops qcom_cpufreq_hw_clk_ops = { 646 .recalc_rate = qcom_cpufreq_hw_recalc_rate, 647 .determine_rate = qcom_cpufreq_hw_determine_rate, 648 }; 649 650 static int qcom_cpufreq_hw_driver_probe(struct platform_device *pdev) 651 { 652 struct clk_hw_onecell_data *clk_data; 653 struct device *dev = &pdev->dev; 654 struct device *cpu_dev; 655 struct clk *clk; 656 int ret, i, num_domains; 657 658 clk = clk_get(dev, "xo"); 659 if (IS_ERR(clk)) 660 return PTR_ERR(clk); 661 662 xo_rate = clk_get_rate(clk); 663 clk_put(clk); 664 665 clk = clk_get(dev, "alternate"); 666 if (IS_ERR(clk)) 667 return PTR_ERR(clk); 668 669 cpu_hw_rate = clk_get_rate(clk) / CLK_HW_DIV; 670 clk_put(clk); 671 672 cpufreq_qcom_hw_driver.driver_data = pdev; 673 674 /* Check for optional interconnect paths on CPU0 */ 675 cpu_dev = get_cpu_device(0); 676 if (!cpu_dev) 677 return -EPROBE_DEFER; 678 679 ret = dev_pm_opp_of_find_icc_paths(cpu_dev, NULL); 680 if (ret) 681 return dev_err_probe(dev, ret, "Failed to find icc paths\n"); 682 683 for (num_domains = 0; num_domains < MAX_FREQ_DOMAINS; num_domains++) 684 if (!platform_get_resource(pdev, IORESOURCE_MEM, num_domains)) 685 break; 686 687 qcom_cpufreq.data = devm_kzalloc(dev, sizeof(struct qcom_cpufreq_data) * num_domains, 688 GFP_KERNEL); 689 if (!qcom_cpufreq.data) 690 return -ENOMEM; 691 692 qcom_cpufreq.soc_data = of_device_get_match_data(dev); 693 if (!qcom_cpufreq.soc_data) 694 return -ENODEV; 695 696 clk_data = devm_kzalloc(dev, struct_size(clk_data, hws, num_domains), GFP_KERNEL); 697 if (!clk_data) 698 return -ENOMEM; 699 700 clk_data->num = num_domains; 701 702 for (i = 0; i < num_domains; i++) { 703 struct qcom_cpufreq_data *data = &qcom_cpufreq.data[i]; 704 struct clk_init_data clk_init = {}; 705 void __iomem *base; 706 707 base = devm_platform_ioremap_resource(pdev, i); 708 if (IS_ERR(base)) { 709 dev_err(dev, "Failed to map resource index %d\n", i); 710 return PTR_ERR(base); 711 } 712 713 data->base = base; 714 715 /* Register CPU clock for each frequency domain */ 716 clk_init.name = kasprintf(GFP_KERNEL, "qcom_cpufreq%d", i); 717 if (!clk_init.name) 718 return -ENOMEM; 719 720 clk_init.flags = CLK_GET_RATE_NOCACHE; 721 clk_init.ops = &qcom_cpufreq_hw_clk_ops; 722 data->cpu_clk.init = &clk_init; 723 724 ret = devm_clk_hw_register(dev, &data->cpu_clk); 725 if (ret < 0) { 726 dev_err(dev, "Failed to register clock %d: %d\n", i, ret); 727 kfree(clk_init.name); 728 return ret; 729 } 730 731 clk_data->hws[i] = &data->cpu_clk; 732 kfree(clk_init.name); 733 } 734 735 ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_onecell_get, clk_data); 736 if (ret < 0) { 737 dev_err(dev, "Failed to add clock provider\n"); 738 return ret; 739 } 740 741 ret = cpufreq_register_driver(&cpufreq_qcom_hw_driver); 742 if (ret) 743 dev_err(dev, "CPUFreq HW driver failed to register\n"); 744 else 745 dev_dbg(dev, "QCOM CPUFreq HW driver initialized\n"); 746 747 return ret; 748 } 749 750 static void qcom_cpufreq_hw_driver_remove(struct platform_device *pdev) 751 { 752 cpufreq_unregister_driver(&cpufreq_qcom_hw_driver); 753 } 754 755 static struct platform_driver qcom_cpufreq_hw_driver = { 756 .probe = qcom_cpufreq_hw_driver_probe, 757 .remove = qcom_cpufreq_hw_driver_remove, 758 .driver = { 759 .name = "qcom-cpufreq-hw", 760 .of_match_table = qcom_cpufreq_hw_match, 761 }, 762 }; 763 764 static int __init qcom_cpufreq_hw_init(void) 765 { 766 return platform_driver_register(&qcom_cpufreq_hw_driver); 767 } 768 postcore_initcall(qcom_cpufreq_hw_init); 769 770 static void __exit qcom_cpufreq_hw_exit(void) 771 { 772 platform_driver_unregister(&qcom_cpufreq_hw_driver); 773 } 774 module_exit(qcom_cpufreq_hw_exit); 775 776 MODULE_DESCRIPTION("QCOM CPUFREQ HW Driver"); 777 MODULE_LICENSE("GPL v2"); 778