2 files changed, 121 insertions, 3 deletions
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)