829 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (c) 2020 - 2022, NVIDIA CORPORATION. All rights reserved4 */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 100022#define REF_CLK_MHZ 408 /* 408 MHz */23#define CPUFREQ_TBL_STEP_HZ (50 * KHZ * KHZ)24#define MAX_CNT ~0U25 26#define MAX_DELTA_KHZ 11520027 28#define NDIV_MASK 0x1FF29 30#define CORE_OFFSET(cpu) (cpu * 8)31#define CMU_CLKS_BASE 0x200032#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 42struct tegra_cpu_data {43 u32 cpuid;44 u32 clusterid;45 void __iomem *freq_core_reg;46};47 48struct tegra_cpu_ctr {49 u32 cpu;50 u32 coreclk_cnt, last_coreclk_cnt;51 u32 refclk_cnt, last_refclk_cnt;52};53 54struct read_counters_work {55 struct work_struct work;56 struct tegra_cpu_ctr c;57};58 59struct 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 66struct 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 74struct 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 82static struct workqueue_struct *read_counters_wq;83 84static 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 107static void tegra_get_cpu_mpidr(void *mpidr)108{109 *((u64 *)mpidr) = read_cpuid_mpidr() & MPIDR_HWID_BITMASK;110}111 112static 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 124static 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 133static 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 single144 * 64-bit read. The counter values are used to determine the average145 * 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 cycle148 */149static 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 reference166 * 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 else175 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 184static 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 191static 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 199static 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 207static 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 to222 * determine the average actual frequency a core has run at over223 * 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 cycle226 * where the core is architecturally clocking227 */228static 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 237static inline u32 map_ndiv_to_freq(struct mrq_cpu_ndiv_limits_response238 *nltbl, u16 ndiv)239{240 return nltbl->ref_clk_hz / KHZ * ndiv / (nltbl->pdiv * nltbl->mdiv);241}242 243static 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 reference256 * 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 else265 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 274static 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 counter284 * = 10526880 usec = 10.527 sec to overflow285 *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 at288 * freq of cluster. Assuming max cluster clock ~2000MHz,289 * It will take = 2 ^ 32 / 2000 MHz to overflow core clk counter290 * = ~2.147 sec to overflow291 */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 speed301 * 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 us305 * - Read above cycle counters again306 * - Calculates freq by subtracting current and previous counters307 * divided by the delay time or eqv. of ref_clk_counter in delta time308 * - Return Kcycles/second, freq in KHz309 *310 * delta time period = x sec311 * = delta ref_clk_counter / (408 * 10^6) sec312 * freq in Hz = cycles/sec313 * = (delta cycles / x sec314 * = (delta cycles * 408 * 10^6) / delta ref_clk_counter315 * in KHz = (delta cycles * 408 * 10^3) / delta ref_clk_counter316 *317 * @cpu - logical cpu whose freq to be updated318 * Returns freq in KHz on success, 0 if cpu is offline319 */320static 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 else341 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 else349 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 359static 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 368static 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 373static 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 380static 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 385static 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 from404 * the last written value, then return freq corresponding405 * to the last written ndiv value from freq_table. This is406 * 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 423static 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 500static 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 538static 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 544static int tegra194_cpufreq_offline(struct cpufreq_policy *policy)545{546 /*547 * Preserve policy->driver_data and don't free resources on light-weight548 * tear down.549 */550 551 return 0;552}553 554static 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 562static 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 cores570 * in a cluster run at same frequency which is the maximum frequency571 * 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 581static 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 595static 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 602static 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 609static void tegra194_cpufreq_free_resources(void)610{611 destroy_workqueue(read_counters_wq);612}613 614static struct cpufreq_frequency_table *615tegra_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 match648 * 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 686static 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 708static 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 794err_free_res:795 tegra194_cpufreq_free_resources();796put_bpmp:797 tegra_bpmp_put(bpmp);798 return err;799}800 801static void tegra194_cpufreq_remove(struct platform_device *pdev)802{803 cpufreq_unregister_driver(&tegra194_cpufreq_driver);804 tegra194_cpufreq_free_resources();805}806 807static 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};813MODULE_DEVICE_TABLE(of, tegra194_cpufreq_of_match);814 815static 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_new = tegra194_cpufreq_remove,822};823module_platform_driver(tegra194_ccplex_driver);824 825MODULE_AUTHOR("Mikko Perttunen <mperttunen@nvidia.com>");826MODULE_AUTHOR("Sumit Gupta <sumitg@nvidia.com>");827MODULE_DESCRIPTION("NVIDIA Tegra194 cpufreq driver");828MODULE_LICENSE("GPL v2");829