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>

5 files changed, 979 insertions, 56 deletions

diff --git a/arch/blackfin/Kconfig b/arch/blackfin/Kconfig
index c602727d1a9a..8f21b78b2e6f 100644
--- a/arch/blackfin/Kconfig
+++ b/arch/blackfin/Kconfig
@@ -880,7 +880,7 @@ config ARCH_SUSPEND_POSSIBLE
 	depends on !SMP
 
 choice
-	prompt "Default Power Saving Mode"
+	prompt "Standby Power Saving Mode"
 	depends on PM
 	default PM_BFIN_SLEEP_DEEPER
 config  PM_BFIN_SLEEP_DEEPER
@@ -899,6 +899,8 @@ config  PM_BFIN_SLEEP_DEEPER
 	  normal during Sleep Deeper, due to the reduced SCLK frequency.
 	  When in the sleep mode, system DMA access to L1 memory is not supported.
 
+	  If unsure, select "Sleep Deeper".
+
 config  PM_BFIN_SLEEP
 	bool "Sleep"
 	help
@@ -906,15 +908,17 @@ config  PM_BFIN_SLEEP
 	  dissipation by disabling the clock to the processor core (CCLK).
 	  The PLL and system clock (SCLK), however, continue to operate in
 	  this mode. Typically an external event or RTC activity will wake
-	  up the processor. When in the sleep mode,
-	  system DMA access to L1 memory is not supported.
+	  up the processor. When in the sleep mode, system DMA access to L1
+	  memory is not supported.
+
+	  If unsure, select "Sleep Deeper".
 endchoice
 
 config PM_WAKEUP_BY_GPIO
-	bool "Cause Wakeup Event by GPIO"
+	bool "Allow Wakeup from Standby by GPIO"
 
 config PM_WAKEUP_GPIO_NUMBER
-	int "Wakeup GPIO number"
+	int "GPIO number"
 	range 0 47
 	depends on PM_WAKEUP_BY_GPIO
 	default 2 if BFIN537_STAMP
@@ -935,6 +939,58 @@ config  PM_WAKEUP_GPIO_POLAR_EDGE_B
 	bool "Both EDGE"
 endchoice
 
+comment "Possible Suspend Mem / Hibernate Wake-Up Sources"
+	depends on PM
+
+config PM_BFIN_WAKE_RTC
+	bool "Allow Wake-Up from RESET and on-chip RTC"
+	depends on PM
+	default n
+	help
+	  Enable RTC Wake-Up (Voltage Regulator Power-Up)
+
+config PM_BFIN_WAKE_PH6
+	bool "Allow Wake-Up from on-chip PHY or PH6 GP"
+	depends on PM && (BF52x || BF534 || BF536 || BF537)
+	default n
+	help
+	  Enable PHY and PH6 GP Wake-Up (Voltage Regulator Power-Up)
+
+config PM_BFIN_WAKE_CAN
+	bool "Allow Wake-Up from on-chip CAN0/1"
+	depends on PM && (BF54x || BF534 || BF536 || BF537)
+	default n
+	help
+	  Enable CAN0/1 Wake-Up (Voltage Regulator Power-Up)
+
+config PM_BFIN_WAKE_GP
+	bool "Allow Wake-Up from GPIOs"
+	depends on PM && BF54x
+	default n
+	help
+	  Enable General-Purpose Wake-Up (Voltage Regulator Power-Up)
+
+config PM_BFIN_WAKE_USB
+	bool "Allow Wake-Up from on-chip USB"
+	depends on PM && (BF54x || BF52x)
+	default n
+	help
+	  Enable USB Wake-Up (Voltage Regulator Power-Up)
+
+config PM_BFIN_WAKE_KEYPAD
+	bool "Allow Wake-Up from on-chip Keypad"
+	depends on PM && BF54x
+	default n
+	help
+	  Enable Keypad Wake-Up (Voltage Regulator Power-Up)
+
+config PM_BFIN_WAKE_ROTARY
+	bool "Allow Wake-Up from on-chip Rotary"
+	depends on PM && BF54x
+	default n
+	help
+	  Enable Rotary Wake-Up (Voltage Regulator Power-Up)
+
 endmenu
 
 menu "CPU Frequency scaling"
diff --git a/arch/blackfin/kernel/bfin_dma_5xx.c b/arch/blackfin/kernel/bfin_dma_5xx.c
index d54f19085f37..ad0e75845ac2 100644
--- a/arch/blackfin/kernel/bfin_dma_5xx.c
+++ b/arch/blackfin/kernel/bfin_dma_5xx.c
@@ -472,6 +472,32 @@ unsigned long get_dma_curr_addr(unsigned int channel)
 }
 EXPORT_SYMBOL(get_dma_curr_addr);
 
+#ifdef CONFIG_PM
+int blackfin_dma_suspend(void)
+{
+	int i;
+
+	for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++) {
+		if (dma_ch[i].chan_status == DMA_CHANNEL_ENABLED) {
+			printk(KERN_ERR "DMA Channel %d failed to suspend\n", i);
+			return -EBUSY;
+		}
+
+		dma_ch[i].saved_peripheral_map = dma_ch[i].regs->peripheral_map;
+	}
+
+	return 0;
+}
+
+void blackfin_dma_resume(void)
+{
+	int i;
+
+	for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++)
+		dma_ch[i].regs->peripheral_map = dma_ch[i].saved_peripheral_map;
+}
+#endif
+
 static void *__dma_memcpy(void *dest, const void *src, size_t size)
 {
 	int direction;	/* 1 - address decrease, 0 - address increase */
diff --git a/arch/blackfin/kernel/bfin_gpio.c b/arch/blackfin/kernel/bfin_gpio.c
index b6d89d1644cc..ecbd141e0ef2 100644
--- a/arch/blackfin/kernel/bfin_gpio.c
+++ b/arch/blackfin/kernel/bfin_gpio.c
@@ -186,7 +186,10 @@ static struct str_ident {
 	char name[RESOURCE_LABEL_SIZE];
 } str_ident[MAX_RESOURCES];
 
-#if defined(CONFIG_PM) && !defined(CONFIG_BF54x)
+#if defined(CONFIG_PM)
+#if defined(CONFIG_BF54x)
+static struct gpio_port_s gpio_bank_saved[gpio_bank(MAX_BLACKFIN_GPIOS)];
+#else
 static unsigned short wakeup_map[gpio_bank(MAX_BLACKFIN_GPIOS)];
 static unsigned char wakeup_flags_map[MAX_BLACKFIN_GPIOS];
 static struct gpio_port_s gpio_bank_saved[gpio_bank(MAX_BLACKFIN_GPIOS)];
@@ -206,7 +209,7 @@ static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PORTF_INT
 #ifdef BF561_FAMILY
 static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PROG0_INTB, IRQ_PROG1_INTB, IRQ_PROG2_INTB};
 #endif
-
+#endif
 #endif /* CONFIG_PM */
 
 #if defined(BF548_FAMILY)
@@ -667,7 +670,7 @@ static int bfin_gpio_wakeup_type(unsigned gpio, unsigned char type)
 	return 0;
 }
 
-u32 bfin_pm_setup(void)
+u32 bfin_pm_standby_setup(void)
 {
 	u16 bank, mask, i, gpio;
 
@@ -679,7 +682,7 @@ u32 bfin_pm_setup(void)
 		gpio_bankb[bank]->maskb = 0;
 
 		if (mask) {
-#ifdef BF537_FAMILY
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
 			gpio_bank_saved[bank].fer   = *port_fer[bank];
 #endif
 			gpio_bank_saved[bank].inen  = gpio_bankb[bank]->inen;
@@ -715,7 +718,7 @@ u32 bfin_pm_setup(void)
 	return 0;
 }
 
-void bfin_pm_restore(void)
+void bfin_pm_standby_restore(void)
 {
 	u16 bank, mask, i;
 
@@ -724,7 +727,7 @@ void bfin_pm_restore(void)
 		bank = gpio_bank(i);
 
 		if (mask) {
-#ifdef BF537_FAMILY
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
 			*port_fer[bank]   	= gpio_bank_saved[bank].fer;
 #endif
 			gpio_bankb[bank]->inen  = gpio_bank_saved[bank].inen;
@@ -743,8 +746,111 @@ void bfin_pm_restore(void)
 	AWA_DUMMY_READ(maskb);
 }
 
+void bfin_gpio_pm_hibernate_suspend(void)
+{
+	int i, bank;
+
+	for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
+		bank = gpio_bank(i);
+
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+			gpio_bank_saved[bank].fer   = *port_fer[bank];
+#ifdef BF527_FAMILY
+			gpio_bank_saved[bank].mux   = *port_mux[bank];
+#else
+			if (bank == 0)
+				gpio_bank_saved[bank].mux   = bfin_read_PORT_MUX();
+#endif
+#endif
+			gpio_bank_saved[bank].data  = gpio_bankb[bank]->data;
+			gpio_bank_saved[bank].inen  = gpio_bankb[bank]->inen;
+			gpio_bank_saved[bank].polar = gpio_bankb[bank]->polar;
+			gpio_bank_saved[bank].dir   = gpio_bankb[bank]->dir;
+			gpio_bank_saved[bank].edge  = gpio_bankb[bank]->edge;
+			gpio_bank_saved[bank].both  = gpio_bankb[bank]->both;
+			gpio_bank_saved[bank].maska  = gpio_bankb[bank]->maska;
+	}
+
+	AWA_DUMMY_READ(maska);
+}
+
+void bfin_gpio_pm_hibernate_restore(void)
+{
+	int i, bank;
+
+	for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
+			bank = gpio_bank(i);
+
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+#ifdef BF527_FAMILY
+			*port_mux[bank] = gpio_bank_saved[bank].mux;
+#else
+			if (bank == 0)
+				bfin_write_PORT_MUX(gpio_bank_saved[bank].mux);
+#endif
+			*port_fer[bank]   	= gpio_bank_saved[bank].fer;
+#endif
+			gpio_bankb[bank]->inen  = gpio_bank_saved[bank].inen;
+			gpio_bankb[bank]->dir   = gpio_bank_saved[bank].dir;
+			gpio_bankb[bank]->polar = gpio_bank_saved[bank].polar;
+			gpio_bankb[bank]->edge  = gpio_bank_saved[bank].edge;
+			gpio_bankb[bank]->both  = gpio_bank_saved[bank].both;
+
+			gpio_bankb[bank]->data_set = gpio_bank_saved[bank].data
+							| gpio_bank_saved[bank].dir;
+
+			gpio_bankb[bank]->maska = gpio_bank_saved[bank].maska;
+	}
+	AWA_DUMMY_READ(maska);
+}
+
+
 #endif
 #else /* BF548_FAMILY */
+#ifdef CONFIG_PM
+
+u32 bfin_pm_standby_setup(void)
+{
+	return 0;
+}
+
+void bfin_pm_standby_restore(void)
+{
+
+}
+
+void bfin_gpio_pm_hibernate_suspend(void)
+{
+	int i, bank;
+
+	for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
+		bank = gpio_bank(i);
+
+			gpio_bank_saved[bank].fer  = gpio_array[bank]->port_fer;
+			gpio_bank_saved[bank].mux  = gpio_array[bank]->port_mux;
+			gpio_bank_saved[bank].data  = gpio_array[bank]->port_data;
+			gpio_bank_saved[bank].data  = gpio_array[bank]->port_data;
+			gpio_bank_saved[bank].inen  = gpio_array[bank]->port_inen;
+			gpio_bank_saved[bank].dir   = gpio_array[bank]->port_dir_set;
+	}
+}
+
+void bfin_gpio_pm_hibernate_restore(void)
+{
+	int i, bank;
+
+	for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
+			bank = gpio_bank(i);
+
+			gpio_array[bank]->port_mux  = gpio_bank_saved[bank].mux;
+			gpio_array[bank]->port_fer  = gpio_bank_saved[bank].fer;
+			gpio_array[bank]->port_inen  = gpio_bank_saved[bank].inen;
+			gpio_array[bank]->port_dir_set   = gpio_bank_saved[bank].dir;
+			gpio_array[bank]->port_set = gpio_bank_saved[bank].data
+							| gpio_bank_saved[bank].dir;
+	}
+}
+#endif
 
 unsigned short get_gpio_dir(unsigned gpio)
 {
diff --git a/arch/blackfin/mach-common/dpmc_modes.S b/arch/blackfin/mach-common/dpmc_modes.S
index b7981d31c392..46ee77afed20 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 0be805ca423f..4fe6a2366b13 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;
 	}