[PATCH v3 0/7] Add common cpuidle code for consolidation.

@@ -2,4 +2,4 @@
 # Makefile for cpuidle.
 #
 
-obj-y += cpuidle.o driver.o governor.o sysfs.o governors/
+obj-y += cpuidle.o driver.o governor.o sysfs.o common.o governors/

@@ -0,0 +1,152 @@
+/*
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ * Copyright 2011 Linaro Ltd.
+ *
+ * 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
+ */
+
+/*
+ * This code performs provides some commonly used cpuidle setup functionality
+ * used by many ARM SoC platforms.  Providing this functionality here
+ * reduces the duplication of this code for each ARM platform that uses it.
+ */
+
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/cpuidle.h>
+#include <linux/hrtimer.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <asm/proc-fns.h>
+
+static struct cpuidle_device __percpu * common_cpuidle_devices;
+
+static int (*do_idle[CPUIDLE_STATE_MAX])(struct cpuidle_device *dev,
+			       struct cpuidle_driver *drv, int index);
+
+int cpuidle_def_idle(struct cpuidle_device *dev,
+			       struct cpuidle_driver *drv, int index)
+{
+	cpu_do_idle();
+	return index;
+}
+
+static int simple_enter(struct cpuidle_device *dev,
+			       struct cpuidle_driver *drv, int index)
+{
+	ktime_t time_start, time_end;
+
+	local_irq_disable();
+
+	time_start = ktime_get();
+
+	index = do_idle[index](dev, drv, index);
+
+	time_end = ktime_get();
+
+	local_irq_enable();
+
+	dev->last_residency =
+		(int)ktime_to_us(ktime_sub(time_end, time_start));
+
+	return index;
+}
+
+void common_cpuidle_devices_uninit(void)
+{
+	int cpu_id;
+	struct cpuidle_device *dev;
+
+	for_each_possible_cpu(cpu_id) {
+		dev = per_cpu_ptr(common_cpuidle_devices, cpu_id);
+		cpuidle_unregister_device(dev);
+	}
+
+	free_percpu(common_cpuidle_devices);
+}
+
+/**
+ * common_cpuidle_init() - Provides some commonly used init functionality.
+ * @pdrv		Pointer to your cpuidle_driver object.
+ * @simple		Use the common simple_enter wrapper?
+ * @driver_data_init	Pointer to your platform function to initialize your
+ *			platform specific driver data.  Use NULL if platform
+ *			specific data is not needed.
+ *
+ * Common cpuidle init interface to provide common cpuidle functionality
+ * used by many platforms.
+ */
+int __init common_cpuidle_init(struct cpuidle_driver *pdrv, bool simple,
+			 void (*driver_data_init)(struct cpuidle_device *dev))
+{
+	struct cpuidle_device *dev;
+	struct cpuidle_driver *drv;
+	int i, cpu_id, ret;
+
+	if (!pdrv || pdrv->state_count > CPUIDLE_STATE_MAX) {
+		pr_err("%s: Invalid Input\n", __func__);
+		return -EINVAL;
+	}
+
+	drv = kmalloc(sizeof(struct cpuidle_driver), GFP_KERNEL);
+	if (!drv) {
+		pr_err("%s: no memory for cpuidle driver\n", __func__);
+		return -ENOMEM;
+	}
+
+	*drv = *pdrv;
+
+	for (i = 0; simple && (i < drv->state_count); i++) {
+		do_idle[i] = drv->states[i].enter;
+		drv->states[i].enter = simple_enter;
+	}
+
+	ret = cpuidle_register_driver(drv);
+	if (ret) {
+		pr_err("%s: Failed to register cpuidle driver\n", __func__);
+		goto free_drv;
+	}
+
+	common_cpuidle_devices = alloc_percpu(struct cpuidle_device);
+	if (common_cpuidle_devices == NULL) {
+		ret = -ENOMEM;
+		goto unregister_drv;
+	}
+
+	/* initialize state data for each cpuidle_device */
+	for_each_possible_cpu(cpu_id) {
+		dev = per_cpu_ptr(common_cpuidle_devices, cpu_id);
+		dev->cpu = cpu_id;
+		dev->state_count = drv->state_count;
+
+		if (driver_data_init)
+			driver_data_init(dev);
+
+		ret = cpuidle_register_device(dev);
+		if (ret) {
+			pr_err("%s: Failed to register cpu %u\n",
+				__func__, cpu_id);
+			goto uninit;
+		}
+	}
+
+	return 0;
+uninit:
+
+	common_cpuidle_devices_uninit();
+
+unregister_drv:
+
+	cpuidle_unregister_driver(drv);
+
+free_drv:
+
+	kfree(drv);
+
+	return ret;
+}

@@ -56,6 +56,16 @@ struct cpuidle_state {
 
 #define CPUIDLE_DRIVER_FLAGS_MASK (0xFFFF0000)
 
+/* Common ARM WFI state */
+#define CPUIDLE_ARM_WFI_STATE {\
+	.enter                  = cpuidle_def_idle,\
+	.exit_latency           = 2,\
+	.target_residency       = 1,\
+	.flags                  = CPUIDLE_FLAG_TIME_VALID,\
+	.name                   = "WFI",\
+	.desc                   = "ARM core clock gating (WFI)",\
+}
+
 /**
  * cpuidle_get_statedata - retrieves private driver state data
  * @st_usage: the state usage statistics

@@ -141,6 +151,13 @@ extern void cpuidle_resume_and_unlock(void);
 extern int cpuidle_enable_device(struct cpuidle_device *dev);
 extern void cpuidle_disable_device(struct cpuidle_device *dev);
 
+/* provide a default idle function */
+extern int cpuidle_def_idle(struct cpuidle_device *dev,
+		       struct cpuidle_driver *drv, int index);
+extern int common_cpuidle_init(struct cpuidle_driver *drv, bool simple,
+			 void (*driver_data_init)(struct cpuidle_device *dev));
+extern void common_cpuidle_devices_uninit(void);
+
 #else
 static inline void disable_cpuidle(void) { }
 static inline int cpuidle_idle_call(void) { return -ENODEV; }

@@ -157,6 +174,13 @@ static inline void cpuidle_resume_and_unlock(void) { }
 static inline int cpuidle_enable_device(struct cpuidle_device *dev)
 {return -ENODEV; }
 static inline void cpuidle_disable_device(struct cpuidle_device *dev) { }
+static inline int cpuidle_def_idle(struct cpuidle_device *dev,
+		       struct cpuidle_driver *drv, int index)
+{return -ENODEV; }
+static inline int common_cpuidle_init(struct cpuidle_driver *pdrv,
+	bool simple, void (*driver_data_init)(struct cpuidle_device *dev))
+{return -ENODEV; }
+static inline void common_cpuidle_devices_uninit(void) { }
 
 #endif
 

@@ -13,80 +13,19 @@
 #include <linux/cpuidle.h>
 #include <linux/io.h>
 #include <linux/export.h>
-#include <linux/time.h>
-
 #include <asm/proc-fns.h>
 
-static int exynos4_enter_idle(struct cpuidle_device *dev,
-			struct cpuidle_driver *drv,
-			      int index);
-
-static struct cpuidle_state exynos4_cpuidle_set[] = {
-	[0] = {
-		.enter			= exynos4_enter_idle,
-		.exit_latency		= 1,
-		.target_residency	= 100000,
-		.flags			= CPUIDLE_FLAG_TIME_VALID,
-		.name			= "IDLE",
-		.desc			= "ARM clock gating(WFI)",
-	},
-};
-
-static DEFINE_PER_CPU(struct cpuidle_device, exynos4_cpuidle_device);
-
-static struct cpuidle_driver exynos4_idle_driver = {
+static struct cpuidle_driver exynos4_idle_driver __initdata = {
 	.name		= "exynos4_idle",
 	.owner		= THIS_MODULE,
+	.states[0]	= CPUIDLE_ARM_WFI_STATE,
+	.state_count	= 1,
 };
 
-static int exynos4_enter_idle(struct cpuidle_device *dev,
-				struct cpuidle_driver *drv,
-			      int index)
-{
-	struct timeval before, after;
-	int idle_time;
-
-	local_irq_disable();
-	do_gettimeofday(&before);
-
-	cpu_do_idle();
-
-	do_gettimeofday(&after);
-	local_irq_enable();
-	idle_time = (after.tv_sec - before.tv_sec) * USEC_PER_SEC +
-		    (after.tv_usec - before.tv_usec);
-
-	dev->last_residency = idle_time;
-	return index;
-}
-
 static int __init exynos4_init_cpuidle(void)
 {
-	int i, max_cpuidle_state, cpu_id;
-	struct cpuidle_device *device;
-	struct cpuidle_driver *drv = &exynos4_idle_driver;
-
-	/* Setup cpuidle driver */
-	drv->state_count = (sizeof(exynos4_cpuidle_set) /
-				       sizeof(struct cpuidle_state));
-	max_cpuidle_state = drv->state_count;
-	for (i = 0; i < max_cpuidle_state; i++) {
-		memcpy(&drv->states[i], &exynos4_cpuidle_set[i],
-				sizeof(struct cpuidle_state));
-	}
-	cpuidle_register_driver(&exynos4_idle_driver);
-
-	for_each_cpu(cpu_id, cpu_online_mask) {
-		device = &per_cpu(exynos4_cpuidle_device, cpu_id);
-		device->cpu = cpu_id;
-
-		device->state_count = drv->state_count;
-
-		if (cpuidle_register_device(device)) {
-			printk(KERN_ERR "CPUidle register device failed\n,");
-			return -EIO;
-		}
-	}
-	return 0;
+	return common_cpuidle_init(&exynos4_idle_driver,
+					true,
+					NULL);
 }
 device_initcall(exynos4_init_cpuidle);

@@ -16,71 +16,38 @@
 #include <asm/system.h>
 #include <asm/io.h>
 
-static void shmobile_enter_wfi(void)
-{
-	cpu_do_idle();
-}
-
-void (*shmobile_cpuidle_modes[CPUIDLE_STATE_MAX])(void) = {
-	shmobile_enter_wfi, /* regular sleep mode */
-};
+void (*shmobile_cpuidle_modes[CPUIDLE_STATE_MAX])(void);
 
 static int shmobile_cpuidle_enter(struct cpuidle_device *dev,
 				  struct cpuidle_driver *drv,
 				  int index)
 {
-	ktime_t before, after;
-
-	before = ktime_get();
-
-	local_irq_disable();
-	local_fiq_disable();
-
 	shmobile_cpuidle_modes[index]();
 
-	local_irq_enable();
-	local_fiq_enable();
-
-	after = ktime_get();
-	dev->last_residency = ktime_to_ns(ktime_sub(after, before)) >> 10;
-
 	return index;
 }
 
-static struct cpuidle_device shmobile_cpuidle_dev;
-static struct cpuidle_driver shmobile_cpuidle_driver = {
+static struct cpuidle_driver shmobile_cpuidle_driver __initdata = {
 	.name =		"shmobile_cpuidle",
 	.owner =	THIS_MODULE,
-	.states[0] = {
-		.name = "C1",
-		.desc = "WFI",
-		.exit_latency = 1,
-		.target_residency = 1 * 2,
-		.flags = CPUIDLE_FLAG_TIME_VALID,
-	},
-	.safe_state_index = 0, /* C1 */
+	.states[0] = CPUIDLE_ARM_WFI_STATE,
+	.safe_state_index = 0,
 	.state_count = 1,
 };
 
 void (*shmobile_cpuidle_setup)(struct cpuidle_driver *drv);
 
-static int shmobile_cpuidle_init(void)
+static int __init shmobile_cpuidle_init(void)
 {
-	struct cpuidle_device *dev = &shmobile_cpuidle_dev;
-	struct cpuidle_driver *drv = &shmobile_cpuidle_driver;
 	int i;
+	struct cpuidle_driver *drv = &shmobile_cpuidle_driver;
 
-	for (i = 0; i < CPUIDLE_STATE_MAX; i++)
+	for (i = drv->state_count; i < CPUIDLE_STATE_MAX; i++)
 		drv->states[i].enter = shmobile_cpuidle_enter;
 
 	if (shmobile_cpuidle_setup)
-		shmobile_cpuidle_setup(drv);
-
-	cpuidle_register_driver(drv);
-
-	dev->state_count = drv->state_count;
-	cpuidle_register_device(dev);
+		shmobile_cpuidle_setup(&shmobile_cpuidle_driver);
 
-	return 0;
+	return common_cpuidle_init(&shmobile_cpuidle_driver, true, NULL);
 }
 late_initcall(shmobile_cpuidle_init);

@@ -24,80 +24,37 @@
 
 #define KIRKWOOD_MAX_STATES	2
 
-static struct cpuidle_driver kirkwood_idle_driver = {
-	.name =         "kirkwood_idle",
-	.owner =        THIS_MODULE,
-};
-
-static DEFINE_PER_CPU(struct cpuidle_device, kirkwood_cpuidle_device);
-
 /* Actual code that puts the SoC in different idle states */
-static int kirkwood_enter_idle(struct cpuidle_device *dev,
+static int kirkwood_idle_ddr(struct cpuidle_device *dev,
 				struct cpuidle_driver *drv,
-			       int index)
+				int index)
 {
-	struct timeval before, after;
-	int idle_time;
-
-	local_irq_disable();
-	do_gettimeofday(&before);
-	if (index == 0)
-		/* Wait for interrupt state */
-		cpu_do_idle();
-	else if (index == 1) {
-		/*
-		 * Following write will put DDR in self refresh.
-		 * Note that we have 256 cycles before DDR puts it
-		 * self in self-refresh, so the wait-for-interrupt
-		 * call afterwards won't get the DDR from self refresh
-		 * mode.
-		 */
-		writel(0x7, DDR_OPERATION_BASE);
-		cpu_do_idle();
-	}
-	do_gettimeofday(&after);
-	local_irq_enable();
-	idle_time = (after.tv_sec - before.tv_sec) * USEC_PER_SEC +
-			(after.tv_usec - before.tv_usec);
-
-	/* Update last residency */
-	dev->last_residency = idle_time;
+	writel(0x7, DDR_OPERATION_BASE);
+	cpu_do_idle();
 
 	return index;
 }
 
+static struct cpuidle_driver kirkwood_idle_driver __initdata = {
+	.name =         "kirkwood_idle",
+	.owner =        THIS_MODULE,
+	.states[0] = CPUIDLE_ARM_WFI_STATE,
+	.states[1] = {
+		.enter			= kirkwood_idle_ddr,
+		.exit_latency		= 10,
+		.target_residency	= 10000,
+		.flags			= CPUIDLE_FLAG_TIME_VALID,
+		.name			= "DDR SR",
+		.desc			= "WFI and DDR Self Refresh",
+	},
+	.state_count = KIRKWOOD_MAX_STATES,
+};
+
 /* Initialize CPU idle by registering the idle states */
-static int kirkwood_init_cpuidle(void)
+static int __init kirkwood_init_cpuidle(void)
 {
-	struct cpuidle_device *device;
-	struct cpuidle_driver *driver = &kirkwood_idle_driver;
-
-	device = &per_cpu(kirkwood_cpuidle_device, smp_processor_id());
-	device->state_count = KIRKWOOD_MAX_STATES;
-	driver->state_count = KIRKWOOD_MAX_STATES;
-
-	/* Wait for interrupt state */
-	driver->states[0].enter = kirkwood_enter_idle;
-	driver->states[0].exit_latency = 1;
-	driver->states[0].target_residency = 10000;
-	driver->states[0].flags = CPUIDLE_FLAG_TIME_VALID;
-	strcpy(driver->states[0].name, "WFI");
-	strcpy(driver->states[0].desc, "Wait for interrupt");
-
-	/* Wait for interrupt and DDR self refresh state */
-	driver->states[1].enter = kirkwood_enter_idle;
-	driver->states[1].exit_latency = 10;
-	driver->states[1].target_residency = 10000;
-	driver->states[1].flags = CPUIDLE_FLAG_TIME_VALID;
-	strcpy(driver->states[1].name, "DDR SR");
-	strcpy(driver->states[1].desc, "WFI and DDR Self Refresh");
-
-	cpuidle_register_driver(&kirkwood_idle_driver);
-	if (cpuidle_register_device(device)) {
-		printk(KERN_ERR "kirkwood_init_cpuidle: Failed registering\n");
-		return -EIO;
-	}
-	return 0;
+	return common_cpuidle_init(&kirkwood_idle_driver,
+					true,
+					NULL);
 }
-
 device_initcall(kirkwood_init_cpuidle);

@@ -24,98 +24,57 @@
 
 #define DAVINCI_CPUIDLE_MAX_STATES	2
 
-struct davinci_ops {
-	void (*enter) (u32 flags);
-	void (*exit) (u32 flags);
-	u32 flags;
-};
-
-/* fields in davinci_ops.flags */
-#define DAVINCI_CPUIDLE_FLAGS_DDR2_PWDN	BIT(0)
-
-static struct cpuidle_driver davinci_idle_driver = {
-	.name	= "cpuidle-davinci",
-	.owner	= THIS_MODULE,
-};
-
-static DEFINE_PER_CPU(struct cpuidle_device, davinci_cpuidle_device);
+u32 __initdata ddr_reg_mask;
 static void __iomem *ddr2_reg_base;
 
-static void davinci_save_ddr_power(int enter, bool pdown)
+/* idle that includes ddr low power */
+static int davinci_idle_ddr(struct cpuidle_device *dev,
+				struct cpuidle_driver *drv,
+				int index)
 {
 	u32 val;
 
 	val = __raw_readl(ddr2_reg_base + DDR2_SDRCR_OFFSET);
 
-	if (enter) {
-		if (pdown)
-			val |= DDR2_SRPD_BIT;
-		else
-			val &= ~DDR2_SRPD_BIT;
-		val |= DDR2_LPMODEN_BIT;
-	} else {
-		val &= ~(DDR2_SRPD_BIT | DDR2_LPMODEN_BIT);
-	}
+	val |= (u32)dev->states_usage[index].driver_data;
 
 	__raw_writel(val, ddr2_reg_base + DDR2_SDRCR_OFFSET);
-}
 
-static void davinci_c2state_enter(u32 flags)
-{
-	davinci_save_ddr_power(1, !!(flags & DAVINCI_CPUIDLE_FLAGS_DDR2_PWDN));
-}
+	/* Wait for interrupt state */
+	cpu_do_idle();
 
-static void davinci_c2state_exit(u32 flags)
-{
-	davinci_save_ddr_power(0, !!(flags & DAVINCI_CPUIDLE_FLAGS_DDR2_PWDN));
+	val &= ~(DDR2_SRPD_BIT | DDR2_LPMODEN_BIT);
+	__raw_writel(val, ddr2_reg_base + DDR2_SDRCR_OFFSET);
+
+	return index;
 }
 
-static struct davinci_ops davinci_states[DAVINCI_CPUIDLE_MAX_STATES] = {
-	[1] = {
-		.enter	= davinci_c2state_enter,
-		.exit	= davinci_c2state_exit,
+static struct cpuidle_driver davinci_idle_driver __initdata = {
+	.name	= "cpuidle-davinci",
+	.owner	= THIS_MODULE,
+	.states[0] = CPUIDLE_ARM_WFI_STATE,
+	.states[1] = {
+		.enter			= davinci_idle_ddr,
+		.exit_latency		= 10,
+		.target_residency	= 100000,
+		.flags			= CPUIDLE_FLAG_TIME_VALID,
+		.name			= "DDR SR",
+		.desc			= "WFI and DDR Self Refresh",
 	},
+	.state_count = DAVINCI_CPUIDLE_MAX_STATES,
 };
 
-/* Actual code that puts the SoC in different idle states */
-static int davinci_enter_idle(struct cpuidle_device *dev,
-				struct cpuidle_driver *drv,
-						int index)
+/* use drive_data field to hold the configured ddr low power bitmask */
+static void __init davinci_cpuidle_dd_init(struct cpuidle_device * dev)
 {
-	struct cpuidle_state_usage *state_usage = &dev->states_usage[index];
-	struct davinci_ops *ops = cpuidle_get_statedata(state_usage);
-	struct timeval before, after;
-	int idle_time;
-
-	local_irq_disable();
-	do_gettimeofday(&before);
-
-	if (ops && ops->enter)
-		ops->enter(ops->flags);
-	/* Wait for interrupt state */
-	cpu_do_idle();
-	if (ops && ops->exit)
-		ops->exit(ops->flags);
-
-	do_gettimeofday(&after);
-	local_irq_enable();
-	idle_time = (after.tv_sec - before.tv_sec) * USEC_PER_SEC +
-			(after.tv_usec - before.tv_usec);
-
-	dev->last_residency = idle_time;
-
-	return index;
+	dev->states_usage[1].driver_data = (void *)ddr_reg_mask;
 }
 
 static int __init davinci_cpuidle_probe(struct platform_device *pdev)
 {
 	int ret;
-	struct cpuidle_device *device;
-	struct cpuidle_driver *driver = &davinci_idle_driver;
 	struct davinci_cpuidle_config *pdata = pdev->dev.platform_data;
 
-	device = &per_cpu(davinci_cpuidle_device, smp_processor_id());
-
 	if (!pdata) {
 		dev_err(&pdev->dev, "cannot get platform data\n");
 		return -ENOENT;

@@ -123,42 +82,15 @@ static int __init davinci_cpuidle_probe(struct platform_device *pdev)
 
 	ddr2_reg_base = pdata->ddr2_ctlr_base;
 
-	/* Wait for interrupt state */
-	driver->states[0].enter = davinci_enter_idle;
-	driver->states[0].exit_latency = 1;
-	driver->states[0].target_residency = 10000;
-	driver->states[0].flags = CPUIDLE_FLAG_TIME_VALID;
-	strcpy(driver->states[0].name, "WFI");
-	strcpy(driver->states[0].desc, "Wait for interrupt");
-
-	/* Wait for interrupt and DDR self refresh state */
-	driver->states[1].enter = davinci_enter_idle;
-	driver->states[1].exit_latency = 10;
-	driver->states[1].target_residency = 10000;
-	driver->states[1].flags = CPUIDLE_FLAG_TIME_VALID;
-	strcpy(driver->states[1].name, "DDR SR");
-	strcpy(driver->states[1].desc, "WFI and DDR Self Refresh");
 	if (pdata->ddr2_pdown)
-		davinci_states[1].flags |= DAVINCI_CPUIDLE_FLAGS_DDR2_PWDN;
-	cpuidle_set_statedata(&device->states_usage[1], &davinci_states[1]);
+		ddr_reg_mask = (DDR2_SRPD_BIT | DDR2_LPMODEN_BIT);
+	else
+		ddr_reg_mask = (DDR2_LPMODEN_BIT);
 
-	device->state_count = DAVINCI_CPUIDLE_MAX_STATES;
-	driver->state_count = DAVINCI_CPUIDLE_MAX_STATES;
+	ret = common_cpuidle_init(&davinci_idle_driver, true,
+					davinci_cpuidle_dd_init);
 
-	ret = cpuidle_register_driver(&davinci_idle_driver);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to register driver\n");
-		return ret;
-	}
-
-	ret = cpuidle_register_device(device);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to register device\n");
-		cpuidle_unregister_driver(&davinci_idle_driver);
-		return ret;
-	}
-
-	return 0;
+	return ret;
 }
 
 static struct platform_driver davinci_cpuidle_driver = {

@@ -174,4 +106,3 @@ static int __init davinci_cpuidle_init(void)
 						davinci_cpuidle_probe);
 }
 device_initcall(davinci_cpuidle_init);
-

@@ -1533,6 +1533,7 @@ static struct clk_lookup mx53_lookups[] = {
 	_REGISTER_CLOCK("imx53-ahci.0", "ahci", sata_clk)
 	_REGISTER_CLOCK("imx53-ahci.0", "ahci_phy", ahci_phy_clk)
 	_REGISTER_CLOCK("imx53-ahci.0", "ahci_dma", ahci_dma_clk)
+	_REGISTER_CLOCK(NULL, "gpc_dvfs", gpc_dvfs_clk)
 };
 
 static void clk_tree_init(void)

@@ -1572,6 +1573,7 @@ int __init mx51_clocks_init(unsigned long ckil, unsigned long osc,
 
 	clk_enable(&cpu_clk);
 	clk_enable(&main_bus_clk);
+	clk_enable(&gpc_dvfs_clk);
 
 	clk_enable(&iim_clk);
 	imx_print_silicon_rev("i.MX51", mx51_revision());

@@ -1615,6 +1617,7 @@ int __init mx53_clocks_init(unsigned long ckil, unsigned long osc,
 	clk_set_parent(&uart_root_clk, &pll3_sw_clk);
 	clk_enable(&cpu_clk);
 	clk_enable(&main_bus_clk);
+	clk_enable(&gpc_dvfs_clk);
 
 	clk_enable(&iim_clk);
 	imx_print_silicon_rev("i.MX53", mx53_revision());

@@ -18,13 +18,6 @@
 #include <mach/hardware.h>
 #include "crm_regs.h"
 
-static struct clk *gpc_dvfs_clk;
-
-static int mx5_suspend_prepare(void)
-{
-	return clk_enable(gpc_dvfs_clk);
-}
-
 static int mx5_suspend_enter(suspend_state_t state)
 {
 	switch (state) {

@@ -50,11 +43,6 @@ static int mx5_suspend_enter(suspend_state_t state)
 	return 0;
 }
 
-static void mx5_suspend_finish(void)
-{
-	clk_disable(gpc_dvfs_clk);
-}
-
 static int mx5_pm_valid(suspend_state_t state)
 {
 	return (state > PM_SUSPEND_ON && state <= PM_SUSPEND_MAX);

@@ -62,21 +50,13 @@ static int mx5_pm_valid(suspend_state_t state)
 
 static const struct platform_suspend_ops mx5_suspend_ops = {
 	.valid = mx5_pm_valid,
-	.prepare = mx5_suspend_prepare,
 	.enter = mx5_suspend_enter,
-	.finish = mx5_suspend_finish,
 };
 
 static int __init mx5_pm_init(void)
 {
-	if (gpc_dvfs_clk == NULL)
-		gpc_dvfs_clk = clk_get(NULL, "gpc_dvfs");
-
-	if (!IS_ERR(gpc_dvfs_clk)) {
-		if (cpu_is_mx51())
-			suspend_set_ops(&mx5_suspend_ops);
-	} else
-		return -EPERM;
+	if (cpu_is_mx51())
+		suspend_set_ops(&mx5_suspend_ops);
 
 	return 0;
 }

@@ -3,8 +3,9 @@
 #
 
 # Object file lists.
-obj-y   := cpu.o mm.o clock-mx51-mx53.o ehci.o system.o
 
+obj-y   := cpu.o mm.o clock-mx51-mx53.o ehci.o system.o
+obj-$(CONFIG_CPU_IDLE) += cpuidle.o
 obj-$(CONFIG_PM) += pm-imx5.o
 obj-$(CONFIG_CPU_FREQ_IMX)    += cpu_op-mx51.o
 obj-$(CONFIG_MACH_MX51_BABBAGE) += board-mx51_babbage.o

@@ -0,0 +1,64 @@
+/*
+ * Copyright 2011 Freescale Semiconductor, Inc.
+ * Copyright 2011 Linaro Ltd.
+ *
+ * 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/kernel.h>
+#include <linux/init.h>
+#include <linux/export.h>
+#include <linux/cpuidle.h>
+#include <asm/proc-fns.h>
+#include <mach/common.h>
+
+int imx5_enter(struct cpuidle_device *dev,
+			struct cpuidle_driver *drv,
+			int index)
+{
+	mx5_cpu_lp_set((unsigned int)dev->states_usage[index].driver_data);
+	cpu_do_idle();
+	return index;
+}
+
+static struct cpuidle_driver imx5_cpuidle_driver __initdata = {
+	.name	= "imx5_cpuidle",
+	.owner	= THIS_MODULE,
+	.states[0] = {
+		.enter			= imx5_enter,
+		.exit_latency		= 12, /* max latency at 160MHz */
+		.target_residency	= 1,
+		.flags			= CPUIDLE_FLAG_TIME_VALID,
+		.name			= "IMX WFI",
+		.desc			= "CPU and related clocks gated",
+	},
+	.states[1] = {
+		.enter			= imx5_enter,
+		.exit_latency		= 20, /* max latency at 160MHz */
+		.target_residency	= 1,
+		.flags			= CPUIDLE_FLAG_TIME_VALID,
+		.name			= "IMX SRPG",
+		.desc			= "CPU state retained,powered off",
+	},
+	.state_count = 2,
+};
+
+/* use driver_data field to hold the mx5 idle parameter */
+static void __init imx5_dd_init(struct cpuidle_device * dev)
+{
+	dev->states_usage[0].driver_data = (void *)WAIT_UNCLOCKED;
+	dev->states_usage[1].driver_data = (void *)WAIT_UNCLOCKED_POWER_OFF;
+}
+
+static int __init imx5_init_cpuidle(void)
+{
+	return common_cpuidle_init(&imx5_cpuidle_driver,
+					true,
+					imx5_dd_init);
+}
+late_initcall(imx5_init_cpuidle);