6 files changed, 345 insertions, 0 deletions

diff --git a/arch/arm/mach-shmobile/Makefile b/arch/arm/mach-shmobile/Makefile
index a3cad5360a71..f443c5860995 100644
--- a/arch/arm/mach-shmobile/Makefile
+++ b/arch/arm/mach-shmobile/Makefile
@@ -32,6 +32,7 @@ obj-$(CONFIG_ARCH_SH73A0)	+= entry-gic.o
 
 # PM objects
 obj-$(CONFIG_SUSPEND)           += suspend.o
+obj-$(CONFIG_ARCH_SH7372)       += pm-sh7372.o sleep-sh7372.o
 
 # Board objects
 obj-$(CONFIG_MACH_G3EVM)        += board-g3evm.o
diff --git a/arch/arm/mach-shmobile/board-ap4evb.c b/arch/arm/mach-shmobile/board-ap4evb.c
index d82d5369e36d..08acb6ec8139 100644
--- a/arch/arm/mach-shmobile/board-ap4evb.c
+++ b/arch/arm/mach-shmobile/board-ap4evb.c
@@ -1354,6 +1354,7 @@ static void __init ap4evb_init(void)
 
         hdmi_init_pm_clock();
         fsi_init_pm_clock();
+        sh7372_pm_init();
 }
 
 static void __init ap4evb_timer_init(void)
diff --git a/arch/arm/mach-shmobile/board-mackerel.c b/arch/arm/mach-shmobile/board-mackerel.c
index efceb1d4d17a..0d882754bff5 100644
--- a/arch/arm/mach-shmobile/board-mackerel.c
+++ b/arch/arm/mach-shmobile/board-mackerel.c
@@ -1230,6 +1230,7 @@ static void __init mackerel_init(void)
         platform_add_devices(mackerel_devices, ARRAY_SIZE(mackerel_devices));
 
         hdmi_init_pm_clock();
+        sh7372_pm_init();
 }
 
 static void __init mackerel_timer_init(void)
diff --git a/arch/arm/mach-shmobile/include/mach/common.h b/arch/arm/mach-shmobile/include/mach/common.h
index 39b78bb80e09..4b653e92d72e 100644
--- a/arch/arm/mach-shmobile/include/mach/common.h
+++ b/arch/arm/mach-shmobile/include/mach/common.h
@@ -31,6 +31,9 @@ extern void sh7372_add_early_devices(void);
 extern void sh7372_add_standard_devices(void);
 extern void sh7372_clock_init(void);
 extern void sh7372_pinmux_init(void);
+extern void sh7372_pm_init(void);
+extern void sh7372_cpu_suspend(void);
+extern void sh7372_cpu_resume(void);
 extern struct clk sh7372_extal1_clk;
 extern struct clk sh7372_extal2_clk;
 
diff --git a/arch/arm/mach-shmobile/pm-sh7372.c b/arch/arm/mach-shmobile/pm-sh7372.c
new file mode 100644
index 000000000000..92c374d411d5
--- /dev/null
+++ b/arch/arm/mach-shmobile/pm-sh7372.c
@@ -0,0 +1,79 @@
+/*
+ * sh7372 Power management support
+ *
+ *  Copyright (C) 2011 Magnus Damm
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/pm.h>
+#include <linux/suspend.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <asm/system.h>
+#include <asm/io.h>
+#include <asm/tlbflush.h>
+#include <mach/common.h>
+
+#define SMFRAM 0xe6a70000
+#define SYSTBCR 0xe6150024
+#define SBAR 0xe6180020
+#define APARMBAREA 0xe6f10020
+
+#ifdef CONFIG_SUSPEND
+static void sh7372_enter_core_standby(void)
+{
+        void __iomem *smfram = (void __iomem *)SMFRAM;
+
+        __raw_writel(0, APARMBAREA); /* translate 4k */
+        __raw_writel(__pa(sh7372_cpu_resume), SBAR); /* set reset vector */
+        __raw_writel(0x10, SYSTBCR); /* enable core standby */
+
+        __raw_writel(0, smfram + 0x3c); /* clear page table address */
+
+        sh7372_cpu_suspend();
+        cpu_init();
+
+        /* if page table address is non-NULL then we have been powered down */
+        if (__raw_readl(smfram + 0x3c)) {
+                __raw_writel(__raw_readl(smfram + 0x40),
+                             __va(__raw_readl(smfram + 0x3c)));
+
+                flush_tlb_all();
+                set_cr(__raw_readl(smfram + 0x38));
+        }
+
+        __raw_writel(0, SYSTBCR); /* disable core standby */
+        __raw_writel(0, SBAR); /* disable reset vector translation */
+}
+
+static int sh7372_enter_suspend(suspend_state_t suspend_state)
+{
+        sh7372_enter_core_standby();
+        return 0;
+}
+
+static void sh7372_suspend_init(void)
+{
+        shmobile_suspend_ops.enter = sh7372_enter_suspend;
+}
+#else
+static void sh7372_suspend_init(void) {}
+#endif
+
+#define DBGREG1 0xe6100020
+#define DBGREG9 0xe6100040
+
+void __init sh7372_pm_init(void)
+{
+        /* enable DBG hardware block to kick SYSC */
+        __raw_writel(0x0000a500, DBGREG9);
+        __raw_writel(0x0000a501, DBGREG9);
+        __raw_writel(0x00000000, DBGREG1);
+
+        sh7372_suspend_init();
+}
diff --git a/arch/arm/mach-shmobile/sleep-sh7372.S b/arch/arm/mach-shmobile/sleep-sh7372.S
new file mode 100644
index 000000000000..d37d3ca4d18f
--- /dev/null
+++ b/arch/arm/mach-shmobile/sleep-sh7372.S
@@ -0,0 +1,260 @@
+/*
+ * sh7372 lowlevel sleep code for "Core Standby Mode"
+ *
+ * Copyright (C) 2011 Magnus Damm
+ *
+ * In "Core Standby Mode" the ARM core is off, but L2 cache is still on
+ *
+ * Based on mach-omap2/sleep34xx.S
+ *
+ * (C) Copyright 2007 Texas Instruments
+ * Karthik Dasu <karthik-dp@ti.com>
+ *
+ * (C) Copyright 2004 Texas Instruments, <www.ti.com>
+ * Richard Woodruff <r-woodruff2@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR /PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+#define SMFRAM 0xe6a70000
+
+        .align
+kernel_flush:
+        .word   v7_flush_dcache_all
+
+        .align  3
+ENTRY(sh7372_cpu_suspend)
+        stmfd   sp!, {r0-r12, lr}       @ save registers on stack
+
+        ldr     r8, =SMFRAM
+
+        mov     r4, sp                  @ Store sp
+        mrs     r5, spsr                @ Store spsr
+        mov     r6, lr                  @ Store lr
+        stmia   r8!, {r4-r6}
+
+        mrc     p15, 0, r4, c1, c0, 2   @ Coprocessor access control register
+        mrc     p15, 0, r5, c2, c0, 0   @ TTBR0
+        mrc     p15, 0, r6, c2, c0, 1   @ TTBR1
+        mrc     p15, 0, r7, c2, c0, 2   @ TTBCR
+        stmia   r8!, {r4-r7}
+
+        mrc     p15, 0, r4, c3, c0, 0   @ Domain access Control Register
+        mrc     p15, 0, r5, c10, c2, 0  @ PRRR
+        mrc     p15, 0, r6, c10, c2, 1  @ NMRR
+        stmia   r8!,{r4-r6}
+
+        mrc     p15, 0, r4, c13, c0, 1  @ Context ID
+        mrc     p15, 0, r5, c13, c0, 2  @ User r/w thread and process ID
+        mrc     p15, 0, r6, c12, c0, 0  @ Secure or NS vector base address
+        mrs     r7, cpsr                @ Store current cpsr
+        stmia   r8!, {r4-r7}
+
+        mrc     p15, 0, r4, c1, c0, 0   @ save control register
+        stmia   r8!, {r4}
+
+        /*
+         * jump out to kernel flush routine
+         *  - reuse that code is better
+         *  - it executes in a cached space so is faster than refetch per-block
+         *  - should be faster and will change with kernel
+         *  - 'might' have to copy address, load and jump to it
+         * Flush all data from the L1 data cache before disabling
+         * SCTLR.C bit.
+         */
+        ldr     r1, kernel_flush
+        mov     lr, pc
+        bx      r1
+
+        /*
+         * Clear the SCTLR.C bit to prevent further data cache
+         * allocation. Clearing SCTLR.C would make all the data accesses
+         * strongly ordered and would not hit the cache.
+         */
+        mrc     p15, 0, r0, c1, c0, 0
+        bic     r0, r0, #(1 << 2)       @ Disable the C bit
+        mcr     p15, 0, r0, c1, c0, 0
+        isb
+
+        /*
+         * Invalidate L1 data cache. Even though only invalidate is
+         * necessary exported flush API is used here. Doing clean
+         * on already clean cache would be almost NOP.
+         */
+        ldr     r1, kernel_flush
+        blx     r1
+        /*
+         * The kernel doesn't interwork: v7_flush_dcache_all in particluar will
+         * always return in Thumb state when CONFIG_THUMB2_KERNEL is enabled.
+         * This sequence switches back to ARM.  Note that .align may insert a
+         * nop: bx pc needs to be word-aligned in order to work.
+         */
+ THUMB( .thumb          )
+ THUMB( .align          )
+ THUMB( bx      pc      )
+ THUMB( nop             )
+        .arm
+
+        /* Data memory barrier and Data sync barrier */
+        dsb
+        dmb
+
+/*
+ * ===================================
+ * == WFI instruction => Enter idle ==
+ * ===================================
+ */
+        wfi                             @ wait for interrupt
+
+/*
+ * ===================================
+ * == Resume path for non-OFF modes ==
+ * ===================================
+ */
+        mrc     p15, 0, r0, c1, c0, 0
+        tst     r0, #(1 << 2)           @ Check C bit enabled?
+        orreq   r0, r0, #(1 << 2)       @ Enable the C bit if cleared
+        mcreq   p15, 0, r0, c1, c0, 0
+        isb
+
+/*
+ * ===================================
+ * == Exit point from non-OFF modes ==
+ * ===================================
+ */
+        ldmfd   sp!, {r0-r12, pc}       @ restore regs and return
+
+        .pool
+
+        .align  12
+        .text
+        .global sh7372_cpu_resume
+sh7372_cpu_resume:
+
+        mov     r1, #0
+        /*
+         * Invalidate all instruction caches to PoU
+         * and flush branch target cache
+         */
+        mcr     p15, 0, r1, c7, c5, 0
+
+        ldr     r3, =SMFRAM
+
+        ldmia   r3!, {r4-r6}
+        mov     sp, r4                  @ Restore sp
+        msr     spsr_cxsf, r5           @ Restore spsr
+        mov     lr, r6                  @ Restore lr
+
+        ldmia   r3!, {r4-r7}
+        mcr     p15, 0, r4, c1, c0, 2   @ Coprocessor access Control Register
+        mcr     p15, 0, r5, c2, c0, 0   @ TTBR0
+        mcr     p15, 0, r6, c2, c0, 1   @ TTBR1
+        mcr     p15, 0, r7, c2, c0, 2   @ TTBCR
+
+        ldmia   r3!,{r4-r6}
+        mcr     p15, 0, r4, c3, c0, 0   @ Domain access Control Register
+        mcr     p15, 0, r5, c10, c2, 0  @ PRRR
+        mcr     p15, 0, r6, c10, c2, 1  @ NMRR
+
+        ldmia   r3!,{r4-r7}
+        mcr     p15, 0, r4, c13, c0, 1  @ Context ID
+        mcr     p15, 0, r5, c13, c0, 2  @ User r/w thread and process ID
+        mrc     p15, 0, r6, c12, c0, 0  @ Secure or NS vector base address
+        msr     cpsr, r7                @ store cpsr
+
+        /* Starting to enable MMU here */
+        mrc     p15, 0, r7, c2, c0, 2   @ Read TTBRControl
+        /* Extract N (0:2) bits and decide whether to use TTBR0 or TTBR1 */
+        and     r7, #0x7
+        cmp     r7, #0x0
+        beq     usettbr0
+ttbr_error:
+        /*
+         * More work needs to be done to support N[0:2] value other than 0
+         * So looping here so that the error can be detected
+         */
+        b       ttbr_error
+
+        .align
+cache_pred_disable_mask:
+        .word   0xFFFFE7FB
+ttbrbit_mask:
+        .word   0xFFFFC000
+table_index_mask:
+        .word   0xFFF00000
+table_entry:
+        .word   0x00000C02
+usettbr0:
+
+        mrc     p15, 0, r2, c2, c0, 0
+        ldr     r5, ttbrbit_mask
+        and     r2, r5
+        mov     r4, pc
+        ldr     r5, table_index_mask
+        and     r4, r5                  @ r4 = 31 to 20 bits of pc
+        /* Extract the value to be written to table entry */
+        ldr     r6, table_entry
+        /* r6 has the value to be written to table entry */
+        add     r6, r6, r4
+        /* Getting the address of table entry to modify */
+        lsr     r4, #18
+        /* r2 has the location which needs to be modified */
+        add     r2, r4
+        ldr     r4, [r2]
+        str     r6, [r2] /* modify the table entry */
+
+        mov     r7, r6
+        mov     r5, r2
+        mov     r6, r4
+        /* r5 = original page table address */
+        /* r6 = original page table data */
+
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 4   @ Flush prefetch buffer
+        mcr     p15, 0, r0, c7, c5, 6   @ Invalidate branch predictor array
+        mcr     p15, 0, r0, c8, c5, 0   @ Invalidate instruction TLB
+        mcr     p15, 0, r0, c8, c6, 0   @ Invalidate data TLB
+
+        /*
+         * Restore control register. This enables the MMU.
+         * The caches and prediction are not enabled here, they
+         * will be enabled after restoring the MMU table entry.
+         */
+        ldmia   r3!, {r4}
+        stmia   r3!, {r5} /* save original page table address */
+        stmia   r3!, {r6} /* save original page table data */
+        stmia   r3!, {r7} /* save modified page table data */
+
+        ldr     r2, cache_pred_disable_mask
+        and     r4, r2
+        mcr     p15, 0, r4, c1, c0, 0
+        dsb
+        isb
+
+        ldr     r0, =restoremmu_on
+        bx      r0
+
+/*
+ * ==============================
+ * == Exit point from OFF mode ==
+ * ==============================
+ */
+restoremmu_on:
+
+        ldmfd   sp!, {r0-r12, pc}       @ restore regs and return