Blackfin arch: Functional power management support

Enable: PM_SUSPEND_MEM -> Blackfin Hibernate to SDRAM
This feature requires a special bootloader (u-boot)
supporting return from hibernate.

Signed-off-by: Michael Hennerich <michael.hennerich@analog.com>
Signed-off-by: Bryan Wu <cooloney@kernel.org>

2 files changed, 780 insertions, 45 deletions

diff --git a/arch/blackfin/mach-common/dpmc_modes.S b/arch/blackfin/mach-common/dpmc_modes.S
index b7981d31c39..46ee77afed2 100644
--- a/arch/blackfin/mach-common/dpmc_modes.S
+++ b/arch/blackfin/mach-common/dpmc_modes.S
@@ -7,7 +7,7 @@
 #include <linux/linkage.h>
 #include <asm/blackfin.h>
 #include <asm/mach/irq.h>
-
+#include <asm/dpmc.h>
 
 .section .l1.text
 
@@ -56,26 +56,25 @@ ENTRY(_hibernate_mode)
 	[--SP] = ( R7:0, P5:0 );
 	[--SP] =  RETS;
 
+	R3 = R0;
+	R0 = IWR_DISABLE_ALL;
+	R1 = IWR_DISABLE_ALL;
+	R2 = IWR_DISABLE_ALL;
 	call _set_sic_iwr;
+	call _set_dram_srfs;
+	SSYNC;
 
 	R0 = 0xFFFF (Z);
 	call _set_rtc_istat;
 
 	P0.H = hi(VR_CTL);
 	P0.L = lo(VR_CTL);
-	R1 = W[P0](z);
-	BITSET (R1, 8);
-	BITCLR (R1, 0);
-	BITCLR (R1, 1);
-	W[P0] = R1.L;
-	SSYNC;
 
+	W[P0] = R3.L;
 	CLI R2;
 	IDLE;
-
-	/* Actually, adding anything may not be necessary...SDRAM contents
-	 * are lost
-	 */
+.Lforever:
+	jump .Lforever;
 
 ENTRY(_deep_sleep)
 	[--SP] = ( R7:0, P5:0 );
@@ -233,51 +232,70 @@ ENTRY(_sleep_deeper)
 	( R7:0, P5:0 ) = [SP++];
 	RTS;
 
+
 ENTRY(_set_dram_srfs)
 	/*  set the dram to self refresh mode */
-#if defined(CONFIG_BF54x)
+	SSYNC;
+#if defined(EBIU_RSTCTL)	/* DDR */
 	P0.H = hi(EBIU_RSTCTL);
 	P0.L = lo(EBIU_RSTCTL);
 	R2 = [P0];
-	R3.H = hi(SRREQ);
-	R3.L = lo(SRREQ);
-#else
-	P0.H = hi(EBIU_SDGCTL);
+	BITSET(R2, 3); /* SRREQ enter self-refresh mode */
+	[P0] = R2;
+	SSYNC;
+1:
+	R2 = [P0];
+	CC = BITTST(R2, 4);
+	if !CC JUMP 1b;
+#else 				/* SDRAM */
 	P0.L = lo(EBIU_SDGCTL);
+	P0.H = hi(EBIU_SDGCTL);
 	R2 = [P0];
-	R3.H = hi(SRFS);
-	R3.L = lo(SRFS);
-#endif
-	R2 = R2|R3;
+	BITSET(R2, 24); /* SRFS enter self-refresh mode */
 	[P0] = R2;
-	ssync;
-#if defined(CONFIG_BF54x)
-.LSRR_MODE:
+	SSYNC;
+
+	P0.L = lo(EBIU_SDSTAT);
+	P0.H = hi(EBIU_SDSTAT);
+1:
+	R2 = w[P0];
+	SSYNC;
+	cc = BITTST(R2, 1); /* SDSRA poll self-refresh status */
+	if !cc jump 1b;
+
+	P0.L = lo(EBIU_SDGCTL);
+	P0.H = hi(EBIU_SDGCTL);
 	R2 = [P0];
-	CC = BITTST(R2, 4);
-	if !CC JUMP .LSRR_MODE;
+	BITCLR(R2, 0); /* SCTLE disable CLKOUT */
+	[P0] = R2;
 #endif
 	RTS;
 
+
 ENTRY(_unset_dram_srfs)
 	/*  set the dram out of self refresh mode */
-#if defined(CONFIG_BF54x)
+#if defined(EBIU_RSTCTL)	/* DDR */
 	P0.H = hi(EBIU_RSTCTL);
 	P0.L = lo(EBIU_RSTCTL);
 	R2 = [P0];
-	R3.H = hi(SRREQ);
-	R3.L = lo(SRREQ);
-#else
+	BITCLR(R2, 3); /* clear SRREQ bit */
+	[P0] = R2;
+#elif defined(EBIU_SDGCTL)	/* SDRAM */
+
+	P0.L = lo(EBIU_SDGCTL); /* release CLKOUT from self-refresh */
 	P0.H = hi(EBIU_SDGCTL);
-	P0.L = lo(EBIU_SDGCTL);
 	R2 = [P0];
-	R3.H = hi(SRFS);
-	R3.L = lo(SRFS);
+	BITSET(R2, 0); /* SCTLE enable CLKOUT */
+	[P0] = R2
+	SSYNC;
+
+	P0.L = lo(EBIU_SDGCTL); /* release SDRAM from self-refresh */
+	P0.H = hi(EBIU_SDGCTL);
+	R2 = [P0];
+	BITCLR(R2, 24); /* clear SRFS bit */
+	[P0] = R2
 #endif
-	R3 = ~R3;
-	R2 = R2&R3;
-	[P0] = R2;
-	ssync;
+	SSYNC;
 	RTS;
 
 ENTRY(_set_sic_iwr)
@@ -307,6 +325,11 @@ ENTRY(_set_rtc_istat)
 	P0.L = lo(RTC_ISTAT);
 	w[P0] = R0.L;
 	SSYNC;
+#elif (ANOMALY_05000371)
+	nop;
+	nop;
+	nop;
+	nop;
 #endif
 	RTS;
 
@@ -318,3 +341,508 @@ ENTRY(_test_pll_locked)
 	CC = BITTST(R0,5);
 	IF !CC JUMP 1b;
 	RTS;
+
+.section .text
+
+
+ENTRY(_do_hibernate)
+	[--SP] = ( R7:0, P5:0 );
+	[--SP] =  RETS;
+	/* Save System MMRs */
+	R2 = R0;
+	P0.H = hi(PLL_CTL);
+	P0.L = lo(PLL_CTL);
+
+#ifdef SIC_IMASK0
+	PM_SYS_PUSH(SIC_IMASK0)
+#endif
+#ifdef SIC_IMASK1
+	PM_SYS_PUSH(SIC_IMASK1)
+#endif
+#ifdef SIC_IMASK2
+	PM_SYS_PUSH(SIC_IMASK2)
+#endif
+#ifdef SIC_IMASK
+	PM_SYS_PUSH(SIC_IMASK)
+#endif
+#ifdef SICA_IMASK0
+	PM_SYS_PUSH(SICA_IMASK0)
+#endif
+#ifdef SICA_IMASK1
+	PM_SYS_PUSH(SICA_IMASK1)
+#endif
+#ifdef SIC_IAR2
+	PM_SYS_PUSH(SIC_IAR0)
+	PM_SYS_PUSH(SIC_IAR1)
+	PM_SYS_PUSH(SIC_IAR2)
+#endif
+#ifdef SIC_IAR3
+	PM_SYS_PUSH(SIC_IAR3)
+#endif
+#ifdef SIC_IAR4
+	PM_SYS_PUSH(SIC_IAR4)
+	PM_SYS_PUSH(SIC_IAR5)
+	PM_SYS_PUSH(SIC_IAR6)
+#endif
+#ifdef SIC_IAR7
+	PM_SYS_PUSH(SIC_IAR7)
+#endif
+#ifdef SIC_IAR8
+	PM_SYS_PUSH(SIC_IAR8)
+	PM_SYS_PUSH(SIC_IAR9)
+	PM_SYS_PUSH(SIC_IAR10)
+	PM_SYS_PUSH(SIC_IAR11)
+#endif
+
+#ifdef SICA_IAR0
+	PM_SYS_PUSH(SICA_IAR0)
+	PM_SYS_PUSH(SICA_IAR1)
+	PM_SYS_PUSH(SICA_IAR2)
+	PM_SYS_PUSH(SICA_IAR3)
+	PM_SYS_PUSH(SICA_IAR4)
+	PM_SYS_PUSH(SICA_IAR5)
+	PM_SYS_PUSH(SICA_IAR6)
+	PM_SYS_PUSH(SICA_IAR7)
+#endif
+
+#ifdef SIC_IWR
+	PM_SYS_PUSH(SIC_IWR)
+#endif
+#ifdef SIC_IWR0
+	PM_SYS_PUSH(SIC_IWR0)
+#endif
+#ifdef SIC_IWR1
+	PM_SYS_PUSH(SIC_IWR1)
+#endif
+#ifdef SIC_IWR2
+	PM_SYS_PUSH(SIC_IWR2)
+#endif
+#ifdef SICA_IWR0
+	PM_SYS_PUSH(SICA_IWR0)
+#endif
+#ifdef SICA_IWR1
+	PM_SYS_PUSH(SICA_IWR1)
+#endif
+
+#ifdef PINT0_ASSIGN
+	PM_SYS_PUSH(PINT0_ASSIGN)
+	PM_SYS_PUSH(PINT1_ASSIGN)
+	PM_SYS_PUSH(PINT2_ASSIGN)
+	PM_SYS_PUSH(PINT3_ASSIGN)
+#endif
+
+	PM_SYS_PUSH(EBIU_AMBCTL0)
+	PM_SYS_PUSH(EBIU_AMBCTL1)
+	PM_SYS_PUSH16(EBIU_AMGCTL)
+
+#ifdef EBIU_FCTL
+	PM_SYS_PUSH(EBIU_MBSCTL)
+	PM_SYS_PUSH(EBIU_MODE)
+	PM_SYS_PUSH(EBIU_FCTL)
+#endif
+
+	PM_SYS_PUSH16(SYSCR)
+
+	/* Save Core MMRs */
+	P0.H = hi(SRAM_BASE_ADDRESS);
+	P0.L = lo(SRAM_BASE_ADDRESS);
+
+	PM_PUSH(DMEM_CONTROL)
+	PM_PUSH(DCPLB_ADDR0)
+	PM_PUSH(DCPLB_ADDR1)
+	PM_PUSH(DCPLB_ADDR2)
+	PM_PUSH(DCPLB_ADDR3)
+	PM_PUSH(DCPLB_ADDR4)
+	PM_PUSH(DCPLB_ADDR5)
+	PM_PUSH(DCPLB_ADDR6)
+	PM_PUSH(DCPLB_ADDR7)
+	PM_PUSH(DCPLB_ADDR8)
+	PM_PUSH(DCPLB_ADDR9)
+	PM_PUSH(DCPLB_ADDR10)
+	PM_PUSH(DCPLB_ADDR11)
+	PM_PUSH(DCPLB_ADDR12)
+	PM_PUSH(DCPLB_ADDR13)
+	PM_PUSH(DCPLB_ADDR14)
+	PM_PUSH(DCPLB_ADDR15)
+	PM_PUSH(DCPLB_DATA0)
+	PM_PUSH(DCPLB_DATA1)
+	PM_PUSH(DCPLB_DATA2)
+	PM_PUSH(DCPLB_DATA3)
+	PM_PUSH(DCPLB_DATA4)
+	PM_PUSH(DCPLB_DATA5)
+	PM_PUSH(DCPLB_DATA6)
+	PM_PUSH(DCPLB_DATA7)
+	PM_PUSH(DCPLB_DATA8)
+	PM_PUSH(DCPLB_DATA9)
+	PM_PUSH(DCPLB_DATA10)
+	PM_PUSH(DCPLB_DATA11)
+	PM_PUSH(DCPLB_DATA12)
+	PM_PUSH(DCPLB_DATA13)
+	PM_PUSH(DCPLB_DATA14)
+	PM_PUSH(DCPLB_DATA15)
+	PM_PUSH(IMEM_CONTROL)
+	PM_PUSH(ICPLB_ADDR0)
+	PM_PUSH(ICPLB_ADDR1)
+	PM_PUSH(ICPLB_ADDR2)
+	PM_PUSH(ICPLB_ADDR3)
+	PM_PUSH(ICPLB_ADDR4)
+	PM_PUSH(ICPLB_ADDR5)
+	PM_PUSH(ICPLB_ADDR6)
+	PM_PUSH(ICPLB_ADDR7)
+	PM_PUSH(ICPLB_ADDR8)
+	PM_PUSH(ICPLB_ADDR9)
+	PM_PUSH(ICPLB_ADDR10)
+	PM_PUSH(ICPLB_ADDR11)
+	PM_PUSH(ICPLB_ADDR12)
+	PM_PUSH(ICPLB_ADDR13)
+	PM_PUSH(ICPLB_ADDR14)
+	PM_PUSH(ICPLB_ADDR15)
+	PM_PUSH(ICPLB_DATA0)
+	PM_PUSH(ICPLB_DATA1)
+	PM_PUSH(ICPLB_DATA2)
+	PM_PUSH(ICPLB_DATA3)
+	PM_PUSH(ICPLB_DATA4)
+	PM_PUSH(ICPLB_DATA5)
+	PM_PUSH(ICPLB_DATA6)
+	PM_PUSH(ICPLB_DATA7)
+	PM_PUSH(ICPLB_DATA8)
+	PM_PUSH(ICPLB_DATA9)
+	PM_PUSH(ICPLB_DATA10)
+	PM_PUSH(ICPLB_DATA11)
+	PM_PUSH(ICPLB_DATA12)
+	PM_PUSH(ICPLB_DATA13)
+	PM_PUSH(ICPLB_DATA14)
+	PM_PUSH(ICPLB_DATA15)
+	PM_PUSH(EVT0)
+	PM_PUSH(EVT1)
+	PM_PUSH(EVT2)
+	PM_PUSH(EVT3)
+	PM_PUSH(EVT4)
+	PM_PUSH(EVT5)
+	PM_PUSH(EVT6)
+	PM_PUSH(EVT7)
+	PM_PUSH(EVT8)
+	PM_PUSH(EVT9)
+	PM_PUSH(EVT10)
+	PM_PUSH(EVT11)
+	PM_PUSH(EVT12)
+	PM_PUSH(EVT13)
+	PM_PUSH(EVT14)
+	PM_PUSH(EVT15)
+	PM_PUSH(IMASK)
+	PM_PUSH(ILAT)
+	PM_PUSH(IPRIO)
+	PM_PUSH(TCNTL)
+	PM_PUSH(TPERIOD)
+	PM_PUSH(TSCALE)
+	PM_PUSH(TCOUNT)
+	PM_PUSH(TBUFCTL)
+
+	/* Save Core Registers */
+	[--sp] = SYSCFG;
+	[--sp] = ( R7:0, P5:0 );
+	[--sp] = fp;
+	[--sp] = usp;
+
+	[--sp] = i0;
+	[--sp] = i1;
+	[--sp] = i2;
+	[--sp] = i3;
+
+	[--sp] = m0;
+	[--sp] = m1;
+	[--sp] = m2;
+	[--sp] = m3;
+
+	[--sp] = l0;
+	[--sp] = l1;
+	[--sp] = l2;
+	[--sp] = l3;
+
+	[--sp] = b0;
+	[--sp] = b1;
+	[--sp] = b2;
+	[--sp] = b3;
+	[--sp] = a0.x;
+	[--sp] = a0.w;
+	[--sp] = a1.x;
+	[--sp] = a1.w;
+
+	[--sp] = LC0;
+	[--sp] = LC1;
+	[--sp] = LT0;
+	[--sp] = LT1;
+	[--sp] = LB0;
+	[--sp] = LB1;
+
+	[--sp] = ASTAT;
+	[--sp] = CYCLES;
+	[--sp] = CYCLES2;
+
+	[--sp] = RETS;
+	r0 = RETI;
+	[--sp] = r0;
+	[--sp] = RETX;
+	[--sp] = RETN;
+	[--sp] = RETE;
+	[--sp] = SEQSTAT;
+
+	/* Save Magic, return address and Stack Pointer */
+	P0.H = 0;
+	P0.L = 0;
+	R0.H = 0xDEAD;	/* Hibernate Magic */
+	R0.L = 0xBEEF;
+	[P0++] = R0;	/* Store Hibernate Magic */
+	R0.H = pm_resume_here;
+	R0.L = pm_resume_here;
+	[P0++] = R0;	/* Save Return Address */
+	[P0++] = SP;	/* Save Stack Pointer */
+	P0.H = _hibernate_mode;
+	P0.L = _hibernate_mode;
+	R0 = R2;
+	call (P0); /* Goodbye */
+
+pm_resume_here:
+
+	/* Restore Core Registers */
+	SEQSTAT = [sp++];
+	RETE = [sp++];
+	RETN = [sp++];
+	RETX = [sp++];
+	r0 = [sp++];
+	RETI = r0;
+	RETS = [sp++];
+
+	CYCLES2 = [sp++];
+	CYCLES = [sp++];
+	ASTAT = [sp++];
+
+	LB1 = [sp++];
+	LB0 = [sp++];
+	LT1 = [sp++];
+	LT0 = [sp++];
+	LC1 = [sp++];
+	LC0 = [sp++];
+
+	a1.w = [sp++];
+	a1.x = [sp++];
+	a0.w = [sp++];
+	a0.x = [sp++];
+	b3 = [sp++];
+	b2 = [sp++];
+	b1 = [sp++];
+	b0 = [sp++];
+
+	l3 = [sp++];
+	l2 = [sp++];
+	l1 = [sp++];
+	l0 = [sp++];
+
+	m3 = [sp++];
+	m2 = [sp++];
+	m1 = [sp++];
+	m0 = [sp++];
+
+	i3 = [sp++];
+	i2 = [sp++];
+	i1 = [sp++];
+	i0 = [sp++];
+
+	usp = [sp++];
+	fp = [sp++];
+
+	( R7 : 0, P5 : 0) = [ SP ++ ];
+	SYSCFG = [sp++];
+
+	/* Restore Core MMRs */
+
+	PM_POP(TBUFCTL)
+	PM_POP(TCOUNT)
+	PM_POP(TSCALE)
+	PM_POP(TPERIOD)
+	PM_POP(TCNTL)
+	PM_POP(IPRIO)
+	PM_POP(ILAT)
+	PM_POP(IMASK)
+	PM_POP(EVT15)
+	PM_POP(EVT14)
+	PM_POP(EVT13)
+	PM_POP(EVT12)
+	PM_POP(EVT11)
+	PM_POP(EVT10)
+	PM_POP(EVT9)
+	PM_POP(EVT8)
+	PM_POP(EVT7)
+	PM_POP(EVT6)
+	PM_POP(EVT5)
+	PM_POP(EVT4)
+	PM_POP(EVT3)
+	PM_POP(EVT2)
+	PM_POP(EVT1)
+	PM_POP(EVT0)
+	PM_POP(ICPLB_DATA15)
+	PM_POP(ICPLB_DATA14)
+	PM_POP(ICPLB_DATA13)
+	PM_POP(ICPLB_DATA12)
+	PM_POP(ICPLB_DATA11)
+	PM_POP(ICPLB_DATA10)
+	PM_POP(ICPLB_DATA9)
+	PM_POP(ICPLB_DATA8)
+	PM_POP(ICPLB_DATA7)
+	PM_POP(ICPLB_DATA6)
+	PM_POP(ICPLB_DATA5)
+	PM_POP(ICPLB_DATA4)
+	PM_POP(ICPLB_DATA3)
+	PM_POP(ICPLB_DATA2)
+	PM_POP(ICPLB_DATA1)
+	PM_POP(ICPLB_DATA0)
+	PM_POP(ICPLB_ADDR15)
+	PM_POP(ICPLB_ADDR14)
+	PM_POP(ICPLB_ADDR13)
+	PM_POP(ICPLB_ADDR12)
+	PM_POP(ICPLB_ADDR11)
+	PM_POP(ICPLB_ADDR10)
+	PM_POP(ICPLB_ADDR9)
+	PM_POP(ICPLB_ADDR8)
+	PM_POP(ICPLB_ADDR7)
+	PM_POP(ICPLB_ADDR6)
+	PM_POP(ICPLB_ADDR5)
+	PM_POP(ICPLB_ADDR4)
+	PM_POP(ICPLB_ADDR3)
+	PM_POP(ICPLB_ADDR2)
+	PM_POP(ICPLB_ADDR1)
+	PM_POP(ICPLB_ADDR0)
+	PM_POP(IMEM_CONTROL)
+	PM_POP(DCPLB_DATA15)
+	PM_POP(DCPLB_DATA14)
+	PM_POP(DCPLB_DATA13)
+	PM_POP(DCPLB_DATA12)
+	PM_POP(DCPLB_DATA11)
+	PM_POP(DCPLB_DATA10)
+	PM_POP(DCPLB_DATA9)
+	PM_POP(DCPLB_DATA8)
+	PM_POP(DCPLB_DATA7)
+	PM_POP(DCPLB_DATA6)
+	PM_POP(DCPLB_DATA5)
+	PM_POP(DCPLB_DATA4)
+	PM_POP(DCPLB_DATA3)
+	PM_POP(DCPLB_DATA2)
+	PM_POP(DCPLB_DATA1)
+	PM_POP(DCPLB_DATA0)
+	PM_POP(DCPLB_ADDR15)
+	PM_POP(DCPLB_ADDR14)
+	PM_POP(DCPLB_ADDR13)
+	PM_POP(DCPLB_ADDR12)
+	PM_POP(DCPLB_ADDR11)
+	PM_POP(DCPLB_ADDR10)
+	PM_POP(DCPLB_ADDR9)
+	PM_POP(DCPLB_ADDR8)
+	PM_POP(DCPLB_ADDR7)
+	PM_POP(DCPLB_ADDR6)
+	PM_POP(DCPLB_ADDR5)
+	PM_POP(DCPLB_ADDR4)
+	PM_POP(DCPLB_ADDR3)
+	PM_POP(DCPLB_ADDR2)
+	PM_POP(DCPLB_ADDR1)
+	PM_POP(DCPLB_ADDR0)
+	PM_POP(DMEM_CONTROL)
+
+	/* Restore System MMRs */
+
+	P0.H = hi(PLL_CTL);
+	P0.L = lo(PLL_CTL);
+	PM_SYS_POP16(SYSCR)
+
+#ifdef EBIU_FCTL
+	PM_SYS_POP(EBIU_FCTL)
+	PM_SYS_POP(EBIU_MODE)
+	PM_SYS_POP(EBIU_MBSCTL)
+#endif
+	PM_SYS_POP16(EBIU_AMGCTL)
+	PM_SYS_POP(EBIU_AMBCTL1)
+	PM_SYS_POP(EBIU_AMBCTL0)
+
+#ifdef PINT0_ASSIGN
+	PM_SYS_POP(PINT3_ASSIGN)
+	PM_SYS_POP(PINT2_ASSIGN)
+	PM_SYS_POP(PINT1_ASSIGN)
+	PM_SYS_POP(PINT0_ASSIGN)
+#endif
+
+#ifdef SICA_IWR1
+	PM_SYS_POP(SICA_IWR1)
+#endif
+#ifdef SICA_IWR0
+	PM_SYS_POP(SICA_IWR0)
+#endif
+#ifdef SIC_IWR2
+	PM_SYS_POP(SIC_IWR2)
+#endif
+#ifdef SIC_IWR1
+	PM_SYS_POP(SIC_IWR1)
+#endif
+#ifdef SIC_IWR0
+	PM_SYS_POP(SIC_IWR0)
+#endif
+#ifdef SIC_IWR
+	PM_SYS_POP(SIC_IWR)
+#endif
+
+#ifdef SICA_IAR0
+	PM_SYS_POP(SICA_IAR7)
+	PM_SYS_POP(SICA_IAR6)
+	PM_SYS_POP(SICA_IAR5)
+	PM_SYS_POP(SICA_IAR4)
+	PM_SYS_POP(SICA_IAR3)
+	PM_SYS_POP(SICA_IAR2)
+	PM_SYS_POP(SICA_IAR1)
+	PM_SYS_POP(SICA_IAR0)
+#endif
+
+#ifdef SIC_IAR8
+	PM_SYS_POP(SIC_IAR11)
+	PM_SYS_POP(SIC_IAR10)
+	PM_SYS_POP(SIC_IAR9)
+	PM_SYS_POP(SIC_IAR8)
+#endif
+#ifdef SIC_IAR7
+	PM_SYS_POP(SIC_IAR7)
+#endif
+#ifdef SIC_IAR6
+	PM_SYS_POP(SIC_IAR6)
+	PM_SYS_POP(SIC_IAR5)
+	PM_SYS_POP(SIC_IAR4)
+#endif
+#ifdef SIC_IAR3
+	PM_SYS_POP(SIC_IAR3)
+#endif
+#ifdef SIC_IAR2
+	PM_SYS_POP(SIC_IAR2)
+	PM_SYS_POP(SIC_IAR1)
+	PM_SYS_POP(SIC_IAR0)
+#endif
+#ifdef SICA_IMASK1
+	PM_SYS_POP(SICA_IMASK1)
+#endif
+#ifdef SICA_IMASK0
+	PM_SYS_POP(SICA_IMASK0)
+#endif
+#ifdef SIC_IMASK
+	PM_SYS_POP(SIC_IMASK)
+#endif
+#ifdef SIC_IMASK2
+	PM_SYS_POP(SIC_IMASK2)
+#endif
+#ifdef SIC_IMASK1
+	PM_SYS_POP(SIC_IMASK1)
+#endif
+#ifdef SIC_IMASK0
+	PM_SYS_POP(SIC_IMASK0)
+#endif
+
+	[--sp] = RETI;	/* Clear Global Interrupt Disable */
+	SP += 4;
+
+	RETS = [SP++];
+	( R7:0, P5:0 ) = [SP++];
+	RTS;
diff --git a/arch/blackfin/mach-common/pm.c b/arch/blackfin/mach-common/pm.c
index 0be805ca423..4fe6a2366b1 100644
--- a/arch/blackfin/mach-common/pm.c
+++ b/arch/blackfin/mach-common/pm.c
@@ -38,8 +38,9 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 
-#include <asm/dpmc.h>
 #include <asm/gpio.h>
+#include <asm/dma.h>
+#include <asm/dpmc.h>
 
 #ifdef CONFIG_PM_WAKEUP_GPIO_POLAR_H
 #define WAKEUP_TYPE	PM_WAKE_HIGH
@@ -61,16 +62,17 @@
 #define WAKEUP_TYPE	PM_WAKE_BOTH_EDGES
 #endif
 
+
 void bfin_pm_suspend_standby_enter(void)
 {
+	unsigned long flags;
+
 #ifdef CONFIG_PM_WAKEUP_BY_GPIO
 	gpio_pm_wakeup_request(CONFIG_PM_WAKEUP_GPIO_NUMBER, WAKEUP_TYPE);
 #endif
 
-	u32 flags;
-
 	local_irq_save(flags);
-	bfin_pm_setup();
+	bfin_pm_standby_setup();
 
 #ifdef CONFIG_PM_BFIN_SLEEP_DEEPER
 	sleep_deeper(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
@@ -78,7 +80,7 @@ void bfin_pm_suspend_standby_enter(void)
 	sleep_mode(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
 #endif
 
-	bfin_pm_restore();
+	bfin_pm_standby_restore();
 
 #if defined(CONFIG_BF54x) || defined(CONFIG_BF52x)  || defined(CONFIG_BF561)
 	bfin_write_SIC_IWR0(IWR_ENABLE_ALL);
@@ -93,6 +95,195 @@ void bfin_pm_suspend_standby_enter(void)
 	local_irq_restore(flags);
 }
 
+int bf53x_suspend_l1_mem(unsigned char *memptr)
+{
+	dma_memcpy(memptr, (const void *) L1_CODE_START, L1_CODE_LENGTH);
+	dma_memcpy(memptr + L1_CODE_LENGTH, (const void *) L1_DATA_A_START,
+			L1_DATA_A_LENGTH);
+	dma_memcpy(memptr + L1_CODE_LENGTH + L1_DATA_A_LENGTH,
+			(const void *) L1_DATA_B_START, L1_DATA_B_LENGTH);
+	memcpy(memptr + L1_CODE_LENGTH + L1_DATA_A_LENGTH +
+			L1_DATA_B_LENGTH, (const void *) L1_SCRATCH_START,
+			L1_SCRATCH_LENGTH);
+
+	return 0;
+}
+
+int bf53x_resume_l1_mem(unsigned char *memptr)
+{
+	dma_memcpy((void *) L1_CODE_START, memptr, L1_CODE_LENGTH);
+	dma_memcpy((void *) L1_DATA_A_START, memptr + L1_CODE_LENGTH,
+			L1_DATA_A_LENGTH);
+	dma_memcpy((void *) L1_DATA_B_START, memptr + L1_CODE_LENGTH +
+			L1_DATA_A_LENGTH, L1_DATA_B_LENGTH);
+	memcpy((void *) L1_SCRATCH_START, memptr + L1_CODE_LENGTH +
+			L1_DATA_A_LENGTH + L1_DATA_B_LENGTH, L1_SCRATCH_LENGTH);
+
+	return 0;
+}
+
+#ifdef CONFIG_BFIN_WB
+static void flushinv_all_dcache(void)
+{
+	u32 way, bank, subbank, set;
+	u32 status, addr;
+	u32 dmem_ctl = bfin_read_DMEM_CONTROL();
+
+	for (bank = 0; bank < 2; ++bank) {
+		if (!(dmem_ctl & (1 << (DMC1_P - bank))))
+			continue;
+
+		for (way = 0; way < 2; ++way)
+			for (subbank = 0; subbank < 4; ++subbank)
+				for (set = 0; set < 64; ++set) {
+
+					bfin_write_DTEST_COMMAND(
+						way << 26 |
+						bank << 23 |
+						subbank << 16 |
+						set << 5
+					);
+					CSYNC();
+					status = bfin_read_DTEST_DATA0();
+
+					/* only worry about valid/dirty entries */
+					if ((status & 0x3) != 0x3)
+						continue;
+
+					/* construct the address using the tag */
+					addr = (status & 0xFFFFC800) | (subbank << 12) | (set << 5);
+
+					/* flush it */
+					__asm__ __volatile__("FLUSHINV[%0];" : : "a"(addr));
+				}
+	}
+}
+#endif
+
+static inline void dcache_disable(void)
+{
+#ifdef CONFIG_BFIN_DCACHE
+	unsigned long ctrl;
+
+#ifdef CONFIG_BFIN_WB
+	flushinv_all_dcache();
+#endif
+	SSYNC();
+	ctrl = bfin_read_DMEM_CONTROL();
+	ctrl &= ~ENDCPLB;
+	bfin_write_DMEM_CONTROL(ctrl);
+	SSYNC();
+#endif
+}
+
+static inline void dcache_enable(void)
+{
+#ifdef CONFIG_BFIN_DCACHE
+	unsigned long ctrl;
+	SSYNC();
+	ctrl = bfin_read_DMEM_CONTROL();
+	ctrl |= ENDCPLB;
+	bfin_write_DMEM_CONTROL(ctrl);
+	SSYNC();
+#endif
+}
+
+static inline void icache_disable(void)
+{
+#ifdef CONFIG_BFIN_ICACHE
+	unsigned long ctrl;
+	SSYNC();
+	ctrl = bfin_read_IMEM_CONTROL();
+	ctrl &= ~ENICPLB;
+	bfin_write_IMEM_CONTROL(ctrl);
+	SSYNC();
+#endif
+}
+
+static inline void icache_enable(void)
+{
+#ifdef CONFIG_BFIN_ICACHE
+	unsigned long ctrl;
+	SSYNC();
+	ctrl = bfin_read_IMEM_CONTROL();
+	ctrl |= ENICPLB;
+	bfin_write_IMEM_CONTROL(ctrl);
+	SSYNC();
+#endif
+}
+
+int bfin_pm_suspend_mem_enter(void)
+{
+	unsigned long flags;
+	int wakeup, ret;
+
+	unsigned char *memptr = kmalloc(L1_CODE_LENGTH + L1_DATA_A_LENGTH
+					 + L1_DATA_B_LENGTH + L1_SCRATCH_LENGTH,
+					  GFP_KERNEL);
+
+	if (memptr == NULL) {
+		panic("bf53x_suspend_l1_mem malloc failed");
+		return -ENOMEM;
+	}
+
+	wakeup = bfin_read_VR_CTL() & ~FREQ;
+	wakeup |= SCKELOW;
+
+	/* FIXME: merge this somehow with set_irq_wake */
+#ifdef CONFIG_PM_BFIN_WAKE_RTC
+	wakeup |= WAKE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_PH6
+	wakeup |= PHYWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_CAN
+	wakeup |= CANWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_GP
+	wakeup |= GPWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_USB
+	wakeup |= USBWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_KEYPAD
+	wakeup |= KPADWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_ROTARY
+	wakeup |= ROTWE;
+#endif
+
+	local_irq_save(flags);
+
+	ret = blackfin_dma_suspend();
+
+	if (ret) {
+		local_irq_restore(flags);
+		kfree(memptr);
+		return ret;
+	}
+
+	bfin_gpio_pm_hibernate_suspend();
+
+	dcache_disable();
+	icache_disable();
+	bf53x_suspend_l1_mem(memptr);
+
+	do_hibernate(wakeup);	/* Goodbye */
+
+	bf53x_resume_l1_mem(memptr);
+
+	icache_enable();
+	dcache_enable();
+
+	bfin_gpio_pm_hibernate_restore();
+	blackfin_dma_resume();
+
+	local_irq_restore(flags);
+	kfree(memptr);
+
+	return 0;
+}
+
 /*
  *	bfin_pm_valid - Tell the PM core that we only support the standby sleep
  *			state
@@ -101,7 +292,24 @@ void bfin_pm_suspend_standby_enter(void)
  */
 static int bfin_pm_valid(suspend_state_t state)
 {
-	return (state == PM_SUSPEND_STANDBY);
+	return (state == PM_SUSPEND_STANDBY
+#ifndef BF533_FAMILY
+	/*
+	 * On BF533/2/1:
+	 * If we enter Hibernate the SCKE Pin is driven Low,
+	 * so that the SDRAM enters Self Refresh Mode.
+	 * However when the reset sequence that follows hibernate
+	 * state is executed, SCKE is driven High, taking the
+	 * SDRAM out of Self Refresh.
+	 *
+	 * If you reconfigure and access the SDRAM "very quickly",
+	 * you are likely to avoid errors, otherwise the SDRAM
+	 * start losing its contents.
+	 * An external HW workaround is possible using logic gates.
+	 */
+	|| state == PM_SUSPEND_MEM
+#endif
+	);
 }
 
 /*
@@ -115,10 +323,9 @@ static int bfin_pm_enter(suspend_state_t state)
 	case PM_SUSPEND_STANDBY:
 		bfin_pm_suspend_standby_enter();
 		break;
-
 	case PM_SUSPEND_MEM:
-		return -ENOTSUPP;
-
+		bfin_pm_suspend_mem_enter();
+		break;
 	default:
 		return -EINVAL;
 	}