ARM: at91/pm: standby mode uses same sram function as suspend to memory mode

To simply the PM code, the suspend to standby mode uses same sram function
as the suspend to memory mode, running in the internal SRAM, instead of the
respective code for each mode.

For the suspend to standby mode, the master clock doesn't switch to the slow
clock, and PLLA and the main oscillator doesn't turn off as well.

Signed-off-by: Wenyou Yang <wenyou.yang@atmel.com>
Tested-by: Sylvain Rochet <sylvain.rochet@finsecur.com>
Signed-off-by: Nicolas Ferre <nicolas.ferre@atmel.com>

1 files changed, 24 insertions, 1 deletions

diff --git a/arch/arm/mach-at91/pm_slowclock.S b/arch/arm/mach-at91/pm_slowclock.S
index 4c5a363646dd..db35f72e7bad 100644
--- a/arch/arm/mach-at91/pm_slowclock.S
+++ b/arch/arm/mach-at91/pm_slowclock.S
@@ -15,6 +15,7 @@
 #include <linux/clk/at91_pmc.h>
 #include <mach/hardware.h>
 #include <mach/at91_ramc.h>
+#include "pm.h"
 
 #define SRAMC_SELF_FRESH_ACTIVE         0x01
 #define SRAMC_SELF_FRESH_EXIT           0x00
@@ -78,12 +79,22 @@ ENTRY(at91_slow_clock)
         str     r0, .pmc_base
         str     r1, .sramc_base
         str     r2, .sramc1_base
-        str     r3, .memtype
+
+        and     r0, r3, #AT91_PM_MEMTYPE_MASK
+        str     r0, .memtype
+
+        lsr     r0, r3, #AT91_PM_MODE_OFFSET
+        and     r0, r0, #AT91_PM_MODE_MASK
+        str     r0, .pm_mode
 
         /* Active the self-refresh mode */
         mov     r0, #SRAMC_SELF_FRESH_ACTIVE
         bl      at91_sramc_self_refresh
 
+        ldr     r0, .pm_mode
+        tst     r0, #AT91_PM_SLOW_CLOCK
+        beq     skip_disable_main_clock
+
         ldr     pmc, .pmc_base
 
         /* Save Master clock setting */
@@ -112,9 +123,18 @@ ENTRY(at91_slow_clock)
         orr     tmp1, tmp1, #AT91_PMC_KEY
         str     tmp1, [pmc, #AT91_CKGR_MOR]
 
+skip_disable_main_clock:
+        ldr     pmc, .pmc_base
+
         /* Wait for interrupt */
         mcr     p15, 0, tmp1, c7, c0, 4
 
+        ldr     r0, .pm_mode
+        tst     r0, #AT91_PM_SLOW_CLOCK
+        beq     skip_enable_main_clock
+
+        ldr     pmc, .pmc_base
+
         /* Turn on the main oscillator */
         ldr     tmp1, [pmc, #AT91_CKGR_MOR]
         orr     tmp1, tmp1, #AT91_PMC_MOSCEN
@@ -143,6 +163,7 @@ ENTRY(at91_slow_clock)
 
         wait_mckrdy
 
+skip_enable_main_clock:
         /* Exit the self-refresh mode */
         mov     r0, #SRAMC_SELF_FRESH_EXIT
         bl      at91_sramc_self_refresh
@@ -284,6 +305,8 @@ ENDPROC(at91_sramc_self_refresh)
         .word 0
 .memtype:
         .word 0
+.pm_mode:
+        .word 0
 .saved_mckr:
         .word 0
 .saved_pllar: