[RFC,02/11] ARM: cpu topology: modify cpu topology

diff --git a/arch/arm/kernel/topology.c b/arch/arm/kernel/topology.c
index d89c66c..34512fe 100644
--- a/arch/arm/kernel/topology.c
+++ b/arch/arm/kernel/topology.c
@@ -44,6 +44,13 @@ 
 struct cputopo_arm cpu_topology[NR_CPUS];
 
 /*
+ * cpu topology mask management
+ */
+
+static void default_cpu_topology_mask(void);
+static void (*set_cpu_topology_mask)(void) = default_cpu_topology_mask;
+
+/*
  * default topology function
  */
 
@@ -66,26 +73,33 @@  static void clear_cpu_topology_mask(void)
 	smp_wmb();
 }
 
-static void default_cpu_topology_mask(unsigned int cpuid)
+/*
+ * default_cpu_topology_mask set the core and thread mask as described in the
+ * ARM ARM
+ */
+static void default_cpu_topology_mask(void)
 {
-	struct cputopo_arm *cpuid_topo = &cpu_topology[cpuid];
-	unsigned int cpu;
+	unsigned int cpuid, cpu;
 
-	for_each_possible_cpu(cpu) {
-		struct cputopo_arm *cpu_topo = &cpu_topology[cpu];
+	for_each_possible_cpu(cpuid) {
+		struct cputopo_arm *cpuid_topo = &cpu_topology[cpuid];
 
-		if (cpuid_topo->socket_id == cpu_topo->socket_id) {
-			cpumask_set_cpu(cpuid, &cpu_topo->core_sibling);
-			if (cpu != cpuid)
-				cpumask_set_cpu(cpu,
-					&cpuid_topo->core_sibling);
+		for_each_possible_cpu(cpu) {
+			struct cputopo_arm *cpu_topo = &cpu_topology[cpu];
 
-			if (cpuid_topo->core_id == cpu_topo->core_id) {
-				cpumask_set_cpu(cpuid,
-					&cpu_topo->thread_sibling);
+			if (cpuid_topo->socket_id == cpu_topo->socket_id) {
+				cpumask_set_cpu(cpuid, &cpu_topo->core_sibling);
 				if (cpu != cpuid)
 					cpumask_set_cpu(cpu,
-						&cpuid_topo->thread_sibling);
+						&cpuid_topo->core_sibling);
+
+				if (cpuid_topo->core_id == cpu_topo->core_id) {
+					cpumask_set_cpu(cpuid,
+						&cpu_topo->thread_sibling);
+					if (cpu != cpuid)
+						cpumask_set_cpu(cpu,
+							&cpuid_topo->thread_sibling);
+				}
 			}
 		}
 	}
@@ -93,6 +107,55 @@  static void default_cpu_topology_mask(unsigned int cpuid)
 }
 
 /*
+ * For Cortex-A9 MPcore dual core, we emulate a multi-package single core
+ * topology in power mode.
+ */
+static void power_cpu_topology_mask_CA9(void)
+{
+	unsigned int cpuid;
+	for_each_possible_cpu(cpuid) {
+		struct cputopo_arm *cpuid_topo = &(cpu_topology[cpuid]);
+
+		cpumask_set_cpu(cpuid, &cpuid_topo->core_sibling);
+		cpumask_set_cpu(cpuid, &cpuid_topo->thread_sibling);
+
+	}
+	smp_wmb();
+}
+
+#define ARM_FAMILY_MASK 0xFF0FFFF0
+#define ARM_CORTEX_A9_FAMILY 0x410FC090
+
+/* update_cpu_topology_policy select a cpu topology policy according to the
+ * available cores.
+ * TODO: The current version assumes that all cores are exactly the same which
+ * might not be true. We need to update it to take into account various
+ * configuration among which system with different kind of core.
+ */
+static int update_cpu_topology_policy(void)
+{
+	unsigned long cpuid;
+
+	if (sched_mc_power_savings == POWERSAVINGS_BALANCE_NONE) {
+		set_cpu_topology_mask = default_cpu_topology_mask;
+		return 0;
+	}
+
+	cpuid = read_cpuid_id();
+	cpuid &= ARM_FAMILY_MASK;
+
+	switch (cpuid) {
+	case ARM_CORTEX_A9_FAMILY:
+		set_cpu_topology_mask = power_cpu_topology_mask_CA9;
+	break;
+	default:
+		set_cpu_topology_mask = default_cpu_topology_mask;
+	break;
+	}
+
+	return 0;
+}
+/*
  * store_cpu_topology is called at boot when only one cpu is running
  * and with the mutex cpu_hotplug.lock locked, when several cpus have booted,
  * which prevents simultaneous write access to cpu_topology array
@@ -159,14 +222,15 @@  void store_cpu_topology(unsigned int cpuid)
  */
 int arch_update_cpu_topology(void)
 {
-	unsigned int cpuid;
-	/* clear core mask */
+
+	/* clear core threads mask */
 	clear_cpu_topology_mask();
 
-	/* update core and thread sibling masks */
-	for_each_possible_cpu(cpuid) {
-		default_cpu_topology_mask(cpuid);
-	}
+	/* set topology policy */
+	update_cpu_topology_policy();
+
+	/* set topology mask and power */
+	(*set_cpu_topology_mask)();
 
 	return 1;
 }