cpufreq: add support for intermediate (stable)

index a05c921..9bf12a2 100644
@@ -1816,12 +1816,43 @@ EXPORT_SYMBOL(cpufreq_unregister_notifier);
  *                              GOVERNORS                            *
  *********************************************************************/
 
+static int __target_index(struct cpufreq_policy *policy,
+			  struct cpufreq_frequency_table *freq_table, int index)
+{
+	struct cpufreq_freqs freqs;
+	int retval = -EINVAL;
+	bool notify;
+
+	notify = !(cpufreq_driver->flags & CPUFREQ_ASYNC_NOTIFICATION);
+
+	if (notify) {
+		freqs.old = policy->cur;
+		freqs.new = freq_table[index].frequency;
+		freqs.flags = 0;
+
+		pr_debug("%s: cpu: %d, oldfreq: %u, new freq: %u\n",
+				__func__, policy->cpu, freqs.old, freqs.new);
+
+		cpufreq_freq_transition_begin(policy, &freqs);
+	}
+
+	retval = cpufreq_driver->target_index(policy, index);
+	if (retval)
+		pr_err("%s: Failed to change cpu frequency: %d\n",
+				__func__, retval);
+
+	if (notify)
+		cpufreq_freq_transition_end(policy, &freqs, retval);
+
+	return retval;
+}
+
 int __cpufreq_driver_target(struct cpufreq_policy *policy,
 			    unsigned int target_freq,
 			    unsigned int relation)
 {
-	int retval = -EINVAL;
 	unsigned int old_target_freq = target_freq;
+	int retval = -EINVAL;
 
 	if (cpufreq_disabled())
 		return -ENODEV;

@@ -1848,8 +1879,6 @@ int __cpufreq_driver_target(struct cpufreq_policy *policy,
 		retval = cpufreq_driver->target(policy, target_freq, relation);
 	else if (cpufreq_driver->target_index) {
 		struct cpufreq_frequency_table *freq_table;
-		struct cpufreq_freqs freqs;
-		bool notify;
 		int index;
 
 		freq_table = cpufreq_frequency_get_table(policy->cpu);

@@ -1870,26 +1899,7 @@ int __cpufreq_driver_target(struct cpufreq_policy *policy,
 			goto out;
 		}
 
-		notify = !(cpufreq_driver->flags & CPUFREQ_ASYNC_NOTIFICATION);
-
-		if (notify) {
-			freqs.old = policy->cur;
-			freqs.new = freq_table[index].frequency;
-			freqs.flags = 0;
-
-			pr_debug("%s: cpu: %d, oldfreq: %u, new freq: %u\n",
-				 __func__, policy->cpu, freqs.old, freqs.new);
-
-			cpufreq_freq_transition_begin(policy, &freqs);
-		}
-
-		retval = cpufreq_driver->target_index(policy, index);
-		if (retval)
-			pr_err("%s: Failed to change cpu frequency: %d\n",
-			       __func__, retval);
-
-		if (notify)
-			cpufreq_freq_transition_end(policy, &freqs, retval);
+		retval = __target_index(policy, freq_table, index);
 	}
 
 out:

index 9bf12a2..f38f2f2 100644
@@ -1819,27 +1819,50 @@ EXPORT_SYMBOL(cpufreq_unregister_notifier);
 static int __target_index(struct cpufreq_policy *policy,
 			  struct cpufreq_frequency_table *freq_table, int index)
 {
-	struct cpufreq_freqs freqs;
+	struct cpufreq_freqs freqs = {.old = policy->cur, .flags = 0};
 	int retval = -EINVAL;
 	bool notify;
 
 	notify = !(cpufreq_driver->flags & CPUFREQ_ASYNC_NOTIFICATION);
+	if (!notify)
+		goto skip_notify;
 
-	if (notify) {
-		freqs.old = policy->cur;
-		freqs.new = freq_table[index].frequency;
-		freqs.flags = 0;
+	/* Handle switching to intermediate frequency */
+	if (cpufreq_driver->get_intermediate) {
+		freqs.new = cpufreq_driver->get_intermediate(policy, index);
 
-		pr_debug("%s: cpu: %d, oldfreq: %u, new freq: %u\n",
+		pr_debug("%s: cpu: %d, switching to intermediate freq: oldfreq: %u, intermediate freq: %u\n",
 				__func__, policy->cpu, freqs.old, freqs.new);
 
 		cpufreq_freq_transition_begin(policy, &freqs);
+		retval = cpufreq_driver->target_intermediate(policy, freqs.new);
+		cpufreq_freq_transition_end(policy, &freqs, retval);
+
+		if (retval) {
+			pr_err("%s: Failed to change to intermediate frequency: %d\n",
+				__func__, retval);
+			return retval;
+		}
+
+		/* Set intermediate as old freq */
+		freqs.old = freqs.new;
 	}
 
+	freqs.new = freq_table[index].frequency;
+
+	pr_debug("%s: cpu: %d, oldfreq: %u, new freq: %u\n", __func__,
+		 policy->cpu, freqs.old, freqs.new);
+
+	cpufreq_freq_transition_begin(policy, &freqs);
+
+skip_notify:
 	retval = cpufreq_driver->target_index(policy, index);
-	if (retval)
+	if (retval) {
+		/* We shouldn't fail after setting intermediate freq */
+		WARN_ON(cpufreq_driver->get_intermediate);
 		pr_err("%s: Failed to change cpu frequency: %d\n",
 				__func__, retval);
+	}
 
 	if (notify)
 		cpufreq_freq_transition_end(policy, &freqs, retval);

@@ -2361,7 +2384,8 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 	    !(driver_data->setpolicy || driver_data->target_index ||
 		    driver_data->target) ||
 	     (driver_data->setpolicy && (driver_data->target_index ||
-		    driver_data->target)))
+		    driver_data->target)) ||
+	     (!!driver_data->get_intermediate ^ !!driver_data->target_intermediate))
 		return -EINVAL;
 
 	pr_debug("trying to register driver %s\n", driver_data->name);

index 3f45889..bfcba11 100644
@@ -226,6 +226,21 @@ struct cpufreq_driver {
 				 unsigned int relation);
 	int	(*target_index)	(struct cpufreq_policy *policy,
 				 unsigned int index);
+	/*
+	 * Only for drivers with target_index() and CPUFREQ_ASYNC_NOTIFICATION
+	 * unset.
+	 *
+	 * get_intermediate should return a stable intermediate frequency
+	 * platform wants to switch to and target_intermediate() should set CPU
+	 * to to that frequency, before jumping to the frequency corresponding
+	 * to 'index'. Core will take care of sending notifications and driver
+	 * doesn't have to handle them in target_intermediate() or
+	 * target_index().
+	 */
+	unsigned int (*get_intermediate)(struct cpufreq_policy *policy,
+					 unsigned int index);
+	int	(*target_intermediate)(struct cpufreq_policy *policy,
+				       unsigned int frequency);
 
 	/* should be defined, if possible */
 	unsigned int	(*get)	(unsigned int cpu);

index 63f0059..6e774c6 100644
@@ -82,9 +82,9 @@ out:
 	return ret;
 }
 
-static int tegra_update_cpu_speed(struct cpufreq_policy *policy,
-		unsigned long rate)
+static int tegra_target(struct cpufreq_policy *policy, unsigned int index)
 {
+	unsigned long rate = freq_table[index].frequency;
 	int ret = 0;
 
 	/*

@@ -106,11 +106,6 @@ static int tegra_update_cpu_speed(struct cpufreq_policy *policy,
 	return ret;
 }
 
-static int tegra_target(struct cpufreq_policy *policy, unsigned int index)
-{
-	return tegra_update_cpu_speed(policy, freq_table[index].frequency);
-}
-
 static int tegra_cpu_init(struct cpufreq_policy *policy)
 {
 	int ret;

index 6e774c6..fc66442 100644
@@ -46,10 +46,19 @@ static struct clk *pll_x_clk;
 static struct clk *pll_p_clk;
 static struct clk *emc_clk;
 
-static int tegra_cpu_clk_set_rate(unsigned long rate)
+static unsigned int
+tegra_get_intermediate(struct cpufreq_policy *policy, unsigned int index)
+{
+	return clk_get_rate(pll_p_clk) / 1000; /* kHz */
+}
+
+static int
+tegra_target_intermediate(struct cpufreq_policy *policy, unsigned int frequency)
 {
 	int ret;
 
+	WARN_ON(frequency != clk_get_rate(pll_p_clk) / 1000);
+
 	/*
 	 * Take an extra reference to the main pll so it doesn't turn
 	 * off when we move the cpu off of it

@@ -57,28 +66,9 @@ static int tegra_cpu_clk_set_rate(unsigned long rate)
 	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;
-	}
+	if (ret)
+		clk_disable_unprepare(pll_x_clk);
 
-out:
-	clk_disable_unprepare(pll_x_clk);
 	return ret;
 }
 

@@ -98,10 +88,21 @@ static int tegra_target(struct cpufreq_policy *policy, unsigned int index)
 	else
 		clk_set_rate(emc_clk, 100000000);  /* emc 50Mhz */
 
-	ret = tegra_cpu_clk_set_rate(rate * 1000);
+	if (rate == clk_get_rate(pll_p_clk))
+		goto out;
+
+	ret = clk_set_rate(pll_x_clk, rate * 1000);
+	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("cpu-tegra: Failed to set cpu frequency to %lu kHz\n",
-			rate);
+		pr_err("Failed to switch cpu to clock pll_x\n");
+
+out:
+	clk_disable_unprepare(pll_x_clk);
 
 	return ret;
 }

@@ -137,16 +138,18 @@ static int tegra_cpu_exit(struct cpufreq_policy *policy)
 }
 
 static struct cpufreq_driver tegra_cpufreq_driver = {
-	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK,
-	.verify		= cpufreq_generic_frequency_table_verify,
-	.target_index	= tegra_target,
-	.get		= cpufreq_generic_get,
-	.init		= tegra_cpu_init,
-	.exit		= tegra_cpu_exit,
-	.name		= "tegra",
-	.attr		= cpufreq_generic_attr,
+	.flags			= CPUFREQ_NEED_INITIAL_FREQ_CHECK,
+	.verify			= cpufreq_generic_frequency_table_verify,
+	.get_intermediate	= tegra_get_intermediate,
+	.target_intermediate	= tegra_target_intermediate,
+	.target_index		= tegra_target,
+	.get			= cpufreq_generic_get,
+	.init			= tegra_cpu_init,
+	.exit			= tegra_cpu_exit,
+	.name			= "tegra",
+	.attr			= cpufreq_generic_attr,
 #ifdef CONFIG_PM
-	.suspend	= cpufreq_generic_suspend,
+	.suspend		= cpufreq_generic_suspend,
 #endif
 };