ARM: at91/pm_suspend: add the WFI instruction support for ARMv7

Add the WFI instruction to make the cpu to the idle state.
In the meanwhile, disable the processor's clock.

Signed-off-by: Wenyou Yang <wenyou.yang@atmel.com>
Acked-by: Alexandre Belloni <alexandre.belloni@free-electrons.com>
Tested-by: Sylvain Rochet <sylvain.rochet@finsecur.com>
Signed-off-by: Nicolas Ferre <nicolas.ferre@atmel.com>

diff --git a/arch/arm/mach-at91/pm_suspend.S b/arch/arm/mach-at91/pm_suspend.S
index 1002bb8..7c444c2 100644 (file)
--- a/arch/arm/mach-at91/pm_suspend.S
+++ b/arch/arm/mach-at91/pm_suspend.S
@@ -51,6 +51,24 @@ tmp2 .req    r5
        beq     1b
        .endm
 
+/*
+ * Put the processor to enter the idle state
+ */
+       .macro at91_cpu_idle
+
+#if defined(CONFIG_CPU_V7)
+       mov     tmp1, #AT91_PMC_PCK
+       str     tmp1, [pmc, #AT91_PMC_SCDR]
+
+       dsb
+
+       wfi             @ Wait For Interrupt
+#else
+       mcr     p15, 0, tmp1, c7, c0, 4
+#endif
+
+       .endm
+
        .text
 
        .arm
@@ -123,7 +141,7 @@ skip_disable_main_clock:
        ldr     pmc, .pmc_base
 
        /* Wait for interrupt */
-       mcr     p15, 0, tmp1, c7, c0, 4
+       at91_cpu_idle
 
        ldr     r0, .pm_mode
        tst     r0, #AT91_PM_SLOW_CLOCK