6 files changed, 61 insertions, 38 deletions

diff --git a/arch/arm/mach-imx/Makefile b/arch/arm/mach-imx/Makefile
index 190d57006163..1ca9558723c2 100644
--- a/arch/arm/mach-imx/Makefile
+++ b/arch/arm/mach-imx/Makefile
@@ -8,8 +8,8 @@ obj-$(CONFIG_SOC_IMX25) += clock-imx25.o mm-imx25.o ehci-imx25.o cpu-imx25.o
 obj-$(CONFIG_SOC_IMX27) += cpu-imx27.o pm-imx27.o
 obj-$(CONFIG_SOC_IMX27) += clock-imx27.o mm-imx27.o ehci-imx27.o
 
-obj-$(CONFIG_SOC_IMX31) += mm-imx3.o cpu-imx31.o clock-imx31.o iomux-imx31.o ehci-imx31.o
-obj-$(CONFIG_SOC_IMX35) += mm-imx3.o cpu-imx35.o clock-imx35.o ehci-imx35.o
+obj-$(CONFIG_SOC_IMX31) += mm-imx3.o cpu-imx31.o clock-imx31.o iomux-imx31.o ehci-imx31.o pm-imx3.o
+obj-$(CONFIG_SOC_IMX35) += mm-imx3.o cpu-imx35.o clock-imx35.o ehci-imx35.o pm-imx3.o
 
 obj-$(CONFIG_SOC_IMX5) += cpu-imx5.o mm-imx5.o clock-mx51-mx53.o ehci-imx5.o pm-imx5.o cpu_op-mx51.o
 
diff --git a/arch/arm/mach-imx/cpu-imx5.c b/arch/arm/mach-imx/cpu-imx5.c
index 5e2e7a843860..aa15c517d06e 100644
--- a/arch/arm/mach-imx/cpu-imx5.c
+++ b/arch/arm/mach-imx/cpu-imx5.c
@@ -149,39 +149,3 @@ int mx50_revision(void)
         return mx5_cpu_rev;
 }
 EXPORT_SYMBOL(mx50_revision);
-
-static int __init post_cpu_init(void)
-{
-        unsigned int reg;
-        void __iomem *base;
-
-        if (cpu_is_mx51() || cpu_is_mx53()) {
-                if (cpu_is_mx51())
-                        base = MX51_IO_ADDRESS(MX51_AIPS1_BASE_ADDR);
-                else
-                        base = MX53_IO_ADDRESS(MX53_AIPS1_BASE_ADDR);
-
-                __raw_writel(0x0, base + 0x40);
-                __raw_writel(0x0, base + 0x44);
-                __raw_writel(0x0, base + 0x48);
-                __raw_writel(0x0, base + 0x4C);
-                reg = __raw_readl(base + 0x50) & 0x00FFFFFF;
-                __raw_writel(reg, base + 0x50);
-
-                if (cpu_is_mx51())
-                        base = MX51_IO_ADDRESS(MX51_AIPS2_BASE_ADDR);
-                else
-                        base = MX53_IO_ADDRESS(MX53_AIPS2_BASE_ADDR);
-
-                __raw_writel(0x0, base + 0x40);
-                __raw_writel(0x0, base + 0x44);
-                __raw_writel(0x0, base + 0x48);
-                __raw_writel(0x0, base + 0x4C);
-                reg = __raw_readl(base + 0x50) & 0x00FFFFFF;
-                __raw_writel(reg, base + 0x50);
-        }
-
-        return 0;
-}
-
-postcore_initcall(post_cpu_init);
diff --git a/arch/arm/mach-imx/crmregs-imx3.h b/arch/arm/mach-imx/crmregs-imx3.h
index d7691e2362c1..53141273df45 100644
--- a/arch/arm/mach-imx/crmregs-imx3.h
+++ b/arch/arm/mach-imx/crmregs-imx3.h
@@ -77,6 +77,7 @@ MX31_IO_ADDRESS(MX31_CCM_BASE_ADDR) : MX35_IO_ADDRESS(MX35_CCM_BASE_ADDR))
 #define MXC_CCM_CCMR_SSI2S_MASK                 (0x3 << 21)
 #define MXC_CCM_CCMR_LPM_OFFSET                 14
 #define MXC_CCM_CCMR_LPM_MASK                   (0x3 << 14)
+#define MXC_CCM_CCMR_LPM_WAIT_MX35              (0x1 << 14)
 #define MXC_CCM_CCMR_FIRS_OFFSET                11
 #define MXC_CCM_CCMR_FIRS_MASK                  (0x3 << 11)
 #define MXC_CCM_CCMR_UPE                        (1 << 9)
diff --git a/arch/arm/mach-imx/mm-imx3.c b/arch/arm/mach-imx/mm-imx3.c
index 07699196b461..f8ca96c354f2 100644
--- a/arch/arm/mach-imx/mm-imx3.c
+++ b/arch/arm/mach-imx/mm-imx3.c
@@ -34,6 +34,8 @@ static void imx3_idle(void)
 {
         unsigned long reg = 0;
 
+        mx3_cpu_lp_set(MX3_WAIT);
+
         __asm__ __volatile__(
                 /* disable I and D cache */
                 "mrc p15, 0, %0, c1, c0, 0\n"
@@ -177,6 +179,10 @@ void __init imx31_soc_init(void)
         }
 
         imx_add_imx_sdma("imx31-sdma", MX31_SDMA_BASE_ADDR, MX31_INT_SDMA, &imx31_sdma_pdata);
+
+        imx_set_aips(MX31_IO_ADDRESS(MX31_AIPS1_BASE_ADDR));
+        imx_set_aips(MX31_IO_ADDRESS(MX31_AIPS2_BASE_ADDR));
+
         platform_device_register_simple("imx31-audmux", 0, imx31_audmux_res,
                                         ARRAY_SIZE(imx31_audmux_res));
 }
@@ -267,6 +273,11 @@ void __init imx35_soc_init(void)
         }
 
         imx_add_imx_sdma("imx35-sdma", MX35_SDMA_BASE_ADDR, MX35_INT_SDMA, &imx35_sdma_pdata);
+
+        /* Setup AIPS registers */
+        imx_set_aips(MX35_IO_ADDRESS(MX35_AIPS1_BASE_ADDR));
+        imx_set_aips(MX35_IO_ADDRESS(MX35_AIPS2_BASE_ADDR));
+
         /* i.mx35 has the i.mx31 type audmux */
         platform_device_register_simple("imx31-audmux", 0, imx35_audmux_res,
                                         ARRAY_SIZE(imx35_audmux_res));
diff --git a/arch/arm/mach-imx/mm-imx5.c b/arch/arm/mach-imx/mm-imx5.c
index dc7c4ed81531..51af9fa56944 100644
--- a/arch/arm/mach-imx/mm-imx5.c
+++ b/arch/arm/mach-imx/mm-imx5.c
@@ -201,6 +201,11 @@ void __init imx51_soc_init(void)
 
         /* i.mx51 has the i.mx35 type sdma */
         imx_add_imx_sdma("imx35-sdma", MX51_SDMA_BASE_ADDR, MX51_INT_SDMA, &imx51_sdma_pdata);
+
+        /* Setup AIPS registers */
+        imx_set_aips(MX51_IO_ADDRESS(MX51_AIPS1_BASE_ADDR));
+        imx_set_aips(MX51_IO_ADDRESS(MX51_AIPS2_BASE_ADDR));
+
         /* i.mx51 has the i.mx31 type audmux */
         platform_device_register_simple("imx31-audmux", 0, imx51_audmux_res,
                                         ARRAY_SIZE(imx51_audmux_res));
@@ -219,6 +224,11 @@ void __init imx53_soc_init(void)
 
         /* i.mx53 has the i.mx35 type sdma */
         imx_add_imx_sdma("imx35-sdma", MX53_SDMA_BASE_ADDR, MX53_INT_SDMA, &imx53_sdma_pdata);
+
+        /* Setup AIPS registers */
+        imx_set_aips(MX53_IO_ADDRESS(MX53_AIPS1_BASE_ADDR));
+        imx_set_aips(MX53_IO_ADDRESS(MX53_AIPS2_BASE_ADDR));
+
         /* i.mx53 has the i.mx31 type audmux */
         platform_device_register_simple("imx31-audmux", 0, imx53_audmux_res,
                                         ARRAY_SIZE(imx53_audmux_res));
diff --git a/arch/arm/mach-imx/pm-imx3.c b/arch/arm/mach-imx/pm-imx3.c
new file mode 100644
index 000000000000..b3752439632e
--- /dev/null
+++ b/arch/arm/mach-imx/pm-imx3.c
@@ -0,0 +1,37 @@
+/*
+ *  Copyright (C) 2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/io.h>
+#include <mach/common.h>
+#include <mach/hardware.h>
+#include <mach/devices-common.h>
+#include "crmregs-imx3.h"
+
+/*
+ * Set cpu low power mode before WFI instruction. This function is called
+ * mx3 because it can be used for mx31 and mx35.
+ * Currently only WAIT_MODE is supported.
+ */
+void mx3_cpu_lp_set(enum mx3_cpu_pwr_mode mode)
+{
+        int reg = __raw_readl(MXC_CCM_CCMR);
+        reg &= ~MXC_CCM_CCMR_LPM_MASK;
+
+        switch (mode) {
+        case MX3_WAIT:
+                if (cpu_is_mx35())
+                        reg |= MXC_CCM_CCMR_LPM_WAIT_MX35;
+                __raw_writel(reg, MXC_CCM_CCMR);
+                break;
+        default:
+                pr_err("Unknown cpu power mode: %d\n", mode);
+                return;
+        }
+}