3 files changed, 55 insertions, 8 deletions

diff --git a/arch/arm/mach-tegra/cpu-tegra.c b/arch/arm/mach-tegra/cpu-tegra.c
index baf543674c3d..1d61146b50cc 100644
--- a/arch/arm/mach-tegra/cpu-tegra.c
+++ b/arch/arm/mach-tegra/cpu-tegra.c
@@ -362,6 +362,17 @@ static int tegra_update_cpu_speed(unsigned long rate)
 	return 0;
 }
 
+unsigned int tegra_count_slow_cpus(unsigned long speed_limit)
+{
+	unsigned int cnt = 0;
+	int i;
+
+	for_each_online_cpu(i)
+		if (target_cpu_speed[i] <= speed_limit)
+			cnt++;
+	return cnt;
+}
+
 unsigned int tegra_get_slowest_cpu_n(void) {
 	unsigned int cpu = nr_cpu_ids;
 	unsigned long rate = ULONG_MAX;
diff --git a/arch/arm/mach-tegra/cpu-tegra3.c b/arch/arm/mach-tegra/cpu-tegra3.c
index 88adea4fd792..acd6889e54f7 100644
--- a/arch/arm/mach-tegra/cpu-tegra3.c
+++ b/arch/arm/mach-tegra/cpu-tegra3.c
@@ -178,6 +178,26 @@ static struct kernel_param_ops tegra_hp_state_ops = {
 module_param_cb(auto_hotplug, &tegra_hp_state_ops, &hp_state, 0644);
 
 
+enum {
+	TEGRA_CPU_SPEED_BALANCED,
+	TEGRA_CPU_SPEED_BIASED,
+	TEGRA_CPU_SPEED_SKEWED,
+};
+
+static int tegra_cpu_speed_balance(void)
+{
+	unsigned long highest_speed = tegra_cpu_highest_speed();
+
+	/* balanced: freq targets for all CPUs are above 50% of highest speed
+	   biased: freq target for at least one CPU is below 50% threshold
+	   skewed: freq targets for at least 2 CPUs are below 25% threshold */
+	if (tegra_count_slow_cpus(highest_speed / 4) >= 2)
+		return TEGRA_CPU_SPEED_SKEWED;
+	else if (tegra_count_slow_cpus(highest_speed / 2) >= 1)
+		return TEGRA_CPU_SPEED_BIASED;
+	return TEGRA_CPU_SPEED_BALANCED;
+}
+
 static void tegra_auto_hotplug_work_func(struct work_struct *work)
 {
 	bool up = false;
@@ -213,17 +233,32 @@ static void tegra_auto_hotplug_work_func(struct work_struct *work)
 				/* catch-up with governor target speed */
 				tegra_cpu_cap_highest_speed(NULL);
 			}
-			queue_delayed_work(
-				hotplug_wq, &hotplug_work, up2gn_delay);
 		} else {
-			cpu = cpumask_next_zero(0, cpu_online_mask);
-			if (cpu < nr_cpu_ids) {
-				up = true;
-				queue_delayed_work(
-					hotplug_wq, &hotplug_work, up2gn_delay);
-				hp_stats_update(cpu, true);
+			switch (tegra_cpu_speed_balance()) {
+			/* cpu speed is up and balanced - one more on-line */
+			case TEGRA_CPU_SPEED_BALANCED:
+				cpu = cpumask_next_zero(0, cpu_online_mask);
+				if (cpu < nr_cpu_ids) {
+					up = true;
+					hp_stats_update(cpu, true);
+				}
+				break;
+			/* cpu speed is up, but skewed - remove one core */
+			case TEGRA_CPU_SPEED_SKEWED:
+				cpu = tegra_get_slowest_cpu_n();
+				if (cpu < nr_cpu_ids) {
+					up = false;
+					hp_stats_update(cpu, false);
+				}
+				break;
+			/* cpu speed is up, but under-utilized - do nothing */
+			case TEGRA_CPU_SPEED_BIASED:
+			default:
+				break;
 			}
 		}
+		queue_delayed_work(
+			hotplug_wq, &hotplug_work, up2gn_delay);
 		break;
 	default:
 		pr_err("%s: invalid tegra hotplug state %d\n",
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
index 0f1719ed5a36..55c2cc044466 100644
--- a/arch/arm/mach-tegra/pm.h
+++ b/arch/arm/mach-tegra/pm.h
@@ -81,6 +81,7 @@ void __init tegra_init_suspend(struct tegra_suspend_platform_data *plat);
 
 void tegra_idle_lp2(void);
 
+unsigned int tegra_count_slow_cpus(unsigned long speed_limit);
 unsigned int tegra_get_slowest_cpu_n(void);
 unsigned long tegra_cpu_lowest_speed(void);
 unsigned long tegra_cpu_highest_speed(void);