ARM: mx51: Implement code to allow mx51 to enter WFI

Implement code for MX51 that allows the SoC to enter WFI when
arch_idle is called.

This patch is also necessary for correctly suspending the system.

Signed-off-by: Dinh Nguyen <Dinh.Nguyen@freescale.com>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

2 files changed, 14 insertions, 1 deletions

diff --git a/arch/arm/plat-mxc/include/mach/mxc.h b/arch/arm/plat-mxc/include/mach/mxc.h
index 3322c7a79c57..1aea818d9d31 100644
--- a/arch/arm/plat-mxc/include/mach/mxc.h
+++ b/arch/arm/plat-mxc/include/mach/mxc.h
@@ -195,6 +195,15 @@ struct cpu_op {
         u32 cpu_rate;
 };
 
+int tzic_enable_wake(int is_idle);
+enum mxc_cpu_pwr_mode {
+        WAIT_CLOCKED,           /* wfi only */
+        WAIT_UNCLOCKED,         /* WAIT */
+        WAIT_UNCLOCKED_POWER_OFF,       /* WAIT + SRPG */
+        STOP_POWER_ON,          /* just STOP */
+        STOP_POWER_OFF,         /* STOP + SRPG */
+};
+
 extern struct cpu_op *(*get_cpu_op)(int *op);
 #endif
 
diff --git a/arch/arm/plat-mxc/include/mach/system.h b/arch/arm/plat-mxc/include/mach/system.h
index 95be51bfe9a9..0417da9f710d 100644
--- a/arch/arm/plat-mxc/include/mach/system.h
+++ b/arch/arm/plat-mxc/include/mach/system.h
@@ -20,6 +20,8 @@
 #include <mach/hardware.h>
 #include <mach/common.h>
 
+extern void mx5_cpu_lp_set(enum mxc_cpu_pwr_mode mode);
+
 static inline void arch_idle(void)
 {
 #ifdef CONFIG_ARCH_MXC91231
@@ -54,7 +56,9 @@ static inline void arch_idle(void)
                         "orr %0, %0, #0x00000004\n"
                         "mcr p15, 0, %0, c1, c0, 0\n"
                         : "=r" (reg));
-        } else
+        } else if (cpu_is_mx51())
+                mx5_cpu_lp_set(WAIT_UNCLOCKED_POWER_OFF);
+        else
                 cpu_do_idle();
 }