5 files changed, 829 insertions, 63 deletions

diff --git a/arch/blackfin/mach-common/arch_checks.c b/arch/blackfin/mach-common/arch_checks.c
index caaab49e9cfa..f9160d83b91f 100644
--- a/arch/blackfin/mach-common/arch_checks.c
+++ b/arch/blackfin/mach-common/arch_checks.c
@@ -53,9 +53,3 @@
 # endif
 
 #endif /* CONFIG_BFIN_KERNEL_CLOCK */
-
-#ifdef CONFIG_MEM_SIZE
-#if (CONFIG_MEM_SIZE % 4)
-#error "SDRAM mem size must be multible of 4MB"
-#endif
-#endif
diff --git a/arch/blackfin/mach-common/dpmc_modes.S b/arch/blackfin/mach-common/dpmc_modes.S
index b7981d31c392..5e3f1d8a4fb8 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
 
@@ -51,31 +51,32 @@ ENTRY(_sleep_mode)
         RETS = [SP++];
         ( R7:0, P5:0 ) = [SP++];
         RTS;
+ENDPROC(_sleep_mode)
 
 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;
+ENDPROC(_hibernate_mode)
 
 ENTRY(_deep_sleep)
         [--SP] = ( R7:0, P5:0 );
@@ -131,6 +132,7 @@ ENTRY(_deep_sleep)
         RETS = [SP++];
         ( R7:0, P5:0 ) = [SP++];
         RTS;
+ENDPROC(_deep_sleep)
 
 ENTRY(_sleep_deeper)
         [--SP] = ( R7:0, P5:0 );
@@ -232,53 +234,73 @@ ENTRY(_sleep_deeper)
         RETS = [SP++];
         ( R7:0, P5:0 ) = [SP++];
         RTS;
+ENDPROC(_sleep_deeper)
 
 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;
+ENDPROC(_set_dram_srfs)
 
 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);
+        R2 = [P0];
+        BITSET(R2, 0); /* SCTLE enable CLKOUT */
+        [P0] = R2
+        SSYNC;
+
+        P0.L = lo(EBIU_SDGCTL); /* release SDRAM from self-refresh */
         P0.H = hi(EBIU_SDGCTL);
-        P0.L = lo(EBIU_SDGCTL);
         R2 = [P0];
-        R3.H = hi(SRFS);
-        R3.L = lo(SRFS);
+        BITCLR(R2, 24); /* clear SRFS bit */
+        [P0] = R2
 #endif
-        R3 = ~R3;
-        R2 = R2&R3;
-        [P0] = R2;
-        ssync;
+        SSYNC;
         RTS;
+ENDPROC(_unset_dram_srfs)
 
 ENTRY(_set_sic_iwr)
 #if defined(CONFIG_BF54x) || defined(CONFIG_BF52x)  || defined(CONFIG_BF561)
@@ -300,6 +322,7 @@ ENTRY(_set_sic_iwr)
 
         SSYNC;
         RTS;
+ENDPROC(_set_sic_iwr)
 
 ENTRY(_set_rtc_istat)
 #ifndef CONFIG_BF561
@@ -307,8 +330,14 @@ ENTRY(_set_rtc_istat)
         P0.L = lo(RTC_ISTAT);
         w[P0] = R0.L;
         SSYNC;
+#elif (ANOMALY_05000371)
+        nop;
+        nop;
+        nop;
+        nop;
 #endif
         RTS;
+ENDPROC(_set_rtc_istat)
 
 ENTRY(_test_pll_locked)
         P0.H = hi(PLL_STAT);
@@ -318,3 +347,509 @@ ENTRY(_test_pll_locked)
         CC = BITTST(R0,5);
         IF !CC JUMP 1b;
         RTS;
+ENDPROC(_test_pll_locked)
+
+.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 = .Lpm_resume_here;
+        R0.L = .Lpm_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 */
+
+.Lpm_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;
+ENDPROC(_do_hibernate)
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
index 038f70e0be65..eceb484d90f9 100644
--- a/arch/blackfin/mach-common/entry.S
+++ b/arch/blackfin/mach-common/entry.S
@@ -158,23 +158,45 @@ ENTRY(_ex_single_step)
         cc = r7 == r6;
         if cc jump _bfin_return_from_exception;
 
+        /* Don't do single step in hardware exception handler */
+        p5.l = lo(IPEND);
+        p5.h = hi(IPEND);
+        r6 = [p5];
+        cc = bittst(r6, 5);
+        if cc jump _bfin_return_from_exception;
+
+#ifdef CONFIG_KGDB
+        /* skip single step if current interrupt priority is higher than
+         * that of the first instruction, from which gdb starts single step */
+        r6 >>= 6;
+        r7 = 10;
+.Lfind_priority_start:
+        cc = bittst(r6, 0);
+        if cc jump .Lfind_priority_done;
+        r6 >>= 1;
+        r7 += -1;
+        cc = r7 == 0;
+        if cc jump .Lfind_priority_done;
+        jump.s .Lfind_priority_start;
+.Lfind_priority_done:
+        p4.l = _debugger_step;
+        p4.h = _debugger_step;
+        r6 = [p4];
+        cc = r6 == 0;
+        if cc jump .Ldo_single_step;
+        r6 += -1;
+        cc = r6 < r7;
+        if cc jump _bfin_return_from_exception;
+.Ldo_single_step:
+#endif
+
         /* If we were in user mode, do the single step normally.  */
-        p5.l = lo(IPEND);
-        p5.h = hi(IPEND);
         r6 = [p5];
         r7 = 0xffe0 (z);
         r7 = r7 & r6;
         cc = r7 == 0;
-        if !cc jump 1f;
-
-        /* Single stepping only a single instruction, so clear the trace
-         * bit here.  */
-        r7 = syscfg;
-        bitclr (r7, 0);
-        syscfg = R7;
-        jump _ex_trap_c;
+        if cc jump 1f;
 
-1:
         /*
          * We were in an interrupt handler.  By convention, all of them save
          * SYSCFG with their first instruction, so by checking whether our
@@ -202,11 +224,15 @@ ENTRY(_ex_single_step)
         cc = R7 == R6;
         if !cc jump _bfin_return_from_exception;
 
+1:
+        /* Single stepping only a single instruction, so clear the trace
+         * bit here.  */
         r7 = syscfg;
         bitclr (r7, 0);
         syscfg = R7;
 
-        /* Fall through to _bfin_return_from_exception.  */
+        jump _ex_trap_c;
+
 ENDPROC(_ex_single_step)
 
 ENTRY(_bfin_return_from_exception)
diff --git a/arch/blackfin/mach-common/ints-priority.c b/arch/blackfin/mach-common/ints-priority.c
index f5fd768022ea..64d746114e4b 100644
--- a/arch/blackfin/mach-common/ints-priority.c
+++ b/arch/blackfin/mach-common/ints-priority.c
@@ -459,6 +459,8 @@ static struct irq_chip bfin_gpio_irqchip = {
         .mask = bfin_gpio_mask_irq,
         .mask_ack = bfin_gpio_mask_ack_irq,
         .unmask = bfin_gpio_unmask_irq,
+        .disable = bfin_gpio_mask_irq,
+        .enable = bfin_gpio_unmask_irq,
         .set_type = bfin_gpio_irq_type,
         .startup = bfin_gpio_irq_startup,
         .shutdown = bfin_gpio_irq_shutdown,
@@ -846,6 +848,8 @@ static struct irq_chip bfin_gpio_irqchip = {
         .mask = bfin_gpio_mask_irq,
         .mask_ack = bfin_gpio_mask_ack_irq,
         .unmask = bfin_gpio_unmask_irq,
+        .disable = bfin_gpio_mask_irq,
+        .enable = bfin_gpio_unmask_irq,
         .set_type = bfin_gpio_irq_type,
         .startup = bfin_gpio_irq_startup,
         .shutdown = bfin_gpio_irq_shutdown,
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;
         }