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>

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