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) {
+        else if (index == 1)
-                asm("b 1f; .align 5; 1:");
+                at91_standby();
-                asm("mcr p15, 0, r0, c7, c10, 4");      /* drain write buffer */
-                saved_lpr = sdram_selfrefresh_enable();
-                cpu_do_idle();
-                sdram_selfrefresh_disable(saved_lpr);
-        }
        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"
+                        at91_standby();
-                                        "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);
                        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);
+        u32 lpr = at91_sys_read(AT91_SDRAMC_LPR);
-        at91_sys_write(AT91_SDRAMC_LPR, 0);
+        asm volatile(
-        at91_sys_write(AT91_SDRAMC_SRR, 1);
+                "b    1f\n\t"
-        return saved_lpr;
+                ".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) \
+#define at91_standby at91rm9200_standby
-        at91_sys_write(AT91_SDRAMC_LPR, saved_lpr)
 #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 void at91sam9g45_standby(void)
-static inline u32 sdram_selfrefresh_enable(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)   \
+#define at91_standby at91sam9g45_standby
-        do { \
-                at91_ramc_write(0, AT91_DDRSDRC_LPR, saved_lpr0); \
-                at91_ramc_write(1, AT91_DDRSDRC_LPR, saved_lpr1); \
-        } while (0)
 #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) \
+#define at91_standby at91sam9_standby
-        at91_ramc_write(0, AT91_SDRAMC_LPR, saved_lpr)
 #endif

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,
39	{	39	{
40	struct timeval before, after;	40	struct timeval before, after;
41	int idle_time;	41	int idle_time;
42	u32 saved_lpr;
43		42
44	local_irq_disable();	43	local_irq_disable();
45	do_gettimeofday(&before);	44	do_gettimeofday(&before);
46	if (index == 0)	45	if (index == 0)
47	/* Wait for interrupt state */	46	/* Wait for interrupt state */
48	cpu_do_idle();	47	cpu_do_idle();
49	else if (index == 1) {	48	else if (index == 1)
50	asm("b 1f; .align 5; 1:");	49	at91_standby();
51	asm("mcr p15, 0, r0, c7, c10, 4"); /* drain write buffer */	50
52	saved_lpr = sdram_selfrefresh_enable();
53	cpu_do_idle();
54	sdram_selfrefresh_disable(saved_lpr);
55	}
56	do_gettimeofday(&after);	51	do_gettimeofday(&after);
57	local_irq_enable();	52	local_irq_enable();
58	idle_time = (after.tv_sec - before.tv_sec) * USEC_PER_SEC +	53	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;
198		198
199	static int at91_pm_enter(suspend_state_t state)	199	static int at91_pm_enter(suspend_state_t state)
200	{	200	{
201	u32 saved_lpr;
202	at91_gpio_suspend();	201	at91_gpio_suspend();
203	at91_irq_suspend();	202	at91_irq_suspend();
204		203
@@ -254,16 +253,7 @@ static int at91_pm_enter(suspend_state_t state)
254	* For ARM 926 based chips, this requirement is weaker	253	* For ARM 926 based chips, this requirement is weaker
255	* as at91sam9 can access a RAM in self-refresh mode.	254	* as at91sam9 can access a RAM in self-refresh mode.
256	*/	255	*/
257	asm volatile ( "mov r0, #0\n\t"	256	at91_standby();
258	"b 1f\n\t"
259	".align 5\n\t"
260	"1: mcr p15, 0, r0, c7, c10, 4\n\t"
261	: /* no output */
262	: /* no input */
263	: "r0");
264	saved_lpr = sdram_selfrefresh_enable();
265	cpu_do_idle();
266	sdram_selfrefresh_disable(saved_lpr);
267	break;	257	break;
268		258
269	case PM_SUSPEND_ON:	259	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 @@
24	* still in self-refresh is "not recommended", but seems to work.	24	* still in self-refresh is "not recommended", but seems to work.
25	*/	25	*/
26		26
27	static inline u32 sdram_selfrefresh_enable(void)	27	static inline void at91rm9200_standby(void)
28	{	28	{
29	u32 saved_lpr = at91_sys_read(AT91_SDRAMC_LPR);	29	u32 lpr = at91_sys_read(AT91_SDRAMC_LPR);
30		30
31	at91_sys_write(AT91_SDRAMC_LPR, 0);	31	asm volatile(
32	at91_sys_write(AT91_SDRAMC_SRR, 1);	32	"b 1f\n\t"
33	return saved_lpr;	33	".align 5\n\t"
		34	"1: mcr p15, 0, %0, c7, c10, 4\n\t"
		35	" str %0, [%1, %2]\n\t"
		36	" str %3, [%1, %4]\n\t"
		37	" mcr p15, 0, %0, c7, c0, 4\n\t"
		38	" str %5, [%1, %2]"
		39	:
		40	: "r" (0), "r" (AT91_BASE_SYS), "r" (AT91_SDRAMC_LPR),
		41	"r" (1), "r" (AT91_SDRAMC_SRR),
		42	"r" (lpr));
34	}	43	}
35		44
36	#define sdram_selfrefresh_disable(saved_lpr) \	45	#define at91_standby at91rm9200_standby
37	at91_sys_write(AT91_SDRAMC_LPR, saved_lpr)
38		46
39	#elif defined(CONFIG_ARCH_AT91SAM9G45)	47	#elif defined(CONFIG_ARCH_AT91SAM9G45)
40	#include <mach/at91sam9_ddrsdr.h>	48	#include <mach/at91sam9_ddrsdr.h>
@@ -42,14 +50,12 @@ static inline u32 sdram_selfrefresh_enable(void)
42	/* We manage both DDRAM/SDRAM controllers, we need more than one value to	50	/* We manage both DDRAM/SDRAM controllers, we need more than one value to
43	* remember.	51	* remember.
44	*/	52	*/
45	static u32 saved_lpr1;	53	static inline void at91sam9g45_standby(void)
46
47	static inline u32 sdram_selfrefresh_enable(void)
48	{	54	{
49	/* Those tow values allow us to delay self-refresh activation	55	/* Those two values allow us to delay self-refresh activation
50	* to the maximum. */	56	* to the maximum. */
51	u32 lpr0, lpr1;	57	u32 lpr0, lpr1;
52	u32 saved_lpr0;	58	u32 saved_lpr0, saved_lpr1;
53		59
54	saved_lpr1 = at91_ramc_read(1, AT91_DDRSDRC_LPR);	60	saved_lpr1 = at91_ramc_read(1, AT91_DDRSDRC_LPR);
55	lpr1 = saved_lpr1 & ~AT91_DDRSDRC_LPCB;	61	lpr1 = saved_lpr1 & ~AT91_DDRSDRC_LPCB;
@@ -63,14 +69,13 @@ static inline u32 sdram_selfrefresh_enable(void)
63	at91_ramc_write(0, AT91_DDRSDRC_LPR, lpr0);	69	at91_ramc_write(0, AT91_DDRSDRC_LPR, lpr0);
64	at91_ramc_write(1, AT91_DDRSDRC_LPR, lpr1);	70	at91_ramc_write(1, AT91_DDRSDRC_LPR, lpr1);
65		71
66	return saved_lpr0;	72	cpu_do_idle();
		73
		74	at91_ramc_write(0, AT91_DDRSDRC_LPR, saved_lpr0);
		75	at91_ramc_write(1, AT91_DDRSDRC_LPR, saved_lpr1);
67	}	76	}
68		77
69	#define sdram_selfrefresh_disable(saved_lpr0) \	78	#define at91_standby at91sam9g45_standby
70	do { \
71	at91_ramc_write(0, AT91_DDRSDRC_LPR, saved_lpr0); \
72	at91_ramc_write(1, AT91_DDRSDRC_LPR, saved_lpr1); \
73	} while (0)
74		79
75	#else	80	#else
76	#include <mach/at91sam9_sdramc.h>	81	#include <mach/at91sam9_sdramc.h>
@@ -83,7 +88,7 @@ static inline u32 sdram_selfrefresh_enable(void)
83	#warning Assuming EB1 SDRAM controller is NOT used	88	#warning Assuming EB1 SDRAM controller is NOT used
84	#endif	89	#endif
85		90
86	static inline u32 sdram_selfrefresh_enable(void)	91	static inline void at91sam9_standby(void)
87	{	92	{
88	u32 saved_lpr, lpr;	93	u32 saved_lpr, lpr;
89		94
@@ -92,11 +97,13 @@ static inline u32 sdram_selfrefresh_enable(void)
92	lpr = saved_lpr & ~AT91_SDRAMC_LPCB;	97	lpr = saved_lpr & ~AT91_SDRAMC_LPCB;
93	at91_ramc_write(0, AT91_SDRAMC_LPR, lpr \|	98	at91_ramc_write(0, AT91_SDRAMC_LPR, lpr \|
94	AT91_SDRAMC_LPCB_SELF_REFRESH);	99	AT91_SDRAMC_LPCB_SELF_REFRESH);
95	return saved_lpr;	100
		101	cpu_do_idle();
		102
		103	at91_ramc_write(0, AT91_SDRAMC_LPR, saved_lpr);
96	}	104	}
97		105
98	#define sdram_selfrefresh_disable(saved_lpr) \	106	#define at91_standby at91sam9_standby
99	at91_ramc_write(0, AT91_SDRAMC_LPR, saved_lpr)
100		107
101	#endif	108	#endif
102		109