[RFC,07/11] ARM: cpu topology: Modify cpu_power according to cpufreq

diff --git a/arch/arm/kernel/topology.c b/arch/arm/kernel/topology.c
index 1196f2d..9f75967 100644
--- a/arch/arm/kernel/topology.c
+++ b/arch/arm/kernel/topology.c
@@ -20,6 +20,10 @@ 
 #include <linux/sched.h>
 #include <linux/cpumask.h>
 
+#ifdef CONFIG_CPU_FREQ
+#include <linux/cpufreq.h>
+#endif
+
 #include <asm/cputype.h>
 #include <asm/topology.h>
 
@@ -65,29 +69,35 @@  static DEFINE_PER_CPU(unsigned int, cpu_scale);
 
 struct cputopo_scale {
 	int scale;
+	int freq;
 };
 
 static struct cputopo_scale cpu_power[NR_CPUS];
 
-#define CPU_TOPO_MAX_SCALING 2
+#define CPU_MAX_SCALING 2
+#define CPU_MAX_FREQ 10
+/* we use a 200Mhz step for scaling cpu power */
+#define CPU_TOPO_FREQ_STEP 200000
 
 #define ARM_CORTEX_A9_DEFAULT_SCALE 0
 #define ARM_CORTEX_A9_POWER_SCALE 1
 
-/* This table sets the cpu_power scale of a cpu according to the sched_mc mode.
- * The content of this table could be SoC specific so we should add a method to
- * overwrite this default table.
+/* This table sets the cpu_power scale of a cpu according to 2 inputs which are
+ * the frequency and the sched_mc mode. The content of this table could be SoC
+ * specific so we should add a method to overwrite this default table.
  * TODO: Study how to use DT for setting this table
  */
-static unsigned long table_cpu_power[CPU_TOPO_MAX_SCALING] = {
-	{1024}, /* default */
-	{4096}, /* Power save mode CA9 MP */
+static unsigned long table_cpu_power[CPU_MAX_SCALING][CPU_MAX_FREQ] = {
+/*	    0   200   400   600   800  1000  1200  1400  1600  1800  */
+	{1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024, 1024}, /* default */
+	{4096, 4096, 4096, 1024, 1024, 1024, 1024, 1024, 1024, 1024}, /* Power save mode CA9 MP */
 };
 
 static void set_power_scale(unsigned int cpuid, unsigned int idx)
 {
 	cpu_power[cpuid].scale = idx;
-	per_cpu(cpu_scale, cpuid) = table_cpu_power[cpu_power[cpuid].scale];
+	per_cpu(cpu_scale, cpuid) =
+		table_cpu_power[idx][cpu_power[cpuid].freq];
 	smp_wmb();
 }
 
@@ -97,6 +107,54 @@  static int topo_cpuscale_init(void)
 	return 0;
 }
 
+#ifdef CONFIG_CPU_FREQ
+static void set_cpufreq_scale(unsigned int cpuid, unsigned int idx)
+{
+	cpu_power[cpuid].freq = idx;
+	per_cpu(cpu_scale, cpuid) =
+		table_cpu_power[cpu_power[cpuid].scale][idx];
+	smp_wmb();
+}
+
+static int topo_cpufreq_transition(struct notifier_block *nb,
+	unsigned long state, void *data)
+{
+	struct cpufreq_freqs *freqs = data;
+
+	if (state == CPUFREQ_POSTCHANGE || state == CPUFREQ_RESUMECHANGE)
+		set_cpufreq_scale(freqs->cpu, freqs->new / CPU_TOPO_FREQ_STEP);
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block topo_cpufreq_nb = {
+	.notifier_call = topo_cpufreq_transition,
+};
+
+static int topo_cpufreq_init(void)
+{
+	/* TODO set initial value according to current freq */
+
+	return cpufreq_register_notifier(&topo_cpufreq_nb,
+			CPUFREQ_TRANSITION_NOTIFIER);
+}
+#else
+static inline int topo_cpufreq_init(void) {return 0; }
+#endif
+
+static int init_cpu_power_scale(void)
+{
+	/* select cpu scale configuration */
+	topo_cpuscale_init();
+
+	/* register cpufreq notifer */
+	topo_cpufreq_init();
+
+	return 0;
+}
+
+core_initcall(init_cpu_power_scale);
+
 /*
  * Update the cpu power
  */
@@ -339,6 +397,7 @@  void init_cpu_topology(void)
 		per_cpu(cpu_scale, cpu) = SCHED_POWER_SCALE;
 
 		cpu_power[cpu].scale = ARM_CORTEX_A9_DEFAULT_SCALE;
+		cpu_power[cpu].freq = 0;
 	}
 	smp_wmb();
 }