sh: SH-Mobile R-standby register save/restore

Add code to save/restore registers during
R-standby sleep on SH-Mobile processors.

Signed-off-by: Magnus Damm <damm@opensource.se>
Signed-off-by: Paul Mundt <lethal@linux-sh.org>

1 files changed, 119 insertions, 2 deletions

diff --git a/arch/sh/kernel/cpu/shmobile/sleep.S b/arch/sh/kernel/cpu/shmobile/sleep.S
index e9dd7fa0abd2..e6aac65f5750 100644
--- 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)