Frequency returned by 'cpuinfo_cur_freq' using counters is not fixed and keeps changing slightly. This change returns a consistent value from freq_table. If the reconstructed frequency has acceptable delta from the last written value, then return the frequency corresponding to the last written ndiv value from freq_table. Otherwise, print a warning and return the reconstructed freq.
Signed-off-by: Sumit Gupta <[email protected]> --- drivers/cpufreq/tegra194-cpufreq.c | 66 ++++++++++++++++++++++++++++++++------ 1 file changed, 57 insertions(+), 9 deletions(-) diff --git a/drivers/cpufreq/tegra194-cpufreq.c b/drivers/cpufreq/tegra194-cpufreq.c index e1d931c..d5b608d 100644 --- a/drivers/cpufreq/tegra194-cpufreq.c +++ b/drivers/cpufreq/tegra194-cpufreq.c @@ -180,9 +180,65 @@ static unsigned int tegra194_get_speed_common(u32 cpu, u32 delay) return (rate_mhz * KHZ); /* in KHz */ } +static void get_cpu_ndiv(void *ndiv) +{ + u64 ndiv_val; + + asm volatile("mrs %0, s3_0_c15_c0_4" : "=r" (ndiv_val) : ); + + *(u64 *)ndiv = ndiv_val; +} + +static void set_cpu_ndiv(void *data) +{ + struct cpufreq_frequency_table *tbl = data; + u64 ndiv_val = (u64)tbl->driver_data; + + asm volatile("msr s3_0_c15_c0_4, %0" : : "r" (ndiv_val)); +} + static unsigned int tegra194_get_speed(u32 cpu) { - return tegra194_get_speed_common(cpu, US_DELAY); + struct cpufreq_frequency_table *table, *pos; + struct cpufreq_policy policy; + unsigned int rate; + u64 ndiv; + int err; + + cpufreq_get_policy(&policy, cpu); + table = policy.freq_table; + + /* reconstruct actual cpu freq using counters*/ + rate = tegra194_get_speed_common(cpu, US_DELAY); + + /* get last written ndiv value*/ + err = smp_call_function_single(cpu, get_cpu_ndiv, &ndiv, true); + if (err) { + pr_err("cpufreq: Failed to get ndiv for CPU%d, ret:%d\n", + cpu, err); + return rate; + } + + /* if the reconstructed frequency has acceptable delta from + * the last written value, then return freq corresponding + * to the last written ndiv value from freq_table. This is + * done to return consistent value. + */ + cpufreq_for_each_valid_entry(pos, table) { + if (pos->driver_data != ndiv) + continue; + + if (abs(pos->frequency - rate) > 115200) { + pr_warn("cpufreq: high delta (%d) on CPU%d\n", + abs(pos->frequency - rate), cpu); + pr_warn("cpufreq: cur:%u, set:%u, set ndiv:%llu\n", + rate, pos->frequency, ndiv); + } else { + rate = pos->frequency; + } + break; + } + return rate; } static int tegra194_cpufreq_init(struct cpufreq_policy *policy) @@ -209,14 +265,6 @@ static int tegra194_cpufreq_init(struct cpufreq_policy *policy) return 0; } -static void set_cpu_ndiv(void *data) -{ - struct cpufreq_frequency_table *tbl = data; - u64 ndiv_val = (u64)tbl->driver_data; - - asm volatile("msr s3_0_c15_c0_4, %0" : : "r" (ndiv_val)); -} - static int tegra194_cpufreq_set_target(struct cpufreq_policy *policy, unsigned int index) { -- 2.7.4
