2 files changed, 62 insertions, 31 deletions

diff --git a/arch/mips/alchemy/common/power.c b/arch/mips/alchemy/common/power.c
index 14eb8c492da2..5ef06a164a82 100644
--- a/arch/mips/alchemy/common/power.c
+++ b/arch/mips/alchemy/common/power.c
@@ -193,9 +193,15 @@ static void restore_core_regs(void)
 
 void au_sleep(void)
 {
-	save_core_regs();
-	au1xxx_save_and_sleep();
-	restore_core_regs();
+	int cpuid = alchemy_get_cputype();
+	if (cpuid != ALCHEMY_CPU_UNKNOWN) {
+		save_core_regs();
+		if (cpuid <= ALCHEMY_CPU_AU1500)
+			alchemy_sleep_au1000();
+		else if (cpuid <= ALCHEMY_CPU_AU1200)
+			alchemy_sleep_au1550();
+		restore_core_regs();
+	}
 }
 
 #endif	/* CONFIG_PM */
diff --git a/arch/mips/alchemy/common/sleeper.S b/arch/mips/alchemy/common/sleeper.S
index 4f4b16741d12..77f3c743b716 100644
--- a/arch/mips/alchemy/common/sleeper.S
+++ b/arch/mips/alchemy/common/sleeper.S
@@ -22,10 +22,9 @@
 	.set noat
 	.align	5
 
-/* Save all of the processor general registers and go to sleep.
- * A wakeup condition will get us back here to restore the registers.
- */
-LEAF(au1xxx_save_and_sleep)
+
+/* preparatory stuff */
+.macro	SETUP_SLEEP
 	subu	sp, PT_SIZE
 	sw	$1, PT_R1(sp)
 	sw	$2, PT_R2(sp)
@@ -69,12 +68,32 @@ LEAF(au1xxx_save_and_sleep)
 	 */
 	lui	t3, 0xb190		/* sys_xxx */
 	sw	sp, 0x0018(t3)
-	la	k0, 3f			/* resume path */
+	la	k0, alchemy_sleep_wakeup	/* resume path */
 	sw	k0, 0x001c(t3)
+.endm
 
-	/* Put SDRAM into self refresh:  Preload instructions into cache,
-	 * issue a precharge, auto/self refresh, then sleep commands to it.
-	 */
+.macro	DO_SLEEP
+	/* put power supply and processor to sleep */
+	sw	zero, 0x0078(t3)	/* sys_slppwr */
+	sync
+	sw	zero, 0x007c(t3)	/* sys_sleep */
+	sync
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+	nop
+.endm
+
+/* sleep code for Au1000/Au1100/Au1500 memory controller type */
+LEAF(alchemy_sleep_au1000)
+
+	SETUP_SLEEP
+
+	/* cache following instructions, as memory gets put to sleep */
 	la	t0, 1f
 	.set	mips3
 	cache	0x14, 0(t0)
@@ -84,17 +103,32 @@ LEAF(au1xxx_save_and_sleep)
 	.set	mips0
 
 1:	lui 	a0, 0xb400		/* mem_xxx */
-#if defined(CONFIG_SOC_AU1000) || defined(CONFIG_SOC_AU1100) ||	\
-    defined(CONFIG_SOC_AU1500)
 	sw	zero, 0x001c(a0) 	/* Precharge */
 	sync
 	sw	zero, 0x0020(a0)	/* Auto Refresh */
 	sync
 	sw	zero, 0x0030(a0)  	/* Sleep */
 	sync
-#endif
 
-#if defined(CONFIG_SOC_AU1550) || defined(CONFIG_SOC_AU1200)
+	DO_SLEEP
+
+END(alchemy_sleep_au1000)
+
+/* sleep code for Au1550/Au1200 memory controller type */
+LEAF(alchemy_sleep_au1550)
+
+	SETUP_SLEEP
+
+	/* cache following instructions, as memory gets put to sleep */
+	la	t0, 1f
+	.set	mips3
+	cache	0x14, 0(t0)
+	cache	0x14, 32(t0)
+	cache	0x14, 64(t0)
+	cache	0x14, 96(t0)
+	.set	mips0
+
+1:	lui 	a0, 0xb400		/* mem_xxx */
 	sw	zero, 0x08c0(a0) 	/* Precharge */
 	sync
 	sw	zero, 0x08d0(a0)	/* Self Refresh */
@@ -114,26 +148,17 @@ LEAF(au1xxx_save_and_sleep)
 	and 	t1, t0, t1		/* clear CE[1:0] */
 	sw 	t1, 0x0840(a0)		/* mem_sdconfiga */
 	sync
-#endif
 
-	/* put power supply and processor to sleep */
-	sw	zero, 0x0078(t3)	/* sys_slppwr */
-	sync
-	sw	zero, 0x007c(t3)	/* sys_sleep */
-	sync
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
-	nop
+	DO_SLEEP
+
+END(alchemy_sleep_au1550)
+
 
 	/* This is where we return upon wakeup.
 	 * Reload all of the registers and return.
 	 */
-3:	lw	k0, 0x20(sp)
+LEAF(alchemy_sleep_wakeup)
+	lw	k0, 0x20(sp)
 	mtc0	k0, CP0_STATUS
 	lw	k0, 0x1c(sp)
 	mtc0	k0, CP0_CONTEXT
@@ -169,4 +194,4 @@ LEAF(au1xxx_save_and_sleep)
 	lw	$31, PT_R31(sp)
 	jr	ra
 	 addiu	sp, PT_SIZE
-END(au1xxx_save_and_sleep)
+END(alchemy_sleep_wakeup)