PMIC: Add DA9053 Driver for LOCO board

Add DA9053 init in LOCO.
Modify Makefile for DA9053 driver.

Signed-off-by: Ying-Chun Liu (PaulLiu) <paul.liu@linaro.org>

8 files changed, 767 insertions, 3 deletions

diff --git a/arch/arm/mach-mx5/Makefile b/arch/arm/mach-mx5/Makefile
index 8c9b1b6b41f..6f49252d0a6 100644
--- a/arch/arm/mach-mx5/Makefile
+++ b/arch/arm/mach-mx5/Makefile
@@ -3,7 +3,7 @@
 #
 
 # Object file lists.
-obj-y   := cpu.o mm.o devices.o ehci.o system.o bus_freq.o sdram_autogating.o
+obj-y   := cpu.o mm.o devices.o ehci.o system.o bus_freq.o sdram_autogating.o suspend.o
 obj-$(CONFIG_SOC_IMX50) += mm-mx50.o
 obj-$(CONFIG_SOC_IMX51)	+= clock.o
 obj-$(CONFIG_SOC_IMX53)	+= clock.o
@@ -14,7 +14,7 @@ obj-$(CONFIG_MACH_MX51_BABBAGE) += board-mx51_babbage.o
 obj-$(CONFIG_MACH_MX51_3DS) += board-mx51_3ds.o
 obj-$(CONFIG_MACH_MX53_EVK) += board-mx53_evk.o
 obj-$(CONFIG_MACH_MX53_SMD) += board-mx53_smd.o
-obj-$(CONFIG_MACH_MX53_LOCO) += board-mx53_loco.o
+obj-$(CONFIG_MACH_MX53_LOCO) += board-mx53_loco.o mx53_loco_pmic_da9053.o
 obj-$(CONFIG_MACH_MX53_ARD) += board-mx53_ard.o
 obj-$(CONFIG_MACH_EUKREA_CPUIMX51) += board-cpuimx51.o
 obj-$(CONFIG_MACH_EUKREA_MBIMX51_BASEBOARD) += eukrea_mbimx51-baseboard.o
diff --git a/arch/arm/mach-mx5/board-mx53_loco.c b/arch/arm/mach-mx5/board-mx53_loco.c
index 5d1c377bfb0..c5c33845d6d 100644
--- a/arch/arm/mach-mx5/board-mx53_loco.c
+++ b/arch/arm/mach-mx5/board-mx53_loco.c
@@ -22,6 +22,7 @@
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
+#include <linux/fsl_devices.h>
 
 #include <mach/common.h>
 #include <mach/hardware.h>
@@ -33,6 +34,7 @@
 
 #include "crm_regs.h"
 #include "devices-imx53.h"
+#include "devices.h"
 
 #define MX53_LOCO_POWER			IMX_GPIO_NR(1, 8)
 #define MX53_LOCO_UI1			IMX_GPIO_NR(2, 14)
@@ -295,6 +297,26 @@ static struct mxc_iim_platform_data iim_data = {
 	.disable_fuse = mxc_iim_disable_fuse,
 };
 
+static void loco_suspend_enter(void)
+{
+	/* da9053 suspend preparation */
+}
+
+static void loco_suspend_exit(void)
+{
+	/*clear the EMPGC0/1 bits */
+	__raw_writel(0, MXC_SRPG_EMPGC0_SRPGCR);
+	__raw_writel(0, MXC_SRPG_EMPGC1_SRPGCR);
+	/* da9053 resmue resore */
+}
+
+static struct mxc_pm_platform_data loco_pm_data = {
+        .suspend_enter = loco_suspend_enter,
+        .suspend_exit = loco_suspend_exit,
+};
+
+extern int __init mx53_loco_init_da9052(void);
+
 static void __init mx53_loco_board_init(void)
 {
 	imx53_soc_init();
@@ -319,6 +341,8 @@ static void __init mx53_loco_board_init(void)
 				ARRAY_SIZE(mxc_i2c0_board_info));
 
 	imx53_add_ahci_imx(0, NULL);
+	mxc_register_device(&mxc_pm_device, &loco_pm_data);
+	mx53_loco_init_da9052();
 }
 
 static void __init mx53_loco_timer_init(void)
diff --git a/arch/arm/mach-mx5/devices.c b/arch/arm/mach-mx5/devices.c
index 5fd8f0c7c84..a8d94fbccf5 100644
--- a/arch/arm/mach-mx5/devices.c
+++ b/arch/arm/mach-mx5/devices.c
@@ -118,3 +118,8 @@ struct platform_device mxc_usbh2_device = {
 		.coherent_dma_mask = DMA_BIT_MASK(32),
 	},
 };
+
+struct platform_device mxc_pm_device = {
+  .name = "mx5_pm",
+  .id = 0,
+};
diff --git a/arch/arm/mach-mx5/devices.h b/arch/arm/mach-mx5/devices.h
index 55a5129bc29..02e83ea9962 100644
--- a/arch/arm/mach-mx5/devices.h
+++ b/arch/arm/mach-mx5/devices.h
@@ -3,3 +3,4 @@ extern struct platform_device mxc_usbh1_device;
 extern struct platform_device mxc_usbh2_device;
 extern struct platform_device mxc_usbdr_udc_device;
 extern struct platform_device mxc_hsi2c_device;
+extern struct platform_device mxc_pm_device;
diff --git a/arch/arm/mach-mx5/mx53_loco_pmic_da9053.c b/arch/arm/mach-mx5/mx53_loco_pmic_da9053.c
new file mode 100644
index 00000000000..98f62f96c4a
--- /dev/null
+++ b/arch/arm/mach-mx5/mx53_loco_pmic_da9053.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (C) 2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+
+/*
+ * mx53_loco_pmic_da9053.c  --  i.MX53 LOCO driver for pmic da9053
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/fixed.h>
+#include <linux/mfd/da9052/da9052.h>
+#include <linux/mfd/da9052/pm.h>
+#include <linux/mfd/da9052/led.h>
+#include <linux/mfd/da9052/tsi.h>
+#include <mach/irqs.h>
+#include <mach/iomux-mx53.h>
+#include <mach/gpio.h>
+
+#define DA9052_LDO(max, min, rname, suspend_mv) \
+{\
+	.constraints = {\
+		.name		= (rname), \
+		.max_uV		= (max) * 1000,\
+		.min_uV		= (min) * 1000,\
+		.valid_ops_mask	= REGULATOR_CHANGE_VOLTAGE\
+		|REGULATOR_CHANGE_STATUS | REGULATOR_CHANGE_MODE,\
+		.valid_modes_mask = REGULATOR_MODE_NORMAL,\
+		.state_mem = { \
+			.uV = suspend_mv * 1000, \
+			.mode = REGULATOR_MODE_NORMAL, \
+			.enabled = (0 == suspend_mv) ? 0 : 1, \
+			.disabled = 0, \
+		}, \
+	},\
+}
+
+#ifdef CONFIG_SND_SOC_SGTL5000
+
+static struct regulator_consumer_supply sgtl5000_consumer[] = {
+	REGULATOR_SUPPLY("VDDA", NULL),
+	REGULATOR_SUPPLY("VDDIO", NULL),
+};
+
+static struct regulator_init_data sgtl5000_reg_initdata = {
+	.constraints = {
+		.valid_ops_mask = REGULATOR_CHANGE_STATUS,
+	},
+	.num_consumer_supplies = ARRAY_SIZE(sgtl5000_consumer),
+	.consumer_supplies = &sgtl5000_consumer[0],
+};
+
+static struct fixed_voltage_config sgtl5000_reg_config = {
+	.supply_name		= "VDDA",
+	.microvolts		= 3200000,
+	.gpio			= -1,
+	.init_data		= &sgtl5000_reg_initdata,
+};
+
+static struct platform_device sgtl5000_reg_devices = {
+
+	.name	= "reg-fixed-voltage",
+	.id	= 1,
+	.dev	= {
+		.platform_data = &sgtl5000_reg_config,
+	},
+};
+#endif /* CONFIG_SND_SOC_SGTL5000 */
+
+/* currently the suspend_mv here takes no effects for DA9053
+preset-voltage have to be done in the latest stage during
+suspend*/
+static struct regulator_init_data da9052_regulators_init[] = {
+	DA9052_LDO(DA9052_LDO1_VOLT_UPPER,
+		DA9052_LDO1_VOLT_LOWER, "DA9052_LDO1", 1300),
+	DA9052_LDO(DA9052_LDO2_VOLT_UPPER,
+		DA9052_LDO2_VOLT_LOWER, "DA9052_LDO2", 1300),
+	DA9052_LDO(DA9052_LDO34_VOLT_UPPER,
+		DA9052_LDO34_VOLT_LOWER, "DA9052_LDO3", 3300),
+	DA9052_LDO(DA9052_LDO34_VOLT_UPPER,
+		DA9052_LDO34_VOLT_LOWER, "DA9052_LDO4", 2775),
+	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO5", 1300),
+	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO6", 1200),
+	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO7", 2750),
+	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO8", 1800),
+	DA9052_LDO(DA9052_LDO9_VOLT_UPPER,
+		DA9052_LDO9_VOLT_LOWER, "DA9052_LDO9", 2500),
+	DA9052_LDO(DA9052_LDO567810_VOLT_UPPER,
+		DA9052_LDO567810_VOLT_LOWER, "DA9052_LDO10", 1200),
+
+	/* BUCKS */
+	DA9052_LDO(DA9052_BUCK_CORE_PRO_VOLT_UPPER,
+		DA9052_BUCK_CORE_PRO_VOLT_LOWER, "DA9052_BUCK_CORE", 850),
+	DA9052_LDO(DA9052_BUCK_CORE_PRO_VOLT_UPPER,
+		DA9052_BUCK_CORE_PRO_VOLT_LOWER, "DA9052_BUCK_PRO", 950),
+	DA9052_LDO(DA9052_BUCK_MEM_VOLT_UPPER,
+		DA9052_BUCK_MEM_VOLT_LOWER, "DA9052_BUCK_MEM", 1500),
+	DA9052_LDO(DA9052_BUCK_PERI_VOLT_UPPER,
+		DA9052_BUCK_PERI_VOLT_LOWER, "DA9052_BUCK_PERI", 2500)
+};
+
+
+static struct da9052_tsi_platform_data da9052_tsi = {
+	.pen_up_interval = 50,
+	.tsi_delay_bit_shift = 6,
+	.tsi_skip_bit_shift = 3,
+	.num_gpio_tsi_register = 3,
+	.tsi_supply_voltage = 2500,
+	 /* This is the DA9052 LDO number used for powering the TSI */
+	.tsi_ref_source = 9,
+	.max_tsi_delay = TSI_DELAY_4SLOTS,
+	.max_tsi_skip_slot = TSI_SKIP_330SLOTS,
+};
+
+static struct da9052_led_platform_data da9052_gpio_led[] = {
+	{
+		.id = DA9052_LED_4,
+		.name = "LED_GPIO14",
+	},
+	{
+		.id = DA9052_LED_5,
+		.name = "LED_GPIO15",
+	},
+};
+
+static struct da9052_leds_platform_data da9052_gpio_leds = {
+	.num_leds = ARRAY_SIZE(da9052_gpio_led),
+	.led = da9052_gpio_led,
+};
+
+
+static void da9052_init_ssc_cache(struct da9052 *da9052)
+{
+	unsigned char cnt;
+
+	/* First initialize all registers as Non-volatile */
+	for (cnt = 0; cnt < DA9052_REG_CNT; cnt++) {
+		da9052->ssc_cache[cnt].type = NON_VOLATILE;
+		da9052->ssc_cache[cnt].status = INVALID;
+		da9052->ssc_cache[cnt].val = 0;
+	}
+
+	/* Now selectively set type for all Volatile registers */
+	/* Reg 1 - 9 */
+	da9052->ssc_cache[DA9052_STATUSA_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_STATUSB_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_STATUSC_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_STATUSD_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_EVENTA_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_EVENTB_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_EVENTC_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_EVENTD_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_FAULTLOG_REG].type = VOLATILE;
+
+	/* Reg 15 */
+	da9052->ssc_cache[DA9052_CONTROLB_REG].type = VOLATILE;
+	/* Reg - 17 */
+	da9052->ssc_cache[DA9052_CONTROLD_REG].type = VOLATILE;
+	/* Reg - 60 */
+	da9052->ssc_cache[DA9052_SUPPLY_REG].type = VOLATILE;
+	/* Reg - 62 */
+	da9052->ssc_cache[DA9052_CHGBUCK_REG].type = VOLATILE;
+
+	/* Reg 67 - 68 */
+	da9052->ssc_cache[DA9052_INPUTCONT_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_CHGTIME_REG].type = VOLATILE;
+
+	/* Reg - 70 */
+	da9052->ssc_cache[DA9052_BOOST_REG].type = VOLATILE;
+
+	/* Reg - 81 */
+	da9052->ssc_cache[DA9052_ADCMAN_REG].type = VOLATILE;
+
+	/* Reg - 83 - 85 */
+	da9052->ssc_cache[DA9052_ADCRESL_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_ADCRESH_REG].type = VOLATILE;
+	da9052->ssc_cache[DA9052_VDDRES_REG].type = VOLATILE;
+
+	/* Reg - 87 */
+	da9052->ssc_cache[DA9052_ICHGAV_REG].type = VOLATILE;
+
+	/* Reg - 90 */
+	da9052->ssc_cache[DA9052_TBATRES_REG].type = VOLATILE;
+
+	/* Reg - 95 */
+	da9052->ssc_cache[DA9052_ADCIN4RES_REG].type = VOLATILE;
+
+	/* Reg - 98 */
+	da9052->ssc_cache[DA9052_ADCIN5RES_REG].type = VOLATILE;
+
+	/* Reg - 101 */
+	da9052->ssc_cache[DA9052_ADCIN6RES_REG].type = VOLATILE;
+
+	/* Reg - 104 */
+	da9052->ssc_cache[DA9052_TJUNCRES_REG].type = VOLATILE;
+
+	/* Reg 106 - 110 */
+	da9052->ssc_cache[DA9052_TSICONTB_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_TSIXMSB_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_TSIYMSB_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_TSILSB_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_TSIZMSB_REG].type	= VOLATILE;
+
+	/* Reg 111 - 117 */
+	da9052->ssc_cache[DA9052_COUNTS_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_COUNTMI_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_COUNTH_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_COUNTD_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_COUNTMO_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_COUNTY_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_ALARMMI_REG].type	= VOLATILE;
+
+	/* Reg 122 - 125 */
+	da9052->ssc_cache[DA9052_SECONDA_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_SECONDB_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_SECONDC_REG].type	= VOLATILE;
+	da9052->ssc_cache[DA9052_SECONDD_REG].type	= VOLATILE;
+
+	/* Following addresses are not assigned to any register */
+	da9052->ssc_cache[126].type			= VOLATILE;
+	da9052->ssc_cache[127].type			= VOLATILE;
+}
+
+
+#define MX53_LOCO_DA9052_IRQ			(6*32 + 11)	/* GPIO7_11 */
+
+static int __init loco_da9052_init(struct da9052 *da9052)
+{
+	/* Configuring for DA9052 interrupt servce */
+	/* s3c_gpio_setpull(DA9052_IRQ_PIN, S3C_GPIO_PULL_UP);*/
+
+	/* Set interrupt as LOW LEVEL interrupt source */
+	irq_set_irq_type(gpio_to_irq(MX53_LOCO_DA9052_IRQ), IRQF_TRIGGER_LOW);
+
+	da9052_init_ssc_cache(da9052);
+
+#ifdef CONFIG_SND_SOC_SGTL5000
+	platform_device_register(&sgtl5000_reg_devices);
+#endif
+
+	return 0;
+}
+
+static struct da9052_platform_data __initdata da9052_plat = {
+	.init = loco_da9052_init,
+	.num_regulators = ARRAY_SIZE(da9052_regulators_init),
+	.regulators = da9052_regulators_init,
+	.led_data = &da9052_gpio_leds,
+	.tsi_data = &da9052_tsi,
+	/* .bat_data = &da9052_bat, */
+	/* .gpio_base = GPIO_BOARD_START, */
+};
+
+
+static struct i2c_board_info __initdata da9052_i2c_device = {
+	I2C_BOARD_INFO(DA9052_SSC_I2C_DEVICE_NAME, DA9052_I2C_ADDR >> 1),
+	.irq = gpio_to_irq(MX53_LOCO_DA9052_IRQ),
+	.platform_data = &da9052_plat,
+};
+
+int __init mx53_loco_init_da9052(void)
+{
+	return i2c_register_board_info(0, &da9052_i2c_device, 1);
+}
diff --git a/arch/arm/mach-mx5/pm.c b/arch/arm/mach-mx5/pm.c
new file mode 100644
index 00000000000..8d671ad2b0f
--- /dev/null
+++ b/arch/arm/mach-mx5/pm.c
@@ -0,0 +1,283 @@
+/*
+ *  Copyright (C) 2008-2011 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/module.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/i2c.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/suspend.h>
+#include <linux/regulator/machine.h>
+#include <linux/proc_fs.h>
+#include <linux/cpufreq.h>
+#include <mach/iram.h>
+#include <linux/fsl_devices.h>
+#include <asm/mach-types.h>
+#include <asm/cacheflush.h>
+#include <asm/tlb.h>
+#include <asm/delay.h>
+#include <asm/mach/map.h>
+#include <mach/hardware.h>
+#include <mach/gpio.h>
+#ifdef CONFIG_ARCH_MX50
+#include <mach/iomux-mx50.h>
+#endif
+#include "crm_regs.h"
+
+#define DATABAHN_CTL_REG0	0
+#define DATABAHN_CTL_REG19	0x4c
+#define DATABAHN_CTL_REG79	0x13c
+#define DATABAHN_PHY_REG25	0x264
+#define MX53_OFFSET 0x20000000
+
+static struct cpu_op *cpu_op_tbl;
+static int cpu_op_nr;
+static struct clk *cpu_clk;
+static struct mxc_pm_platform_data *pm_data;
+
+#if defined(CONFIG_CPU_FREQ)
+static int org_freq;
+extern int set_cpu_freq(int wp);
+#endif
+
+
+static struct device *pm_dev;
+struct clk *gpc_dvfs_clk;
+extern void cpu_do_suspend_workaround(u32 sdclk_iomux_addr);
+extern void mx50_suspend(u32 databahn_addr);
+extern struct cpu_op *(*get_cpu_op)(int *wp);
+extern void __iomem *databahn_base;
+extern void da9053_suspend_cmd(void);
+extern void da9053_resume_dump(void);
+extern void pm_da9053_i2c_init(u32 base_addr);
+
+extern int iram_ready;
+void *suspend_iram_base;
+void (*suspend_in_iram)(void *sdclk_iomux_addr) = NULL;
+void __iomem *suspend_param1;
+
+#define TZIC_WAKEUP0_OFFSET            0x0E00
+#define TZIC_WAKEUP1_OFFSET            0x0E04
+#define TZIC_WAKEUP2_OFFSET            0x0E08
+#define TZIC_WAKEUP3_OFFSET            0x0E0C
+#define GPIO7_0_11_IRQ_BIT		(0x1<<11)
+
+static void mx53_smd_loco_irq_wake_fixup(void)
+{
+	void __iomem *tzic_base;
+	tzic_base = ioremap(MX53_TZIC_BASE_ADDR, SZ_4K);
+	if (NULL == tzic_base) {
+		pr_err("fail to map MX53_TZIC_BASE_ADDR\n");
+		return;
+	}
+	__raw_writel(0, tzic_base + TZIC_WAKEUP0_OFFSET);
+	__raw_writel(0, tzic_base + TZIC_WAKEUP1_OFFSET);
+	__raw_writel(0, tzic_base + TZIC_WAKEUP2_OFFSET);
+	/* only enable irq wakeup for da9053 */
+	__raw_writel(GPIO7_0_11_IRQ_BIT, tzic_base + TZIC_WAKEUP3_OFFSET);
+	iounmap(tzic_base);
+	pr_debug("only da9053 irq is wakeup-enabled\n");
+}
+
+extern void mx5_cpu_lp_set(enum mxc_cpu_pwr_mode mode);
+
+static int mx5_suspend_enter(suspend_state_t state)
+{
+	if (gpc_dvfs_clk == NULL)
+		gpc_dvfs_clk = clk_get(NULL, "gpc_dvfs_clk");
+	/* gpc clock is needed for SRPG */
+	clk_enable(gpc_dvfs_clk);
+	switch (state) {
+	case PM_SUSPEND_MEM:
+		mx5_cpu_lp_set(STOP_POWER_OFF);
+		break;
+	case PM_SUSPEND_STANDBY:
+		mx5_cpu_lp_set(WAIT_UNCLOCKED_POWER_OFF);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (tzic_enable_wake(0) != 0)
+		return -EAGAIN;
+
+	if (state == PM_SUSPEND_MEM) {
+		local_flush_tlb_all();
+		flush_cache_all();
+
+		if (pm_data && pm_data->suspend_enter)
+			pm_data->suspend_enter();
+
+		suspend_in_iram(suspend_param1);
+
+		if (pm_data && pm_data->suspend_exit)
+			pm_data->suspend_exit();
+	} else {
+			cpu_do_idle();
+	}
+	clk_disable(gpc_dvfs_clk);
+
+	return 0;
+}
+
+
+/*
+ * Called after processes are frozen, but before we shut down devices.
+ */
+static int mx5_suspend_prepare(void)
+{
+#if defined(CONFIG_CPU_FREQ)
+#define MX53_SUSPEND_CPU_WP 1000000000
+	struct cpufreq_freqs freqs;
+	u32 suspend_wp = 0;
+	org_freq = clk_get_rate(cpu_clk);
+	/* workaround for mx53 to suspend on 400MHZ wp */
+	if (cpu_is_mx53())
+		for (suspend_wp = 0; suspend_wp < cpu_op_nr; suspend_wp++)
+			if (cpu_op_tbl[suspend_wp].cpu_rate
+				== MX53_SUSPEND_CPU_WP)
+				break;
+	if (suspend_wp == cpu_op_nr)
+		suspend_wp = 0;
+	pr_info("suspend wp cpu=%d\n", cpu_op_tbl[suspend_wp].cpu_rate);
+	freqs.old = org_freq / 1000;
+	freqs.new = cpu_op_tbl[suspend_wp].cpu_rate / 1000;
+	freqs.cpu = 0;
+	freqs.flags = 0;
+
+	if (clk_get_rate(cpu_clk) != cpu_op_tbl[suspend_wp].cpu_rate) {
+		set_cpu_freq(cpu_op_tbl[suspend_wp].cpu_rate);
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+#endif
+	return 0;
+}
+
+/*
+ * Called before devices are re-setup.
+ */
+static void mx5_suspend_finish(void)
+{
+#if defined(CONFIG_CPU_FREQ)
+	struct cpufreq_freqs freqs;
+
+	freqs.old = clk_get_rate(cpu_clk) / 1000;
+	freqs.new = org_freq / 1000;
+	freqs.cpu = 0;
+	freqs.flags = 0;
+
+
+	if (org_freq != clk_get_rate(cpu_clk)) {
+		set_cpu_freq(org_freq);
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+#endif
+}
+
+/*
+ * Called after devices are re-setup, but before processes are thawed.
+ */
+static void mx5_suspend_end(void)
+{
+}
+
+static int mx5_pm_valid(suspend_state_t state)
+{
+	return (state > PM_SUSPEND_ON && state <= PM_SUSPEND_MAX);
+}
+
+struct platform_suspend_ops mx5_suspend_ops = {
+	.valid = mx5_pm_valid,
+	.prepare = mx5_suspend_prepare,
+	.enter = mx5_suspend_enter,
+	.finish = mx5_suspend_finish,
+	.end = mx5_suspend_end,
+};
+
+static int __devinit mx5_pm_probe(struct platform_device *pdev)
+{
+	pm_dev = &pdev->dev;
+	pm_data = pdev->dev.platform_data;
+
+	return 0;
+}
+
+static struct platform_driver mx5_pm_driver = {
+	.driver = {
+		   .name = "mx5_pm",
+		   },
+	.probe = mx5_pm_probe,
+};
+
+static int __init pm_init(void)
+{
+	unsigned long iram_paddr, cpaddr;
+
+	pr_info("Static Power Management for Freescale i.MX5\n");
+	if (platform_driver_register(&mx5_pm_driver) != 0) {
+		printk(KERN_ERR "mx5_pm_driver register failed\n");
+		return -ENODEV;
+	}
+	suspend_set_ops(&mx5_suspend_ops);
+	/* Move suspend routine into iRAM */
+	cpaddr = iram_alloc(SZ_4K, &iram_paddr);
+	/* Need to remap the area here since we want the memory region
+		 to be executable. */
+	suspend_iram_base = __arm_ioremap(iram_paddr, SZ_4K,
+					  MT_HIGH_VECTORS);
+	pr_info("cpaddr = %x suspend_iram_base=%x\n", cpaddr, suspend_iram_base);
+
+	if (cpu_is_mx51() || cpu_is_mx53()) {
+		suspend_param1 = MX51_IO_ADDRESS(MX51_IOMUXC_BASE_ADDR + 0x4b8);
+		memcpy(cpaddr, cpu_do_suspend_workaround,
+			SZ_4K);
+	} else if (cpu_is_mx50()) {
+		/*
+		 * Need to run the suspend code from IRAM as the DDR needs
+		 * to be put into self refresh mode manually.
+		 */
+		memcpy(cpaddr, mx50_suspend, SZ_4K);
+
+		suspend_param1 = databahn_base;
+	}
+	suspend_in_iram = (void *)suspend_iram_base;
+
+	cpu_op_tbl = get_cpu_op(&cpu_op_nr);
+
+	cpu_clk = clk_get(NULL, "cpu_clk");
+	if (IS_ERR(cpu_clk)) {
+		printk(KERN_DEBUG "%s: failed to get cpu_clk\n", __func__);
+		return PTR_ERR(cpu_clk);
+	}
+	printk(KERN_INFO "PM driver module loaded\n");
+
+	return 0;
+}
+
+
+static void __exit pm_cleanup(void)
+{
+	/* Unregister the device structure */
+	platform_driver_unregister(&mx5_pm_driver);
+}
+
+module_init(pm_init);
+module_exit(pm_cleanup);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("PM driver");
+MODULE_LICENSE("GPL");
diff --git a/arch/arm/mach-mx5/suspend.S b/arch/arm/mach-mx5/suspend.S
new file mode 100644
index 00000000000..c7937ec94d9
--- /dev/null
+++ b/arch/arm/mach-mx5/suspend.S
@@ -0,0 +1,162 @@
+/*
+ * Copyright (C) 2008-2011 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/linkage.h>
+
+#define ARM_CTRL_DCACHE  1 << 2
+#define ARM_CTRL_ICACHE  1 << 12
+#define ARM_AUXCR_L2EN   1 << 1
+
+
+/*
+ *	cpu_do_suspend_workaround()
+ *
+ *	Suspend the processor (eg, wait for interrupt).
+ *
+ *	IRQs are already disabled.
+ */
+ENTRY(cpu_do_suspend_workaround)
+    stmfd   sp!, {r4,r5,r6,r7,r9,r10,r11}     @ Save registers
+
+   mov    r6, r0                       @save iomux address
+    /* Disable L1 caches */
+    mrc     p15, 0, r0, c1, c0, 0    @ R0 = system control reg
+    bic     r0, r0, #ARM_CTRL_ICACHE @ Disable ICache
+    bic     r0, r0, #ARM_CTRL_DCACHE @ Disable DCache
+    mcr     p15, 0, r0, c1, c0, 0    @ Update system control reg
+
+    mrc     p15, 1, r0, c0, c0, 1   @ Read CLIDR
+    ands    r3, r0, #0x7000000       @ Isolate level of coherency
+    mov     r3, r3, lsr #23         @ Cache level value (naturally aligned)
+    beq     FinishedClean
+    mov     r10, #0
+Loop1Clean:
+    add     r2, r10, r10, lsr #1    @ Work out cache level
+    mov     r1, r0, lsr r2          @ R0 bottom 3 bits = Cache Type for this level
+    and     r1, r1, #7              @ Get those 3 bits alone
+    cmp     r1, #2
+    blt     SkipClean                @ No cache or only instruction cache at this level
+    mcr     p15, 2, r10, c0, c0, 0  @ Write the Cache Size selection register
+    mov     r1, #0
+    .long   0xF57FF06F              @ ISB
+    mrc     p15, 1, r1, c0, c0, 0   @ Reads current Cache Size ID register
+    and     r2, r1, #7             @ Extract the line length field
+    add     r2, r2, #4              @ Add 4 for the line length offset (log2 16 bytes)
+    ldr     r4, =0x3FF
+    ands    r4, r4, r1, lsr #3      @ R4 is the max number on the way size (right aligned)
+    clz     r5, r4                  @ R5 is the bit position of the way size increment
+    ldr     r7, =0x00007FFF
+    ands    r7, r7, r1, lsr #13     @ R7 is the max number of the index size (right aligned)
+Loop2Clean:
+    mov     r9, r4                  @ R9 working copy of the max way size (right aligned)
+Loop3Clean:
+    orr     r11, r10, r9, lsl r5    @ Factor in the way number and cache number into R11
+    orr     r11, r11, r7, lsl r2    @ Factor in the index number
+    mcr     p15, 0, r11, c7, c14, 2 @ Clean and invalidate by set/way
+    subs    r9, r9, #1              @ Decrement the way number
+    bge     Loop3Clean
+    subs    r7, r7, #1              @ Decrement the index
+    bge     Loop2Clean
+SkipClean:
+    add     r10, r10, #2            @ Increment the cache number
+    cmp     r3, r10
+    bgt     Loop1Clean
+
+FinishedClean:
+
+    /* Disable L2 cache */
+    mrc     p15, 0, r0, c1, c0, 1   @ R0 = auxiliary control reg
+    bic     r0, r0, #ARM_AUXCR_L2EN @ Disable L2 cache
+    mcr     p15, 0, r0, c1, c0, 1   @ Update aux control reg
+
+#if 0
+	/*Set the DDR drive strength to low */
+	ldr        r10, [r6]
+	and       r10, r10, #0xF1        @ clear bits 2-1
+	str        r10, [r6]
+#endif
+
+    .long     0xe320f003              @ Opcode for WFI
+
+#if 0
+	/*Set the DDR drive strength to max */
+	orr       r10, r10, #0x06        @ set bits 2-1
+	str        r10, [r6]
+#endif
+
+	ldr     r11, =0x0000fFFF
+TestLoop:
+	subs    r11,r11, #1              @ Decrement the index
+	bge     TestLoop
+
+    mov     r0, #0
+    mcr     p15, 0, r0, c7, c5, 0   @ Invalidate inst cache
+
+    /* Invalidate data caches */
+    mrc     p15, 1, r0, c0, c0, 1   @ Read CLIDR
+    ands    r3, r0, #0x7000000      @ Isolate level of coherency
+    mov     r3, r3, lsr #23         @ Cache level value (naturally aligned)
+    beq     FinishedInvalidate
+    mov     r10, #0
+Loop1Invalidate:
+    add     r2, r10, r10, lsr #1    @ Work out cache level
+    mov     r1, r0, lsr r2          @ R0 bottom 3 bits = Cache Type for this level
+    and     r1, r1, #7              @ Get those 3 bits alone
+    cmp     r1, #2
+    blt     SkipInvalidate          @ No cache or only instruction cache at this level
+    mcr     p15, 2, r10, c0, c0, 0  @ Write the Cache Size selection register
+    mov     r1, #0
+    .long   0xF57FF06F              @ ISB
+    mrc     p15, 1, r1, c0, c0, 0   @ Reads current Cache Size ID register
+    and     r2, r1, #7              @ Extract the line length field
+    add     r2, r2, #4              @ Add 4 for the line length offset (log2 16 bytes)
+    ldr     r4, =0x3FF
+    ands    r4, r4, r1, lsr #3      @ R4 is the max number on the way size (right aligned)
+    clz     r5, r4                  @ R5 is the bit position of the way size increment
+    ldr     r7, =0x00007FFF
+    ands    r7, r7, r1, lsr #13     @ R7 is the max number of the index size (right aligned)
+Loop2Invalidate:
+    mov     r9, r4                  @ R9 working copy of the max way size (right aligned)
+Loop3Invalidate:
+    orr     r11, r10, r9, lsl r5    @ Factor in the way number and cache number into R11
+    orr     r11, r11, r7, lsl r2    @ Factor in the index number
+    mcr     p15, 0, r11, c7, c6, 2  @ Invalidate by set/way
+    subs    r9, r9, #1              @ Decrement the way number
+    bge     Loop3Invalidate
+    subs    r7, r7, #1              @ Decrement the index
+    bge     Loop2Invalidate
+SkipInvalidate:
+    add     r10, r10, #2            @ Increment the cache number
+    cmp     r3, r10
+    bgt     Loop1Invalidate
+
+FinishedInvalidate:
+
+    /* Enable L2 cache */
+    mrc     p15, 0, r0, c1, c0, 1   @ R0 = auxiliary control reg
+    orr     r0, r0, #ARM_AUXCR_L2EN @ Enable L2 cache
+    mcr     p15, 0, r0, c1, c0, 1   @ Update aux control reg
+
+    /* Enable L1 caches */
+    mrc     p15, 0, r0, c1, c0, 0    @ R0 = system control reg
+    orr     r0, r0, #ARM_CTRL_ICACHE @ Enable ICache
+    orr     r0, r0, #ARM_CTRL_DCACHE @ Enable DCache
+    mcr     p15, 0, r0, c1, c0, 0    @ Update system control reg
+
+    /* Restore registers */
+    ldmfd sp!, {r4,r5,r6,r7,r9,r10,r11}
+    mov		pc, lr
+
+	.type	cpu_do_suspend, #object
+ENTRY(cpu_do_suspend)
+	.word	cpu_do_suspend_workaround
+	.size	cpu_do_suspend_workaround, . - cpu_do_suspend_workaround
diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
index 74aac96cda2..0ca65fad210 100644
--- a/arch/arm/plat-mxc/cpufreq.c
+++ b/arch/arm/plat-mxc/cpufreq.c
@@ -38,7 +38,7 @@ static struct cpufreq_frequency_table *imx_freq_table;
 static int cpu_op_nr;
 static struct cpu_op *cpu_op_tbl;
 
-static int set_cpu_freq(int freq)
+int set_cpu_freq(int freq)
 {
 	int ret = 0;
 	int org_cpu_rate;