1 files changed, 47 insertions, 1 deletions

diff --git a/arch/arm/mach-tegra/cpu-tegra.c b/arch/arm/mach-tegra/cpu-tegra.c
index ceb52db1e2f1..627bf0f4262e 100644
--- a/arch/arm/mach-tegra/cpu-tegra.c
+++ b/arch/arm/mach-tegra/cpu-tegra.c
@@ -49,6 +49,8 @@ static struct cpufreq_frequency_table freq_table[] = {
 #define NUM_CPUS        2
 
 static struct clk *cpu_clk;
+static struct clk *pll_x_clk;
+static struct clk *pll_p_clk;
 static struct clk *emc_clk;
 
 static unsigned long target_cpu_speed[NUM_CPUS];
@@ -71,6 +73,42 @@ static unsigned int tegra_getspeed(unsigned int cpu)
         return rate;
 }
 
+static int tegra_cpu_clk_set_rate(unsigned long rate)
+{
+        int ret;
+
+        /*
+         * Take an extra reference to the main pll so it doesn't turn
+         * off when we move the cpu off of it
+         */
+        clk_prepare_enable(pll_x_clk);
+
+        ret = clk_set_parent(cpu_clk, pll_p_clk);
+        if (ret) {
+                pr_err("Failed to switch cpu to clock pll_p\n");
+                goto out;
+        }
+
+        if (rate == clk_get_rate(pll_p_clk))
+                goto out;
+
+        ret = clk_set_rate(pll_x_clk, rate);
+        if (ret) {
+                pr_err("Failed to change pll_x to %lu\n", rate);
+                goto out;
+        }
+
+        ret = clk_set_parent(cpu_clk, pll_x_clk);
+        if (ret) {
+                pr_err("Failed to switch cpu to clock pll_x\n");
+                goto out;
+        }
+
+out:
+        clk_disable_unprepare(pll_x_clk);
+        return ret;
+}
+
 static int tegra_update_cpu_speed(unsigned long rate)
 {
         int ret = 0;
@@ -101,7 +139,7 @@ static int tegra_update_cpu_speed(unsigned long rate)
                freqs.old, freqs.new);
 #endif
 
-        ret = clk_set_rate(cpu_clk, freqs.new * 1000);
+        ret = tegra_cpu_clk_set_rate(freqs.new * 1000);
         if (ret) {
                 pr_err("cpu-tegra: Failed to set cpu frequency to %d kHz\n",
                         freqs.new);
@@ -183,6 +221,14 @@ static int tegra_cpu_init(struct cpufreq_policy *policy)
         if (IS_ERR(cpu_clk))
                 return PTR_ERR(cpu_clk);
 
+        pll_x_clk = clk_get_sys(NULL, "pll_x");
+        if (IS_ERR(pll_x_clk))
+                return PTR_ERR(pll_x_clk);
+
+        pll_p_clk = clk_get_sys(NULL, "pll_p");
+        if (IS_ERR(pll_p_clk))
+                return PTR_ERR(pll_p_clk);
+
         emc_clk = clk_get_sys("cpu", "emc");
         if (IS_ERR(emc_clk)) {
                 clk_put(cpu_clk);