diff --git a/arch/arm/mach-at91/cpuidle.c b/arch/arm/mach-at91/cpuidle.c
index a851e6c984218d14533fd5157c7e58fc76c5fc81..555d956b3a574b3a4d96be46c199c087d991080d 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 d7e8d4cbc5ef52d27be7f87270b2cd0fd418c99a..d554e6771b4e65acf5c90e39dad5e95dcc254f0d 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 38f9a13ee794d6082867ce3ac100b17cb8eadfc3..bba9ce1aaaecc75f48df2c3377b4bc7197b234b5 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