6 files changed, 42 insertions, 20 deletions

diff --git a/arch/arm/mach-highbank/highbank.c b/arch/arm/mach-highbank/highbank.c
index dc248167d206..981dc1e1da51 100644
--- a/arch/arm/mach-highbank/highbank.c
+++ b/arch/arm/mach-highbank/highbank.c
@@ -135,7 +135,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..70af9d13fcef 100644
--- a/arch/arm/mach-highbank/sysregs.h
+++ b/arch/arm/mach-highbank/sysregs.h
@@ -44,28 +44,43 @@ static inline void highbank_set_core_pwr(void)
                 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 = cpu_logical_map(smp_processor_id());
+        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();