From 6c20f0e49d265a2c4904944228df05e33fcc9031 Mon Sep 17 00:00:00 2001
From: Stephen Warren <swarren@nvidia.com>
Date: Mon, 10 Sep 2012 17:05:01 -0600
Subject: [PATCH] --- yaml --- r: 326649 b: refs/heads/master c:
 ce32ddaa7087da7c9d43751db423281711a1ff2e h: refs/heads/master i:   326647:
 fbd6dce0c0dce017c8806cab15fa274d1f6816f7 v: v3

---
 [refs]                                |  2 +-
 trunk/arch/arm/mach-tegra/cpu-tegra.c | 48 ++++++++++++++++++++++++++-
 2 files changed, 48 insertions(+), 2 deletions(-)

diff --git a/[refs] b/[refs]
index c7a5764a90df..6f00910224cb 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 7a74a4436b20980f8e19138e857bc7e39db5ad4b
+refs/heads/master: ce32ddaa7087da7c9d43751db423281711a1ff2e
diff --git a/trunk/arch/arm/mach-tegra/cpu-tegra.c b/trunk/arch/arm/mach-tegra/cpu-tegra.c
index ceb52db1e2f1..627bf0f4262e 100644
--- a/trunk/arch/arm/mach-tegra/cpu-tegra.c
+++ b/trunk/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);