1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (c) 2020 - 2022, NVIDIA CORPORATION. All rights reserved 4 */ 5 6 #include <linux/cpu.h> 7 #include <linux/cpufreq.h> 8 #include <linux/dma-mapping.h> 9 #include <linux/module.h> 10 #include <linux/of.h> 11 #include <linux/of_platform.h> 12 #include <linux/platform_device.h> 13 #include <linux/slab.h> 14 #include <linux/units.h> 15 16 #include <asm/smp_plat.h> 17 18 #include <soc/tegra/bpmp.h> 19 #include <soc/tegra/bpmp-abi.h> 20 21 #define KHZ 1000 22 #define REF_CLK_MHZ 408 /* 408 MHz */ 23 #define CPUFREQ_TBL_STEP_HZ (50 * KHZ * KHZ) 24 #define MAX_CNT ~0U 25 26 #define MAX_DELTA_KHZ 115200 27 28 #define NDIV_MASK 0x1FF 29 30 #define CORE_OFFSET(cpu) (cpu * 8) 31 #define CMU_CLKS_BASE 0x2000 32 #define SCRATCH_FREQ_CORE_REG(data, cpu) (data->regs + CMU_CLKS_BASE + CORE_OFFSET(cpu)) 33 34 #define MMCRAB_CLUSTER_BASE(cl) (0x30000 + (cl * 0x10000)) 35 #define CLUSTER_ACTMON_BASE(data, cl) \ 36 (data->regs + (MMCRAB_CLUSTER_BASE(cl) + data->soc->actmon_cntr_base)) 37 #define CORE_ACTMON_CNTR_REG(data, cl, cpu) (CLUSTER_ACTMON_BASE(data, cl) + CORE_OFFSET(cpu)) 38 39 /* cpufreq transisition latency */ 40 #define TEGRA_CPUFREQ_TRANSITION_LATENCY (300 * 1000) /* unit in nanoseconds */ 41 42 struct tegra_cpu_data { 43 u32 cpuid; 44 u32 clusterid; 45 void __iomem *freq_core_reg; 46 }; 47 48 struct tegra_cpu_ctr { 49 u32 cpu; 50 u32 coreclk_cnt, last_coreclk_cnt; 51 u32 refclk_cnt, last_refclk_cnt; 52 }; 53 54 struct read_counters_work { 55 struct work_struct work; 56 struct tegra_cpu_ctr c; 57 }; 58 59 struct tegra_cpufreq_ops { 60 void (*read_counters)(struct tegra_cpu_ctr *c); 61 void (*set_cpu_ndiv)(struct cpufreq_policy *policy, u64 ndiv); 62 void (*get_cpu_cluster_id)(u32 cpu, u32 *cpuid, u32 *clusterid); 63 int (*get_cpu_ndiv)(u32 cpu, u32 cpuid, u32 clusterid, u64 *ndiv); 64 }; 65 66 struct tegra_cpufreq_soc { 67 struct tegra_cpufreq_ops *ops; 68 int maxcpus_per_cluster; 69 unsigned int num_clusters; 70 phys_addr_t actmon_cntr_base; 71 u32 refclk_delta_min; 72 }; 73 74 struct tegra194_cpufreq_data { 75 void __iomem *regs; 76 struct cpufreq_frequency_table **bpmp_luts; 77 const struct tegra_cpufreq_soc *soc; 78 bool icc_dram_bw_scaling; 79 struct tegra_cpu_data *cpu_data; 80 }; 81 82 static struct workqueue_struct *read_counters_wq; 83 84 static int tegra_cpufreq_set_bw(struct cpufreq_policy *policy, unsigned long freq_khz) 85 { 86 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 87 struct dev_pm_opp *opp; 88 struct device *dev; 89 int ret; 90 91 dev = get_cpu_device(policy->cpu); 92 if (!dev) 93 return -ENODEV; 94 95 opp = dev_pm_opp_find_freq_exact(dev, freq_khz * KHZ, true); 96 if (IS_ERR(opp)) 97 return PTR_ERR(opp); 98 99 ret = dev_pm_opp_set_opp(dev, opp); 100 if (ret) 101 data->icc_dram_bw_scaling = false; 102 103 dev_pm_opp_put(opp); 104 return ret; 105 } 106 107 static void tegra_get_cpu_mpidr(void *mpidr) 108 { 109 *((u64 *)mpidr) = read_cpuid_mpidr() & MPIDR_HWID_BITMASK; 110 } 111 112 static void tegra234_get_cpu_cluster_id(u32 cpu, u32 *cpuid, u32 *clusterid) 113 { 114 u64 mpidr; 115 116 smp_call_function_single(cpu, tegra_get_cpu_mpidr, &mpidr, true); 117 118 if (cpuid) 119 *cpuid = MPIDR_AFFINITY_LEVEL(mpidr, 1); 120 if (clusterid) 121 *clusterid = MPIDR_AFFINITY_LEVEL(mpidr, 2); 122 } 123 124 static int tegra234_get_cpu_ndiv(u32 cpu, u32 cpuid, u32 clusterid, u64 *ndiv) 125 { 126 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 127 128 *ndiv = readl(data->cpu_data[cpu].freq_core_reg) & NDIV_MASK; 129 130 return 0; 131 } 132 133 static void tegra234_set_cpu_ndiv(struct cpufreq_policy *policy, u64 ndiv) 134 { 135 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 136 u32 cpu; 137 138 for_each_cpu(cpu, policy->cpus) 139 writel(ndiv, data->cpu_data[cpu].freq_core_reg); 140 } 141 142 /* 143 * This register provides access to two counter values with a single 144 * 64-bit read. The counter values are used to determine the average 145 * actual frequency a core has run at over a period of time. 146 * [63:32] PLLP counter: Counts at fixed frequency (408 MHz) 147 * [31:0] Core clock counter: Counts on every core clock cycle 148 */ 149 static void tegra234_read_counters(struct tegra_cpu_ctr *c) 150 { 151 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 152 void __iomem *actmon_reg; 153 u32 delta_refcnt; 154 int cnt = 0; 155 u64 val; 156 157 actmon_reg = CORE_ACTMON_CNTR_REG(data, data->cpu_data[c->cpu].clusterid, 158 data->cpu_data[c->cpu].cpuid); 159 160 val = readq(actmon_reg); 161 c->last_refclk_cnt = upper_32_bits(val); 162 c->last_coreclk_cnt = lower_32_bits(val); 163 164 /* 165 * The sampling window is based on the minimum number of reference 166 * clock cycles which is known to give a stable value of CPU frequency. 167 */ 168 do { 169 val = readq(actmon_reg); 170 c->refclk_cnt = upper_32_bits(val); 171 c->coreclk_cnt = lower_32_bits(val); 172 if (c->refclk_cnt < c->last_refclk_cnt) 173 delta_refcnt = c->refclk_cnt + (MAX_CNT - c->last_refclk_cnt); 174 else 175 delta_refcnt = c->refclk_cnt - c->last_refclk_cnt; 176 if (++cnt >= 0xFFFF) { 177 pr_warn("cpufreq: problem with refclk on cpu:%d, delta_refcnt:%u, cnt:%d\n", 178 c->cpu, delta_refcnt, cnt); 179 break; 180 } 181 } while (delta_refcnt < data->soc->refclk_delta_min); 182 } 183 184 static struct tegra_cpufreq_ops tegra234_cpufreq_ops = { 185 .read_counters = tegra234_read_counters, 186 .get_cpu_cluster_id = tegra234_get_cpu_cluster_id, 187 .get_cpu_ndiv = tegra234_get_cpu_ndiv, 188 .set_cpu_ndiv = tegra234_set_cpu_ndiv, 189 }; 190 191 static const struct tegra_cpufreq_soc tegra234_cpufreq_soc = { 192 .ops = &tegra234_cpufreq_ops, 193 .actmon_cntr_base = 0x9000, 194 .maxcpus_per_cluster = 4, 195 .num_clusters = 3, 196 .refclk_delta_min = 16000, 197 }; 198 199 static const struct tegra_cpufreq_soc tegra239_cpufreq_soc = { 200 .ops = &tegra234_cpufreq_ops, 201 .actmon_cntr_base = 0x4000, 202 .maxcpus_per_cluster = 8, 203 .num_clusters = 1, 204 .refclk_delta_min = 16000, 205 }; 206 207 static void tegra194_get_cpu_cluster_id(u32 cpu, u32 *cpuid, u32 *clusterid) 208 { 209 u64 mpidr; 210 211 smp_call_function_single(cpu, tegra_get_cpu_mpidr, &mpidr, true); 212 213 if (cpuid) 214 *cpuid = MPIDR_AFFINITY_LEVEL(mpidr, 0); 215 if (clusterid) 216 *clusterid = MPIDR_AFFINITY_LEVEL(mpidr, 1); 217 } 218 219 /* 220 * Read per-core Read-only system register NVFREQ_FEEDBACK_EL1. 221 * The register provides frequency feedback information to 222 * determine the average actual frequency a core has run at over 223 * a period of time. 224 * [31:0] PLLP counter: Counts at fixed frequency (408 MHz) 225 * [63:32] Core clock counter: counts on every core clock cycle 226 * where the core is architecturally clocking 227 */ 228 static u64 read_freq_feedback(void) 229 { 230 u64 val = 0; 231 232 asm volatile("mrs %0, s3_0_c15_c0_5" : "=r" (val) : ); 233 234 return val; 235 } 236 237 static inline u32 map_ndiv_to_freq(struct mrq_cpu_ndiv_limits_response 238 *nltbl, u16 ndiv) 239 { 240 return nltbl->ref_clk_hz / KHZ * ndiv / (nltbl->pdiv * nltbl->mdiv); 241 } 242 243 static void tegra194_read_counters(struct tegra_cpu_ctr *c) 244 { 245 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 246 u32 delta_refcnt; 247 int cnt = 0; 248 u64 val; 249 250 val = read_freq_feedback(); 251 c->last_refclk_cnt = lower_32_bits(val); 252 c->last_coreclk_cnt = upper_32_bits(val); 253 254 /* 255 * The sampling window is based on the minimum number of reference 256 * clock cycles which is known to give a stable value of CPU frequency. 257 */ 258 do { 259 val = read_freq_feedback(); 260 c->refclk_cnt = lower_32_bits(val); 261 c->coreclk_cnt = upper_32_bits(val); 262 if (c->refclk_cnt < c->last_refclk_cnt) 263 delta_refcnt = c->refclk_cnt + (MAX_CNT - c->last_refclk_cnt); 264 else 265 delta_refcnt = c->refclk_cnt - c->last_refclk_cnt; 266 if (++cnt >= 0xFFFF) { 267 pr_warn("cpufreq: problem with refclk on cpu:%d, delta_refcnt:%u, cnt:%d\n", 268 c->cpu, delta_refcnt, cnt); 269 break; 270 } 271 } while (delta_refcnt < data->soc->refclk_delta_min); 272 } 273 274 static void tegra_read_counters(struct work_struct *work) 275 { 276 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 277 struct read_counters_work *read_counters_work; 278 struct tegra_cpu_ctr *c; 279 280 /* 281 * ref_clk_counter(32 bit counter) runs on constant clk, 282 * pll_p(408MHz). 283 * It will take = 2 ^ 32 / 408 MHz to overflow ref clk counter 284 * = 10526880 usec = 10.527 sec to overflow 285 * 286 * Like wise core_clk_counter(32 bit counter) runs on core clock. 287 * It's synchronized to crab_clk (cpu_crab_clk) which runs at 288 * freq of cluster. Assuming max cluster clock ~2000MHz, 289 * It will take = 2 ^ 32 / 2000 MHz to overflow core clk counter 290 * = ~2.147 sec to overflow 291 */ 292 read_counters_work = container_of(work, struct read_counters_work, 293 work); 294 c = &read_counters_work->c; 295 296 data->soc->ops->read_counters(c); 297 } 298 299 /* 300 * Return instantaneous cpu speed 301 * Instantaneous freq is calculated as - 302 * -Takes sample on every query of getting the freq. 303 * - Read core and ref clock counters; 304 * - Delay for X us 305 * - Read above cycle counters again 306 * - Calculates freq by subtracting current and previous counters 307 * divided by the delay time or eqv. of ref_clk_counter in delta time 308 * - Return Kcycles/second, freq in KHz 309 * 310 * delta time period = x sec 311 * = delta ref_clk_counter / (408 * 10^6) sec 312 * freq in Hz = cycles/sec 313 * = (delta cycles / x sec 314 * = (delta cycles * 408 * 10^6) / delta ref_clk_counter 315 * in KHz = (delta cycles * 408 * 10^3) / delta ref_clk_counter 316 * 317 * @cpu - logical cpu whose freq to be updated 318 * Returns freq in KHz on success, 0 if cpu is offline 319 */ 320 static unsigned int tegra194_calculate_speed(u32 cpu) 321 { 322 struct read_counters_work read_counters_work; 323 struct tegra_cpu_ctr c; 324 u32 delta_refcnt; 325 u32 delta_ccnt; 326 u32 rate_mhz; 327 328 /* 329 * Reconstruct cpu frequency over an observation/sampling window. 330 * Using workqueue to keep interrupts enabled during the interval. 331 */ 332 read_counters_work.c.cpu = cpu; 333 INIT_WORK_ONSTACK(&read_counters_work.work, tegra_read_counters); 334 queue_work_on(cpu, read_counters_wq, &read_counters_work.work); 335 flush_work(&read_counters_work.work); 336 c = read_counters_work.c; 337 338 if (c.coreclk_cnt < c.last_coreclk_cnt) 339 delta_ccnt = c.coreclk_cnt + (MAX_CNT - c.last_coreclk_cnt); 340 else 341 delta_ccnt = c.coreclk_cnt - c.last_coreclk_cnt; 342 if (!delta_ccnt) 343 return 0; 344 345 /* ref clock is 32 bits */ 346 if (c.refclk_cnt < c.last_refclk_cnt) 347 delta_refcnt = c.refclk_cnt + (MAX_CNT - c.last_refclk_cnt); 348 else 349 delta_refcnt = c.refclk_cnt - c.last_refclk_cnt; 350 if (!delta_refcnt) { 351 pr_debug("cpufreq: %d is idle, delta_refcnt: 0\n", cpu); 352 return 0; 353 } 354 rate_mhz = ((unsigned long)(delta_ccnt * REF_CLK_MHZ)) / delta_refcnt; 355 356 return (rate_mhz * KHZ); /* in KHz */ 357 } 358 359 static void tegra194_get_cpu_ndiv_sysreg(void *ndiv) 360 { 361 u64 ndiv_val; 362 363 asm volatile("mrs %0, s3_0_c15_c0_4" : "=r" (ndiv_val) : ); 364 365 *(u64 *)ndiv = ndiv_val; 366 } 367 368 static int tegra194_get_cpu_ndiv(u32 cpu, u32 cpuid, u32 clusterid, u64 *ndiv) 369 { 370 return smp_call_function_single(cpu, tegra194_get_cpu_ndiv_sysreg, &ndiv, true); 371 } 372 373 static void tegra194_set_cpu_ndiv_sysreg(void *data) 374 { 375 u64 ndiv_val = *(u64 *)data; 376 377 asm volatile("msr s3_0_c15_c0_4, %0" : : "r" (ndiv_val)); 378 } 379 380 static void tegra194_set_cpu_ndiv(struct cpufreq_policy *policy, u64 ndiv) 381 { 382 on_each_cpu_mask(policy->cpus, tegra194_set_cpu_ndiv_sysreg, &ndiv, true); 383 } 384 385 static unsigned int tegra194_get_speed(u32 cpu) 386 { 387 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 388 u32 clusterid = data->cpu_data[cpu].clusterid; 389 struct cpufreq_frequency_table *pos; 390 unsigned int rate; 391 u64 ndiv; 392 int ret; 393 394 /* reconstruct actual cpu freq using counters */ 395 rate = tegra194_calculate_speed(cpu); 396 397 /* get last written ndiv value */ 398 ret = data->soc->ops->get_cpu_ndiv(cpu, data->cpu_data[cpu].cpuid, clusterid, &ndiv); 399 if (WARN_ON_ONCE(ret)) 400 return rate; 401 402 /* 403 * If the reconstructed frequency has acceptable delta from 404 * the last written value, then return freq corresponding 405 * to the last written ndiv value from freq_table. This is 406 * done to return consistent value. 407 */ 408 cpufreq_for_each_valid_entry(pos, data->bpmp_luts[clusterid]) { 409 if (pos->driver_data != ndiv) 410 continue; 411 412 if (abs(pos->frequency - rate) > MAX_DELTA_KHZ) { 413 pr_warn("cpufreq: cpu%d,cur:%u,set:%u,delta:%d,set ndiv:%llu\n", 414 cpu, rate, pos->frequency, abs(rate - pos->frequency), ndiv); 415 } else { 416 rate = pos->frequency; 417 } 418 break; 419 } 420 return rate; 421 } 422 423 static int tegra_cpufreq_init_cpufreq_table(struct cpufreq_policy *policy, 424 struct cpufreq_frequency_table *bpmp_lut, 425 struct cpufreq_frequency_table **opp_table) 426 { 427 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 428 struct cpufreq_frequency_table *freq_table = NULL; 429 struct cpufreq_frequency_table *pos; 430 struct device *cpu_dev; 431 struct dev_pm_opp *opp; 432 unsigned long rate; 433 int ret, max_opps; 434 int j = 0; 435 436 cpu_dev = get_cpu_device(policy->cpu); 437 if (!cpu_dev) { 438 pr_err("%s: failed to get cpu%d device\n", __func__, policy->cpu); 439 return -ENODEV; 440 } 441 442 /* Initialize OPP table mentioned in operating-points-v2 property in DT */ 443 ret = dev_pm_opp_of_add_table_indexed(cpu_dev, 0); 444 if (!ret) { 445 max_opps = dev_pm_opp_get_opp_count(cpu_dev); 446 if (max_opps <= 0) { 447 dev_err(cpu_dev, "Failed to add OPPs\n"); 448 return max_opps; 449 } 450 451 /* Disable all opps and cross-validate against LUT later */ 452 for (rate = 0; ; rate++) { 453 opp = dev_pm_opp_find_freq_ceil(cpu_dev, &rate); 454 if (IS_ERR(opp)) 455 break; 456 457 dev_pm_opp_put(opp); 458 dev_pm_opp_disable(cpu_dev, rate); 459 } 460 } else { 461 dev_err(cpu_dev, "Invalid or empty opp table in device tree\n"); 462 data->icc_dram_bw_scaling = false; 463 return ret; 464 } 465 466 freq_table = kcalloc((max_opps + 1), sizeof(*freq_table), GFP_KERNEL); 467 if (!freq_table) 468 return -ENOMEM; 469 470 /* 471 * Cross check the frequencies from BPMP-FW LUT against the OPP's present in DT. 472 * Enable only those DT OPP's which are present in LUT also. 473 */ 474 cpufreq_for_each_valid_entry(pos, bpmp_lut) { 475 opp = dev_pm_opp_find_freq_exact(cpu_dev, pos->frequency * KHZ, false); 476 if (IS_ERR(opp)) 477 continue; 478 479 dev_pm_opp_put(opp); 480 481 ret = dev_pm_opp_enable(cpu_dev, pos->frequency * KHZ); 482 if (ret < 0) 483 return ret; 484 485 freq_table[j].driver_data = pos->driver_data; 486 freq_table[j].frequency = pos->frequency; 487 j++; 488 } 489 490 freq_table[j].driver_data = pos->driver_data; 491 freq_table[j].frequency = CPUFREQ_TABLE_END; 492 493 *opp_table = &freq_table[0]; 494 495 dev_pm_opp_set_sharing_cpus(cpu_dev, policy->cpus); 496 497 return ret; 498 } 499 500 static int tegra194_cpufreq_init(struct cpufreq_policy *policy) 501 { 502 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 503 int maxcpus_per_cluster = data->soc->maxcpus_per_cluster; 504 u32 clusterid = data->cpu_data[policy->cpu].clusterid; 505 struct cpufreq_frequency_table *freq_table; 506 struct cpufreq_frequency_table *bpmp_lut; 507 u32 start_cpu, cpu; 508 int ret; 509 510 if (clusterid >= data->soc->num_clusters || !data->bpmp_luts[clusterid]) 511 return -EINVAL; 512 513 start_cpu = rounddown(policy->cpu, maxcpus_per_cluster); 514 /* set same policy for all cpus in a cluster */ 515 for (cpu = start_cpu; cpu < (start_cpu + maxcpus_per_cluster); cpu++) { 516 if (cpu_possible(cpu)) 517 cpumask_set_cpu(cpu, policy->cpus); 518 } 519 policy->cpuinfo.transition_latency = TEGRA_CPUFREQ_TRANSITION_LATENCY; 520 521 bpmp_lut = data->bpmp_luts[clusterid]; 522 523 if (data->icc_dram_bw_scaling) { 524 ret = tegra_cpufreq_init_cpufreq_table(policy, bpmp_lut, &freq_table); 525 if (!ret) { 526 policy->freq_table = freq_table; 527 return 0; 528 } 529 } 530 531 data->icc_dram_bw_scaling = false; 532 policy->freq_table = bpmp_lut; 533 pr_info("OPP tables missing from DT, EMC frequency scaling disabled\n"); 534 535 return 0; 536 } 537 538 static int tegra194_cpufreq_online(struct cpufreq_policy *policy) 539 { 540 /* We did light-weight tear down earlier, nothing to do here */ 541 return 0; 542 } 543 544 static int tegra194_cpufreq_offline(struct cpufreq_policy *policy) 545 { 546 /* 547 * Preserve policy->driver_data and don't free resources on light-weight 548 * tear down. 549 */ 550 551 return 0; 552 } 553 554 static void tegra194_cpufreq_exit(struct cpufreq_policy *policy) 555 { 556 struct device *cpu_dev = get_cpu_device(policy->cpu); 557 558 dev_pm_opp_remove_all_dynamic(cpu_dev); 559 dev_pm_opp_of_cpumask_remove_table(policy->related_cpus); 560 } 561 562 static int tegra194_cpufreq_set_target(struct cpufreq_policy *policy, 563 unsigned int index) 564 { 565 struct cpufreq_frequency_table *tbl = policy->freq_table + index; 566 struct tegra194_cpufreq_data *data = cpufreq_get_driver_data(); 567 568 /* 569 * Each core writes frequency in per core register. Then both cores 570 * in a cluster run at same frequency which is the maximum frequency 571 * request out of the values requested by both cores in that cluster. 572 */ 573 data->soc->ops->set_cpu_ndiv(policy, (u64)tbl->driver_data); 574 575 if (data->icc_dram_bw_scaling) 576 tegra_cpufreq_set_bw(policy, tbl->frequency); 577 578 return 0; 579 } 580 581 static struct cpufreq_driver tegra194_cpufreq_driver = { 582 .name = "tegra194", 583 .flags = CPUFREQ_CONST_LOOPS | CPUFREQ_NEED_INITIAL_FREQ_CHECK | 584 CPUFREQ_IS_COOLING_DEV, 585 .verify = cpufreq_generic_frequency_table_verify, 586 .target_index = tegra194_cpufreq_set_target, 587 .get = tegra194_get_speed, 588 .init = tegra194_cpufreq_init, 589 .exit = tegra194_cpufreq_exit, 590 .online = tegra194_cpufreq_online, 591 .offline = tegra194_cpufreq_offline, 592 .attr = cpufreq_generic_attr, 593 }; 594 595 static struct tegra_cpufreq_ops tegra194_cpufreq_ops = { 596 .read_counters = tegra194_read_counters, 597 .get_cpu_cluster_id = tegra194_get_cpu_cluster_id, 598 .get_cpu_ndiv = tegra194_get_cpu_ndiv, 599 .set_cpu_ndiv = tegra194_set_cpu_ndiv, 600 }; 601 602 static const struct tegra_cpufreq_soc tegra194_cpufreq_soc = { 603 .ops = &tegra194_cpufreq_ops, 604 .maxcpus_per_cluster = 2, 605 .num_clusters = 4, 606 .refclk_delta_min = 16000, 607 }; 608 609 static void tegra194_cpufreq_free_resources(void) 610 { 611 destroy_workqueue(read_counters_wq); 612 } 613 614 static struct cpufreq_frequency_table * 615 tegra_cpufreq_bpmp_read_lut(struct platform_device *pdev, struct tegra_bpmp *bpmp, 616 unsigned int cluster_id) 617 { 618 struct cpufreq_frequency_table *freq_table; 619 struct mrq_cpu_ndiv_limits_response resp; 620 unsigned int num_freqs, ndiv, delta_ndiv; 621 struct mrq_cpu_ndiv_limits_request req; 622 struct tegra_bpmp_message msg; 623 u16 freq_table_step_size; 624 int err, index; 625 626 memset(&req, 0, sizeof(req)); 627 req.cluster_id = cluster_id; 628 629 memset(&msg, 0, sizeof(msg)); 630 msg.mrq = MRQ_CPU_NDIV_LIMITS; 631 msg.tx.data = &req; 632 msg.tx.size = sizeof(req); 633 msg.rx.data = &resp; 634 msg.rx.size = sizeof(resp); 635 636 err = tegra_bpmp_transfer(bpmp, &msg); 637 if (err) 638 return ERR_PTR(err); 639 if (msg.rx.ret == -BPMP_EINVAL) { 640 /* Cluster not available */ 641 return NULL; 642 } 643 if (msg.rx.ret) 644 return ERR_PTR(-EINVAL); 645 646 /* 647 * Make sure frequency table step is a multiple of mdiv to match 648 * vhint table granularity. 649 */ 650 freq_table_step_size = resp.mdiv * 651 DIV_ROUND_UP(CPUFREQ_TBL_STEP_HZ, resp.ref_clk_hz); 652 653 dev_dbg(&pdev->dev, "cluster %d: frequency table step size: %d\n", 654 cluster_id, freq_table_step_size); 655 656 delta_ndiv = resp.ndiv_max - resp.ndiv_min; 657 658 if (unlikely(delta_ndiv == 0)) { 659 num_freqs = 1; 660 } else { 661 /* We store both ndiv_min and ndiv_max hence the +1 */ 662 num_freqs = delta_ndiv / freq_table_step_size + 1; 663 } 664 665 num_freqs += (delta_ndiv % freq_table_step_size) ? 1 : 0; 666 667 freq_table = devm_kcalloc(&pdev->dev, num_freqs + 1, 668 sizeof(*freq_table), GFP_KERNEL); 669 if (!freq_table) 670 return ERR_PTR(-ENOMEM); 671 672 for (index = 0, ndiv = resp.ndiv_min; 673 ndiv < resp.ndiv_max; 674 index++, ndiv += freq_table_step_size) { 675 freq_table[index].driver_data = ndiv; 676 freq_table[index].frequency = map_ndiv_to_freq(&resp, ndiv); 677 } 678 679 freq_table[index].driver_data = resp.ndiv_max; 680 freq_table[index++].frequency = map_ndiv_to_freq(&resp, resp.ndiv_max); 681 freq_table[index].frequency = CPUFREQ_TABLE_END; 682 683 return freq_table; 684 } 685 686 static int tegra194_cpufreq_store_physids(unsigned int cpu, struct tegra194_cpufreq_data *data) 687 { 688 int num_cpus = data->soc->maxcpus_per_cluster * data->soc->num_clusters; 689 u32 cpuid, clusterid; 690 u64 mpidr_id; 691 692 if (cpu > (num_cpus - 1)) { 693 pr_err("cpufreq: wrong num of cpus or clusters in soc data\n"); 694 return -EINVAL; 695 } 696 697 data->soc->ops->get_cpu_cluster_id(cpu, &cpuid, &clusterid); 698 699 mpidr_id = (clusterid * data->soc->maxcpus_per_cluster) + cpuid; 700 701 data->cpu_data[cpu].cpuid = cpuid; 702 data->cpu_data[cpu].clusterid = clusterid; 703 data->cpu_data[cpu].freq_core_reg = SCRATCH_FREQ_CORE_REG(data, mpidr_id); 704 705 return 0; 706 } 707 708 static int tegra194_cpufreq_probe(struct platform_device *pdev) 709 { 710 const struct tegra_cpufreq_soc *soc; 711 struct tegra194_cpufreq_data *data; 712 struct tegra_bpmp *bpmp; 713 struct device *cpu_dev; 714 int err, i; 715 u32 cpu; 716 717 data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL); 718 if (!data) 719 return -ENOMEM; 720 721 soc = of_device_get_match_data(&pdev->dev); 722 723 if (soc->ops && soc->maxcpus_per_cluster && soc->num_clusters && soc->refclk_delta_min) { 724 data->soc = soc; 725 } else { 726 dev_err(&pdev->dev, "soc data missing\n"); 727 return -EINVAL; 728 } 729 730 data->bpmp_luts = devm_kcalloc(&pdev->dev, data->soc->num_clusters, 731 sizeof(*data->bpmp_luts), GFP_KERNEL); 732 if (!data->bpmp_luts) 733 return -ENOMEM; 734 735 if (soc->actmon_cntr_base) { 736 /* mmio registers are used for frequency request and re-construction */ 737 data->regs = devm_platform_ioremap_resource(pdev, 0); 738 if (IS_ERR(data->regs)) 739 return PTR_ERR(data->regs); 740 } 741 742 data->cpu_data = devm_kcalloc(&pdev->dev, data->soc->num_clusters * 743 data->soc->maxcpus_per_cluster, 744 sizeof(*data->cpu_data), GFP_KERNEL); 745 if (!data->cpu_data) 746 return -ENOMEM; 747 748 platform_set_drvdata(pdev, data); 749 750 bpmp = tegra_bpmp_get(&pdev->dev); 751 if (IS_ERR(bpmp)) 752 return PTR_ERR(bpmp); 753 754 read_counters_wq = alloc_workqueue("read_counters_wq", __WQ_LEGACY, 1); 755 if (!read_counters_wq) { 756 dev_err(&pdev->dev, "fail to create_workqueue\n"); 757 err = -EINVAL; 758 goto put_bpmp; 759 } 760 761 for (i = 0; i < data->soc->num_clusters; i++) { 762 data->bpmp_luts[i] = tegra_cpufreq_bpmp_read_lut(pdev, bpmp, i); 763 if (IS_ERR(data->bpmp_luts[i])) { 764 err = PTR_ERR(data->bpmp_luts[i]); 765 goto err_free_res; 766 } 767 } 768 769 for_each_possible_cpu(cpu) { 770 err = tegra194_cpufreq_store_physids(cpu, data); 771 if (err) 772 goto err_free_res; 773 } 774 775 tegra194_cpufreq_driver.driver_data = data; 776 777 /* Check for optional OPPv2 and interconnect paths on CPU0 to enable ICC scaling */ 778 cpu_dev = get_cpu_device(0); 779 if (!cpu_dev) { 780 err = -EPROBE_DEFER; 781 goto err_free_res; 782 } 783 784 if (dev_pm_opp_of_get_opp_desc_node(cpu_dev)) { 785 err = dev_pm_opp_of_find_icc_paths(cpu_dev, NULL); 786 if (!err) 787 data->icc_dram_bw_scaling = true; 788 } 789 790 err = cpufreq_register_driver(&tegra194_cpufreq_driver); 791 if (!err) 792 goto put_bpmp; 793 794 err_free_res: 795 tegra194_cpufreq_free_resources(); 796 put_bpmp: 797 tegra_bpmp_put(bpmp); 798 return err; 799 } 800 801 static void tegra194_cpufreq_remove(struct platform_device *pdev) 802 { 803 cpufreq_unregister_driver(&tegra194_cpufreq_driver); 804 tegra194_cpufreq_free_resources(); 805 } 806 807 static const struct of_device_id tegra194_cpufreq_of_match[] = { 808 { .compatible = "nvidia,tegra194-ccplex", .data = &tegra194_cpufreq_soc }, 809 { .compatible = "nvidia,tegra234-ccplex-cluster", .data = &tegra234_cpufreq_soc }, 810 { .compatible = "nvidia,tegra239-ccplex-cluster", .data = &tegra239_cpufreq_soc }, 811 { /* sentinel */ } 812 }; 813 MODULE_DEVICE_TABLE(of, tegra194_cpufreq_of_match); 814 815 static struct platform_driver tegra194_ccplex_driver = { 816 .driver = { 817 .name = "tegra194-cpufreq", 818 .of_match_table = tegra194_cpufreq_of_match, 819 }, 820 .probe = tegra194_cpufreq_probe, 821 .remove = tegra194_cpufreq_remove, 822 }; 823 module_platform_driver(tegra194_ccplex_driver); 824 825 MODULE_AUTHOR("Mikko Perttunen <mperttunen@nvidia.com>"); 826 MODULE_AUTHOR("Sumit Gupta <sumitg@nvidia.com>"); 827 MODULE_DESCRIPTION("NVIDIA Tegra194 cpufreq driver"); 828 MODULE_LICENSE("GPL v2"); 829