ENGR00126863: MX50: Add LPAPM support

Add support for Low Power Audio Playback Mode. System will enter this mode
whenever no modules that need high bus frequencies are active. The LP domain
and DDR will run at 24Mhz and CPU is at 160MHz in LPAPM mode.

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

7 files changed, 841 insertions, 191 deletions

diff --git a/arch/arm/mach-mx5/Makefile b/arch/arm/mach-mx5/Makefile
index 8b92cf0dbb53..2cbfae762f25 100644
--- a/arch/arm/mach-mx5/Makefile
+++ b/arch/arm/mach-mx5/Makefile
@@ -10,7 +10,7 @@ sdram_autogating.o bus_freq.o usb_dr.o usb_h1.o usb_h2.o dummy_gpio.o  early_set
 
 obj-$(CONFIG_ARCH_MX51) += clock.o suspend.o
 obj-$(CONFIG_ARCH_MX53) += clock.o suspend.o
-obj-$(CONFIG_ARCH_MX50) += clock_mx50.o dmaengine.o dma-apbh.o mx50_suspend.o
+obj-$(CONFIG_ARCH_MX50) += clock_mx50.o dmaengine.o dma-apbh.o mx50_suspend.o mx50_ddr_freq.o
 
 obj-$(CONFIG_MACH_MX51_3DS) += mx51_3stack.o mx51_3stack_gpio.o mx51_3stack_pmic_mc13892.o
 obj-$(CONFIG_MACH_MX51_BABBAGE) += mx51_babbage.o mx51_babbage_pmic_mc13892.o
diff --git a/arch/arm/mach-mx5/bus_freq.c b/arch/arm/mach-mx5/bus_freq.c
index 49e50d08bb60..098b3357505a 100644
--- a/arch/arm/mach-mx5/bus_freq.c
+++ b/arch/arm/mach-mx5/bus_freq.c
@@ -27,12 +27,19 @@
 #include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
+#include <linux/iram_alloc.h>
+#include <linux/mutex.h>
 #include <mach/hardware.h>
 #include <mach/clock.h>
 #include <mach/mxc_dvfs.h>
 #include <mach/sdram_autogating.h>
+#include <asm/mach/map.h>
+#include <asm/cacheflush.h>
+#include <asm/tlb.h>
 #include "crm_regs.h"
 
+#define LP_LOW_VOLTAGE		1050000
+#define LP_NORMAL_VOLTAGE		1250000
 #define LP_APM_CLK   			24000000
 #define NAND_LP_APM_CLK			12000000
 #define AXI_A_NORMAL_CLK		166250000
@@ -43,6 +50,7 @@
 #define NFC_CLK_NORMAL_DIV      	4
 #define SPIN_DELAY	1000000 /* in nanoseconds */
 
+DEFINE_SPINLOCK(ddr_freq_lock);
 
 static unsigned long lp_normal_rate;
 static unsigned long lp_med_rate;
@@ -60,18 +68,15 @@ static struct clk *axi_a_clk;
 static struct clk *axi_b_clk;
 static struct clk *cpu_clk;
 static struct clk *ddr_hf_clk;
-static struct clk *nfc_clk;
 static struct clk *ahb_clk;
-static struct clk *vpu_clk;
-static struct clk *vpu_core_clk;
-static struct clk *emi_slow_clk;
 static struct clk *ddr_clk;
-static struct clk *ipu_clk;
 static struct clk *periph_apm_clk;
 static struct clk *lp_apm;
 static struct clk *osc;
 static struct clk *gpc_dvfs_clk;
 static struct clk *emi_garb_clk;
+static void __iomem *pll1_base;
+static void __iomem *pll4_base;
 
 struct regulator *lp_regulator;
 int low_bus_freq_mode;
@@ -91,9 +96,20 @@ int cpu_wp_nr;
 int lp_high_freq;
 int lp_med_freq;
 
+void enter_lpapm_mode_mx50(void);
+void enter_lpapm_mode_mx51(void);
+void exit_lpapm_mode_mx50(void);
+void exit_lpapm_mode_mx51(void);
+void *ddr_freq_change_iram_base;
+void (*change_ddr_freq)(void *ccm_addr, void *databahn_addr, u32 freq) = NULL;
+
+extern void mx50_ddr_freq_change(u32 ccm_base,
+					u32 databahn_addr, u32 freq);
 extern int dvfs_core_is_active;
 extern struct cpu_wp *(*get_cpu_wp)(int *wp);
 extern void propagate_rate(struct clk *tclk);
+extern void __iomem *ccm_base;
+extern void __iomem *databahn_base;
 
 struct dvfs_wp dvfs_core_setpoint[] = {
 						{33, 8, 33, 10, 10, 0x08},
@@ -101,9 +117,6 @@ struct dvfs_wp dvfs_core_setpoint[] = {
 						{28, 8, 33, 20, 30, 0x08},
 						{29, 0, 33, 20, 10, 0x08},};
 
-static void __iomem *pll1_base;
-static void __iomem *pll4_base;
-
 int set_low_bus_freq(void)
 {
 	u32 reg;
@@ -115,58 +128,34 @@ int set_low_bus_freq(void)
 
 	if (bus_freq_scaling_initialized) {
 		/* can not enter low bus freq, when cpu is in highest freq */
-		if (clk_get_rate(cpu_clk) != cpu_wp_tbl[cpu_wp_nr - 1].cpu_rate)
+		if (clk_get_rate(cpu_clk) !=
+				cpu_wp_tbl[cpu_wp_nr - 1].cpu_rate) {
 			return 0;
+		}
 
 		stop_dvfs_per();
 
 		stop_sdram_autogating();
-		/*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) && cpu_is_mx51()) {
+		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));
-			/* 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 AHB dividers to be 1. */
-			/* 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);
+			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);
 
-			low_bus_freq_mode = 1;
-			high_bus_freq_mode = 0;
-		} else {
+		} else if (cpu_is_mx53()) {
+			/*Change the DDR freq to 133Mhz. */
+			clk_set_rate(ddr_hf_clk,
+			     clk_round_rate(ddr_hf_clk, ddr_low_rate));
+
 			/* move cpu clk to pll2, 400 / 3 = 133Mhz for cpu  */
 			clk_set_parent(pll1_sw_clk, pll2);
 
@@ -216,11 +205,123 @@ int set_low_bus_freq(void)
 				__raw_writel(reg, pll4_base + MXC_PLL_DP_CTL);
 			}
 		}
-
 	}
 	return 0;
 }
 
+void enter_lpapm_mode_mx50()
+{
+	u32 reg;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ddr_freq_lock, flags);
+
+	/* 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.
+	 */
+	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);
+	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))
+		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);
+
+	local_flush_tlb_all();
+	flush_cache_all();
+
+	memcpy(ddr_freq_change_iram_base, mx50_ddr_freq_change, SZ_8K);
+	change_ddr_freq = (void *)ddr_freq_change_iram_base;
+
+	/* 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);
+
+	udelay(100);
+
+	spin_unlock_irqrestore(&ddr_freq_lock, flags);
+
+}
+
+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 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);
+}
+
 int set_high_bus_freq(int high_bus_freq)
 {
 	u32 reg;
@@ -233,43 +334,22 @@ 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_mx51()) {
+			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));
-				clk_set_parent(periph_apm_clk, pll3);
-				/* Set the dividers to the default dividers */
-				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 |= (3 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
-					| 4 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
-					| 4 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
-					| 4 << 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);
-
-				low_bus_freq_mode = 0;
-				high_bus_freq_mode = 1;
-				clk_disable(emi_garb_clk);
-
-				/*Set the main_bus_clk parent to be PLL2. */
-				clk_set_parent(main_bus_clk, pll2);
+				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 {
+			} else if (cpu_is_mx53()) {
 				/* move cpu clk to pll1 */
 				reg = __raw_readl(MXC_CCM_CDHIPR);
 				if ((reg & MXC_CCM_CDHIPR_ARM_PODF_BUSY) == 0)
@@ -309,12 +389,10 @@ int set_high_bus_freq(int high_bus_freq)
 
 				propagate_rate(main_bus_clk);
 				propagate_rate(pll1_sw_clk);
+				/*Change the DDR freq to mormal_rate*/
+				clk_set_rate(ddr_hf_clk,
+					clk_round_rate(ddr_hf_clk, ddr_normal_rate));
 			}
-
-			/*Change the DDR freq to mormal_rate*/
-			clk_set_rate(ddr_hf_clk,
-			    clk_round_rate(ddr_hf_clk, ddr_normal_rate));
-
 			start_dvfs_per();
 		}
 		if (bus_freq_scaling_is_active) {
@@ -355,11 +433,117 @@ int set_high_bus_freq(int high_bus_freq)
 	return 0;
 }
 
+void exit_lpapm_mode_mx50()
+{
+	u32 reg, ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ddr_freq_lock, flags);
+
+	local_flush_tlb_all();
+	flush_cache_all();
+
+	memcpy(ddr_freq_change_iram_base, mx50_ddr_freq_change, SZ_8K);
+	change_ddr_freq = (void *)ddr_freq_change_iram_base;
+
+	/* Set the DDR to default freq.
+	 */
+	change_ddr_freq(ccm_base, databahn_base, ddr_normal_rate);
+
+	udelay(100);
+
+	/* 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);
+
+	/* 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);
+
+	/* 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)
+		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 */
+	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
+		|1 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
+		|2 << 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) & 0xF)
+		udelay(10);
+
+	low_bus_freq_mode = 0;
+	high_bus_freq_mode = 1;
+
+	/*Set the main_bus_clk parent to be PLL2. */
+	clk_set_parent(main_bus_clk, pll2);
+	spin_unlock_irqrestore(&ddr_freq_lock, flags);
+
+	udelay(100);
+}
+
+void exit_lpapm_mode_mx51()
+{
+	u32 reg;
+
+	clk_set_parent(periph_apm_clk, pll3);
+
+	/* Set the dividers to the default dividers */
+	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 |= (3 << MXC_CCM_CBCDR_AXI_A_PODF_OFFSET
+		| 4 << MXC_CCM_CBCDR_AXI_B_PODF_OFFSET
+		| 4 << MXC_CCM_CBCDR_AHB_PODF_OFFSET
+		| 4 << 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);
+
+	low_bus_freq_mode = 0;
+	high_bus_freq_mode = 1;
+	clk_disable(emi_garb_clk);
+
+	/*Set the main_bus_clk parent to be PLL2. */
+	clk_set_parent(main_bus_clk, pll2);
+
+	/*Change the DDR freq to 200MHz*/
+	clk_set_rate(ddr_hf_clk,
+	    clk_round_rate(ddr_hf_clk, ddr_normal_rate));
+}
+
 int low_freq_bus_used(void)
 {
-	if ((clk_get_usecount(ipu_clk) == 0)
-	    && (clk_get_usecount(vpu_clk) == 0)
-	    && (lp_high_freq == 0)
+	if ((lp_high_freq == 0)
 	    && (lp_med_freq == 0))
 		return 1;
 	else
@@ -438,9 +622,11 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 {
 	int err = 0;
 	unsigned long pll2_rate, pll1_rate;
+	unsigned long iram_paddr;
 
 	pll1_base = ioremap(MX53_BASE_ADDR(PLL1_BASE_ADDR), SZ_4K);
-	pll4_base = ioremap(MX53_BASE_ADDR(PLL4_BASE_ADDR), SZ_4K);
+	if (cpu_is_mx53())
+		pll4_base = ioremap(MX53_BASE_ADDR(PLL4_BASE_ADDR), SZ_4K);
 
 	busfreq_dev = &pdev->dev;
 
@@ -475,39 +661,12 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 		return PTR_ERR(pll3);
 	}
 
-	pll4 = clk_get(NULL, "pll4");
-	if (IS_ERR(pll4)) {
-		printk(KERN_DEBUG "%s: failed to get pll4\n", __func__);
-		return PTR_ERR(pll4);
-	}
-
-	pll1_rate = clk_get_rate(pll1_sw_clk);
-	pll2_rate = clk_get_rate(pll2);
-
-	if (pll2_rate == 665000000) {
-		/* for mx51 */
-		lp_normal_rate = pll2_rate / 5;
-		lp_med_rate = pll2_rate / 8;
-		ddr_normal_rate = pll1_rate / 4; /* 200M */
-		ddr_low_rate = pll1_rate / 6; /* 133M */
-	} else if (pll2_rate == 600000000) {
-		/* for mx53 evk rev.A */
-		lp_normal_rate = pll2_rate / 5;
-		lp_med_rate = pll2_rate / 8;
-		ddr_normal_rate = pll2_rate / 2;
-		ddr_low_rate = pll2_rate / 2;
-	} else if (pll2_rate == 400000000) {
-		/* for mx53 evk rev.B */
-		lp_normal_rate = pll2_rate / 3;
-		lp_med_rate = pll2_rate / 5;
-		ddr_normal_rate = pll2_rate / 1;
-		ddr_low_rate = pll2_rate / 3;
-	}
-
-	pll3 = clk_get(NULL, "pll3");
-	if (IS_ERR(pll3)) {
-		printk(KERN_DEBUG "%s: failed to get pll3\n", __func__);
-		return PTR_ERR(pll3);
+	if (cpu_is_mx53()) {
+		pll4 = clk_get(NULL, "pll4");
+		if (IS_ERR(pll4)) {
+			printk(KERN_DEBUG "%s: failed to get pll4\n", __func__);
+			return PTR_ERR(pll4);
+		}
 	}
 
 	axi_a_clk = clk_get(NULL, "axi_a_clk");
@@ -524,29 +683,19 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 		return PTR_ERR(axi_b_clk);
 	}
 
-	if (cpu_is_mx51())
-		ddr_hf_clk = clk_get(NULL, "ddr_hf_clk");
-	else
-		ddr_hf_clk = clk_get(NULL, "axi_a_clk");
-
-	if (IS_ERR(ddr_hf_clk)) {
-		printk(KERN_DEBUG "%s: failed to get ddr_hf_clk\n",
+	ddr_clk = clk_get(NULL, "ddr_clk");
+	if (IS_ERR(ddr_clk)) {
+		printk(KERN_DEBUG "%s: failed to get ddr_clk\n",
 		       __func__);
-		return PTR_ERR(ddr_hf_clk);
+		return PTR_ERR(ddr_clk);
 	}
 
-	emi_slow_clk = clk_get(NULL, "emi_slow_clk");
-	if (IS_ERR(emi_slow_clk)) {
-		printk(KERN_DEBUG "%s: failed to get emi_slow_clk\n",
-		       __func__);
-		return PTR_ERR(emi_slow_clk);
-	}
+	ddr_hf_clk = clk_get_parent(ddr_clk);
 
-	nfc_clk = clk_get(NULL, "nfc_clk");
-	if (IS_ERR(nfc_clk)) {
-		printk(KERN_DEBUG "%s: failed to get nfc_clk\n",
+	if (IS_ERR(ddr_hf_clk)) {
+		printk(KERN_DEBUG "%s: failed to get ddr_hf_clk\n",
 		       __func__);
-		return PTR_ERR(nfc_clk);
+		return PTR_ERR(ddr_hf_clk);
 	}
 
 	ahb_clk = clk_get(NULL, "ahb_clk");
@@ -556,20 +705,6 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 		return PTR_ERR(ahb_clk);
 	}
 
-	vpu_core_clk = clk_get(NULL, "vpu_core_clk");
-	if (IS_ERR(vpu_core_clk)) {
-		printk(KERN_DEBUG "%s: failed to get vpu_core_clk\n",
-		       __func__);
-		return PTR_ERR(vpu_core_clk);
-	}
-
-	ddr_clk = clk_get(NULL, "ddr_clk");
-	if (IS_ERR(ddr_clk)) {
-		printk(KERN_DEBUG "%s: failed to get ddr_clk\n",
-		       __func__);
-		return PTR_ERR(ddr_clk);
-	}
-
 	cpu_clk = clk_get(NULL, "cpu_clk");
 	if (IS_ERR(cpu_clk)) {
 		printk(KERN_DEBUG "%s: failed to get cpu_clk\n",
@@ -577,35 +712,25 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 		return PTR_ERR(cpu_clk);
 	}
 
-	ipu_clk = clk_get(NULL, "ipu_clk");
-	if (IS_ERR(ipu_clk)) {
-		printk(KERN_DEBUG "%s: failed to get ipu_clk\n",
-		       __func__);
-		return PTR_ERR(ipu_clk);
-	}
-
 	if (cpu_is_mx51())
 		emi_garb_clk = clk_get(NULL, "emi_garb_clk");
-	else
+	else if (cpu_is_mx53())
 		emi_garb_clk = clk_get(NULL, "emi_intr_clk.1");
+	else
+		emi_garb_clk = clk_get(NULL, "ocram_clk");
 	if (IS_ERR(emi_garb_clk)) {
 		printk(KERN_DEBUG "%s: failed to get emi_garb_clk\n",
 		       __func__);
 		return PTR_ERR(emi_garb_clk);
 	}
 
-	vpu_clk = clk_get(NULL, "vpu_clk");
-	if (IS_ERR(vpu_clk)) {
-		printk(KERN_DEBUG "%s: failed to get vpu_clk\n",
-		       __func__);
-		return PTR_ERR(vpu_clk);
-	}
-
-	periph_apm_clk = clk_get(NULL, "periph_apm_clk");
-	if (IS_ERR(periph_apm_clk)) {
-		printk(KERN_DEBUG "%s: failed to get periph_apm_clk\n",
-		       __func__);
-		return PTR_ERR(periph_apm_clk);
+	if (cpu_is_mx51()  || cpu_is_mx53()) {
+		periph_apm_clk = clk_get(NULL, "periph_apm_clk");
+		if (IS_ERR(periph_apm_clk)) {
+			printk(KERN_DEBUG "%s: failed to get periph_apm_clk\n",
+			       __func__);
+			return PTR_ERR(periph_apm_clk);
+		}
 	}
 
 	lp_apm = clk_get(NULL, "lp_apm");
@@ -634,6 +759,49 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 		return err;
 	}
 
+	pll1_rate = clk_get_rate(pll1_sw_clk);
+	pll2_rate = clk_get_rate(pll2);
+
+	if (pll2_rate == 665000000) {
+		/* for mx51 */
+		lp_normal_rate = pll2_rate / 5;
+		lp_med_rate = pll2_rate / 8;
+		ddr_normal_rate = pll1_rate / 4; /* 200M */
+		ddr_low_rate = pll1_rate / 6; /* 133M */
+	} else if (pll2_rate == 600000000) {
+		/* for mx53 evk rev.A */
+		lp_normal_rate = pll2_rate / 5;
+		lp_med_rate = pll2_rate / 8;
+		ddr_normal_rate = pll2_rate / 2;
+		ddr_low_rate = pll2_rate / 2;
+	} else if (pll2_rate == 400000000) {
+		/* for mx53 evk rev.B */
+		lp_normal_rate = pll2_rate / 3;
+		lp_med_rate = pll2_rate / 5;
+		if (cpu_is_mx53()) {
+			ddr_normal_rate = pll2_rate / 1;
+			ddr_low_rate = pll2_rate / 3;
+		} else if (cpu_is_mx50()) {
+			ddr_normal_rate = clk_get_rate(ddr_clk);
+			ddr_low_rate = LP_APM_CLK;
+		}
+	}
+	if (cpu_is_mx50()) {
+		iram_alloc(SZ_8K, &iram_paddr);
+		/* Need to remap the area here since we want the memory region
+			 to be executable. */
+		ddr_freq_change_iram_base = __arm_ioremap(iram_paddr,
+							SZ_8K, MT_HIGH_VECTORS);
+		memcpy(ddr_freq_change_iram_base, mx50_ddr_freq_change, SZ_8K);
+		change_ddr_freq = (void *)ddr_freq_change_iram_base;
+
+		lp_regulator = regulator_get(NULL, "SW2");
+		if (IS_ERR(lp_regulator)) {
+			printk(KERN_DEBUG
+			"%s: failed to get lp regulator\n", __func__);
+			return PTR_ERR(lp_regulator);
+		}
+	}
 	cpu_wp_tbl = get_cpu_wp(&cpu_wp_nr);
 	low_bus_freq_mode = 0;
 	high_bus_freq_mode = 1;
diff --git a/arch/arm/mach-mx5/clock_mx50.c b/arch/arm/mach-mx5/clock_mx50.c
index 694d19a80b66..794361ae0de0 100644
--- a/arch/arm/mach-mx5/clock_mx50.c
+++ b/arch/arm/mach-mx5/clock_mx50.c
@@ -1173,15 +1173,25 @@ static int _clk_sys_clk_enable(struct clk *clk)
 
 static void _clk_sys_clk_disable(struct clk *clk)
 {
-	u32 reg;
+	u32 reg, reg1;
 
+	reg1 = (__raw_readl(databahn + DATABAHN_CTL_REG55))
+					& DDR_SYNC_MODE;
 	reg = __raw_readl(MXC_CCM_CLK_SYS);
 	reg &= ~(MXC_CCM_CLK_SYS_SYS_XTAL_CLKGATE_MASK |
 				MXC_CCM_CLK_SYS_SYS_PLL_CLKGATE_MASK);
 	if (__raw_readl(MXC_CCM_CLKSEQ_BYPASS) & 0x1)
 		reg |= 1 << MXC_CCM_CLK_SYS_SYS_PLL_CLKGATE_OFFSET;
-	else
-		reg |= 1 << MXC_CCM_CLK_SYS_SYS_XTAL_CLKGATE_OFFSET;
+	else {
+		/* If DDR is sourced from SYS_CLK (in Sync mode), we cannot
+		 * gate its clock when ARM is in wait if the DDR is not in
+		 * self refresh.
+		 */
+		if (reg1 == DDR_SYNC_MODE)
+			reg |= 3 << MXC_CCM_CLK_SYS_SYS_XTAL_CLKGATE_OFFSET;
+		else
+			reg |= 1 << MXC_CCM_CLK_SYS_SYS_XTAL_CLKGATE_OFFSET;
+	}
 	__raw_writel(reg, MXC_CCM_CLK_SYS);
 }
 
@@ -2379,14 +2389,6 @@ static int _clk_ddr_enable(struct clk *clk)
 
 static void _clk_ddr_disable(struct clk *clk)
 {
-	u32 reg, reg1;
-	reg = __raw_readl(MXC_CCM_CLK_DDR);
-	reg &= ~MXC_CCM_CLK_DDR_DDR_CLKGATE_MASK;
-	reg1 = (__raw_readl(databahn + DATABAHN_CTL_REG55))
-					& DDR_SYNC_MODE;
-	if (reg1 != DDR_SYNC_MODE)
-		reg |= 1 << MXC_CCM_CLK_DDR_DDR_CLKGATE_OFFSET;
-	__raw_writel(reg, MXC_CCM_CLK_DDR);
 	_clk_disable_inwait(clk);
 }
 
@@ -2683,6 +2685,7 @@ static struct clk pxp_axi_clk = {
 	.disable = _clk_disable,
 	.enable_reg = MXC_CCM_CCGR6,
 	.enable_shift = MXC_CCM_CCGR6_CG9_OFFSET,
+	.flags = AHB_MED_SET_POINT | CPU_FREQ_TRIG_UPDATE,
 };
 
 static struct clk elcdif_axi_clk = {
@@ -3510,6 +3513,10 @@ int __init mx50_clocks_init(unsigned long ckil, unsigned long osc, unsigned long
 	clk_set_parent(&weim_clk[0], &ahb_clk);
 	clk_set_rate(&weim_clk[0], clk_round_rate(&weim_clk[0], 130000000));
 
+	/* Do the following just to disable the PLL since its not used */
+	clk_enable(&pll3_sw_clk);
+	clk_disable(&pll3_sw_clk);
+
 	base = ioremap(MX53_BASE_ADDR(GPT1_BASE_ADDR), SZ_4K);
 	mxc_timer_init(&gpt_clk[0], base, MXC_INT_GPT);
 	return 0;
diff --git a/arch/arm/mach-mx5/cpu.c b/arch/arm/mach-mx5/cpu.c
index 7d4ecaa727ff..2dd93a64e972 100644
--- a/arch/arm/mach-mx5/cpu.c
+++ b/arch/arm/mach-mx5/cpu.c
@@ -42,6 +42,7 @@
 void __iomem *arm_plat_base;
 void __iomem *gpc_base;
 void __iomem *ccm_base;
+void __iomem *databahn_base;
 
 struct cpu_wp *(*get_cpu_wp)(int *wp);
 void (*set_num_cpu_wp)(int num);
@@ -184,6 +185,7 @@ static int __init post_cpu_init(void)
 		iounmap(base);
 	}
 
+	databahn_base = ioremap(MX50_DATABAHN_BASE_ADDR, SZ_16K);
 	return 0;
 }
 
diff --git a/arch/arm/mach-mx5/mx50_arm2.c b/arch/arm/mach-mx5/mx50_arm2.c
index 204f4c1b58a8..121a51256b85 100644
--- a/arch/arm/mach-mx5/mx50_arm2.c
+++ b/arch/arm/mach-mx5/mx50_arm2.c
@@ -309,7 +309,7 @@ static struct cpu_wp cpu_wp_auto[] = {
 	 .cpu_voltage = 1050000,},
 	{
 	 .pll_rate = 800000000,
-	 .cpu_rate = 166250000,
+	 .cpu_rate = 160000000,
 	 .pdf = 4,
 	 .mfi = 8,
 	 .mfd = 2,
@@ -1149,9 +1149,10 @@ static void __init mxc_board_init(void)
 	mxc_register_device(&mxc_pxp_client_device, NULL);
 	mxc_register_device(&mxc_pxp_v4l2, NULL);
 	mxc_register_device(&mxc_dvfs_core_device, &dvfs_core_data);
+	mxc_register_device(&busfreq_device, NULL);
+
 	/*
 	mxc_register_device(&mx53_lpmode_device, NULL);
-	mxc_register_device(&busfreq_device, NULL);
 	mxc_register_device(&mxc_dvfs_per_device, &dvfs_per_data);
 	*/
 
diff --git a/arch/arm/mach-mx5/mx50_ddr_freq.S b/arch/arm/mach-mx5/mx50_ddr_freq.S
new file mode 100644
index 000000000000..f478dbe42006
--- /dev/null
+++ b/arch/arm/mach-mx5/mx50_ddr_freq.S
@@ -0,0 +1,471 @@
+/*
+ * Copyright (C) 2010 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.
+ */
+
+#include <linux/linkage.h>
+
+/*
+ *  mx50_ddr_freq_change
+ *
+ *  Idle the processor (eg, wait for interrupt).
+ *  Make sure DDR is in self-refresh.
+ *  IRQs are already disabled.
+ */
+ENTRY(mx50_ddr_freq_change)
+    stmfd   sp!, {r3,r4,r5,r6}      @ Save registers
+
+    mov    r6, r0                       @save CCM address
+    mov    r5, r1                       @save DataBahn address
+    mov    r4, r2                       @save new freq requested
+
+    ldr     r0, [r6, #0x90]
+
+    /* If Databahn is in LPM4, exit that mode first. */
+    ldr     r1,[r5, #0x50]             @Store LPM mode in r1.
+    mov   r0, r1
+    bic     r0, #0x1F
+    str     r0,[r5, #0x50]
+
+    /*
+     * Make sure the DDR is self-refresh, before switching its frequency
+     * and clock source
+     */
+
+    /* Step 1: Enter self-refresh mode */
+    ldr     r0,[r5, #0x4c]
+    orr     r0,r0,#0x1
+    str     r0,[r5, #0x4c]
+
+     /* Step 2: Poll the CKE_STATUS bit. */
+LoopCKE0:
+    /* Wait for CKE = 0 */
+    ldr     r0,[r5, #0xfc]
+    and     r0, r0, #0x10000
+    ldr     r2, =0x10000
+    cmp     r0, r2
+    beq     LoopCKE0
+
+    /* Step 3: Mask the DLL lock state change, set bit 8 in int_mask. */
+    ldr    r0, [r5, #0xac]
+    orr    r0, r0, #0x100
+    str    r0, [r5, #0xac]
+
+    /* Step 4: Stop the Controller. */
+    ldr     r0,[r5]
+    bic     r0, r0, #0x1
+    str     r0,[r5]
+
+    /* Step 5: Clear the DLL lock state change bit 8 in int_ack */
+    ldr     r0, [r5, #0xa8]
+    orr     r0, r0, #0x1000000
+    str     r0, [r5, #0xa8]
+
+    /* Step 6: Clear the interrupt mask for DLL lock state.
+      * Bit 8 in int_mask */
+    ldr     r0, [r5, #0xac]
+    bic     r0, r0, #0x100
+    str     r0, [r5, #0xac]
+
+     /* Change the freq now */
+     /* If the freq req is below 200MHz, set DDR to synchronous mode.
+      * else set to async mode. */
+    ldr     r0, =200000000
+    cmp   r4, r0
+    bgt    Async_Mode
+
+     /* Set the DDR to be Synchronous
+     mode. */
+    /* Set the Databahn to sync mode. */
+    ldr     r0, [r5, #0xdc]
+    orr     r0, r0, #0x30000
+    str    r0, [r5, #0xdc]
+
+    /* Turn OFF the DDR_CKLGATE_MASK in MXC_CCM_DDR */
+    ldr     r0, [r6, #0x98]
+    bic     r0, r0, #0xC0000000
+    str     r0, [r6, #0x98]
+
+    /* Check if XTAL can source the DDR. */
+    ldr     r0, =24000000
+    cmp   r4, r0
+    ble    databahn_ddr_24
+
+    /*Source DDR from PLL1.  Setup the dividers accordingly. */
+    ldr    r0, =800000000
+    ldr    r3, =1
+Loop1:
+    sub    r0, r0, r4
+    cmp   r0, r4
+    blt     Div_Found
+    add    r3, r3, #1
+    bgt     Loop1
+
+Div_Found:
+    ldr     r0, [r6, #0x94]
+    bic     r0, r0, #0x3f
+    orr     r0, r0, r3
+    str     r0, [r6, #0x94]
+    /* Set the DDR to sourced from PLL1 in sync path */
+    ldr     r0, [r6, #0x90]
+    orr     r0, r0, #0x3
+    str     r0, [r6, #0x90]
+
+    /* Turn OFF the DDR_CKLGATE_MASK in MXC_CCM_DDR */
+    ldr     r0, [r6, #0x98]
+    bic     r0, r0, #0xC0000000
+    str     r0, [r6, #0x98]
+
+    ldr     r0, =24000000
+    cmp   r4, r0
+    beq   databahn_ddr_24
+
+    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
+    str    r0, [r5, #0x200]
+    ldr    r0, =0x000f1100
+    str    r0, [r5, #0x204]
+    ldr    r0, =0xf3003a27
+    str    r0, [r5, #0x208]
+    ldr    r0, =0x074002c1
+    str    r0, [r5, #0x20c]
+
+    ldr    r0, =0xf3003a27
+    str    r0, [r5, #0x210]
+    ldr    r0, =0x074002c1
+    str    r0, [r5, #0x214]
+    ldr    r0, =0xf3003a27
+    str    r0, [r5, #0x218]
+    ldr    r0, =0x074002c1
+    str    r0, [r5, #0x21c]
+
+    ldr    r0, =0xf3003a27
+    str    r0, [r5, #0x220]
+    ldr    r0, =0x074002c1
+    str    r0, [r5, #0x224]
+    ldr    r0, =0xf3003a27
+    str    r0, [r5, #0x228]
+    ldr    r0, =0x074002c1
+    str    r0, [r5, #0x22c]
+
+    ldr    r0, =0x00810004
+    str    r0, [r5, #0x234]
+    ldr     r0, =0x30219f14
+    str    r0, [r5, #0x238]
+    ldr    r0, =0x00219f01
+    str    r0, [r5, #0x23c]
+
+    ldr     r0, =0x30219f14
+    str    r0, [r5, #0x240]
+    ldr    r0, =0x00219f01
+    str    r0, [r5, #0x244]
+    ldr     r0, =0x30219f14
+    str    r0, [r5, #0x248]
+    ldr    r0, =0x00219f01
+    str    r0, [r5, #0x24c]
+
+    ldr     r0, =0x30219f14
+    str    r0, [r5, #0x250]
+    ldr    r0, =0x00219f01
+    str    r0, [r5, #0x254]
+    ldr     r0, =0x30219f14
+    str    r0, [r5, #0x258]
+    ldr    r0, =0x00219f01
+    str    r0, [r5, #0x25c]
+
+    b       Setup_Done
+
+Async_Mode:
+    /* Set the Databahn to async mode. */
+    ldr     r0, [r5, #0xdc]
+    and    r0, r0, #0xfffcffff
+    str    r0, [r5, #0xdc]
+
+    /*Source DDR from PLL1.  Setup the dividers accordingly. */
+    ldr    r0, =800000000
+    ldr    r3, =1
+Loop2:
+    sub    r0, r0, r4
+    cmp   r0, r4
+    blt     Div_Found1
+    add    r3, r3, #1
+    bgt     Loop2
+
+Div_Found1:
+    ldr     r0, [r6, #0x98]
+    bic    r0, r0, #0x3f
+    orr     r0, r0, r3
+    str     r0, [r6, #0x98]
+
+    /* Set the DDR to sourced from PLL1 in async path */
+    ldr     r0, [r6, #0x98]
+    bic     r0, r0, #0x40
+    str     r0, [r6, #0x98]
+
+    /* Turn ON the DDR_CKLGATE_MASK in MXC_CCM_DDR */
+    ldr     r0, [r6, #0x98]
+    orr     r0, r0, #0x40000000
+    str     r0, [r6, #0x98]
+
+    ldr     r0, =24000000
+    cmp   r4, r0
+    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]
+    ldr     r0, =0x00000000
+    str     r0, [r5, #0x204]
+    ldr     r0, =0xf5003a27
+    str     r0, [r5, #0x208]
+
+    ldr     r0, =0xf5003a27
+    str     r0, [r5, #0x210]
+    ldr     r0, =0xf5003a27
+    str     r0, [r5, #0x218]
+
+    ldr     r0, =0xf5003a27
+    str     r0, [r5, #0x220]
+    ldr     r0, =0xf5003a27
+    str     r0, [r5, #0x228]
+
+    ldr     r0, =0x074002e1
+    str     r0, [r5, #0x20c]
+    ldr     r0, =0x074002e1
+    str     r0, [r5, #0x214]
+    ldr     r0, =0x074002e1
+    str     r0, [r5, #0x21c]
+    ldr     r0, =0x074002e1
+    str     r0, [r5, #0x224]
+    ldr     r0, =0x074002e1
+    str     r0, [r5, #0x22c]
+
+    ldr     r0, =0x00810006
+    str     r0, [r5, #0x234]
+    ldr     r0, =0x20099414
+    str     r0, [r5, #0x238]
+    ldr     r0, =0x000a1401
+    str     r0, [r5, #0x23c]
+
+    ldr     r0, =0x20099414
+    str     r0, [r5, #0x240]
+    ldr     r0, =0x000a1401
+    str     r0, [r5, #0x244]
+    ldr     r0, =0x20099414
+    str     r0, [r5, #0x248]
+    ldr     r0, =0x000a1401
+    str     r0, [r5, #0x24c]
+
+    ldr     r0, =0x20099414
+    str     r0, [r5, #0x250]
+    ldr     r0, =0x000a1401
+    str     r0, [r5, #0x254]
+    ldr     r0, =0x20099414
+    str     r0, [r5, #0x258]
+    ldr     r0, =0x000a1401
+    str     r0, [r5, #0x25c]
+
+    b       Setup_Done
+
+
+Setup_Done:
+    /* Start controller */
+    ldr     r0,[r5]
+    orr     r0, r0,#0x1
+    str     r0,[r5]
+
+    /* Poll the DLL lock state change in int_status reg*/
+    /* DLL is bypassed in the 24MHz mode, so no waiting for DLL to lock. */
+    ldr     r0, =24000000
+    cmp   r4, r0
+    beq    Exit_Self_Refresh
+DllLock:
+    ldr     r0, [r5, #0xa8]
+    and    r0, r0, #0x100
+    ldr     r2, =0x100
+    cmp   r0, r2
+    bne    DllLock
+
+    /*Leave self-refresh mode */
+Exit_Self_Refresh:
+    ldr     r0,[r5, #0x4c]
+    and    r0,r0,#0xfffffffe
+    str     r0,[r5, #0x4c]
+
+LoopCKE1:
+    /*Wait for CKE = 1 */
+    ldr     r0,[r5, #0xfc]
+    and    r0, r0, #0x10000
+    ldr     r2, =0x10000
+    cmp     r0, r2
+    bne     LoopCKE1
+
+    /* Put the databahn back to into the LPM mode. */
+    str     r1,[r5, #0x50]
+
+    /* Restore registers */
+    ldmfd sp!, {r3,r4,r5,r6}
+    mov     pc, lr
+
+    .type   mx50_do_ddr_freq_change, #object
+ENTRY(mx50_do_ddr_freq_change)
+    .word   mx50_ddr_freq_change
+    .size    mx50_ddr_freq_change, . - mx50_ddr_freq_change
diff --git a/arch/arm/plat-mxc/clock.c b/arch/arm/plat-mxc/clock.c
index 27a559657be0..f271091a7620 100644
--- a/arch/arm/plat-mxc/clock.c
+++ b/arch/arm/plat-mxc/clock.c
@@ -4,7 +4,7 @@
  * Copyright (C) 2004 - 2005 Nokia corporation
  * Written by Tuukka Tikkanen <tuukka.tikkanen@elektrobit.com>
  * Modified for omap shared clock framework by Tony Lindgren <tony@atomide.com>
- * Copyright 2007-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2007-2010 Freescale Semiconductor, Inc.
  * Copyright 2008 Juergen Beisert, kernel@pengutronix.de
  *
  * This program is free software; you can redistribute it and/or
@@ -173,14 +173,8 @@ int clk_enable(struct clk *clk)
 	if (clk == NULL || IS_ERR(clk))
 		return -EINVAL;
 
-	spin_lock_irqsave(&clockfw_lock, flags);
-
-	ret = __clk_enable(clk);
-
-	spin_unlock_irqrestore(&clockfw_lock, flags);
-
 	if ((clk->flags & CPU_FREQ_TRIG_UPDATE)
-			&& (clk_get_usecount(clk) == 1)) {
+			&& (clk_get_usecount(clk) == 0)) {
 #if (defined(CONFIG_ARCH_MX5) || defined(CONFIG_ARCH_MX37))
 		if (low_freq_bus_used() && !low_bus_freq_mode)
 			set_low_bus_freq();
@@ -200,6 +194,13 @@ int clk_enable(struct clk *clk)
 #endif
 	}
 
+
+	spin_lock_irqsave(&clockfw_lock, flags);
+
+	ret = __clk_enable(clk);
+
+	spin_unlock_irqrestore(&clockfw_lock, flags);
+
 	return ret;
 }
 EXPORT_SYMBOL(clk_enable);
@@ -228,12 +229,12 @@ void clk_disable(struct clk *clk)
 			set_low_bus_freq();
 		else {
 			if (!high_bus_freq_mode) {
-				/* Currently at ow or medium set point,
+				/* Currently at low or medium set point,
 				  * need to set to high setpoint
 				  */
 				set_high_bus_freq(0);
 			} else if (high_bus_freq_mode || low_bus_freq_mode) {
-				/* Currently at ow or high set point,
+				/* Currently at low or high set point,
 				  * need to set to medium setpoint
 				  */
 				set_high_bus_freq(0);