ENGR00132139 MX50: Set DDR to 24MHz in LPAPM mode

Added support for DDR at 24MHz in LPAPM mode to achieve low power in idle mode.
In LPAPM mode, all peripheral domain clocks and DDR are sourced from 24MHz XTAL.
When ARM is in WFI, its sourced from 24MHz XTAL too. Thus all PLLs are turned
off.

Signed-off-by: Ranjani Vaidyanathan <ra5478@freescale.com>

5 files changed, 451 insertions, 267 deletions

diff --git a/arch/arm/mach-mx5/bus_freq.c b/arch/arm/mach-mx5/bus_freq.c
index 1095da065e66..922349089082 100644
--- a/arch/arm/mach-mx5/bus_freq.c
+++ b/arch/arm/mach-mx5/bus_freq.c
@@ -22,6 +22,7 @@
  * @ingroup PM
  */
 #include <asm/io.h>
+#include <linux/sched.h>
 #include <linux/proc_fs.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -49,6 +50,8 @@
 #define EMI_SLOW_CLK_NORMAL_DIV		AXI_B_CLK_NORMAL_DIV
 #define NFC_CLK_NORMAL_DIV      	4
 #define SPIN_DELAY	1000000 /* in nanoseconds */
+#define HW_QOS_DISABLE		0x70
+#define HW_QOS_DISABLE_SET		0x74
 
 DEFINE_SPINLOCK(ddr_freq_lock);
 
@@ -78,6 +81,10 @@ static struct clk *emi_garb_clk;
 static void __iomem *pll1_base;
 static void __iomem *pll4_base;
 
+static void __iomem *qosc_base;
+
+struct regulator *pll_regulator;
+
 struct regulator *lp_regulator;
 int low_bus_freq_mode;
 int high_bus_freq_mode;
@@ -96,6 +103,11 @@ int cpu_wp_nr;
 int lp_high_freq;
 int lp_med_freq;
 
+static int lp_voltage;
+struct workqueue_struct *voltage_wq;
+static struct work_struct voltage_change_handler;
+struct completion voltage_change_cmpl;
+
 void enter_lpapm_mode_mx50(void);
 void enter_lpapm_mode_mx51(void);
 void exit_lpapm_mode_mx50(void);
@@ -116,6 +128,23 @@ struct dvfs_wp dvfs_core_setpoint[] = {
 						{28, 8, 33, 20, 30, 0x08},
 						{29, 0, 33, 20, 10, 0x08},};
 
+static DEFINE_SPINLOCK(voltage_lock);
+
+static void voltage_work_handler(struct work_struct *work)
+{
+	if (lp_regulator != NULL) {
+		u32 ret = 0;
+		ret = regulator_set_voltage(lp_regulator,
+					lp_voltage, lp_voltage);
+		udelay(400);
+		if (ret < 0) {
+			printk(KERN_ERR "COULD NOT SET LP VOLTAGE!!!!!!\n");
+			return;
+		}
+	}
+	complete_all(&voltage_change_cmpl);
+}
+
 int set_low_bus_freq(void)
 {
 	u32 reg;
@@ -135,21 +164,12 @@ int set_low_bus_freq(void)
 		stop_dvfs_per();
 
 		stop_sdram_autogating();
-		/* Set PLL3 to 133Mhz if no-one is using it. */
-		if ((clk_get_usecount(pll3) == 0) && !cpu_is_mx53()) {
-			u32 pll3_rate = clk_get_rate(pll3);
-
-			clk_enable(pll3);
-			clk_set_rate(pll3, clk_round_rate(pll3, 133000000));
+		if (!cpu_is_mx53()) {
 			if (cpu_is_mx50())
 				enter_lpapm_mode_mx50();
 			else
 				enter_lpapm_mode_mx51();
 
-			/* Set PLL3 back to original rate. */
-			clk_set_rate(pll3, clk_round_rate(pll3, pll3_rate));
-			clk_disable(pll3);
-
 		} else if (cpu_is_mx53()) {
 			/*Change the DDR freq to 133Mhz. */
 			clk_set_rate(ddr_hf_clk,
@@ -212,95 +232,130 @@ void enter_lpapm_mode_mx50()
 
 	spin_lock_irqsave(&ddr_freq_lock, flags);
 
+	local_flush_tlb_all();
+	flush_cache_all();
+
+	/* Disable all masters from accessing the DDR. */
+	reg = __raw_readl(qosc_base + HW_QOS_DISABLE);
+	reg |= 0xFFE;
+	__raw_writel(reg, qosc_base + HW_QOS_DISABLE_SET);
+	udelay(10);
+
+	/* Set the DDR to run from 24MHz.
+	 * Need to source the DDR from the SYS_CLK after
+	 * setting it into self-refresh mode. This code needs to run from iRAM.
+	 */
+	change_ddr_freq(ccm_base, databahn_base, LP_APM_CLK);
+
+	/* Enable all masters to access the DDR. */
+	reg = __raw_readl(qosc_base + HW_QOS_DISABLE);
+	reg = 0x0;
+	__raw_writel(reg, qosc_base + HW_QOS_DISABLE);
+
+	udelay(100);
+
 	/* Set the parent of main_bus_clk to be PLL3 */
 	clk_set_parent(main_bus_clk, pll3);
 
-	/* Set the AHB dividers to be 1. */
-	/* Set the dividers to be  1, so the clock rates
-	 * are at 133MHz.
+	/* Set the AHB dividers to be 2.
+	 * Set the dividers so that clock rates
+	 * are not greater than current clock rate.
 	 */
 	reg = __raw_readl(MXC_CCM_CBCDR);
 	reg &= ~(MXC_CCM_CBCDR_AXI_A_PODF_MASK
 			| MXC_CCM_CBCDR_AXI_B_PODF_MASK
 			| MXC_CCM_CBCDR_AHB_PODF_MASK
 			| MX50_CCM_CBCDR_WEIM_PODF_MASK);
-	reg |= (0 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
-			| 0 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
-			| 0 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
+	reg |= (1 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
+			| 1 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
+			| 1 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
 			| 0 << MX50_CCM_CBCDR_WEIM_PODF_OFFSET);
 	__raw_writel(reg, MXC_CCM_CBCDR);
 	while (__raw_readl(MXC_CCM_CDHIPR) & 0x0F)
 		udelay(10);
+
 	low_bus_freq_mode = 1;
 	high_bus_freq_mode = 0;
 
 	/* Set the source of main_bus_clk to be lp-apm. */
 	clk_set_parent(main_bus_clk, lp_apm);
 
-	/* Set SYS_CLK to 24MHz. sourced from XTAL*/
-	/* Turn on the XTAL_CLK_GATE. */
-	reg = __raw_readl(MXC_CCM_CLK_SYS);
-	reg |= 3 << MXC_CCM_CLK_SYS_SYS_XTAL_CLKGATE_OFFSET;
-	__raw_writel(reg, MXC_CCM_CLK_SYS);
-
-	/* Set the divider. */
-	reg = __raw_readl(MXC_CCM_CLK_SYS);
-	reg &= ~MXC_CCM_CLK_SYS_DIV_XTAL_MASK;
-	reg |= 1 << MXC_CCM_CLK_SYS_DIV_XTAL_OFFSET;
-	__raw_writel(reg, MXC_CCM_CLK_SYS);
-	while (__raw_readl(MXC_CCM_CSR2) & 0x1)
-		udelay(10);
-
-	/* Set the source to be XTAL. */
-	reg = __raw_readl(MXC_CCM_CLKSEQ_BYPASS);
-	reg &= ~0x1;
-	__raw_writel(reg, MXC_CCM_CLKSEQ_BYPASS);
-	while (!(__raw_readl(MXC_CCM_CSR2) & 0x400))
+	/* Set the AHB dividers to be 1. */
+	/* Set the dividers to be  1, so the clock rates
+	 * are at 24Mhz
+	 */
+	reg = __raw_readl(MXC_CCM_CBCDR);
+	reg &= ~(MXC_CCM_CBCDR_AXI_A_PODF_MASK
+			| MXC_CCM_CBCDR_AXI_B_PODF_MASK
+			| MXC_CCM_CBCDR_AHB_PODF_MASK
+			| MX50_CCM_CBCDR_WEIM_PODF_MASK);
+	reg |= (0 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
+			| 0 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
+			| 0 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
+			| 0 << MX50_CCM_CBCDR_WEIM_PODF_OFFSET);
+	__raw_writel(reg, MXC_CCM_CBCDR);
+	while (__raw_readl(MXC_CCM_CDHIPR) & 0x0F)
 		udelay(10);
 
-	/* Turn OFF the PLL_CLK_GATE. */
-	reg = __raw_readl(MXC_CCM_CLK_SYS);
-	reg &= ~MXC_CCM_CLK_SYS_SYS_PLL_CLKGATE_MASK;
-	__raw_writel(reg, MXC_CCM_CLK_SYS);
 	spin_unlock_irqrestore(&ddr_freq_lock, flags);
 
+	spin_lock_irqsave(&voltage_lock, flags);
+	lp_voltage = LP_LOW_VOLTAGE;
+	INIT_COMPLETION(voltage_change_cmpl);
+	queue_work(voltage_wq, &voltage_change_handler);
+	spin_unlock_irqrestore(&voltage_lock, flags);
+
+	udelay(100);
 }
 
 void enter_lpapm_mode_mx51()
 {
 	u32 reg;
 
-	/*Change the DDR freq to 133Mhz. */
-	clk_set_rate(ddr_hf_clk,
-	     clk_round_rate(ddr_hf_clk, ddr_low_rate));
+	/* Set PLL3 to 133Mhz if no-one is using it. */
+	if (clk_get_usecount(pll3) == 0) {
+		u32 pll3_rate = clk_get_rate(pll3);
 
-	/* Set the parent of Periph_apm_clk to be PLL3 */
-	clk_set_parent(periph_apm_clk, pll3);
-	clk_set_parent(main_bus_clk, periph_apm_clk);
+		clk_enable(pll3);
+		clk_set_rate(pll3, clk_round_rate(pll3, 133000000));
 
-	/* Set the dividers to be  1, so the clock rates
-	  * are at 133MHz.
-	  */
-	reg = __raw_readl(MXC_CCM_CBCDR);
-	reg &= ~(MXC_CCM_CBCDR_AXI_A_PODF_MASK
-			| MXC_CCM_CBCDR_AXI_B_PODF_MASK
-			| MXC_CCM_CBCDR_AHB_PODF_MASK
-			| MXC_CCM_CBCDR_EMI_PODF_MASK
-			| MXC_CCM_CBCDR_NFC_PODF_OFFSET);
-	reg |= (0 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
-			| 0 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
-			| 0 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
-			| 0 << MXC_CCM_CBCDR_EMI_PODF_OFFSET
-			| 3 << MXC_CCM_CBCDR_NFC_PODF_OFFSET);
-	__raw_writel(reg, MXC_CCM_CBCDR);
+		/*Change the DDR freq to 133Mhz. */
+		clk_set_rate(ddr_hf_clk,
+		     clk_round_rate(ddr_hf_clk, ddr_low_rate));
 
-	clk_enable(emi_garb_clk);
-	while (__raw_readl(MXC_CCM_CDHIPR) & 0x1F)
-		udelay(10);
-	clk_disable(emi_garb_clk);
+		/* Set the parent of Periph_apm_clk to be PLL3 */
+		clk_set_parent(periph_apm_clk, pll3);
+		clk_set_parent(main_bus_clk, periph_apm_clk);
+
+		/* Set the dividers to be  1, so the clock rates
+		  * are at 133MHz.
+		  */
+		reg = __raw_readl(MXC_CCM_CBCDR);
+		reg &= ~(MXC_CCM_CBCDR_AXI_A_PODF_MASK
+				| MXC_CCM_CBCDR_AXI_B_PODF_MASK
+				| MXC_CCM_CBCDR_AHB_PODF_MASK
+				| MXC_CCM_CBCDR_EMI_PODF_MASK
+				| MXC_CCM_CBCDR_NFC_PODF_OFFSET);
+		reg |= (0 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
+				| 0 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
+				| 0 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
+				| 0 << MXC_CCM_CBCDR_EMI_PODF_OFFSET
+				| 3 << MXC_CCM_CBCDR_NFC_PODF_OFFSET);
+		__raw_writel(reg, MXC_CCM_CBCDR);
+
+		clk_enable(emi_garb_clk);
+		while (__raw_readl(MXC_CCM_CDHIPR) & 0x1F)
+			udelay(10);
+		clk_disable(emi_garb_clk);
+
+		/* Set the source of Periph_APM_Clock to be lp-apm. */
+		clk_set_parent(periph_apm_clk, lp_apm);
+
+		/* Set PLL3 back to original rate. */
+		clk_set_rate(pll3, clk_round_rate(pll3, pll3_rate));
+		clk_disable(pll3);
 
-	/* Set the source of Periph_APM_Clock to be lp-apm. */
-	clk_set_parent(periph_apm_clk, lp_apm);
+	}
 }
 
 int set_high_bus_freq(int high_bus_freq)
@@ -315,22 +370,12 @@ int set_high_bus_freq(int high_bus_freq)
 
 		if (low_bus_freq_mode) {
 			/* Relock PLL3 to 133MHz */
-			if ((clk_get_usecount(pll3) == 0) && !cpu_is_mx53()) {
-				u32 pll3_rate = clk_get_rate(pll3);
-
-				clk_enable(pll3);
-				clk_set_rate(pll3,
-					clk_round_rate(pll3, 133000000));
+			if (!cpu_is_mx53()) {
 				if (cpu_is_mx50())
 					exit_lpapm_mode_mx50();
 				else
 					exit_lpapm_mode_mx51();
-
-				/* Relock PLL3 to its original rate */
-				clk_set_rate(pll3,
-					clk_round_rate(pll3, pll3_rate));
-				clk_disable(pll3);
-			} else if (cpu_is_mx53()) {
+			} else {
 				/* move cpu clk to pll1 */
 				reg = __raw_readl(MXC_CCM_CDHIPR);
 				if ((reg & MXC_CCM_CDHIPR_ARM_PODF_BUSY) == 0)
@@ -414,37 +459,65 @@ int set_high_bus_freq(int high_bus_freq)
 
 void exit_lpapm_mode_mx50()
 {
-	u32 reg, ret;
+	u32 reg;
 	unsigned long flags;
 
-	spin_lock_irqsave(&ddr_freq_lock, flags);
+	do {
+		if (completion_done(&voltage_change_cmpl)) {
+			spin_lock_irqsave(&voltage_lock, flags);
+			break;
+		} else {
+			set_user_nice(get_current(), 10);
+			yield();
+			set_user_nice(get_current(), -10);
+		}
+	} while (1);
+	if (completion_done(&voltage_change_cmpl)) {
+		if (lp_voltage != LP_NORMAL_VOLTAGE) {
+			INIT_COMPLETION(voltage_change_cmpl);
+			lp_voltage = LP_NORMAL_VOLTAGE;
+			if (!queue_work(voltage_wq, &voltage_change_handler))
+				printk(KERN_ERR "WORK_NOT_ADDED\n");
+			spin_unlock_irqrestore(&voltage_lock, flags);
+			while (!completion_done(&voltage_change_cmpl)) {
+				set_user_nice(get_current(), 10);
+				yield();
+				set_user_nice(get_current(), -10);
+			}
+		} else
+			spin_unlock_irqrestore(&voltage_lock, flags);
+	} else {
+		spin_unlock_irqrestore(&voltage_lock, flags);
+		while (!completion_done(&voltage_change_cmpl)) {
+			set_user_nice(get_current(), 10);
+			yield();
+			set_user_nice(get_current(), -10);
+		}
+	}
 
-	/* Set SYS_CLK to source from PLL1 */
-	/* Set sys_clk back to 200MHz. */
-	/* Set the divider to 4. */
-	reg = __raw_readl(MXC_CCM_CLK_SYS);
-	reg &= ~MXC_CCM_CLK_SYS_DIV_PLL_MASK;
-	reg |= 0x4 << MXC_CCM_CLK_SYS_DIV_PLL_OFFSET;
-	__raw_writel(reg, MXC_CCM_CLK_SYS);
-	udelay(100);
+	spin_lock_irqsave(&ddr_freq_lock, flags);
+	if (!low_bus_freq_mode) {
+		spin_unlock_irqrestore(&ddr_freq_lock, flags);
+		return;
+	}
 
-	/* Turn ON the PLL CLK_GATE. */
-	reg = __raw_readl(MXC_CCM_CLK_SYS);
-	reg |= 3 << MXC_CCM_CLK_SYS_SYS_PLL_CLKGATE_OFFSET;
-	__raw_writel(reg, MXC_CCM_CLK_SYS);
+	/* Temporarily set the dividers when the source is PLL3.
+	 * No clock rate is above 133MHz.
+	 */
+	reg = __raw_readl(MXC_CCM_CBCDR);
+	reg &= ~(MXC_CCM_CBCDR_AXI_A_PODF_MASK
+		| MXC_CCM_CBCDR_AXI_B_PODF_MASK
+		| MXC_CCM_CBCDR_AHB_PODF_MASK
+		| MX50_CCM_CBCDR_WEIM_PODF_MASK);
+	reg |= (1 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
+		|1 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
+		|1 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
+		|0 << MX50_CCM_CBCDR_WEIM_PODF_OFFSET);
+	__raw_writel(reg, MXC_CCM_CBCDR);
 
-	/* Source the SYS_CLK from PLL */
-	reg = __raw_readl(MXC_CCM_CLKSEQ_BYPASS);
-	reg |= 0x3;
-	__raw_writel(reg, MXC_CCM_CLKSEQ_BYPASS);
-	while (__raw_readl(MXC_CCM_CSR2) & 0x400)
+	while (__raw_readl(MXC_CCM_CDHIPR) & 0xF)
 		udelay(10);
 
-	/* Turn OFF the XTAL_CLK_GATE. */
-	reg = __raw_readl(MXC_CCM_CLK_SYS);
-	reg &= ~MXC_CCM_CLK_SYS_SYS_XTAL_CLKGATE_MASK;
-	__raw_writel(reg, MXC_CCM_CLK_SYS);
-
 	clk_set_parent(main_bus_clk, pll3);
 
 	/* Set the dividers to the default dividers */
@@ -467,8 +540,26 @@ void exit_lpapm_mode_mx50()
 
 	/*Set the main_bus_clk parent to be PLL2. */
 	clk_set_parent(main_bus_clk, pll2);
-	spin_unlock_irqrestore(&ddr_freq_lock, flags);
 
+	/* Disable all masters from accessing the DDR. */
+	reg = __raw_readl(qosc_base + HW_QOS_DISABLE);
+	reg |= 0xFFE;
+	__raw_writel(reg, qosc_base + HW_QOS_DISABLE_SET);
+		udelay(10);
+
+	local_flush_tlb_all();
+	flush_cache_all();
+
+	/* Set the DDR to default freq.
+	 */
+	change_ddr_freq(ccm_base, databahn_base, ddr_normal_rate);
+
+	/* Enable all masters to access the DDR. */
+	reg = __raw_readl(qosc_base + HW_QOS_DISABLE);
+	reg = 0x0;
+	__raw_writel(reg, qosc_base + HW_QOS_DISABLE);
+
+	spin_unlock_irqrestore(&ddr_freq_lock, flags);
 	udelay(100);
 }
 
@@ -476,6 +567,28 @@ void exit_lpapm_mode_mx51()
 {
 	u32 reg;
 
+
+	/* Temporarily Set the dividers  is PLL3.
+	 * No clock rate is above 133MHz.
+	 */
+	reg = __raw_readl(MXC_CCM_CBCDR);
+	reg &= ~(MXC_CCM_CBCDR_AXI_A_PODF_MASK
+		| MXC_CCM_CBCDR_AXI_B_PODF_MASK
+		| MXC_CCM_CBCDR_AHB_PODF_MASK
+		| MXC_CCM_CBCDR_EMI_PODF_MASK
+		| MXC_CCM_CBCDR_NFC_PODF_OFFSET);
+	reg |= (1 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
+		| 1 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
+		| 1 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
+		| 1 << MXC_CCM_CBCDR_EMI_PODF_OFFSET
+		| 3 << MXC_CCM_CBCDR_NFC_PODF_OFFSET);
+	__raw_writel(reg, MXC_CCM_CBCDR);
+
+	clk_enable(emi_garb_clk);
+	while (__raw_readl(MXC_CCM_CDHIPR) & 0x1F)
+		udelay(10);
+	clk_disable(emi_garb_clk);
+
 	clk_set_parent(periph_apm_clk, pll3);
 
 	/* Set the dividers to the default dividers */
@@ -707,12 +820,6 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 		return PTR_ERR(lp_apm);
 	}
 
-	osc = clk_get(NULL, "osc");
-	if (IS_ERR(osc)) {
-		printk(KERN_DEBUG "%s: failed to get osc\n", __func__);
-		return PTR_ERR(osc);
-	}
-
 	gpc_dvfs_clk = clk_get(NULL, "gpc_dvfs_clk");
 	if (IS_ERR(gpc_dvfs_clk)) {
 		printk(KERN_DEBUG "%s: failed to get gpc_dvfs_clk\n", __func__);
@@ -755,6 +862,8 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 	}
 
 	if (cpu_is_mx50()) {
+		u32 reg;
+
 		iram_alloc(SZ_8K, &iram_paddr);
 		/* Need to remap the area here since we want the memory region
 			 to be executable. */
@@ -769,6 +878,17 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 			"%s: failed to get lp regulator\n", __func__);
 			return PTR_ERR(lp_regulator);
 		}
+
+		qosc_base = ioremap(QOSC_BASE_ADDR, SZ_4K);
+		/* Enable the QoSC */
+		reg = __raw_readl(qosc_base);
+		reg &= ~0xC0000000;
+		__raw_writel(reg, qosc_base);
+
+		voltage_wq = create_singlethread_workqueue("voltage_change");
+		INIT_WORK(&voltage_change_handler, voltage_work_handler);
+
+		init_completion(&voltage_change_cmpl);
 	}
 	cpu_wp_tbl = get_cpu_wp(&cpu_wp_nr);
 	low_bus_freq_mode = 0;
diff --git a/arch/arm/mach-mx5/clock_mx50.c b/arch/arm/mach-mx5/clock_mx50.c
index 02c0dbb664d4..a60a181d6282 100644
--- a/arch/arm/mach-mx5/clock_mx50.c
+++ b/arch/arm/mach-mx5/clock_mx50.c
@@ -66,7 +66,7 @@ static struct cpu_wp *cpu_wp_tbl;
 static void __iomem *pll1_base;
 static void __iomem *pll2_base;
 static void __iomem *pll3_base;
-static void __iomem *apll_base;
+void __iomem *apll_base;
 
 extern int cpu_wp_nr;
 extern int lp_high_freq;
@@ -324,7 +324,7 @@ static struct clk apll_clk = {
 	.get_rate = apll_get_rate,
 	.enable = apll_enable,
 	.disable = apll_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static unsigned long pfd_round_rate(struct clk *clk, unsigned long rate)
@@ -423,7 +423,7 @@ static struct clk pfd0_clk = {
 	.round_rate = pfd_round_rate,
 	.enable = pfd_enable,
 	.disable = pfd_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static struct clk pfd1_clk = {
@@ -434,7 +434,7 @@ static struct clk pfd1_clk = {
 	.round_rate = pfd_round_rate,
 	.enable = pfd_enable,
 	.disable = pfd_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static struct clk pfd2_clk = {
@@ -445,7 +445,7 @@ static struct clk pfd2_clk = {
 	.round_rate = pfd_round_rate,
 	.enable = pfd_enable,
 	.disable = pfd_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static struct clk pfd3_clk = {
@@ -456,7 +456,7 @@ static struct clk pfd3_clk = {
 	.round_rate = pfd_round_rate,
 	.enable = pfd_enable,
 	.disable = pfd_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static struct clk pfd4_clk = {
@@ -467,7 +467,7 @@ static struct clk pfd4_clk = {
 	.round_rate = pfd_round_rate,
 	.enable = pfd_enable,
 	.disable = pfd_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static struct clk pfd5_clk = {
@@ -478,7 +478,7 @@ static struct clk pfd5_clk = {
 	.round_rate = pfd_round_rate,
 	.enable = pfd_enable,
 	.disable = pfd_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static struct clk pfd6_clk = {
@@ -489,7 +489,7 @@ static struct clk pfd6_clk = {
 	.round_rate = pfd_round_rate,
 	.enable = pfd_enable,
 	.disable = pfd_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static struct clk pfd7_clk = {
@@ -500,7 +500,7 @@ static struct clk pfd7_clk = {
 	.round_rate = pfd_round_rate,
 	.enable = pfd_enable,
 	.disable = pfd_disable,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static unsigned long _clk_pll_get_rate(struct clk *clk)
@@ -664,12 +664,13 @@ static int _clk_pll1_sw_set_parent(struct clk *clk, struct clk *parent)
 		    (mux << MXC_CCM_CCSR_STEP_SEL_OFFSET);
 	} else {
 		if (parent == &lp_apm_clk) {
-			reg |= MXC_CCM_CCSR_PLL1_SW_CLK_SEL;
-			reg = __raw_readl(MXC_CCM_CCSR);
 			mux = _get_mux(parent, &lp_apm_clk, NULL, &pll2_sw_clk,
 				       &pll3_sw_clk);
 			reg = (reg & ~MXC_CCM_CCSR_STEP_SEL_MASK) |
 			    (mux << MXC_CCM_CCSR_STEP_SEL_OFFSET);
+			__raw_writel(reg, MXC_CCM_CCSR);
+			reg = __raw_readl(MXC_CCM_CCSR);
+			reg |= MXC_CCM_CCSR_PLL1_SW_CLK_SEL;
 		} else {
 			mux = _get_mux(parent, &lp_apm_clk, NULL, &pll2_sw_clk,
 				       &pll3_sw_clk);
@@ -2592,7 +2593,7 @@ static struct clk display_axi_clk = {
 	.disable = _clk_disable,
 	.enable_reg = MXC_CCM_DISPLAY_AXI,
 	.enable_shift = MXC_CCM_DISPLAY_AXI_CLKGATE_OFFSET,
-	.flags = RATE_PROPAGATES,
+	.flags = RATE_PROPAGATES | AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 /* TODO: check Auto-Slow Mode */
diff --git a/arch/arm/mach-mx5/mx50_ddr_freq.S b/arch/arm/mach-mx5/mx50_ddr_freq.S
index 7628b6a7c7e6..ee675d671a31 100644
--- a/arch/arm/mach-mx5/mx50_ddr_freq.S
+++ b/arch/arm/mach-mx5/mx50_ddr_freq.S
@@ -32,7 +32,29 @@ ENTRY(mx50_ddr_freq_change)
     mov    r5, r1                       @save DataBahn address
     mov    r4, r2                       @save new freq requested
 
-    ldr     r0, [r6, #0x90]
+    /* Make sure no TLB miss will occur when the DDR is in self refresh. */
+    /* Invalidate TLB single entry to ensure that the address is not
+      * already in the TLB.
+      */
+    adr   r3, LoopCKE2               @Address in this function.
+    mcr   p15, 0, r3, c8, c7, 1      @ Make sure freq code address
+                                             @ is not already in TLB.
+    mcr   p15, 0, r6, c8, c7, 1      @ Make sure CCM address
+                                             @ is not already in TLB.
+    mcr   p15, 0, r5, c8, c7, 1      @ make sure Databahn address
+                                             @ is not already in TLB.
+    mrc   p15, 0, r0, c10, c0, 0    @ Read the TLB lockdown register
+    orr    r0, r0, #1                    @ Set the Preserve bit.
+    mcr   p15, 0, r0, c10, c0, 0    @ Write to the lockdown register
+    ldr    r2, [r6]                        @ TLB will miss,
+                                             @CCM address will be loaded
+    ldr    r2, [r5]                        @ TLB will miss,
+                                             @Databahn address will be loaded
+    ldr    r2, [r3]                        @ TLB will miss
+    mrc  p15, 0, r0, c10, c0, 0      @ Read the lockdown register
+                                             @ (victim will be incremented)
+    bic   r0, r0, #1                     @ Clear the preserve bit
+    mcr   p15, 0, r0, c10, c0, 0     @ Write to the lockdown register.
 
     /* If Databahn is in LPM4, exit that mode first. */
     ldr     r1,[r5, #0x50]             @Store LPM mode in r1.
@@ -48,6 +70,17 @@ LoopCKE2:
     cmp     r0, r2
     bne     LoopCKE2
 
+/* Wait for the databahn to idle
+     Meaning, no access to the databahn is
+     being made.
+*/
+NotIdle:
+    ldr     r0,[r5, #0x13c]
+    and    r0, r0, #0x100
+    ldr     r2, =0x100
+    cmp     r0, r2
+    beq     NotIdle
+
     /*
      * Make sure the DDR is self-refresh, before switching its frequency
      * and clock source
@@ -91,7 +124,7 @@ LoopCKE0:
      /* Change the freq now */
      /* If the freq req is below 24MHz, set DDR to synchronous mode.
       * else set to async mode. */
-    ldr     r0, =24000000
+    ldr     r0, =200000000
     cmp   r4, r0
     bgt    Async_Mode
 
@@ -144,81 +177,13 @@ Div_Found:
     b       Ddr_not_24
 
 databahn_ddr_24:
-    ldr    r0, =0x00000003
-    str    r0, [r5, #0x08]
-    ldr    r0, =0x000012c0
-    str    r0, [r5, #0x0c]
-
-    ldr    r0, =0x00000018
-    str    r0, [r5, #0x10]
-    ldr    r0, =0x000000f0
-    str    r0, [r5, #0x14]
-    ldr    r0, =0x02010b0c
-    str    r0, [r5, #0x18]
-    ldr    r0, =0x02020102
-    str    r0, [r5, #0x1c]
-
-    ldr    r0, =0x05010102
-    str    r0, [r5, #0x20]
-    ldr    r0, =0x01000103
-    str    r0, [r5, #0x28]
-    ldr    r0, =0x04030101
-    str    r0, [r5, #0x2c]
-
-    ldr    r0, =0x00000202
-    str    r0, [r5, #0x34]
-    ldr    r0, =0x00000001
-    str    r0, [r5, #0x38]
-    ldr    r0, =0x00000401
-    str    r0, [r5, #0x3c]
-
     ldr    r0, =0x00050056
     str    r0, [r5, #0x40]
-    ldr    r0, =0x00040004
-    str    r0, [r5, #0x48]
-
-    ldr    r0, =0x00040022
-    str    r0, [r5, #0x6c]
-
-    ldr    r0, =0x00040022
-    str    r0, [r5, #0x78]
-
-    ldr    r0, =0x00180000
-    str    r0, [r5, #0x80]
-    ldr    r0, =0x00000009
-    str    r0, [r5, #0x84]
-    ldr    r0, =0x02400003
-    str    r0, [r5, #0x88]
-    ldr    r0, =0x01000200
-    str    r0, [r5, #0x8c]
-
-    ldr    r0, =0x00000000
-    str    r0, [r5, #0xcc]
-
-    ldr    r0, =0x01000201
-    str    r0, [r5, #0xd0]
-    ldr    r0, =0x01010301
-    str    r0, [r5, #0xd4]
-    ldr    r0, =0x00000101
-    str    r0, [r5, #0xd8]
-
-    ldr    r0, =0x02000602
-    str    r0, [r5, #0x104]
-    ldr    r0, =0x00560000
-    str    r0, [r5, #0x108]
-    ldr    r0, =0x00560056
-    str    r0, [r5, #0x10c]
-
-    ldr    r0, =0x00560056
-    str    r0, [r5, #0x110]
-    ldr    r0, =0x03060056
-    str    r0, [r5, #0x114]
-
     /* Set the Databahn DLL in bypass mode */
     /* PHY Register settings. */
-    ldr    r0, =0x00000100
+    ldr    r0, =0x0
     str    r0, [r5, #0x200]
-    ldr    r0, =0x000f1100
+    ldr    r0, =0x0
     str    r0, [r5, #0x204]
     ldr    r0, =0xf3003a27
     str    r0, [r5, #0x208]
@@ -268,9 +233,65 @@ databahn_ddr_24:
     ldr    r0, =0x00219f01
     str    r0, [r5, #0x25c]
 
+    /* Set SYS_CLK to be sourced from 24MHz. */
+    /* Set the SYS_XTAL_DIV */
+    ldr    r0, [r6, #0x94]
+    bic    r0, r0, #0x3c0
+    orr    r0, r0, #0x40
+    str    r0, [r6, #0x94]
+
+    /* Enable SYS_XTAL_CLKGATE. */
+    ldr    r0, [r6, #0x94]
+    orr    r0, r0, #0xC0000000
+    str    r0, [r6, #0x94]
+
+    /* set SYS_CLK to be sourced from XTAL. */
+    ldr     r0, [r6, #0x90]
+    bic     r0, r0, #0x1
+    str     r0, [r6, #0x90]
+
+    /* Disable SYS_PLL_CLKGATE.*/
+    ldr    r0, [r6, #0x94]
+    bic    r0, r0, #0x30000000
+    str    r0, [r6, #0x94]
     b       Setup_Done
 
 Async_Mode:
+    /* If SYS_CLK is running at 24MHz, increase
+      * it to 200MHz.
+      */
+    ldr     r0, [r6, #0x90]
+    and    r0, r0, #0x1
+    cmp   r0, #0
+    bne    Sys_Clk_Not_24
+
+   /* Disable SYS_PLL_CLKGATE. */
+    ldr     r0, [r6, #0x94]
+    bic     r0, r0, #0x30000000
+    str     r0, [r6, #0x94]
+
+    /* Set the new divider. */
+    ldr     r0, [r6, #0x94]
+    bic    r0, r0, #0x3f
+    orr     r0, r0, #4
+    str     r0, [r6, #0x94]
+
+   /* Enable SYS_PLL_CLKGATE. */
+    ldr     r0, [r6, #0x94]
+    orr     r0, r0, #0x30000000
+    str     r0, [r6, #0x94]
+
+    /* SYS_CLK to be sourced from PLL1. */
+    ldr     r0, [r6, #0x90]
+    orr     r0, r0, #0x3
+    str     r0, [r6, #0x90]
+
+   /* Disable SYS_XTAL_CLKGATE. */
+    ldr     r0, [r6, #0x94]
+    bic     r0, r0, #0xC0000000
+    str     r0, [r6, #0x94]
+
+Sys_Clk_Not_24:
     /* Set the Databahn to async mode. */
     ldr     r0, [r5, #0xdc]
     and    r0, r0, #0xfffcffff
@@ -287,8 +308,13 @@ Loop2:
     bgt     Loop2
 
 Div_Found1:
+    /* Turn OFF the DDR_CKLGATE_MASK in MXC_CCM_DDR */
     ldr     r0, [r6, #0x98]
-    bic    r0, r0, #0x3f
+    bic     r0, r0, #0xC0000000
+    str     r0, [r6, #0x98]
+
+    ldr     r0, [r6, #0x98]
+    bic     r0, r0, #0x3f
     orr     r0, r0, r3
     str     r0, [r6, #0x98]
 
@@ -299,7 +325,7 @@ Div_Found1:
 
     /* Turn ON the DDR_CKLGATE_MASK in MXC_CCM_DDR */
     ldr     r0, [r6, #0x98]
-    orr     r0, r0, #0x40000000
+    orr     r0, r0, #0xC0000000
     str     r0, [r6, #0x98]
 
     ldr     r0, =24000000
@@ -307,76 +333,8 @@ Div_Found1:
     beq    databahn_ddr_24
 
 Ddr_not_24:
-    ldr     r0, =0x0000001b
-    str     r0, [r5, #0x8]
-    ldr     r0, =0x0000d056
-    str     r0, [r5, #0xc]
-
-    ldr     r0, =0x0000010b
-    str     r0, [r5, #0x10]
-    ldr     r0, =0x00000a6b
-    str     r0, [r5, #0x14]
-    ldr     r0, =0x02020d0c
-    str     r0, [r5, #0x18]
-    ldr     r0, =0x0c110302
-    str     r0, [r5, #0x1c]
-
-    ldr     r0, =0x05020503
-    str     r0, [r5, #0x20]
-    ldr     r0, =0x01000403
-    str     r0, [r5, #0x28]
-    ldr     r0, =0x09040501
-    str     r0, [r5, #0x2c]
-
-    ldr     r0, =0x00000e02
-    str     r0, [r5, #0x34]
-    ldr     r0, =0x00000006
-    str     r0, [r5, #0x38]
-    ldr     r0, =0x00002301
-    str     r0, [r5, #0x3c]
-
     ldr     r0, =0x00050408
     str     r0, [r5, #0x40]
-    ldr     r0, =0x00260026
-    str     r0, [r5, #0x48]
-
-    ldr     r0, =0x00040042
-    str     r0, [r5, #0x6c]
-
-    ldr     r0, =0x00040042
-    str     r0, [r5, #0x78]
-
-    ldr     r0, =0x010b0000
-    str     r0, [r5, #0x80]
-    ldr     r0, =0x00000060
-    str     r0, [r5, #0x84]
-    ldr     r0, =0x02400018
-    str     r0, [r5, #0x88]
-    ldr     r0, =0x01000e00
-    str     r0, [r5, #0x8c]
-
-    ldr     r0, =0x01000000
-    str     r0, [r5, #0xcc]
-
-    ldr     r0, =0x01000201
-    str     r0, [r5, #0xd0]
-    ldr     r0, =0x00000200
-    str     r0, [r5, #0xd4]
-    ldr     r0, =0x00000102
-    str     r0, [r5, #0xd8]
-
-    ldr     r0, =0x02000802
-    str     r0, [r5, #0x104]
-    ldr     r0, =0x04080000
-    str     r0, [r5, #0x108]
-    ldr     r0, =0x04080408
-    str     r0, [r5, #0x10c]
-
-    ldr     r0, =0x04080408
-    str     r0, [r5, #0x110]
-    ldr     r0, =0x03060408
-    str     r0, [r5, #0x114]
-
     /* PHY setting for 266MHz */
     ldr     r0, =0x00000000
     str     r0, [r5, #0x200]
diff --git a/arch/arm/mach-mx5/mx50_wfi.S b/arch/arm/mach-mx5/mx50_wfi.S
index b0d984c4527d..3335b0f320a4 100644
--- a/arch/arm/mach-mx5/mx50_wfi.S
+++ b/arch/arm/mach-mx5/mx50_wfi.S
@@ -44,6 +44,12 @@ LoopCKE0:
     cmp     r0, r2
     beq     LoopCKE0
 
+    /* Check if Databahn is in SYNC or ASYNC mode. */
+    ldr      r4, [r5, #0xdc]
+    and     r4, r4, #0x30000
+    cmp    r4, #0x30000
+    beq     Sync_mode
+
     /* Set the DDR_CLKGATE to 0x1. */
     ldr      r0, [r6, #0x98]
     bic     r0, r0, #0x80000000
@@ -55,7 +61,59 @@ LoopCKE0:
     ldr      r0, [r6, #0x98]
     orr     r0, r0, #0xC0000000
     str     r0, [r6, #0x98]
+    b       Wfi_Done
+
+Sync_mode:
+    /* Check if PLL1 is sourcing SYS_CLK. */
+    ldr     r5, [r6, #0x90]
+    and    r5, r0, #0x1
+    cmp   r5, #0x1
+    beq    pll1_source
+
+    /* Set the SYS_XTAL_CLKGATE to 0x1. */
+    ldr      r0, [r6, #0x94]
+    bic     r0, r0, #0x80000000
+    str     r0, [r6, #0x94]
+
+    /* Set the SYS_XTAL_DIV to 0xF (1.6MHz) to reduce power.
+     * since this clock is not gated when ARM is in WFI.
+     */
+
+    ldr      r0, [r6, #0x94]
+    orr      r0, r0, #0x3c0
+    str      r0, [r6, #0x94]
+
+    b       do_wfi
+pll1_source:
+    /* Set the SYS_PLL_CLKGATE to 0x1. */
+    ldr      r0, [r6, #0x94]
+    bic     r0, r0, #0x40000000
+    str     r0, [r6, #0x94]
+
+do_wfi:
+    .long     0xe320f003              @ Opcode for WFI
+
+     cmp    r5, #1
+     beq    pll1_source1
+    /* Set the SYS_XTAL_DIV to 24MHz.*/
+    ldr      r0, [r6, #0x94]
+    bic      r0, r0, #0x3c0
+    orr      r0, r0, #0x40
+    str      r0, [r6, #0x94]
+
+    /* Set the SYS_XTAL_CLKGATE to 0x3. */
+    ldr      r0, [r6, #0x94]
+    orr     r0, r0, #0xC0000000
+    str     r0, [r6, #0x94]
+    b       Wfi_Done
+
+pll1_source1:
+    /* Set the SYS_PLL_CLKGATE to 0x3. */
+    ldr      r0, [r6, #0x94]
+    orr     r0, r0, #0x30000000
+    str     r0, [r6, #0x94]
 
+Wfi_Done:
     /* Restore registers */
     ldmfd sp!, {r3,r4,r5,r6,r7,r8,r9,r10,r11}
     mov     pc, lr
diff --git a/arch/arm/mach-mx5/system.c b/arch/arm/mach-mx5/system.c
index f12b2b3304b7..e513f8e34714 100644
--- a/arch/arm/mach-mx5/system.c
+++ b/arch/arm/mach-mx5/system.c
@@ -14,6 +14,8 @@
 #include <linux/kernel.h>
 #include <linux/clk.h>
 #include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pmic_external.h>
 #include <asm/io.h>
 #include <mach/hardware.h>
 #include <mach/clock.h>
@@ -37,12 +39,20 @@ extern int iram_ready;
 extern int dvfs_core_is_active;
 extern void __iomem *ccm_base;
 extern void __iomem *databahn_base;
+extern int low_bus_freq_mode;
 extern void (*wait_in_iram)(void *ccm_addr, void *databahn_addr);
-extern void *wait_in_iram_base;
 extern void mx50_wait(u32 ccm_base, u32 databahn_addr);
 extern void stop_dvfs(void);
+extern void *wait_in_iram_base;
+extern void __iomem *apll_base;
 
 static struct clk *gpc_dvfs_clk;
+static struct regulator *vpll;
+static struct clk *pll1_sw_clk;
+static struct clk *osc;
+static struct clk *pll1_main_clk;
+static struct clk *ddr_clk ;
+static int dvfs_core_paused;
 
 /* set cpu low power mode before WFI instruction */
 void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
@@ -159,16 +169,53 @@ static int arch_idle_mode = WAIT_UNCLOCKED_POWER_OFF;
 void arch_idle(void)
 {
 	if (likely(!mxc_jtag_enabled)) {
-		struct clk *ddr_clk = clk_get(NULL, "ddr_clk");
+		if (ddr_clk == NULL)
+			ddr_clk = clk_get(NULL, "ddr_clk");
 		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);
 		mxc_cpu_lp_set(arch_idle_mode);
+
 		if (cpu_is_mx50() && (clk_get_usecount(ddr_clk) == 0)) {
 			memcpy(wait_in_iram_base, mx50_wait, SZ_4K);
 			wait_in_iram = (void *)wait_in_iram_base;
-			wait_in_iram(ccm_base, databahn_base);
+			if (low_bus_freq_mode) {
+				u32 reg, cpu_podf;
+
+				reg = __raw_readl(apll_base + 0x50);
+				reg = 0x120490;
+				__raw_writel(reg, apll_base + 0x50);
+				reg = __raw_readl(apll_base + 0x80);
+				reg |= 1;
+				__raw_writel(reg, apll_base + 0x80);
+
+				/* Move ARM to be sourced from 24MHz XTAL.
+				 * when ARM is in WFI.
+				 */
+				if (pll1_sw_clk == NULL)
+					pll1_sw_clk = clk_get(NULL,
+							"pll1_sw_clk");
+				if (osc == NULL)
+					osc = clk_get(NULL, "lp_apm");
+				if (pll1_main_clk == NULL)
+					pll1_main_clk = clk_get(NULL,
+							"pll1_main_clk");
+
+				clk_set_parent(pll1_sw_clk, osc);
+				/* Set the ARM-PODF divider to 1. */
+				cpu_podf = __raw_readl(MXC_CCM_CACRR);
+				__raw_writel(0x01, MXC_CCM_CACRR);
+
+				wait_in_iram(ccm_base, databahn_base);
+
+				/* Set the ARM-POD divider back
+				 * to the original.
+				 */
+				__raw_writel(cpu_podf, MXC_CCM_CACRR);
+				clk_set_parent(pll1_sw_clk, pll1_main_clk);
+			} else
+				wait_in_iram(ccm_base, databahn_base);
 		} else
 			cpu_do_idle();
 		clk_disable(gpc_dvfs_clk);