3 files changed, 117 insertions, 0 deletions

diff --git a/arch/sh/kernel/asm-offsets.c b/arch/sh/kernel/asm-offsets.c
index 9bdeff962e1c..6026b0f849a1 100644
--- a/arch/sh/kernel/asm-offsets.c
+++ b/arch/sh/kernel/asm-offsets.c
@@ -44,5 +44,15 @@ int main(void)
         DEFINE(SH_SLEEP_BASE_ADDR, offsetof(struct sh_sleep_data, addr));
         DEFINE(SH_SLEEP_BASE_DATA, offsetof(struct sh_sleep_data, data));
         DEFINE(SH_SLEEP_REG_STBCR, offsetof(struct sh_sleep_regs, stbcr));
+        DEFINE(SH_SLEEP_REG_PTEH, offsetof(struct sh_sleep_regs, pteh));
+        DEFINE(SH_SLEEP_REG_PTEL, offsetof(struct sh_sleep_regs, ptel));
+        DEFINE(SH_SLEEP_REG_TTB, offsetof(struct sh_sleep_regs, ttb));
+        DEFINE(SH_SLEEP_REG_TEA, offsetof(struct sh_sleep_regs, tea));
+        DEFINE(SH_SLEEP_REG_MMUCR, offsetof(struct sh_sleep_regs, mmucr));
+        DEFINE(SH_SLEEP_REG_PTEA, offsetof(struct sh_sleep_regs, ptea));
+        DEFINE(SH_SLEEP_REG_PASCR, offsetof(struct sh_sleep_regs, pascr));
+        DEFINE(SH_SLEEP_REG_IRMCR, offsetof(struct sh_sleep_regs, irmcr));
+        DEFINE(SH_SLEEP_REG_CCR, offsetof(struct sh_sleep_regs, ccr));
+        DEFINE(SH_SLEEP_REG_RAMCR, offsetof(struct sh_sleep_regs, ramcr));
         return 0;
 }
diff --git a/arch/sh/kernel/cpu/shmobile/pm.c b/arch/sh/kernel/cpu/shmobile/pm.c
index a94dc480f0c1..ca642f39e2e3 100644
--- a/arch/sh/kernel/cpu/shmobile/pm.c
+++ b/arch/sh/kernel/cpu/shmobile/pm.c
@@ -15,6 +15,7 @@
 #include <linux/suspend.h>
 #include <asm/suspend.h>
 #include <asm/uaccess.h>
+#include <asm/cacheflush.h>
 
 /*
  * Notifier lists for pre/post sleep notification
@@ -54,6 +55,10 @@ void sh_mobile_call_standby(unsigned long mode)
         atomic_notifier_call_chain(&sh_mobile_pre_sleep_notifier_list,
                                    mode, NULL);
 
+        /* flush the caches if MMU flag is set */
+        if (mode & SUSP_SH_MMU)
+                flush_cache_all();
+
         /* Let assembly snippet in on-chip memory handle the rest */
         standby_onchip_mem(mode, ILRAM_BASE);
 
@@ -81,6 +86,16 @@ void sh_mobile_register_self_refresh(unsigned long flags,
         /* part 0: data area */
         sdp = onchip_mem;
         sdp->addr.stbcr = 0xa4150020; /* STBCR */
+        sdp->addr.pteh = 0xff000000; /* PTEH */
+        sdp->addr.ptel = 0xff000004; /* PTEL */
+        sdp->addr.ttb = 0xff000008; /* TTB */
+        sdp->addr.tea = 0xff00000c; /* TEA */
+        sdp->addr.mmucr = 0xff000010; /* MMUCR */
+        sdp->addr.ptea = 0xff000034; /* PTEA */
+        sdp->addr.pascr = 0xff000070; /* PASCR */
+        sdp->addr.irmcr = 0xff000078; /* IRMCR */
+        sdp->addr.ccr = 0xff00001c; /* CCR */
+        sdp->addr.ramcr = 0xff000074; /* RAMCR */
         vp = sdp + 1;
 
         /* part 1: common code to enter sleep mode */
diff --git a/arch/sh/kernel/cpu/shmobile/sleep.S b/arch/sh/kernel/cpu/shmobile/sleep.S
index d3221d9b88be..e620bf397af5 100644
--- a/arch/sh/kernel/cpu/shmobile/sleep.S
+++ b/arch/sh/kernel/cpu/shmobile/sleep.S
@@ -52,6 +52,57 @@ ENTRY(sh_mobile_sleep_enter_start)
         bsr     save_register
          mov    #SH_SLEEP_REG_STBCR, r0
 
+        /* save mmu and cache context if needed */
+        mov.l   @(SH_SLEEP_MODE, r5), r0
+        tst     #SUSP_SH_MMU, r0
+        bt      skip_mmu_save_disable
+
+       /* save mmu state */
+        bsr     save_register
+         mov    #SH_SLEEP_REG_PTEH, r0
+
+        bsr     save_register
+         mov    #SH_SLEEP_REG_PTEL, r0
+
+        bsr     save_register
+         mov    #SH_SLEEP_REG_TTB, r0
+
+        bsr     save_register
+         mov    #SH_SLEEP_REG_TEA, r0
+
+        bsr     save_register
+         mov    #SH_SLEEP_REG_MMUCR, r0
+
+        bsr     save_register
+         mov    #SH_SLEEP_REG_PTEA, r0
+
+        bsr     save_register
+         mov    #SH_SLEEP_REG_PASCR, r0
+
+        bsr     save_register
+         mov    #SH_SLEEP_REG_IRMCR, r0
+
+        /* invalidate TLBs and disable the MMU */
+        bsr     get_register
+         mov    #SH_SLEEP_REG_MMUCR, r0
+        mov     #4, r1
+        mov.l   r1, @r0
+        icbi    @r0
+
+        /* save cache registers and disable caches */
+        bsr     save_register
+         mov    #SH_SLEEP_REG_CCR, r0
+
+        bsr     save_register
+         mov    #SH_SLEEP_REG_RAMCR, r0
+
+        bsr     get_register
+         mov    #SH_SLEEP_REG_CCR, r0
+        mov     #0, r1
+        mov.l   r1, @r0
+        icbi    @r0
+
+skip_mmu_save_disable:
         /* call self-refresh entering code if needed */
         mov.l   @(SH_SLEEP_MODE, r5), r0
         tst     #SUSP_SH_SF, r0
@@ -166,6 +217,47 @@ ENTRY(sh_mobile_sleep_resume_start)
          nop
 
 skip_restore_sf:
+        /* restore mmu and cache state if needed */
+        mov.l   @(SH_SLEEP_MODE, r5), r0
+        tst     #SUSP_SH_MMU, r0
+        bt      skip_restore_mmu
+
+        /* restore mmu state */
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_PTEH, r0
+
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_PTEL, r0
+
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_TTB, r0
+
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_TEA, r0
+
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_PTEA, r0
+
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_PASCR, r0
+
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_IRMCR, r0
+
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_MMUCR, r0
+        icbi    @r0
+
+        /* restore cache settings */
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_RAMCR, r0
+        icbi    @r0
+
+        bsr     restore_register
+         mov    #SH_SLEEP_REG_CCR, r0
+        icbi    @r0
+
+skip_restore_mmu:
         rte
          nop