Merge branch 'davinci-for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git...

[pandora-kernel.git] / arch / sh / kernel / cpu / shmobile / sleep.S

diff --git a/arch/sh/kernel/cpu/shmobile/sleep.S b/arch/sh/kernel/cpu/shmobile/sleep.S
index e9dd7fa..e6aac65 100644 (file)
--- a/arch/sh/kernel/cpu/shmobile/sleep.S
+++ b/arch/sh/kernel/cpu/shmobile/sleep.S
@@ -48,8 +48,48 @@ ENTRY(sh_mobile_sleep_enter_start)
        stc     sr, r0
        mov.l   r0, @(SH_SLEEP_SR, r5)
 
-       /* save sp */
+       /* save general purpose registers to stack if needed */
+       mov.l   @(SH_SLEEP_MODE, r5), r0
+       tst     #SUSP_SH_REGS, r0
+       bt      skip_regs_save
+
+       sts.l   pr, @-r15
+       mov.l   r14, @-r15
+       mov.l   r13, @-r15
+       mov.l   r12, @-r15
+       mov.l   r11, @-r15
+       mov.l   r10, @-r15
+       mov.l   r9, @-r15
+       mov.l   r8, @-r15
+
+       /* make sure bank0 is selected, save low registers */
+       mov.l   rb_bit, r9
+       not     r9, r9
+       bsr     set_sr
+        mov    #0, r10
+
+       bsr     save_low_regs
+        nop
+
+       /* switch to bank 1, save low registers */
+       mov.l   rb_bit, r10
+       bsr     set_sr
+        mov    #-1, r9
+
+       bsr     save_low_regs
+        nop
+
+       /* switch back to bank 0 */
+       mov.l   rb_bit, r9
+       not     r9, r9
+       bsr     set_sr
+        mov    #0, r10
+
+skip_regs_save:
+
+       /* save sp, also set to internal ram */
        mov.l   r15, @(SH_SLEEP_SP, r5)
+       mov     r5, r15
 
        /* save stbcr */
        bsr     save_register
@@ -60,7 +100,7 @@ ENTRY(sh_mobile_sleep_enter_start)
        tst     #SUSP_SH_MMU, r0
        bt      skip_mmu_save_disable
 
-       /* save mmu state */
+       /* save mmu state */
        bsr     save_register
         mov    #SH_SLEEP_REG_PTEH, r0
 
@@ -177,6 +217,29 @@ get_register:
        mov.l   @(r0, r5), r0
        rts
         nop
+
+set_sr:
+       stc     sr, r8
+       and     r9, r8
+       or      r10, r8
+       ldc     r8, sr
+       rts
+        nop
+
+save_low_regs:
+       mov.l   r7, @-r15
+       mov.l   r6, @-r15
+       mov.l   r5, @-r15
+       mov.l   r4, @-r15
+       mov.l   r3, @-r15
+       mov.l   r2, @-r15
+       mov.l   r1, @-r15
+       rts
+        mov.l  r0, @-r15
+
+       .balign 4
+rb_bit:        .long   0x20000000 ! RB=1
+
 ENTRY(sh_mobile_sleep_enter_end)
 
        .balign 4
@@ -270,6 +333,40 @@ skip_restore_sf:
        icbi    @r0
 
 skip_restore_mmu:
+
+       /* restore general purpose registers if needed */
+       mov.l   @(SH_SLEEP_MODE, r5), r0
+       tst     #SUSP_SH_REGS, r0
+       bt      skip_restore_regs
+
+       /* switch to bank 1, restore low registers */
+       mov.l   _rb_bit, r10
+       bsr     _set_sr
+        mov    #-1, r9
+
+       bsr     restore_low_regs
+        nop
+
+       /* switch to bank0, restore low registers */
+       mov.l   _rb_bit, r9
+       not     r9, r9
+       bsr     _set_sr
+        mov    #0, r10
+
+       bsr     restore_low_regs
+        nop
+
+       /* restore the rest of the registers */
+       mov.l   @r15+, r8
+       mov.l   @r15+, r9
+       mov.l   @r15+, r10
+       mov.l   @r15+, r11
+       mov.l   @r15+, r12
+       mov.l   @r15+, r13
+       mov.l   @r15+, r14
+       lds.l   @r15+, pr
+
+skip_restore_regs:
        rte
         nop
 
@@ -283,6 +380,26 @@ restore_register:
        rts
         nop
 
+_set_sr:
+       stc     sr, r8
+       and     r9, r8
+       or      r10, r8
+       ldc     r8, sr
+       rts
+        nop
+
+restore_low_regs:
+       mov.l   @r15+, r0
+       mov.l   @r15+, r1
+       mov.l   @r15+, r2
+       mov.l   @r15+, r3
+       mov.l   @r15+, r4
+       mov.l   @r15+, r5
+       mov.l   @r15+, r6
+       rts
+        mov.l  @r15+, r7
+
        .balign 4
+_rb_bit:       .long   0x20000000 ! RB=1
 1:     .long   ~0x7ff
 ENTRY(sh_mobile_sleep_resume_end)