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>

9 files changed, 1033 insertions, 119 deletions

diff --git a/arch/blackfin/Kconfig b/arch/blackfin/Kconfig
index c602727..8f21b78 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 d54f190..ad0e758 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 b6d89d1..ecbd141 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 b7981d3..46ee77a 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 0be805c..4fe6a23 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;
 	}
diff --git a/include/asm-blackfin/dma.h b/include/asm-blackfin/dma.h
index c0d5259..3cd4b52 100644
--- a/include/asm-blackfin/dma.h
+++ b/include/asm-blackfin/dma.h
@@ -144,8 +144,16 @@ struct dma_channel {
 	void *data;
 	unsigned int dma_enable_flag;
 	unsigned int loopback_flag;
+#ifdef CONFIG_PM
+	unsigned short saved_peripheral_map;
+#endif
 };
 
+#ifdef CONFIG_PM
+int blackfin_dma_suspend(void);
+void blackfin_dma_resume(void);
+#endif
+
 /*******************************************************************************
 *	DMA API's
 *******************************************************************************/
diff --git a/include/asm-blackfin/dpmc.h b/include/asm-blackfin/dpmc.h
index 7f34cd3..de28e6e 100644
--- a/include/asm-blackfin/dpmc.h
+++ b/include/asm-blackfin/dpmc.h
@@ -7,63 +7,18 @@
 #ifndef _BLACKFIN_DPMC_H_
 #define _BLACKFIN_DPMC_H_
 
-#define SLEEP_MODE		1
-#define DEEP_SLEEP_MODE		2
-#define ACTIVE_PLL_DISABLED	3
-#define FULLON_MODE		4
-#define ACTIVE_PLL_ENABLED	5
-#define HIBERNATE_MODE		6
-
-#define IOCTL_FULL_ON_MODE	_IO('s', 0xA0)
-#define IOCTL_ACTIVE_MODE	_IO('s', 0xA1)
-#define IOCTL_SLEEP_MODE	_IO('s', 0xA2)
-#define IOCTL_DEEP_SLEEP_MODE	_IO('s', 0xA3)
-#define IOCTL_HIBERNATE_MODE	_IO('s', 0xA4)
-#define IOCTL_CHANGE_FREQUENCY	_IOW('s', 0xA5, unsigned long)
-#define IOCTL_CHANGE_VOLTAGE	_IOW('s', 0xA6, unsigned long)
-#define IOCTL_SET_CCLK		_IOW('s', 0xA7, unsigned long)
-#define IOCTL_SET_SCLK		_IOW('s', 0xA8, unsigned long)
-#define IOCTL_GET_PLLSTATUS	_IOW('s', 0xA9, unsigned long)
-#define IOCTL_GET_CORECLOCK	_IOW('s', 0xAA, unsigned long)
-#define IOCTL_GET_SYSTEMCLOCK	_IOW('s', 0xAB, unsigned long)
-#define IOCTL_GET_VCO		_IOW('s', 0xAC, unsigned long)
-#define IOCTL_DISABLE_WDOG_TIMER _IO('s', 0xAD)
-#define IOCTL_UNMASK_WDOG_WAKEUP_EVENT _IO('s',0xAE)
-#define IOCTL_PROGRAM_WDOG_TIMER _IOW('s',0xAF,unsigned long)
-#define IOCTL_CLEAR_WDOG_WAKEUP_EVENT _IO('s',0xB0)
-#define IOCTL_SLEEP_DEEPER_MODE _IO('s',0xB1)
-
-#define DPMC_MINOR		254
-
-#define ON	0
-#define OFF	1
-
 #ifdef __KERNEL__
+#ifndef __ASSEMBLY__
 
-unsigned long calc_volt(void);
-int calc_vlev(int vlt);
-unsigned long change_voltage(unsigned long volt);
-int calc_msel(int vco_hz);
-unsigned long change_frequency(unsigned long vco_mhz);
-int set_pll_div(unsigned short sel, unsigned char flag);
-int get_vco(void);
-unsigned long change_system_clock(unsigned long clock);
-unsigned long change_core_clock(unsigned long clock);
-unsigned long get_pll_status(void);
-void change_baud(int baud);
-void fullon_mode(void);
-void active_mode(void);
 void sleep_mode(u32 sic_iwr0, u32 sic_iwr1, u32 sic_iwr2);
 void deep_sleep(u32 sic_iwr0, u32 sic_iwr1, u32 sic_iwr2);
 void hibernate_mode(u32 sic_iwr0, u32 sic_iwr1, u32 sic_iwr2);
 void sleep_deeper(u32 sic_iwr0, u32 sic_iwr1, u32 sic_iwr2);
-void program_wdog_timer(unsigned long);
-void unmask_wdog_wakeup_evt(void);
-void clear_wdog_wakeup_evt(void);
-void disable_wdog_timer(void);
+void do_hibernate(int wakeup);
+void set_dram_srfs(void);
+void unset_dram_srfs(void);
 
-extern unsigned long get_cclk(void);
-extern unsigned long get_sclk(void);
+#define VRPAIR(vlev, freq) (((vlev) << 16) | ((freq) >> 16))
 
 struct bfin_dpmc_platform_data {
 	const unsigned int *tuple_tab;
@@ -71,8 +26,33 @@ struct bfin_dpmc_platform_data {
 	unsigned short vr_settling_time; /* in us */
 };
 
-#define VRPAIR(vlev, freq) (((vlev) << 16) | ((freq) >> 16))
+#else
+
+#define PM_PUSH(x) \
+	R0 = [P0 + (x - SRAM_BASE_ADDRESS)];\
+	[--SP] =  R0;\
+
+#define PM_POP(x) \
+	R0 = [SP++];\
+	[P0 + (x - SRAM_BASE_ADDRESS)] = R0;\
+
+#define PM_SYS_PUSH(x) \
+	R0 = [P0 + (x - PLL_CTL)];\
+	[--SP] =  R0;\
+
+#define PM_SYS_POP(x) \
+	R0 = [SP++];\
+	[P0 + (x - PLL_CTL)] = R0;\
+
+#define PM_SYS_PUSH16(x) \
+	R0 = w[P0 + (x - PLL_CTL)];\
+	[--SP] =  R0;\
+
+#define PM_SYS_POP16(x) \
+	R0 = [SP++];\
+	w[P0 + (x - PLL_CTL)] = R0;\
 
+#endif
 #endif	/* __KERNEL__ */
 
 #endif	/*_BLACKFIN_DPMC_H_*/
diff --git a/include/asm-blackfin/gpio.h b/include/asm-blackfin/gpio.h
index ff95e9d..168f125 100644
--- a/include/asm-blackfin/gpio.h
+++ b/include/asm-blackfin/gpio.h
@@ -376,8 +376,12 @@ struct gpio_port_t {
 #endif
 
 #ifdef CONFIG_PM
-unsigned int bfin_pm_setup(void);
-void bfin_pm_restore(void);
+
+unsigned int bfin_pm_standby_setup(void);
+void bfin_pm_standby_restore(void);
+
+void bfin_gpio_pm_hibernate_restore(void);
+void bfin_gpio_pm_hibernate_suspend(void);
 
 #ifndef CONFIG_BF54x
 #define PM_WAKE_RISING	0x1
@@ -392,17 +396,8 @@ void gpio_pm_wakeup_free(unsigned gpio);
 
 struct gpio_port_s {
 	unsigned short data;
-	unsigned short data_clear;
-	unsigned short data_set;
-	unsigned short toggle;
 	unsigned short maska;
-	unsigned short maska_clear;
-	unsigned short maska_set;
-	unsigned short maska_toggle;
 	unsigned short maskb;
-	unsigned short maskb_clear;
-	unsigned short maskb_set;
-	unsigned short maskb_toggle;
 	unsigned short dir;
 	unsigned short polar;
 	unsigned short edge;
@@ -411,10 +406,10 @@ struct gpio_port_s {
 
 	unsigned short fer;
 	unsigned short reserved;
+	unsigned short mux;
 };
 #endif /*CONFIG_BF54x*/
 #endif /*CONFIG_PM*/
-
 /***********************************************************
 *
 * FUNCTIONS: Blackfin GPIO Driver
diff --git a/include/asm-blackfin/mach-bf548/gpio.h b/include/asm-blackfin/mach-bf548/gpio.h
index cb8b0f1..bba82dc 100644
--- a/include/asm-blackfin/mach-bf548/gpio.h
+++ b/include/asm-blackfin/mach-bf548/gpio.h
@@ -209,3 +209,11 @@ struct gpio_port_t {
 	unsigned short dummy7;
 	unsigned int port_mux;
 };
+
+struct gpio_port_s {
+	unsigned short fer;
+	unsigned short data;
+	unsigned short dir;
+	unsigned short inen;
+	unsigned int mux;
+};