3 files changed, 122 insertions, 3 deletions

diff --git a/arch/sh/include/asm/suspend.h b/arch/sh/include/asm/suspend.h
index fe9c2a1ad047..64eb41a063e8 100644
--- a/arch/sh/include/asm/suspend.h
+++ b/arch/sh/include/asm/suspend.h
@@ -92,5 +92,6 @@ extern unsigned long sh_mobile_sleep_supported;
 #define SUSP_SH_USTANDBY        (1 << 3) /* SH-Mobile U-standby mode */
 #define SUSP_SH_SF              (1 << 4) /* Enable self-refresh */
 #define SUSP_SH_MMU             (1 << 5) /* Save/restore MMU and cache */
+#define SUSP_SH_REGS            (1 << 6) /* Save/restore registers */
 
 #endif /* _ASM_SH_SUSPEND_H */
diff --git a/arch/sh/kernel/cpu/shmobile/pm.c b/arch/sh/kernel/cpu/shmobile/pm.c
index ca029a44743c..e55968712706 100644
--- a/arch/sh/kernel/cpu/shmobile/pm.c
+++ b/arch/sh/kernel/cpu/shmobile/pm.c
@@ -33,7 +33,8 @@ ATOMIC_NOTIFIER_HEAD(sh_mobile_post_sleep_notifier_list);
 #define SUSP_MODE_SLEEP         (SUSP_SH_SLEEP)
 #define SUSP_MODE_SLEEP_SF      (SUSP_SH_SLEEP | SUSP_SH_SF)
 #define SUSP_MODE_STANDBY_SF    (SUSP_SH_STANDBY | SUSP_SH_SF)
-#define SUSP_MODE_RSTANDBY      (SUSP_SH_RSTANDBY | SUSP_SH_MMU | SUSP_SH_SF)
+#define SUSP_MODE_RSTANDBY_SF \
+        (SUSP_SH_RSTANDBY | SUSP_SH_MMU | SUSP_SH_REGS | SUSP_SH_SF)
  /*
   * U-standby mode is unsupported since it needs bootloader hacks
   */
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)