blackfin: bf60x: add power management support
authorSteven Miao <realmz6@gmail.com>
Wed, 16 May 2012 10:26:10 +0000 (18:26 +0800)
committerBob Liu <lliubbo@gmail.com>
Mon, 21 May 2012 06:54:24 +0000 (14:54 +0800)
Add bf60x cpu pm callbacks and change blackfin pm framework to support bf60x.

Signed-off-by: Steven Miao <realmz6@gmail.com>
Signed-off-by: Bob Liu <lliubbo@gmail.com>
arch/blackfin/include/asm/dpmc.h
arch/blackfin/include/asm/pm.h [new file with mode: 0644]
arch/blackfin/mach-bf609/hibernate.S [new file with mode: 0644]
arch/blackfin/mach-bf609/include/mach/pm.h [new file with mode: 0644]
arch/blackfin/mach-bf609/pm.c [new file with mode: 0644]
arch/blackfin/mach-common/Makefile
arch/blackfin/mach-common/dpmc_modes.S
arch/blackfin/mach-common/pm.c

index c4ec959dad78045e48e0d8b6b92d9068e15bf356..b47399cda6ed897b1dbfe490b674df4c7529ec71 100644 (file)
@@ -9,6 +9,637 @@
 #ifndef _BLACKFIN_DPMC_H_
 #define _BLACKFIN_DPMC_H_
 
+#ifdef __ASSEMBLY__
+#define PM_REG0  R7
+#define PM_REG1  R6
+#define PM_REG2  R5
+#define PM_REG3  R4
+#define PM_REG4  R3
+#define PM_REG5  R2
+#define PM_REG6  R1
+#define PM_REG7  R0
+#define PM_REG8  P5
+#define PM_REG9  P4
+#define PM_REG10 P3
+#define PM_REG11 P2
+#define PM_REG12 P1
+#define PM_REG13 P0
+
+#define PM_REGSET0  R7:7
+#define PM_REGSET1  R7:6
+#define PM_REGSET2  R7:5
+#define PM_REGSET3  R7:4
+#define PM_REGSET4  R7:3
+#define PM_REGSET5  R7:2
+#define PM_REGSET6  R7:1
+#define PM_REGSET7  R7:0
+#define PM_REGSET8  R7:0, P5:5
+#define PM_REGSET9  R7:0, P5:4
+#define PM_REGSET10 R7:0, P5:3
+#define PM_REGSET11 R7:0, P5:2
+#define PM_REGSET12 R7:0, P5:1
+#define PM_REGSET13 R7:0, P5:0
+
+#define _PM_PUSH(n, x, w, base) PM_REG##n = w[FP + ((x) - (base))];
+#define _PM_POP(n, x, w, base)  w[FP + ((x) - (base))] = PM_REG##n;
+#define PM_PUSH_SYNC(n)         [--sp] = (PM_REGSET##n);
+#define PM_POP_SYNC(n)          (PM_REGSET##n) = [sp++];
+#define PM_PUSH(n, x)          PM_REG##n = [FP++];
+#define PM_POP(n, x)            [FP--] = PM_REG##n;
+#define PM_CORE_PUSH(n, x)      _PM_PUSH(n, x, , COREMMR_BASE)
+#define PM_CORE_POP(n, x)       _PM_POP(n, x, , COREMMR_BASE)
+#define PM_SYS_PUSH(n, x)       _PM_PUSH(n, x, , SYSMMR_BASE)
+#define PM_SYS_POP(n, x)        _PM_POP(n, x, , SYSMMR_BASE)
+#define PM_SYS_PUSH16(n, x)     _PM_PUSH(n, x, w, SYSMMR_BASE)
+#define PM_SYS_POP16(n, x)      _PM_POP(n, x, w, SYSMMR_BASE)
+
+
+       .macro bfin_cpu_reg_save
+       /*
+        * Save the core regs early so we can blow them away when
+        * saving/restoring MMR states
+        */
+       [--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;
+
+       /* We can't push RETI directly as that'll change IPEND[4] */
+       r7 = RETI;
+       [--sp] = RETS;
+       [--sp] = ASTAT;
+       [--sp] = CYCLES;
+       [--sp] = CYCLES2;
+       [--sp] = SYSCFG;
+       [--sp] = RETX;
+       [--sp] = SEQSTAT;
+       [--sp] = r7;
+
+       /* Save first func arg in M3 */
+       M3 = R0;
+       .endm
+
+       .macro bfin_cpu_reg_restore
+       /* Restore Core Registers */
+       RETI = [sp++];
+       SEQSTAT = [sp++];
+       RETX = [sp++];
+       SYSCFG = [sp++];
+       CYCLES2 = [sp++];
+       CYCLES = [sp++];
+       ASTAT = [sp++];
+       RETS = [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++];
+
+       .endm
+
+       .macro bfin_sys_mmr_save
+       /* Save system MMRs */
+       FP.H = hi(SYSMMR_BASE);
+       FP.L = lo(SYSMMR_BASE);
+#ifdef SIC_IMASK0
+       PM_SYS_PUSH(0, SIC_IMASK0)
+       PM_SYS_PUSH(1, SIC_IMASK1)
+# ifdef SIC_IMASK2
+       PM_SYS_PUSH(2, SIC_IMASK2)
+# endif
+#else
+# ifdef SIC_IMASK
+       PM_SYS_PUSH(0, SIC_IMASK)
+# endif
+#endif
+
+#ifdef SIC_IAR0
+       PM_SYS_PUSH(3, SIC_IAR0)
+       PM_SYS_PUSH(4, SIC_IAR1)
+       PM_SYS_PUSH(5, SIC_IAR2)
+#endif
+#ifdef SIC_IAR3
+       PM_SYS_PUSH(6, SIC_IAR3)
+#endif
+#ifdef SIC_IAR4
+       PM_SYS_PUSH(7, SIC_IAR4)
+       PM_SYS_PUSH(8, SIC_IAR5)
+       PM_SYS_PUSH(9, SIC_IAR6)
+#endif
+#ifdef SIC_IAR7
+       PM_SYS_PUSH(10, SIC_IAR7)
+#endif
+#ifdef SIC_IAR8
+       PM_SYS_PUSH(11, SIC_IAR8)
+       PM_SYS_PUSH(12, SIC_IAR9)
+       PM_SYS_PUSH(13, SIC_IAR10)
+#endif
+       PM_PUSH_SYNC(13)
+#ifdef SIC_IAR11
+       PM_SYS_PUSH(0, SIC_IAR11)
+#endif
+
+#ifdef SIC_IWR
+       PM_SYS_PUSH(1, SIC_IWR)
+#endif
+#ifdef SIC_IWR0
+       PM_SYS_PUSH(1, SIC_IWR0)
+#endif
+#ifdef SIC_IWR1
+       PM_SYS_PUSH(2, SIC_IWR1)
+#endif
+#ifdef SIC_IWR2
+       PM_SYS_PUSH(3, SIC_IWR2)
+#endif
+
+#ifdef PINT0_ASSIGN
+       PM_SYS_PUSH(4, PINT0_MASK_SET)
+       PM_SYS_PUSH(5, PINT1_MASK_SET)
+       PM_SYS_PUSH(6, PINT2_MASK_SET)
+       PM_SYS_PUSH(7, PINT3_MASK_SET)
+       PM_SYS_PUSH(8, PINT0_ASSIGN)
+       PM_SYS_PUSH(9, PINT1_ASSIGN)
+       PM_SYS_PUSH(10, PINT2_ASSIGN)
+       PM_SYS_PUSH(11, PINT3_ASSIGN)
+       PM_SYS_PUSH(12, PINT0_INVERT_SET)
+       PM_SYS_PUSH(13, PINT1_INVERT_SET)
+       PM_PUSH_SYNC(13)
+       PM_SYS_PUSH(0, PINT2_INVERT_SET)
+       PM_SYS_PUSH(1, PINT3_INVERT_SET)
+       PM_SYS_PUSH(2, PINT0_EDGE_SET)
+       PM_SYS_PUSH(3, PINT1_EDGE_SET)
+       PM_SYS_PUSH(4, PINT2_EDGE_SET)
+       PM_SYS_PUSH(5, PINT3_EDGE_SET)
+#endif
+
+#ifdef SYSCR
+       PM_SYS_PUSH16(6, SYSCR)
+#endif
+
+#ifdef EBIU_AMGCTL
+       PM_SYS_PUSH16(7, EBIU_AMGCTL)
+       PM_SYS_PUSH(8, EBIU_AMBCTL0)
+       PM_SYS_PUSH(9, EBIU_AMBCTL1)
+#endif
+#ifdef EBIU_FCTL
+       PM_SYS_PUSH(10, EBIU_MBSCTL)
+       PM_SYS_PUSH(11, EBIU_MODE)
+       PM_SYS_PUSH(12, EBIU_FCTL)
+       PM_PUSH_SYNC(12)
+#else
+       PM_PUSH_SYNC(9)
+#endif
+       .endm
+
+
+       .macro bfin_sys_mmr_restore
+/* Restore System MMRs */
+       FP.H = hi(SYSMMR_BASE);
+       FP.L = lo(SYSMMR_BASE);
+
+#ifdef EBIU_FCTL
+       PM_POP_SYNC(12)
+       PM_SYS_POP(12, EBIU_FCTL)
+       PM_SYS_POP(11, EBIU_MODE)
+       PM_SYS_POP(10, EBIU_MBSCTL)
+#else
+       PM_POP_SYNC(9)
+#endif
+
+#ifdef EBIU_AMBCTL
+       PM_SYS_POP(9, EBIU_AMBCTL1)
+       PM_SYS_POP(8, EBIU_AMBCTL0)
+       PM_SYS_POP16(7, EBIU_AMGCTL)
+#endif
+
+#ifdef SYSCR
+       PM_SYS_POP16(6, SYSCR)
+#endif
+
+#ifdef PINT0_ASSIGN
+       PM_SYS_POP(5, PINT3_EDGE_SET)
+       PM_SYS_POP(4, PINT2_EDGE_SET)
+       PM_SYS_POP(3, PINT1_EDGE_SET)
+       PM_SYS_POP(2, PINT0_EDGE_SET)
+       PM_SYS_POP(1, PINT3_INVERT_SET)
+       PM_SYS_POP(0, PINT2_INVERT_SET)
+       PM_POP_SYNC(13)
+       PM_SYS_POP(13, PINT1_INVERT_SET)
+       PM_SYS_POP(12, PINT0_INVERT_SET)
+       PM_SYS_POP(11, PINT3_ASSIGN)
+       PM_SYS_POP(10, PINT2_ASSIGN)
+       PM_SYS_POP(9, PINT1_ASSIGN)
+       PM_SYS_POP(8, PINT0_ASSIGN)
+       PM_SYS_POP(7, PINT3_MASK_SET)
+       PM_SYS_POP(6, PINT2_MASK_SET)
+       PM_SYS_POP(5, PINT1_MASK_SET)
+       PM_SYS_POP(4, PINT0_MASK_SET)
+#endif
+
+#ifdef SIC_IWR2
+       PM_SYS_POP(3, SIC_IWR2)
+#endif
+#ifdef SIC_IWR1
+       PM_SYS_POP(2, SIC_IWR1)
+#endif
+#ifdef SIC_IWR0
+       PM_SYS_POP(1, SIC_IWR0)
+#endif
+#ifdef SIC_IWR
+       PM_SYS_POP(1, SIC_IWR)
+#endif
+
+#ifdef SIC_IAR11
+       PM_SYS_POP(0, SIC_IAR11)
+#endif
+       PM_POP_SYNC(13)
+#ifdef SIC_IAR8
+       PM_SYS_POP(13, SIC_IAR10)
+       PM_SYS_POP(12, SIC_IAR9)
+       PM_SYS_POP(11, SIC_IAR8)
+#endif
+#ifdef SIC_IAR7
+       PM_SYS_POP(10, SIC_IAR7)
+#endif
+#ifdef SIC_IAR6
+       PM_SYS_POP(9, SIC_IAR6)
+       PM_SYS_POP(8, SIC_IAR5)
+       PM_SYS_POP(7, SIC_IAR4)
+#endif
+#ifdef SIC_IAR3
+       PM_SYS_POP(6, SIC_IAR3)
+#endif
+#ifdef SIC_IAR0
+       PM_SYS_POP(5, SIC_IAR2)
+       PM_SYS_POP(4, SIC_IAR1)
+       PM_SYS_POP(3, SIC_IAR0)
+#endif
+#ifdef SIC_IMASK0
+# ifdef SIC_IMASK2
+       PM_SYS_POP(2, SIC_IMASK2)
+# endif
+       PM_SYS_POP(1, SIC_IMASK1)
+       PM_SYS_POP(0, SIC_IMASK0)
+#else
+# ifdef SIC_IMASK
+       PM_SYS_POP(0, SIC_IMASK)
+# endif
+#endif
+       .endm
+
+       .macro bfin_core_mmr_save
+       /* Save Core MMRs */
+       I0.H = hi(COREMMR_BASE);
+       I0.L = lo(COREMMR_BASE);
+       I1 = I0;
+       I2 = I0;
+       I3 = I0;
+       B0 = I0;
+       B1 = I0;
+       B2 = I0;
+       B3 = I0;
+       I1.L = lo(DCPLB_ADDR0);
+       I2.L = lo(DCPLB_DATA0);
+       I3.L = lo(ICPLB_ADDR0);
+       B0.L = lo(ICPLB_DATA0);
+       B1.L = lo(EVT2);
+       B2.L = lo(IMASK);
+       B3.L = lo(TCNTL);
+
+       /* Event Vectors */
+       FP = B1;
+       PM_PUSH(0, EVT2)
+       PM_PUSH(1, EVT3)
+       FP += 4;        /* EVT4 */
+       PM_PUSH(2, EVT5)
+       PM_PUSH(3, EVT6)
+       PM_PUSH(4, EVT7)
+       PM_PUSH(5, EVT8)
+       PM_PUSH_SYNC(5)
+
+       PM_PUSH(0, EVT9)
+       PM_PUSH(1, EVT10)
+       PM_PUSH(2, EVT11)
+       PM_PUSH(3, EVT12)
+       PM_PUSH(4, EVT13)
+       PM_PUSH(5, EVT14)
+       PM_PUSH(6, EVT15)
+
+       /* CEC */
+       FP = B2;
+       PM_PUSH(7, IMASK)
+       FP += 4;        /* IPEND */
+       PM_PUSH(8, ILAT)
+       PM_PUSH(9, IPRIO)
+
+       /* Core Timer */
+       FP = B3;
+       PM_PUSH(10, TCNTL)
+       PM_PUSH(11, TPERIOD)
+       PM_PUSH(12, TSCALE)
+       PM_PUSH(13, TCOUNT)
+       PM_PUSH_SYNC(13)
+
+       /* Misc non-contiguous registers */
+       FP = I0;
+       PM_CORE_PUSH(0, DMEM_CONTROL);
+       PM_CORE_PUSH(1, IMEM_CONTROL);
+       PM_CORE_PUSH(2, TBUFCTL);
+       PM_PUSH_SYNC(2)
+
+       /* DCPLB Addr */
+       FP = I1;
+       PM_PUSH(0, DCPLB_ADDR0)
+       PM_PUSH(1, DCPLB_ADDR1)
+       PM_PUSH(2, DCPLB_ADDR2)
+       PM_PUSH(3, DCPLB_ADDR3)
+       PM_PUSH(4, DCPLB_ADDR4)
+       PM_PUSH(5, DCPLB_ADDR5)
+       PM_PUSH(6, DCPLB_ADDR6)
+       PM_PUSH(7, DCPLB_ADDR7)
+       PM_PUSH(8, DCPLB_ADDR8)
+       PM_PUSH(9, DCPLB_ADDR9)
+       PM_PUSH(10, DCPLB_ADDR10)
+       PM_PUSH(11, DCPLB_ADDR11)
+       PM_PUSH(12, DCPLB_ADDR12)
+       PM_PUSH(13, DCPLB_ADDR13)
+       PM_PUSH_SYNC(13)
+       PM_PUSH(0, DCPLB_ADDR14)
+       PM_PUSH(1, DCPLB_ADDR15)
+
+       /* DCPLB Data */
+       FP = I2;
+       PM_PUSH(2, DCPLB_DATA0)
+       PM_PUSH(3, DCPLB_DATA1)
+       PM_PUSH(4, DCPLB_DATA2)
+       PM_PUSH(5, DCPLB_DATA3)
+       PM_PUSH(6, DCPLB_DATA4)
+       PM_PUSH(7, DCPLB_DATA5)
+       PM_PUSH(8, DCPLB_DATA6)
+       PM_PUSH(9, DCPLB_DATA7)
+       PM_PUSH(10, DCPLB_DATA8)
+       PM_PUSH(11, DCPLB_DATA9)
+       PM_PUSH(12, DCPLB_DATA10)
+       PM_PUSH(13, DCPLB_DATA11)
+       PM_PUSH_SYNC(13)
+       PM_PUSH(0, DCPLB_DATA12)
+       PM_PUSH(1, DCPLB_DATA13)
+       PM_PUSH(2, DCPLB_DATA14)
+       PM_PUSH(3, DCPLB_DATA15)
+
+       /* ICPLB Addr */
+       FP = I3;
+       PM_PUSH(4, ICPLB_ADDR0)
+       PM_PUSH(5, ICPLB_ADDR1)
+       PM_PUSH(6, ICPLB_ADDR2)
+       PM_PUSH(7, ICPLB_ADDR3)
+       PM_PUSH(8, ICPLB_ADDR4)
+       PM_PUSH(9, ICPLB_ADDR5)
+       PM_PUSH(10, ICPLB_ADDR6)
+       PM_PUSH(11, ICPLB_ADDR7)
+       PM_PUSH(12, ICPLB_ADDR8)
+       PM_PUSH(13, ICPLB_ADDR9)
+       PM_PUSH_SYNC(13)
+       PM_PUSH(0, ICPLB_ADDR10)
+       PM_PUSH(1, ICPLB_ADDR11)
+       PM_PUSH(2, ICPLB_ADDR12)
+       PM_PUSH(3, ICPLB_ADDR13)
+       PM_PUSH(4, ICPLB_ADDR14)
+       PM_PUSH(5, ICPLB_ADDR15)
+
+       /* ICPLB Data */
+       FP = B0;
+       PM_PUSH(6, ICPLB_DATA0)
+       PM_PUSH(7, ICPLB_DATA1)
+       PM_PUSH(8, ICPLB_DATA2)
+       PM_PUSH(9, ICPLB_DATA3)
+       PM_PUSH(10, ICPLB_DATA4)
+       PM_PUSH(11, ICPLB_DATA5)
+       PM_PUSH(12, ICPLB_DATA6)
+       PM_PUSH(13, ICPLB_DATA7)
+       PM_PUSH_SYNC(13)
+       PM_PUSH(0, ICPLB_DATA8)
+       PM_PUSH(1, ICPLB_DATA9)
+       PM_PUSH(2, ICPLB_DATA10)
+       PM_PUSH(3, ICPLB_DATA11)
+       PM_PUSH(4, ICPLB_DATA12)
+       PM_PUSH(5, ICPLB_DATA13)
+       PM_PUSH(6, ICPLB_DATA14)
+       PM_PUSH(7, ICPLB_DATA15)
+       PM_PUSH_SYNC(7)
+       .endm
+
+       .macro bfin_core_mmr_restore
+       /* Restore Core MMRs */
+       I0.H = hi(COREMMR_BASE);
+       I0.L = lo(COREMMR_BASE);
+       I1 = I0;
+       I2 = I0;
+       I3 = I0;
+       B0 = I0;
+       B1 = I0;
+       B2 = I0;
+       B3 = I0;
+       I1.L = lo(DCPLB_ADDR15);
+       I2.L = lo(DCPLB_DATA15);
+       I3.L = lo(ICPLB_ADDR15);
+       B0.L = lo(ICPLB_DATA15);
+       B1.L = lo(EVT15);
+       B2.L = lo(IPRIO);
+       B3.L = lo(TCOUNT);
+
+       /* ICPLB Data */
+       FP = B0;
+       PM_POP_SYNC(7)
+       PM_POP(7, ICPLB_DATA15)
+       PM_POP(6, ICPLB_DATA14)
+       PM_POP(5, ICPLB_DATA13)
+       PM_POP(4, ICPLB_DATA12)
+       PM_POP(3, ICPLB_DATA11)
+       PM_POP(2, ICPLB_DATA10)
+       PM_POP(1, ICPLB_DATA9)
+       PM_POP(0, ICPLB_DATA8)
+       PM_POP_SYNC(13)
+       PM_POP(13, ICPLB_DATA7)
+       PM_POP(12, ICPLB_DATA6)
+       PM_POP(11, ICPLB_DATA5)
+       PM_POP(10, ICPLB_DATA4)
+       PM_POP(9, ICPLB_DATA3)
+       PM_POP(8, ICPLB_DATA2)
+       PM_POP(7, ICPLB_DATA1)
+       PM_POP(6, ICPLB_DATA0)
+
+       /* ICPLB Addr */
+       FP = I3;
+       PM_POP(5, ICPLB_ADDR15)
+       PM_POP(4, ICPLB_ADDR14)
+       PM_POP(3, ICPLB_ADDR13)
+       PM_POP(2, ICPLB_ADDR12)
+       PM_POP(1, ICPLB_ADDR11)
+       PM_POP(0, ICPLB_ADDR10)
+       PM_POP_SYNC(13)
+       PM_POP(13, ICPLB_ADDR9)
+       PM_POP(12, ICPLB_ADDR8)
+       PM_POP(11, ICPLB_ADDR7)
+       PM_POP(10, ICPLB_ADDR6)
+       PM_POP(9, ICPLB_ADDR5)
+       PM_POP(8, ICPLB_ADDR4)
+       PM_POP(7, ICPLB_ADDR3)
+       PM_POP(6, ICPLB_ADDR2)
+       PM_POP(5, ICPLB_ADDR1)
+       PM_POP(4, ICPLB_ADDR0)
+
+       /* DCPLB Data */
+       FP = I2;
+       PM_POP(3, DCPLB_DATA15)
+       PM_POP(2, DCPLB_DATA14)
+       PM_POP(1, DCPLB_DATA13)
+       PM_POP(0, DCPLB_DATA12)
+       PM_POP_SYNC(13)
+       PM_POP(13, DCPLB_DATA11)
+       PM_POP(12, DCPLB_DATA10)
+       PM_POP(11, DCPLB_DATA9)
+       PM_POP(10, DCPLB_DATA8)
+       PM_POP(9, DCPLB_DATA7)
+       PM_POP(8, DCPLB_DATA6)
+       PM_POP(7, DCPLB_DATA5)
+       PM_POP(6, DCPLB_DATA4)
+       PM_POP(5, DCPLB_DATA3)
+       PM_POP(4, DCPLB_DATA2)
+       PM_POP(3, DCPLB_DATA1)
+       PM_POP(2, DCPLB_DATA0)
+
+       /* DCPLB Addr */
+       FP = I1;
+       PM_POP(1, DCPLB_ADDR15)
+       PM_POP(0, DCPLB_ADDR14)
+       PM_POP_SYNC(13)
+       PM_POP(13, DCPLB_ADDR13)
+       PM_POP(12, DCPLB_ADDR12)
+       PM_POP(11, DCPLB_ADDR11)
+       PM_POP(10, DCPLB_ADDR10)
+       PM_POP(9, DCPLB_ADDR9)
+       PM_POP(8, DCPLB_ADDR8)
+       PM_POP(7, DCPLB_ADDR7)
+       PM_POP(6, DCPLB_ADDR6)
+       PM_POP(5, DCPLB_ADDR5)
+       PM_POP(4, DCPLB_ADDR4)
+       PM_POP(3, DCPLB_ADDR3)
+       PM_POP(2, DCPLB_ADDR2)
+       PM_POP(1, DCPLB_ADDR1)
+       PM_POP(0, DCPLB_ADDR0)
+
+
+       /* Misc non-contiguous registers */
+
+       /* icache & dcache will enable later 
+          drop IMEM_CONTROL, DMEM_CONTROL pop
+       */
+       FP = I0;
+       PM_POP_SYNC(2)
+       PM_CORE_POP(2, TBUFCTL)
+       PM_CORE_POP(1, IMEM_CONTROL)
+       PM_CORE_POP(0, DMEM_CONTROL)
+
+       /* Core Timer */
+       FP = B3;
+       R0 = 0x1;
+       [FP - 0xC] = R0;
+
+       PM_POP_SYNC(13)
+       FP = B3;
+       PM_POP(13, TCOUNT)
+       PM_POP(12, TSCALE)
+       PM_POP(11, TPERIOD)
+       PM_POP(10, TCNTL)
+
+       /* CEC */
+       FP = B2;
+       PM_POP(9, IPRIO)
+       PM_POP(8, ILAT)
+       FP += -4;       /* IPEND */
+       PM_POP(7, IMASK)
+
+       /* Event Vectors */
+       FP = B1;
+       PM_POP(6, EVT15)
+       PM_POP(5, EVT14)
+       PM_POP(4, EVT13)
+       PM_POP(3, EVT12)
+       PM_POP(2, EVT11)
+       PM_POP(1, EVT10)
+       PM_POP(0, EVT9)
+       PM_POP_SYNC(5)
+       PM_POP(5, EVT8)
+       PM_POP(4, EVT7)
+       PM_POP(3, EVT6)
+       PM_POP(2, EVT5)
+       FP += -4;       /* EVT4 */
+       PM_POP(1, EVT3)
+       PM_POP(0, EVT2)
+       .endm
+#endif
+
 #include <mach/pll.h>
 
 /* PLL_CTL Masks */
 #define USBWE                  0x0800  /* Enable USB Wakeup From Hibernate */
 #endif
 
+
 #ifndef __ASSEMBLY__
 
 void sleep_mode(u32 sic_iwr0, u32 sic_iwr1, u32 sic_iwr2);
diff --git a/arch/blackfin/include/asm/pm.h b/arch/blackfin/include/asm/pm.h
new file mode 100644 (file)
index 0000000..da63b46
--- /dev/null
@@ -0,0 +1,32 @@
+/*
+ * Blackfin bf609 power management
+ *
+ * Copyright 2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2
+ */
+
+#ifndef __PM_H__
+#define __PM_H__
+
+#include <mach/pm.h>
+#include <linux/suspend.h>
+
+struct bfin_cpu_pm_fns {
+       void    (*save)(unsigned long *);
+       void    (*restore)(unsigned long *);
+       int     (*valid)(suspend_state_t state);
+       void    (*enter)(suspend_state_t state);
+       int     (*prepare)(void);
+       void    (*finish)(void);
+};
+
+extern struct bfin_cpu_pm_fns *bfin_cpu_pm;
+
+# ifdef CONFIG_BFIN_COREB
+void bfin_coreb_start(void);
+void bfin_coreb_stop(void);
+void bfin_coreb_reset(void);
+# endif
+
+#endif
diff --git a/arch/blackfin/mach-bf609/hibernate.S b/arch/blackfin/mach-bf609/hibernate.S
new file mode 100644 (file)
index 0000000..baedd6e
--- /dev/null
@@ -0,0 +1,62 @@
+#include <linux/linkage.h>
+#include <asm/blackfin.h>
+#include <asm/dpmc.h>
+
+#define PM_STACK   (COREA_L1_SCRATCH_START + L1_SCRATCH_LENGTH - 12)
+
+.section .l1.text
+ENTRY(_enter_hibernate)
+       /* switch stack to L1 scratch, prepare for ddr srfr */
+       P0.H = HI(PM_STACK);
+       P0.L = LO(PM_STACK);
+       SP = P0;
+
+       call _bf609_ddr_sr;
+       call _bfin_hibernate_syscontrol;
+
+       P0.H = HI(DPM0_RESTORE4);
+       P0.L = LO(DPM0_RESTORE4);
+       P1.H = _bf609_pm_data;
+       P1.L = _bf609_pm_data;
+       [P0] = P1;
+
+       P0.H = HI(DPM0_CTL);
+       P0.L = LO(DPM0_CTL);
+       R3.H = HI(0x00000010);
+       R3.L = LO(0x00000010);
+       [P0] = R3;
+
+       SSYNC;
+ENDPROC(_enter_hibernate_mode)
+
+.section .text
+ENTRY(_bf609_hibernate)
+       bfin_cpu_reg_save;
+       bfin_core_mmr_save;
+
+       P0.H = _bf609_pm_data;
+       P0.L = _bf609_pm_data;
+       R1.H = 0xDEAD;
+       R1.L = 0xBEEF;
+       R2.H = .Lpm_resume_here;
+       R2.L = .Lpm_resume_here;
+       [P0++] = R1;
+       [P0++] = R2;
+       [P0++] = SP;
+
+       P1.H = _enter_hibernate;
+       P1.L = _enter_hibernate;
+
+       call (P1);
+.Lpm_resume_here:
+
+       bfin_core_mmr_restore;
+       bfin_cpu_reg_restore;
+
+       [--sp] = RETI;  /* Clear Global Interrupt Disable */
+       SP += 4;
+
+       RTS;
+
+ENDPROC(_bf609_hibernate)
+
diff --git a/arch/blackfin/mach-bf609/include/mach/pm.h b/arch/blackfin/mach-bf609/include/mach/pm.h
new file mode 100644 (file)
index 0000000..036d9bd
--- /dev/null
@@ -0,0 +1,21 @@
+/*
+ * Blackfin bf609 power management
+ *
+ * Copyright 2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2
+ */
+
+#ifndef __MACH_BF609_PM_H__
+#define __MACH_BF609_PM_H__
+
+#include <linux/suspend.h>
+
+int bfin609_pm_enter(suspend_state_t state);
+int bf609_pm_prepare(void);
+void bf609_pm_finish(void);
+
+void bf609_hibernate(void);
+void bfin_sec_raise_irq(unsigned int sid);
+void coreb_enable(void);
+#endif
diff --git a/arch/blackfin/mach-bf609/pm.c b/arch/blackfin/mach-bf609/pm.c
new file mode 100644 (file)
index 0000000..bbc5a79
--- /dev/null
@@ -0,0 +1,310 @@
+/*
+ * Blackfin bf609 power management
+ *
+ * Copyright 2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2
+ */
+
+#include <linux/suspend.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/gpio.h>
+#include <linux/irq.h>
+
+#include <linux/delay.h>
+
+#include <asm/dpmc.h>
+#include <asm/pm.h>
+#include <mach/pm.h>
+#include <asm/blackfin.h>
+
+/***********************************************************/
+/*                                                         */
+/* Wakeup Actions for DPM_RESTORE                          */
+/*                                                         */
+/***********************************************************/
+#define BITP_ROM_WUA_CHKHDR             24
+#define BITP_ROM_WUA_DDRLOCK            7
+#define BITP_ROM_WUA_DDRDLLEN           6
+#define BITP_ROM_WUA_DDR                5
+#define BITP_ROM_WUA_CGU                4
+#define BITP_ROM_WUA_MEMBOOT            2
+#define BITP_ROM_WUA_EN                 1
+
+#define BITM_ROM_WUA_CHKHDR             (0xFF000000)
+#define ENUM_ROM_WUA_CHKHDR_AD                  0xAD000000
+
+#define BITM_ROM_WUA_DDRLOCK            (0x00000080)
+#define BITM_ROM_WUA_DDRDLLEN           (0x00000040)
+#define BITM_ROM_WUA_DDR                (0x00000020)
+#define BITM_ROM_WUA_CGU                (0x00000010)
+#define BITM_ROM_WUA_MEMBOOT            (0x00000002)
+#define BITM_ROM_WUA_EN                 (0x00000001)
+
+/***********************************************************/
+/*                                                         */
+/* Syscontrol                                              */
+/*                                                         */
+/***********************************************************/
+#define BITP_ROM_SYSCTRL_CGU_LOCKINGEN  28    /* unlocks CGU_CTL register */
+#define BITP_ROM_SYSCTRL_WUA_OVERRIDE   24
+#define BITP_ROM_SYSCTRL_WUA_DDRDLLEN   20    /* Saves the DDR DLL and PADS registers to the DPM registers */
+#define BITP_ROM_SYSCTRL_WUA_DDR        19    /* Saves the DDR registers to the DPM registers */
+#define BITP_ROM_SYSCTRL_WUA_CGU        18    /* Saves the CGU registers into DPM registers */
+#define BITP_ROM_SYSCTRL_WUA_DPMWRITE   17    /* Saves the Syscontrol structure structure contents into DPM registers */
+#define BITP_ROM_SYSCTRL_WUA_EN         16    /* reads current PLL and DDR configuration into structure */
+#define BITP_ROM_SYSCTRL_DDR_WRITE      13    /* writes the DDR registers from Syscontrol structure for wakeup initialization of DDR */
+#define BITP_ROM_SYSCTRL_DDR_READ       12    /* Read the DDR registers into the Syscontrol structure for storing prior to hibernate */
+#define BITP_ROM_SYSCTRL_CGU_AUTODIS    11    /* Disables auto handling of UPDT and ALGN fields */
+#define BITP_ROM_SYSCTRL_CGU_CLKOUTSEL  7    /* access CGU_CLKOUTSEL register */
+#define BITP_ROM_SYSCTRL_CGU_DIV        6    /* access CGU_DIV register */
+#define BITP_ROM_SYSCTRL_CGU_STAT       5    /* access CGU_STAT register */
+#define BITP_ROM_SYSCTRL_CGU_CTL        4    /* access CGU_CTL register */
+#define BITP_ROM_SYSCTRL_CGU_RTNSTAT    2    /* Update structure STAT field upon error */
+#define BITP_ROM_SYSCTRL_WRITE          1    /* write registers */
+#define BITP_ROM_SYSCTRL_READ           0    /* read registers */
+
+#define BITM_ROM_SYSCTRL_CGU_READ       (0x00000001)    /* Read CGU registers */
+#define BITM_ROM_SYSCTRL_CGU_WRITE      (0x00000002)    /* Write registers */
+#define BITM_ROM_SYSCTRL_CGU_RTNSTAT    (0x00000004)    /* Update structure STAT field upon error or after a write operation */
+#define BITM_ROM_SYSCTRL_CGU_CTL        (0x00000010)    /* Access CGU_CTL register */
+#define BITM_ROM_SYSCTRL_CGU_STAT       (0x00000020)    /* Access CGU_STAT register */
+#define BITM_ROM_SYSCTRL_CGU_DIV        (0x00000040)    /* Access CGU_DIV register */
+#define BITM_ROM_SYSCTRL_CGU_CLKOUTSEL  (0x00000080)    /* Access CGU_CLKOUTSEL register */
+#define BITM_ROM_SYSCTRL_CGU_AUTODIS    (0x00000800)    /* Disables auto handling of UPDT and ALGN fields */
+#define BITM_ROM_SYSCTRL_DDR_READ       (0x00001000)    /* Reads the contents of the DDR registers and stores them into the structure */
+#define BITM_ROM_SYSCTRL_DDR_WRITE      (0x00002000)    /* Writes the DDR registers from the structure, only really intented for wakeup functionality and not for full DDR configuration */
+#define BITM_ROM_SYSCTRL_WUA_EN         (0x00010000)    /* Wakeup entry or exit opertation enable */
+#define BITM_ROM_SYSCTRL_WUA_DPMWRITE   (0x00020000)    /* When set indicates a restore of the PLL and DDR is to be performed otherwise a save is required */
+#define BITM_ROM_SYSCTRL_WUA_CGU        (0x00040000)    /* Only applicable for a PLL and DDR save operation to the DPM, saves the current settings if cleared or the contents of the structure if set */
+#define BITM_ROM_SYSCTRL_WUA_DDR        (0x00080000)    /* Only applicable for a PLL and DDR save operation to the DPM, saves the current settings if cleared or the contents of the structure if set */
+#define BITM_ROM_SYSCTRL_WUA_DDRDLLEN   (0x00100000)    /* Enables saving/restoring of the DDR DLLCTL register */
+#define BITM_ROM_SYSCTRL_WUA_OVERRIDE   (0x01000000)
+#define BITM_ROM_SYSCTRL_CGU_LOCKINGEN  (0x10000000)    /* Unlocks the CGU_CTL register */
+
+
+/* Structures for the syscontrol() function */
+struct STRUCT_ROM_SYSCTRL {
+       uint32_t ulCGU_CTL;
+       uint32_t ulCGU_STAT;
+       uint32_t ulCGU_DIV;
+       uint32_t ulCGU_CLKOUTSEL;
+       uint32_t ulWUA_Flags;
+       uint32_t ulWUA_BootAddr;
+       uint32_t ulWUA_User;
+       uint32_t ulDDR_CTL;
+       uint32_t ulDDR_CFG;
+       uint32_t ulDDR_TR0;
+       uint32_t ulDDR_TR1;
+       uint32_t ulDDR_TR2;
+       uint32_t ulDDR_MR;
+       uint32_t ulDDR_EMR1;
+       uint32_t ulDDR_EMR2;
+       uint32_t ulDDR_PADCTL;
+       uint32_t ulDDR_DLLCTL;
+       uint32_t ulReserved;
+};
+
+struct bfin_pm_data {
+       uint32_t magic;
+       uint32_t resume_addr;
+       uint32_t sp;
+};
+
+struct bfin_pm_data bf609_pm_data;
+
+struct STRUCT_ROM_SYSCTRL configvalues;
+uint32_t dactionflags;
+
+#define FUNC_ROM_SYSCONTROL 0xC8000080
+__attribute__((l1_data))
+static uint32_t (* const bfrom_SysControl)(uint32_t action_flags, struct STRUCT_ROM_SYSCTRL *settings, void *reserved) = (void *)FUNC_ROM_SYSCONTROL;
+
+__attribute__((l1_text))
+void bfin_cpu_suspend(void)
+{
+       __asm__ __volatile__( \
+                       ".align 8;" \
+                       "idle;" \
+                       : : \
+                       );
+}
+
+__attribute__((l1_text))
+void bfin_deepsleep(unsigned long mask)
+{
+       uint32_t dpm0_ctl;
+
+       bfin_write32(DPM0_WAKE_EN, 0x10);
+       bfin_write32(DPM0_WAKE_POL, 0x10);
+       dpm0_ctl = bfin_read32(DPM0_CTL);
+       dpm0_ctl = 0x00000008;
+       bfin_write32(DPM0_CTL, dpm0_ctl);
+       SSYNC();
+       __asm__ __volatile__( \
+                       ".align 8;" \
+                       "idle;" \
+                       : : \
+                       );
+}
+
+__attribute__((l1_text))
+void bf609_ddr_sr(void)
+{
+       uint32_t reg;
+
+       reg = bfin_read_DDR0_CTL();
+       reg |= 0x8;
+       bfin_write_DDR0_CTL(reg);
+
+       while (!(bfin_read_DDR0_STAT() & 0x8))
+               continue;
+}
+
+__attribute__((l1_text))
+void bf609_ddr_sr_exit(void)
+{
+       uint32_t reg;
+       while (!(bfin_read_DDR0_STAT() & 0x1))
+               continue;
+
+       reg = bfin_read_DDR0_CTL();
+       reg &= ~0x8;
+       bfin_write_DDR0_CTL(reg);
+
+       while ((bfin_read_DDR0_STAT() & 0x8))
+               continue;
+}
+
+__attribute__((l1_text))
+void bfin_hibernate_syscontrol(void)
+{
+       configvalues.ulWUA_Flags = (0xAD000000 | BITM_ROM_WUA_EN
+               | BITM_ROM_WUA_CGU | BITM_ROM_WUA_DDR | BITM_ROM_WUA_DDRDLLEN);
+
+       dactionflags = (BITM_ROM_SYSCTRL_WUA_EN
+               | BITM_ROM_SYSCTRL_WUA_DPMWRITE | BITM_ROM_SYSCTRL_WUA_CGU
+               | BITM_ROM_SYSCTRL_WUA_DDR | BITM_ROM_SYSCTRL_WUA_DDRDLLEN);
+
+       bfrom_SysControl(dactionflags, &configvalues, NULL);
+
+       bfin_write32(DPM0_RESTORE5, bfin_read32(DPM0_RESTORE5) | 4);
+}
+
+#ifndef CONFIG_BF60x
+# define SIC_SYSIRQ(irq)       (irq - (IRQ_CORETMR + 1))
+#else
+# define SIC_SYSIRQ(irq)       ((irq) - IVG15)
+#endif
+void bfin_hibernate(unsigned long mask)
+{
+       bfin_write32(DPM0_WAKE_EN, 0x10);
+       bfin_write32(DPM0_WAKE_POL, 0x10);
+       bfin_write32(DPM0_PGCNTR, 0x0000FFFF);
+       bfin_write32(DPM0_HIB_DIS, 0xFFFF);
+
+       printk(KERN_DEBUG "hibernate: restore %x pgcnt %x\n", bfin_read32(DPM0_RESTORE0), bfin_read32(DPM0_PGCNTR));
+
+       bf609_hibernate();
+}
+
+void bf609_cpu_pm_enter(suspend_state_t state)
+{
+       int error;
+       error = irq_set_irq_wake(255, 1);
+       if(error < 0)
+               printk(KERN_DEBUG "Unable to get irq wake\n");
+       error = irq_set_irq_wake(231, 1);
+       if (error < 0)
+               printk(KERN_DEBUG "Unable to get irq wake\n");
+
+       if (state == PM_SUSPEND_STANDBY)
+               bfin_deepsleep(0xffff);
+       else {
+               bfin_hibernate(0xffff);
+       }
+}
+
+int bf609_cpu_pm_prepare(void)
+{
+       return 0;
+}
+
+void bf609_cpu_pm_finish(void)
+{
+
+}
+
+static struct bfin_cpu_pm_fns bf609_cpu_pm = {
+       .enter          = bf609_cpu_pm_enter,
+       .prepare        = bf609_cpu_pm_prepare,
+       .finish         = bf609_cpu_pm_finish,
+};
+
+static irqreturn_t test_isr(int irq, void *dev_id)
+{
+       printk(KERN_DEBUG "gpio irq %d\n", irq);
+       return IRQ_HANDLED;
+}
+
+static irqreturn_t dpm0_isr(int irq, void *dev_id)
+{
+       uint32_t wake_stat;
+
+       wake_stat = bfin_read32(DPM0_WAKE_STAT);
+       printk(KERN_DEBUG "enter %s wake stat %08x\n", __func__, wake_stat);
+
+       bfin_write32(DPM0_WAKE_STAT, wake_stat);
+       return IRQ_HANDLED;
+}
+
+static int __init bf609_init_pm(void)
+{
+       int irq;
+       int error;
+       error = gpio_request(GPIO_PG4, "gpiopg4");
+       if (error < 0) {
+               printk(KERN_DEBUG "failed to request GPIO %d, error %d\n",
+                               GPIO_PG4, error);
+       }
+
+       irq = gpio_to_irq(GPIO_PG4);
+       if (irq < 0) {
+               error = irq;
+               printk(KERN_DEBUG "Unable to get irq number for GPIO %d, error %d\n",
+                               GPIO_PG4, error);
+       }
+
+       printk(KERN_DEBUG "%s gpio %d irq %d\n", __func__, GPIO_PG4, irq);
+
+       error = request_irq(irq, test_isr, IRQF_TRIGGER_FALLING | IRQF_NO_SUSPEND, "gpiopg4", NULL);
+       if(error < 0)
+               printk(KERN_DEBUG "Unable to get irq\n");
+
+#if 1
+       irq = gpio_to_irq(GPIO_PE12);
+       if (irq < 0) {
+               error = irq;
+               printk(KERN_DEBUG "Unable to get irq number for GPIO %d, error %d\n",
+                               GPIO_PE12, error);
+       }
+
+       error = request_irq(irq, test_isr, IRQF_TRIGGER_RISING | IRQF_NO_SUSPEND, "gpiope12", NULL);
+       if(error < 0)
+               printk(KERN_DEBUG "Unable to get irq\n");
+#endif
+
+       error = request_irq(IRQ_CGU_EVT, dpm0_isr, IRQF_NO_SUSPEND, "cgu0 event", NULL);
+       if(error < 0)
+               printk(KERN_DEBUG "Unable to get irq\n");
+
+       error = request_irq(IRQ_DPM, dpm0_isr, IRQF_NO_SUSPEND, "dpm0 event", NULL);
+       if (error < 0)
+               printk(KERN_DEBUG "Unable to get irq\n");
+
+       bfin_cpu_pm = &bf609_cpu_pm;
+       return 0;
+}
+
+late_initcall(bf609_init_pm);
index ff299f24aba0703fe8751004de6ed7345ee8777a..75f0ba29ebb909235f78cd484123e25f77e478a9 100644 (file)
@@ -6,7 +6,10 @@ obj-y := \
        cache.o cache-c.o entry.o head.o \
        interrupt.o arch_checks.o ints-priority.o
 
-obj-$(CONFIG_PM)          += pm.o dpmc_modes.o
+obj-$(CONFIG_PM)          += pm.o
+ifneq ($(CONFIG_BF60x),y)
+obj-$(CONFIG_PM)         += dpmc_modes.o
+endif
 obj-$(CONFIG_CPU_FREQ)    += cpufreq.o
 obj-$(CONFIG_CPU_VOLTAGE) += dpmc.o
 obj-$(CONFIG_SMP)         += smp.o
index 1c534d298de4dad5534bff858885940a74ca545c..2bec509cd7e24210513b8fa606a04fb9c882882a 100644 (file)
@@ -10,7 +10,7 @@
 #include <asm/dpmc.h>
 
 .section .l1.text
-
+#ifndef CONFIG_BF60x
 ENTRY(_sleep_mode)
        [--SP] = (R7:4, P5:3);
        [--SP] = RETS;
@@ -49,6 +49,7 @@ ENTRY(_sleep_mode)
        (R7:4, P5:3) = [SP++];
        RTS;
 ENDPROC(_sleep_mode)
+#endif
 
 /*
  * This func never returns as it puts the part into hibernate, and
@@ -58,12 +59,14 @@ ENDPROC(_sleep_mode)
  *
  * We accept just one argument -- the value to write to VR_CTL.
  */
+
 ENTRY(_hibernate_mode)
        /* Save/setup the regs we need early for minor pipeline optimization */
        R4 = R0;
+
+#ifndef CONFIG_BF60x
        P3.H = hi(VR_CTL);
        P3.L = lo(VR_CTL);
-
        /* Disable all wakeup sources */
        R0 = IWR_DISABLE_ALL;
        R1 = IWR_DISABLE_ALL;
@@ -71,6 +74,7 @@ ENTRY(_hibernate_mode)
        call _set_sic_iwr;
        call _set_dram_srfs;
        SSYNC;
+#endif
 
        /* Finally, we climb into our cave to hibernate */
        W[P3] = R4.L;
@@ -80,6 +84,7 @@ ENTRY(_hibernate_mode)
        jump .Lforever;
 ENDPROC(_hibernate_mode)
 
+#ifndef CONFIG_BF60x
 ENTRY(_sleep_deeper)
        [--SP] = (R7:4, P5:3);
        [--SP] = RETS;
@@ -274,329 +279,13 @@ ENTRY(_test_pll_locked)
        IF !CC JUMP 1b;
        RTS;
 ENDPROC(_test_pll_locked)
+#endif
 
 .section .text
-
-#define PM_REG0  R7
-#define PM_REG1  R6
-#define PM_REG2  R5
-#define PM_REG3  R4
-#define PM_REG4  R3
-#define PM_REG5  R2
-#define PM_REG6  R1
-#define PM_REG7  R0
-#define PM_REG8  P5
-#define PM_REG9  P4
-#define PM_REG10 P3
-#define PM_REG11 P2
-#define PM_REG12 P1
-#define PM_REG13 P0
-
-#define PM_REGSET0  R7:7
-#define PM_REGSET1  R7:6
-#define PM_REGSET2  R7:5
-#define PM_REGSET3  R7:4
-#define PM_REGSET4  R7:3
-#define PM_REGSET5  R7:2
-#define PM_REGSET6  R7:1
-#define PM_REGSET7  R7:0
-#define PM_REGSET8  R7:0, P5:5
-#define PM_REGSET9  R7:0, P5:4
-#define PM_REGSET10 R7:0, P5:3
-#define PM_REGSET11 R7:0, P5:2
-#define PM_REGSET12 R7:0, P5:1
-#define PM_REGSET13 R7:0, P5:0
-
-#define _PM_PUSH(n, x, w, base) PM_REG##n = w[FP + ((x) - (base))];
-#define _PM_POP(n, x, w, base)  w[FP + ((x) - (base))] = PM_REG##n;
-#define PM_PUSH_SYNC(n)         [--sp] = (PM_REGSET##n);
-#define PM_POP_SYNC(n)          (PM_REGSET##n) = [sp++];
-#define PM_PUSH(n, x)           PM_REG##n = [FP++];
-#define PM_POP(n, x)            [FP--] = PM_REG##n;
-#define PM_CORE_PUSH(n, x)      _PM_PUSH(n, x, , COREMMR_BASE)
-#define PM_CORE_POP(n, x)       _PM_POP(n, x, , COREMMR_BASE)
-#define PM_SYS_PUSH(n, x)       _PM_PUSH(n, x, , SYSMMR_BASE)
-#define PM_SYS_POP(n, x)        _PM_POP(n, x, , SYSMMR_BASE)
-#define PM_SYS_PUSH16(n, x)     _PM_PUSH(n, x, w, SYSMMR_BASE)
-#define PM_SYS_POP16(n, x)      _PM_POP(n, x, w, SYSMMR_BASE)
-
 ENTRY(_do_hibernate)
-       /*
-        * Save the core regs early so we can blow them away when
-        * saving/restoring MMR states
-        */
-       [--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;
-
-       /* We can't push RETI directly as that'll change IPEND[4] */
-       r7 = RETI;
-       [--sp] = RETS;
-       [--sp] = ASTAT;
-       [--sp] = CYCLES;
-       [--sp] = CYCLES2;
-       [--sp] = SYSCFG;
-       [--sp] = RETX;
-       [--sp] = SEQSTAT;
-       [--sp] = r7;
-
-       /* Save first func arg in M3 */
-       M3 = R0;
-
-       /* Save system MMRs */
-       FP.H = hi(SYSMMR_BASE);
-       FP.L = lo(SYSMMR_BASE);
-
-#ifdef SIC_IMASK0
-       PM_SYS_PUSH(0, SIC_IMASK0)
-       PM_SYS_PUSH(1, SIC_IMASK1)
-# ifdef SIC_IMASK2
-       PM_SYS_PUSH(2, SIC_IMASK2)
-# endif
-#else
-       PM_SYS_PUSH(0, SIC_IMASK)
-#endif
-#ifdef SIC_IAR0
-       PM_SYS_PUSH(3, SIC_IAR0)
-       PM_SYS_PUSH(4, SIC_IAR1)
-       PM_SYS_PUSH(5, SIC_IAR2)
-#endif
-#ifdef SIC_IAR3
-       PM_SYS_PUSH(6, SIC_IAR3)
-#endif
-#ifdef SIC_IAR4
-       PM_SYS_PUSH(7, SIC_IAR4)
-       PM_SYS_PUSH(8, SIC_IAR5)
-       PM_SYS_PUSH(9, SIC_IAR6)
-#endif
-#ifdef SIC_IAR7
-       PM_SYS_PUSH(10, SIC_IAR7)
-#endif
-#ifdef SIC_IAR8
-       PM_SYS_PUSH(11, SIC_IAR8)
-       PM_SYS_PUSH(12, SIC_IAR9)
-       PM_SYS_PUSH(13, SIC_IAR10)
-#endif
-       PM_PUSH_SYNC(13)
-#ifdef SIC_IAR11
-       PM_SYS_PUSH(0, SIC_IAR11)
-#endif
-
-#ifdef SIC_IWR
-       PM_SYS_PUSH(1, SIC_IWR)
-#endif
-#ifdef SIC_IWR0
-       PM_SYS_PUSH(1, SIC_IWR0)
-#endif
-#ifdef SIC_IWR1
-       PM_SYS_PUSH(2, SIC_IWR1)
-#endif
-#ifdef SIC_IWR2
-       PM_SYS_PUSH(3, SIC_IWR2)
-#endif
-
-#ifdef PINT0_ASSIGN
-       PM_SYS_PUSH(4, PINT0_MASK_SET)
-       PM_SYS_PUSH(5, PINT1_MASK_SET)
-       PM_SYS_PUSH(6, PINT2_MASK_SET)
-       PM_SYS_PUSH(7, PINT3_MASK_SET)
-       PM_SYS_PUSH(8, PINT0_ASSIGN)
-       PM_SYS_PUSH(9, PINT1_ASSIGN)
-       PM_SYS_PUSH(10, PINT2_ASSIGN)
-       PM_SYS_PUSH(11, PINT3_ASSIGN)
-       PM_SYS_PUSH(12, PINT0_INVERT_SET)
-       PM_SYS_PUSH(13, PINT1_INVERT_SET)
-       PM_PUSH_SYNC(13)
-       PM_SYS_PUSH(0, PINT2_INVERT_SET)
-       PM_SYS_PUSH(1, PINT3_INVERT_SET)
-       PM_SYS_PUSH(2, PINT0_EDGE_SET)
-       PM_SYS_PUSH(3, PINT1_EDGE_SET)
-       PM_SYS_PUSH(4, PINT2_EDGE_SET)
-       PM_SYS_PUSH(5, PINT3_EDGE_SET)
-#endif
-
-       PM_SYS_PUSH16(6, SYSCR)
-
-       PM_SYS_PUSH16(7, EBIU_AMGCTL)
-       PM_SYS_PUSH(8, EBIU_AMBCTL0)
-       PM_SYS_PUSH(9, EBIU_AMBCTL1)
-#ifdef EBIU_FCTL
-       PM_SYS_PUSH(10, EBIU_MBSCTL)
-       PM_SYS_PUSH(11, EBIU_MODE)
-       PM_SYS_PUSH(12, EBIU_FCTL)
-       PM_PUSH_SYNC(12)
-#else
-       PM_PUSH_SYNC(9)
-#endif
-
-       /* Save Core MMRs */
-       I0.H = hi(COREMMR_BASE);
-       I0.L = lo(COREMMR_BASE);
-       I1 = I0;
-       I2 = I0;
-       I3 = I0;
-       B0 = I0;
-       B1 = I0;
-       B2 = I0;
-       B3 = I0;
-       I1.L = lo(DCPLB_ADDR0);
-       I2.L = lo(DCPLB_DATA0);
-       I3.L = lo(ICPLB_ADDR0);
-       B0.L = lo(ICPLB_DATA0);
-       B1.L = lo(EVT2);
-       B2.L = lo(IMASK);
-       B3.L = lo(TCNTL);
-
-       /* DCPLB Addr */
-       FP = I1;
-       PM_PUSH(0, DCPLB_ADDR0)
-       PM_PUSH(1, DCPLB_ADDR1)
-       PM_PUSH(2, DCPLB_ADDR2)
-       PM_PUSH(3, DCPLB_ADDR3)
-       PM_PUSH(4, DCPLB_ADDR4)
-       PM_PUSH(5, DCPLB_ADDR5)
-       PM_PUSH(6, DCPLB_ADDR6)
-       PM_PUSH(7, DCPLB_ADDR7)
-       PM_PUSH(8, DCPLB_ADDR8)
-       PM_PUSH(9, DCPLB_ADDR9)
-       PM_PUSH(10, DCPLB_ADDR10)
-       PM_PUSH(11, DCPLB_ADDR11)
-       PM_PUSH(12, DCPLB_ADDR12)
-       PM_PUSH(13, DCPLB_ADDR13)
-       PM_PUSH_SYNC(13)
-       PM_PUSH(0, DCPLB_ADDR14)
-       PM_PUSH(1, DCPLB_ADDR15)
-
-       /* DCPLB Data */
-       FP = I2;
-       PM_PUSH(2, DCPLB_DATA0)
-       PM_PUSH(3, DCPLB_DATA1)
-       PM_PUSH(4, DCPLB_DATA2)
-       PM_PUSH(5, DCPLB_DATA3)
-       PM_PUSH(6, DCPLB_DATA4)
-       PM_PUSH(7, DCPLB_DATA5)
-       PM_PUSH(8, DCPLB_DATA6)
-       PM_PUSH(9, DCPLB_DATA7)
-       PM_PUSH(10, DCPLB_DATA8)
-       PM_PUSH(11, DCPLB_DATA9)
-       PM_PUSH(12, DCPLB_DATA10)
-       PM_PUSH(13, DCPLB_DATA11)
-       PM_PUSH_SYNC(13)
-       PM_PUSH(0, DCPLB_DATA12)
-       PM_PUSH(1, DCPLB_DATA13)
-       PM_PUSH(2, DCPLB_DATA14)
-       PM_PUSH(3, DCPLB_DATA15)
-
-       /* ICPLB Addr */
-       FP = I3;
-       PM_PUSH(4, ICPLB_ADDR0)
-       PM_PUSH(5, ICPLB_ADDR1)
-       PM_PUSH(6, ICPLB_ADDR2)
-       PM_PUSH(7, ICPLB_ADDR3)
-       PM_PUSH(8, ICPLB_ADDR4)
-       PM_PUSH(9, ICPLB_ADDR5)
-       PM_PUSH(10, ICPLB_ADDR6)
-       PM_PUSH(11, ICPLB_ADDR7)
-       PM_PUSH(12, ICPLB_ADDR8)
-       PM_PUSH(13, ICPLB_ADDR9)
-       PM_PUSH_SYNC(13)
-       PM_PUSH(0, ICPLB_ADDR10)
-       PM_PUSH(1, ICPLB_ADDR11)
-       PM_PUSH(2, ICPLB_ADDR12)
-       PM_PUSH(3, ICPLB_ADDR13)
-       PM_PUSH(4, ICPLB_ADDR14)
-       PM_PUSH(5, ICPLB_ADDR15)
-
-       /* ICPLB Data */
-       FP = B0;
-       PM_PUSH(6, ICPLB_DATA0)
-       PM_PUSH(7, ICPLB_DATA1)
-       PM_PUSH(8, ICPLB_DATA2)
-       PM_PUSH(9, ICPLB_DATA3)
-       PM_PUSH(10, ICPLB_DATA4)
-       PM_PUSH(11, ICPLB_DATA5)
-       PM_PUSH(12, ICPLB_DATA6)
-       PM_PUSH(13, ICPLB_DATA7)
-       PM_PUSH_SYNC(13)
-       PM_PUSH(0, ICPLB_DATA8)
-       PM_PUSH(1, ICPLB_DATA9)
-       PM_PUSH(2, ICPLB_DATA10)
-       PM_PUSH(3, ICPLB_DATA11)
-       PM_PUSH(4, ICPLB_DATA12)
-       PM_PUSH(5, ICPLB_DATA13)
-       PM_PUSH(6, ICPLB_DATA14)
-       PM_PUSH(7, ICPLB_DATA15)
-
-       /* Event Vectors */
-       FP = B1;
-       PM_PUSH(8, EVT2)
-       PM_PUSH(9, EVT3)
-       FP += 4;        /* EVT4 */
-       PM_PUSH(10, EVT5)
-       PM_PUSH(11, EVT6)
-       PM_PUSH(12, EVT7)
-       PM_PUSH(13, EVT8)
-       PM_PUSH_SYNC(13)
-       PM_PUSH(0, EVT9)
-       PM_PUSH(1, EVT10)
-       PM_PUSH(2, EVT11)
-       PM_PUSH(3, EVT12)
-       PM_PUSH(4, EVT13)
-       PM_PUSH(5, EVT14)
-       PM_PUSH(6, EVT15)
-
-       /* CEC */
-       FP = B2;
-       PM_PUSH(7, IMASK)
-       FP += 4;        /* IPEND */
-       PM_PUSH(8, ILAT)
-       PM_PUSH(9, IPRIO)
-
-       /* Core Timer */
-       FP = B3;
-       PM_PUSH(10, TCNTL)
-       PM_PUSH(11, TPERIOD)
-       PM_PUSH(12, TSCALE)
-       PM_PUSH(13, TCOUNT)
-       PM_PUSH_SYNC(13)
-
-       /* Misc non-contiguous registers */
-       FP = I0;
-       PM_CORE_PUSH(0, DMEM_CONTROL);
-       PM_CORE_PUSH(1, IMEM_CONTROL);
-       PM_CORE_PUSH(2, TBUFCTL);
-       PM_PUSH_SYNC(2)
+       bfin_cpu_reg_save;
+       bfin_sys_mmr_save;
+       bfin_core_mmr_save;
 
        /* Setup args to hibernate mode early for pipeline optimization */
        R0 = M3;
@@ -618,274 +307,9 @@ ENTRY(_do_hibernate)
 
 .Lpm_resume_here:
 
-       /* Restore Core MMRs */
-       I0.H = hi(COREMMR_BASE);
-       I0.L = lo(COREMMR_BASE);
-       I1 = I0;
-       I2 = I0;
-       I3 = I0;
-       B0 = I0;
-       B1 = I0;
-       B2 = I0;
-       B3 = I0;
-       I1.L = lo(DCPLB_ADDR15);
-       I2.L = lo(DCPLB_DATA15);
-       I3.L = lo(ICPLB_ADDR15);
-       B0.L = lo(ICPLB_DATA15);
-       B1.L = lo(EVT15);
-       B2.L = lo(IPRIO);
-       B3.L = lo(TCOUNT);
-
-       /* Misc non-contiguous registers */
-       FP = I0;
-       PM_POP_SYNC(2)
-       PM_CORE_POP(2, TBUFCTL)
-       PM_CORE_POP(1, IMEM_CONTROL)
-       PM_CORE_POP(0, DMEM_CONTROL)
-
-       /* Core Timer */
-       PM_POP_SYNC(13)
-       FP = B3;
-       PM_POP(13, TCOUNT)
-       PM_POP(12, TSCALE)
-       PM_POP(11, TPERIOD)
-       PM_POP(10, TCNTL)
-
-       /* CEC */
-       FP = B2;
-       PM_POP(9, IPRIO)
-       PM_POP(8, ILAT)
-       FP += -4;       /* IPEND */
-       PM_POP(7, IMASK)
-
-       /* Event Vectors */
-       FP = B1;
-       PM_POP(6, EVT15)
-       PM_POP(5, EVT14)
-       PM_POP(4, EVT13)
-       PM_POP(3, EVT12)
-       PM_POP(2, EVT11)
-       PM_POP(1, EVT10)
-       PM_POP(0, EVT9)
-       PM_POP_SYNC(13)
-       PM_POP(13, EVT8)
-       PM_POP(12, EVT7)
-       PM_POP(11, EVT6)
-       PM_POP(10, EVT5)
-       FP += -4;       /* EVT4 */
-       PM_POP(9, EVT3)
-       PM_POP(8, EVT2)
-
-       /* ICPLB Data */
-       FP = B0;
-       PM_POP(7, ICPLB_DATA15)
-       PM_POP(6, ICPLB_DATA14)
-       PM_POP(5, ICPLB_DATA13)
-       PM_POP(4, ICPLB_DATA12)
-       PM_POP(3, ICPLB_DATA11)
-       PM_POP(2, ICPLB_DATA10)
-       PM_POP(1, ICPLB_DATA9)
-       PM_POP(0, ICPLB_DATA8)
-       PM_POP_SYNC(13)
-       PM_POP(13, ICPLB_DATA7)
-       PM_POP(12, ICPLB_DATA6)
-       PM_POP(11, ICPLB_DATA5)
-       PM_POP(10, ICPLB_DATA4)
-       PM_POP(9, ICPLB_DATA3)
-       PM_POP(8, ICPLB_DATA2)
-       PM_POP(7, ICPLB_DATA1)
-       PM_POP(6, ICPLB_DATA0)
-
-       /* ICPLB Addr */
-       FP = I3;
-       PM_POP(5, ICPLB_ADDR15)
-       PM_POP(4, ICPLB_ADDR14)
-       PM_POP(3, ICPLB_ADDR13)
-       PM_POP(2, ICPLB_ADDR12)
-       PM_POP(1, ICPLB_ADDR11)
-       PM_POP(0, ICPLB_ADDR10)
-       PM_POP_SYNC(13)
-       PM_POP(13, ICPLB_ADDR9)
-       PM_POP(12, ICPLB_ADDR8)
-       PM_POP(11, ICPLB_ADDR7)
-       PM_POP(10, ICPLB_ADDR6)
-       PM_POP(9, ICPLB_ADDR5)
-       PM_POP(8, ICPLB_ADDR4)
-       PM_POP(7, ICPLB_ADDR3)
-       PM_POP(6, ICPLB_ADDR2)
-       PM_POP(5, ICPLB_ADDR1)
-       PM_POP(4, ICPLB_ADDR0)
-
-       /* DCPLB Data */
-       FP = I2;
-       PM_POP(3, DCPLB_DATA15)
-       PM_POP(2, DCPLB_DATA14)
-       PM_POP(1, DCPLB_DATA13)
-       PM_POP(0, DCPLB_DATA12)
-       PM_POP_SYNC(13)
-       PM_POP(13, DCPLB_DATA11)
-       PM_POP(12, DCPLB_DATA10)
-       PM_POP(11, DCPLB_DATA9)
-       PM_POP(10, DCPLB_DATA8)
-       PM_POP(9, DCPLB_DATA7)
-       PM_POP(8, DCPLB_DATA6)
-       PM_POP(7, DCPLB_DATA5)
-       PM_POP(6, DCPLB_DATA4)
-       PM_POP(5, DCPLB_DATA3)
-       PM_POP(4, DCPLB_DATA2)
-       PM_POP(3, DCPLB_DATA1)
-       PM_POP(2, DCPLB_DATA0)
-
-       /* DCPLB Addr */
-       FP = I1;
-       PM_POP(1, DCPLB_ADDR15)
-       PM_POP(0, DCPLB_ADDR14)
-       PM_POP_SYNC(13)
-       PM_POP(13, DCPLB_ADDR13)
-       PM_POP(12, DCPLB_ADDR12)
-       PM_POP(11, DCPLB_ADDR11)
-       PM_POP(10, DCPLB_ADDR10)
-       PM_POP(9, DCPLB_ADDR9)
-       PM_POP(8, DCPLB_ADDR8)
-       PM_POP(7, DCPLB_ADDR7)
-       PM_POP(6, DCPLB_ADDR6)
-       PM_POP(5, DCPLB_ADDR5)
-       PM_POP(4, DCPLB_ADDR4)
-       PM_POP(3, DCPLB_ADDR3)
-       PM_POP(2, DCPLB_ADDR2)
-       PM_POP(1, DCPLB_ADDR1)
-       PM_POP(0, DCPLB_ADDR0)
-
-       /* Restore System MMRs */
-       FP.H = hi(SYSMMR_BASE);
-       FP.L = lo(SYSMMR_BASE);
-
-#ifdef EBIU_FCTL
-       PM_POP_SYNC(12)
-       PM_SYS_POP(12, EBIU_FCTL)
-       PM_SYS_POP(11, EBIU_MODE)
-       PM_SYS_POP(10, EBIU_MBSCTL)
-#else
-       PM_POP_SYNC(9)
-#endif
-       PM_SYS_POP(9, EBIU_AMBCTL1)
-       PM_SYS_POP(8, EBIU_AMBCTL0)
-       PM_SYS_POP16(7, EBIU_AMGCTL)
-
-       PM_SYS_POP16(6, SYSCR)
-
-#ifdef PINT0_ASSIGN
-       PM_SYS_POP(5, PINT3_EDGE_SET)
-       PM_SYS_POP(4, PINT2_EDGE_SET)
-       PM_SYS_POP(3, PINT1_EDGE_SET)
-       PM_SYS_POP(2, PINT0_EDGE_SET)
-       PM_SYS_POP(1, PINT3_INVERT_SET)
-       PM_SYS_POP(0, PINT2_INVERT_SET)
-       PM_POP_SYNC(13)
-       PM_SYS_POP(13, PINT1_INVERT_SET)
-       PM_SYS_POP(12, PINT0_INVERT_SET)
-       PM_SYS_POP(11, PINT3_ASSIGN)
-       PM_SYS_POP(10, PINT2_ASSIGN)
-       PM_SYS_POP(9, PINT1_ASSIGN)
-       PM_SYS_POP(8, PINT0_ASSIGN)
-       PM_SYS_POP(7, PINT3_MASK_SET)
-       PM_SYS_POP(6, PINT2_MASK_SET)
-       PM_SYS_POP(5, PINT1_MASK_SET)
-       PM_SYS_POP(4, PINT0_MASK_SET)
-#endif
-
-#ifdef SIC_IWR2
-       PM_SYS_POP(3, SIC_IWR2)
-#endif
-#ifdef SIC_IWR1
-       PM_SYS_POP(2, SIC_IWR1)
-#endif
-#ifdef SIC_IWR0
-       PM_SYS_POP(1, SIC_IWR0)
-#endif
-#ifdef SIC_IWR
-       PM_SYS_POP(1, SIC_IWR)
-#endif
-
-#ifdef SIC_IAR11
-       PM_SYS_POP(0, SIC_IAR11)
-#endif
-       PM_POP_SYNC(13)
-#ifdef SIC_IAR8
-       PM_SYS_POP(13, SIC_IAR10)
-       PM_SYS_POP(12, SIC_IAR9)
-       PM_SYS_POP(11, SIC_IAR8)
-#endif
-#ifdef SIC_IAR7
-       PM_SYS_POP(10, SIC_IAR7)
-#endif
-#ifdef SIC_IAR6
-       PM_SYS_POP(9, SIC_IAR6)
-       PM_SYS_POP(8, SIC_IAR5)
-       PM_SYS_POP(7, SIC_IAR4)
-#endif
-#ifdef SIC_IAR3
-       PM_SYS_POP(6, SIC_IAR3)
-#endif
-#ifdef SIC_IAR0
-       PM_SYS_POP(5, SIC_IAR2)
-       PM_SYS_POP(4, SIC_IAR1)
-       PM_SYS_POP(3, SIC_IAR0)
-#endif
-#ifdef SIC_IMASK0
-# ifdef SIC_IMASK2
-       PM_SYS_POP(2, SIC_IMASK2)
-# endif
-       PM_SYS_POP(1, SIC_IMASK1)
-       PM_SYS_POP(0, SIC_IMASK0)
-#else
-       PM_SYS_POP(0, SIC_IMASK)
-#endif
-
-       /* Restore Core Registers */
-       RETI = [sp++];
-       SEQSTAT = [sp++];
-       RETX = [sp++];
-       SYSCFG = [sp++];
-       CYCLES2 = [sp++];
-       CYCLES = [sp++];
-       ASTAT = [sp++];
-       RETS = [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++];
+       bfin_core_mmr_restore;
+       bfin_sys_mmr_restore;
+       bfin_cpu_reg_restore;
 
        [--sp] = RETI;  /* Clear Global Interrupt Disable */
        SP += 4;
index 3c648a077e75d746bc11ed1f8b369540ad31d2f4..c25c2f1c819c90cc1fbaa33b8af8131cd7dd4d96 100644 (file)
 #include <asm/gpio.h>
 #include <asm/dma.h>
 #include <asm/dpmc.h>
+#include <asm/pm.h>
 
+#ifdef CONFIG_BF60x
+struct bfin_cpu_pm_fns *bfin_cpu_pm;
+#endif
 
 void bfin_pm_suspend_standby_enter(void)
 {
+#ifndef CONFIG_BF60x
        bfin_pm_standby_setup();
+#endif
 
-#ifdef CONFIG_PM_BFIN_SLEEP_DEEPER
-       sleep_deeper(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
+#ifdef CONFIG_BF60x
+       bfin_cpu_pm->enter(PM_SUSPEND_STANDBY);
 #else
+# ifdef CONFIG_PM_BFIN_SLEEP_DEEPER
+       sleep_deeper(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
+# else
        sleep_mode(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
+# endif
 #endif
 
+#ifndef CONFIG_BF60x
        bfin_pm_standby_restore();
+#endif
 
+#ifndef CONFIG_BF60x
 #ifdef SIC_IWR0
        bfin_write_SIC_IWR0(IWR_DISABLE_ALL);
 # ifdef SIC_IWR1
@@ -52,6 +65,8 @@ void bfin_pm_suspend_standby_enter(void)
 #else
        bfin_write_SIC_IWR(IWR_DISABLE_ALL);
 #endif
+
+#endif
 }
 
 int bf53x_suspend_l1_mem(unsigned char *memptr)
@@ -83,10 +98,13 @@ int bf53x_resume_l1_mem(unsigned char *memptr)
 }
 
 #if defined(CONFIG_BFIN_EXTMEM_WRITEBACK) || defined(CONFIG_BFIN_L2_WRITEBACK)
+# ifdef CONFIG_BF60x
+__attribute__((l1_text))
+# endif
 static void flushinv_all_dcache(void)
 {
-       u32 way, bank, subbank, set;
-       u32 status, addr;
+       register u32 way, bank, subbank, set;
+       register u32 status, addr;
        u32 dmem_ctl = bfin_read_DMEM_CONTROL();
 
        for (bank = 0; bank < 2; ++bank) {
@@ -133,7 +151,11 @@ int bfin_pm_suspend_mem_enter(void)
                return -ENOMEM;
        }
 
+#ifndef CONFIG_BF60x
        wakeup = bfin_read_VR_CTL() & ~FREQ;
+#else
+
+#endif
        wakeup |= SCKELOW;
 
 #ifdef CONFIG_PM_BFIN_WAKE_PH6
@@ -159,7 +181,11 @@ int bfin_pm_suspend_mem_enter(void)
        _disable_icplb();
        bf53x_suspend_l1_mem(memptr);
 
+#ifndef CONFIG_BF60x
        do_hibernate(wakeup | vr_wakeup);       /* See you later! */
+#else
+       bfin_cpu_pm->enter(PM_SUSPEND_MEM);
+#endif
 
        bf53x_resume_l1_mem(memptr);