[ARM] tegra: cpufreq: Adjust twds on frequency scaling

Change-Id: I8d187b861e18dfefcbf4566f4c695dfd1e17fe98
Signed-off-by: Colin Cross <ccross@google.com>

diff --git a/arch/arm/mach-tegra/cpu-tegra.c b/arch/arm/mach-tegra/cpu-tegra.c
index 8363a21a8e69b9cff14a59ee8c4e0d202668a9ee..1a8a27a8649cbecbdafee297aa260242f8be05ac 100644 (file)
--- a/arch/arm/mach-tegra/cpu-tegra.c
+++ b/arch/arm/mach-tegra/cpu-tegra.c
@@ -30,6 +30,7 @@
 #include <linux/io.h>
 #include <linux/suspend.h>
 
+#include <asm/smp_twd.h>
 #include <asm/system.h>
 
 #include <mach/hardware.h>
@@ -72,6 +73,17 @@ unsigned int tegra_getspeed(unsigned int cpu)
        return rate;
 }
 
+static void tegra_cpufreq_rescale_twd_other_cpu(void *data) {
+       unsigned long new_rate = *(unsigned long *)data;
+       twd_recalc_prescaler(new_rate);
+}
+
+static void tegra_cpufreq_rescale_twds(unsigned long new_rate)
+{
+       twd_recalc_prescaler(new_rate);
+       smp_call_function(tegra_cpufreq_rescale_twd_other_cpu, &new_rate, 1);
+}
+
 static int tegra_update_cpu_speed(unsigned long rate)
 {
        int ret = 0;
@@ -86,6 +98,9 @@ static int tegra_update_cpu_speed(unsigned long rate)
        for_each_online_cpu(freqs.cpu)
                cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
 
+       if (freqs.new > freqs.old)
+               tegra_cpufreq_rescale_twds(freqs.new * 1000);
+
 #ifdef CONFIG_CPU_FREQ_DEBUG
        printk(KERN_DEBUG "cpufreq-tegra: transition: %u --> %u\n",
               freqs.old, freqs.new);
@@ -98,6 +113,9 @@ static int tegra_update_cpu_speed(unsigned long rate)
                return ret;
        }
 
+       if (freqs.new < freqs.old)
+               tegra_cpufreq_rescale_twds(freqs.new * 1000);
+
        for_each_online_cpu(freqs.cpu)
                cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);