6 files changed, 45 insertions, 22 deletions

diff --git a/arch/arm/mach-highbank/highbank.c b/arch/arm/mach-highbank/highbank.c
index dc248167d206..e6c061282939 100644
--- a/arch/arm/mach-highbank/highbank.c
+++ b/arch/arm/mach-highbank/highbank.c
@@ -28,6 +28,7 @@
 
 #include <asm/arch_timer.h>
 #include <asm/cacheflush.h>
+#include <asm/cputype.h>
 #include <asm/smp_plat.h>
 #include <asm/smp_twd.h>
 #include <asm/hardware/arm_timer.h>
@@ -59,7 +60,7 @@ static void __init highbank_scu_map_io(void)
 
 void highbank_set_cpu_jump(int cpu, void *jump_addr)
 {
-	cpu = cpu_logical_map(cpu);
+	cpu = MPIDR_AFFINITY_LEVEL(cpu_logical_map(cpu), 0);
 	writel(virt_to_phys(jump_addr), HB_JUMP_TABLE_VIRT(cpu));
 	__cpuc_flush_dcache_area(HB_JUMP_TABLE_VIRT(cpu), 16);
 	outer_clean_range(HB_JUMP_TABLE_PHYS(cpu),
@@ -135,7 +136,7 @@ static struct sys_timer highbank_timer = {
 
 static void highbank_power_off(void)
 {
-	hignbank_set_pwr_shutdown();
+	highbank_set_pwr_shutdown();
 
 	while (1)
 		cpu_do_idle();
diff --git a/arch/arm/mach-highbank/hotplug.c b/arch/arm/mach-highbank/hotplug.c
index 7b60faccd551..f30c52843396 100644
--- a/arch/arm/mach-highbank/hotplug.c
+++ b/arch/arm/mach-highbank/hotplug.c
@@ -30,7 +30,7 @@ void __ref highbank_cpu_die(unsigned int cpu)
 {
 	flush_cache_all();
 
-	highbank_set_cpu_jump(cpu, secondary_startup);
+	highbank_set_cpu_jump(cpu, phys_to_virt(0));
 	highbank_set_core_pwr();
 
 	cpu_do_idle();
diff --git a/arch/arm/mach-highbank/platsmp.c b/arch/arm/mach-highbank/platsmp.c
index 1129957f6c1d..4ecc864ac8b9 100644
--- a/arch/arm/mach-highbank/platsmp.c
+++ b/arch/arm/mach-highbank/platsmp.c
@@ -32,6 +32,7 @@ static void __cpuinit highbank_secondary_init(unsigned int cpu)
 
 static int __cpuinit highbank_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
+	highbank_set_cpu_jump(cpu, secondary_startup);
 	gic_raise_softirq(cpumask_of(cpu), 0);
 	return 0;
 }
@@ -61,19 +62,8 @@ static void __init highbank_smp_init_cpus(void)
 
 static void __init highbank_smp_prepare_cpus(unsigned int max_cpus)
 {
-	int i;
-
 	if (scu_base_addr)
 		scu_enable(scu_base_addr);
-
-	/*
-	 * Write the address of secondary startup into the jump table
-	 * The cores are in wfi and wait until they receive a soft interrupt
-	 * and a non-zero value to jump to. Then the secondary CPU branches
-	 * to this address.
-	 */
-	for (i = 1; i < max_cpus; i++)
-		highbank_set_cpu_jump(i, secondary_startup);
 }
 
 struct smp_operations highbank_smp_ops __initdata = {
diff --git a/arch/arm/mach-highbank/pm.c b/arch/arm/mach-highbank/pm.c
index 74aa135966f0..04eddb4f4380 100644
--- a/arch/arm/mach-highbank/pm.c
+++ b/arch/arm/mach-highbank/pm.c
@@ -14,10 +14,12 @@
  * this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/cpu_pm.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/suspend.h>
 
+#include <asm/cacheflush.h>
 #include <asm/proc-fns.h>
 #include <asm/suspend.h>
 
@@ -26,16 +28,31 @@
 
 static int highbank_suspend_finish(unsigned long val)
 {
+	outer_flush_all();
+	outer_disable();
+
+	highbank_set_pwr_suspend();
+
 	cpu_do_idle();
+
+	highbank_clear_pwr_request();
 	return 0;
 }
 
 static int highbank_pm_enter(suspend_state_t state)
 {
-	hignbank_set_pwr_suspend();
+	cpu_pm_enter();
+	cpu_cluster_pm_enter();
+
 	highbank_set_cpu_jump(0, cpu_resume);
 	cpu_suspend(0, highbank_suspend_finish);
 
+	cpu_cluster_pm_exit();
+	cpu_pm_exit();
+
+	highbank_smc1(0x102, 0x1);
+	if (scu_base_addr)
+		scu_enable(scu_base_addr);
 	return 0;
 }
 
diff --git a/arch/arm/mach-highbank/sysregs.h b/arch/arm/mach-highbank/sysregs.h
index e13e8ea7c6cb..5995df7f2622 100644
--- a/arch/arm/mach-highbank/sysregs.h
+++ b/arch/arm/mach-highbank/sysregs.h
@@ -37,35 +37,50 @@ extern void __iomem *sregs_base;
 
 static inline void highbank_set_core_pwr(void)
 {
-	int cpu = cpu_logical_map(smp_processor_id());
+	int cpu = MPIDR_AFFINITY_LEVEL(cpu_logical_map(smp_processor_id()), 0);
 	if (scu_base_addr)
 		scu_power_mode(scu_base_addr, SCU_PM_POWEROFF);
 	else
 		writel_relaxed(1, sregs_base + SREG_CPU_PWR_CTRL(cpu));
 }
 
-static inline void hignbank_set_pwr_suspend(void)
+static inline void highbank_clear_core_pwr(void)
+{
+	int cpu = MPIDR_AFFINITY_LEVEL(cpu_logical_map(smp_processor_id()), 0);
+	if (scu_base_addr)
+		scu_power_mode(scu_base_addr, SCU_PM_NORMAL);
+	else
+		writel_relaxed(0, sregs_base + SREG_CPU_PWR_CTRL(cpu));
+}
+
+static inline void highbank_set_pwr_suspend(void)
 {
 	writel(HB_PWR_SUSPEND, sregs_base + HB_SREG_A9_PWR_REQ);
 	highbank_set_core_pwr();
 }
 
-static inline void hignbank_set_pwr_shutdown(void)
+static inline void highbank_set_pwr_shutdown(void)
 {
 	writel(HB_PWR_SHUTDOWN, sregs_base + HB_SREG_A9_PWR_REQ);
 	highbank_set_core_pwr();
 }
 
-static inline void hignbank_set_pwr_soft_reset(void)
+static inline void highbank_set_pwr_soft_reset(void)
 {
 	writel(HB_PWR_SOFT_RESET, sregs_base + HB_SREG_A9_PWR_REQ);
 	highbank_set_core_pwr();
 }
 
-static inline void hignbank_set_pwr_hard_reset(void)
+static inline void highbank_set_pwr_hard_reset(void)
 {
 	writel(HB_PWR_HARD_RESET, sregs_base + HB_SREG_A9_PWR_REQ);
 	highbank_set_core_pwr();
 }
 
+static inline void highbank_clear_pwr_request(void)
+{
+	writel(~0UL, sregs_base + HB_SREG_A9_PWR_REQ);
+	highbank_clear_core_pwr();
+}
+
 #endif
diff --git a/arch/arm/mach-highbank/system.c b/arch/arm/mach-highbank/system.c
index aed96ad9bd4a..37d8384dcf19 100644
--- a/arch/arm/mach-highbank/system.c
+++ b/arch/arm/mach-highbank/system.c
@@ -22,9 +22,9 @@
 void highbank_restart(char mode, const char *cmd)
 {
 	if (mode == 'h')
-		hignbank_set_pwr_hard_reset();
+		highbank_set_pwr_hard_reset();
 	else
-		hignbank_set_pwr_soft_reset();
+		highbank_set_pwr_soft_reset();
 
 	while (1)
 		cpu_do_idle();