3 files changed, 34 insertions, 42 deletions

diff --git a/arch/arm/mach-at91/cpuidle.c b/arch/arm/mach-at91/cpuidle.c
index a851e6c98421..555d956b3a57 100644
--- a/arch/arm/mach-at91/cpuidle.c
+++ b/arch/arm/mach-at91/cpuidle.c
@@ -39,20 +39,15 @@ static int at91_enter_idle(struct cpuidle_device *dev,
 {
         struct timeval before, after;
         int idle_time;
-        u32 saved_lpr;
 
         local_irq_disable();
         do_gettimeofday(&before);
         if (index == 0)
                 /* Wait for interrupt state */
                 cpu_do_idle();
-        else if (index == 1) {
-                asm("b 1f; .align 5; 1:");
-                asm("mcr p15, 0, r0, c7, c10, 4");      /* drain write buffer */
-                saved_lpr = sdram_selfrefresh_enable();
-                cpu_do_idle();
-                sdram_selfrefresh_disable(saved_lpr);
-        }
+        else if (index == 1)
+                at91_standby();
+
         do_gettimeofday(&after);
         local_irq_enable();
         idle_time = (after.tv_sec - before.tv_sec) * USEC_PER_SEC +
diff --git a/arch/arm/mach-at91/pm.c b/arch/arm/mach-at91/pm.c
index d7e8d4cbc5ef..d554e6771b4e 100644
--- a/arch/arm/mach-at91/pm.c
+++ b/arch/arm/mach-at91/pm.c
@@ -198,7 +198,6 @@ extern u32 at91_slow_clock_sz;
 
 static int at91_pm_enter(suspend_state_t state)
 {
-        u32 saved_lpr;
         at91_gpio_suspend();
         at91_irq_suspend();
 
@@ -254,16 +253,7 @@ static int at91_pm_enter(suspend_state_t state)
                          * For ARM 926 based chips, this requirement is weaker
                          * as at91sam9 can access a RAM in self-refresh mode.
                          */
-                        asm volatile (  "mov r0, #0\n\t"
-                                        "b 1f\n\t"
-                                        ".align 5\n\t"
-                                        "1: mcr p15, 0, r0, c7, c10, 4\n\t"
-                                        : /* no output */
-                                        : /* no input */
-                                        : "r0");
-                        saved_lpr = sdram_selfrefresh_enable();
-                        cpu_do_idle();
-                        sdram_selfrefresh_disable(saved_lpr);
+                        at91_standby();
                         break;
 
                 case PM_SUSPEND_ON:
diff --git a/arch/arm/mach-at91/pm.h b/arch/arm/mach-at91/pm.h
index 38f9a13ee794..bba9ce1aaaec 100644
--- a/arch/arm/mach-at91/pm.h
+++ b/arch/arm/mach-at91/pm.h
@@ -24,17 +24,25 @@
  * still in self-refresh is "not recommended", but seems to work.
  */
 
-static inline u32 sdram_selfrefresh_enable(void)
+static inline void at91rm9200_standby(void)
 {
-        u32 saved_lpr = at91_sys_read(AT91_SDRAMC_LPR);
-
-        at91_sys_write(AT91_SDRAMC_LPR, 0);
-        at91_sys_write(AT91_SDRAMC_SRR, 1);
-        return saved_lpr;
+        u32 lpr = at91_sys_read(AT91_SDRAMC_LPR);
+
+        asm volatile(
+                "b    1f\n\t"
+                ".align    5\n\t"
+                "1:  mcr    p15, 0, %0, c7, c10, 4\n\t"
+                "    str    %0, [%1, %2]\n\t"
+                "    str    %3, [%1, %4]\n\t"
+                "    mcr    p15, 0, %0, c7, c0, 4\n\t"
+                "    str    %5, [%1, %2]"
+                :
+                : "r" (0), "r" (AT91_BASE_SYS), "r" (AT91_SDRAMC_LPR),
+                  "r" (1), "r" (AT91_SDRAMC_SRR),
+                  "r" (lpr));
 }
 
-#define sdram_selfrefresh_disable(saved_lpr) \
-        at91_sys_write(AT91_SDRAMC_LPR, saved_lpr)
+#define at91_standby at91rm9200_standby
 
 #elif defined(CONFIG_ARCH_AT91SAM9G45)
 #include <mach/at91sam9_ddrsdr.h>
@@ -42,14 +50,12 @@ static inline u32 sdram_selfrefresh_enable(void)
 /* We manage both DDRAM/SDRAM controllers, we need more than one value to
  * remember.
  */
-static u32 saved_lpr1;
-
-static inline u32 sdram_selfrefresh_enable(void)
+static inline void at91sam9g45_standby(void)
 {
-        /* Those tow values allow us to delay self-refresh activation
+        /* Those two values allow us to delay self-refresh activation
          * to the maximum. */
         u32 lpr0, lpr1;
-        u32 saved_lpr0;
+        u32 saved_lpr0, saved_lpr1;
 
         saved_lpr1 = at91_ramc_read(1, AT91_DDRSDRC_LPR);
         lpr1 = saved_lpr1 & ~AT91_DDRSDRC_LPCB;
@@ -63,14 +69,13 @@ static inline u32 sdram_selfrefresh_enable(void)
         at91_ramc_write(0, AT91_DDRSDRC_LPR, lpr0);
         at91_ramc_write(1, AT91_DDRSDRC_LPR, lpr1);
 
-        return saved_lpr0;
+        cpu_do_idle();
+
+        at91_ramc_write(0, AT91_DDRSDRC_LPR, saved_lpr0);
+        at91_ramc_write(1, AT91_DDRSDRC_LPR, saved_lpr1);
 }
 
-#define sdram_selfrefresh_disable(saved_lpr0)   \
-        do { \
-                at91_ramc_write(0, AT91_DDRSDRC_LPR, saved_lpr0); \
-                at91_ramc_write(1, AT91_DDRSDRC_LPR, saved_lpr1); \
-        } while (0)
+#define at91_standby at91sam9g45_standby
 
 #else
 #include <mach/at91sam9_sdramc.h>
@@ -83,7 +88,7 @@ static inline u32 sdram_selfrefresh_enable(void)
 #warning Assuming EB1 SDRAM controller is *NOT* used
 #endif
 
-static inline u32 sdram_selfrefresh_enable(void)
+static inline void at91sam9_standby(void)
 {
         u32 saved_lpr, lpr;
 
@@ -92,11 +97,13 @@ static inline u32 sdram_selfrefresh_enable(void)
         lpr = saved_lpr & ~AT91_SDRAMC_LPCB;
         at91_ramc_write(0, AT91_SDRAMC_LPR, lpr |
                         AT91_SDRAMC_LPCB_SELF_REFRESH);
-        return saved_lpr;
+
+        cpu_do_idle();
+
+        at91_ramc_write(0, AT91_SDRAMC_LPR, saved_lpr);
 }
 
-#define sdram_selfrefresh_disable(saved_lpr) \
-        at91_ramc_write(0, AT91_SDRAMC_LPR, saved_lpr)
+#define at91_standby at91sam9_standby
 
 #endif