ENGR00116479: Ported DVFS-CORE and audio pop issue from Apollo project.

Fix the audio pop issue that is caused whenever the system enters and
exits LP-APM mode.
Added some more working points for DVFS-CORE.

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

12 files changed, 323 insertions, 664 deletions

diff --git a/arch/arm/mach-mx37/Makefile b/arch/arm/mach-mx37/Makefile
index 54d3fa1d71ed..5a484de73ebd 100644
--- a/arch/arm/mach-mx37/Makefile
+++ b/arch/arm/mach-mx37/Makefile
@@ -4,7 +4,7 @@
 
 # Object file lists.
 
-obj-y   := system.o iomux.o cpu.o mm.o clock.o devices.o serial.o dma.o lpmodes.o dptc.o bus_freq.o
+obj-y   := system.o iomux.o cpu.o mm.o clock.o devices.o serial.o dma.o dptc.o bus_freq.o
 
 
 obj-$(CONFIG_MACH_MX37_3DS) += mx37_3stack.o mx37_3stack_gpio.o 
diff --git a/arch/arm/mach-mx37/bus_freq.c b/arch/arm/mach-mx37/bus_freq.c
index 6af034605451..d6cde4a21487 100644
--- a/arch/arm/mach-mx37/bus_freq.c
+++ b/arch/arm/mach-mx37/bus_freq.c
@@ -39,6 +39,7 @@
 #define LP_LPM_VOLTAGE 1050000
 #define LP_LOWFREQ_VOLTAGE 1050000
 #define LP_NORMAL_VOLTAGE 1200000
+#define GP_LPAPM_FREQ   200000000
 
 DEFINE_SPINLOCK(bus_freq_lock);
 
@@ -69,70 +70,47 @@ char *gp_reg_id = "SW1";
 char *lp_reg_id = "SW2";
 static struct cpu_wp *cpu_wp_tbl;
 
-struct dvfs_wp dvfs_core_setpoint[] = {{33, 7, 33, 10, 10, 0x10},
-					    {22, 0, 33, 10, 10, 0x10},};
+struct dvfs_wp dvfs_core_setpoint[] = {
+						{33, 8, 33, 10, 10, 0x08},
+						{26, 0, 33, 20, 10, 0x08},
+						{28, 8, 33, 20, 30, 0x08},
+						{26, 0, 33, 20, 10, 0x08},};
 
 int set_low_bus_freq(void)
 {
 	int ret = 0;
-	unsigned long lp_lpm_clk;
 	unsigned long flags;
+	int reg;
+	unsigned long lp_lpm_clk;
 
-	struct clk *p_clk;
-	struct clk *amode_parent_clk;
-
-	if (low_bus_freq_mode)
-		return ret;
-
-	if (clk_get_rate(cpu_clk) != 200000000)
-		return;
-
-	clk_disable(uart_clk);
-
-	lp_lpm_clk = clk_get_rate(lp_apm);
-	amode_parent_clk = lp_apm;
-	p_clk = clk_get_parent(periph_apm_clk);
 	spin_lock_irqsave(&bus_freq_lock, flags);
-	/* Make sure osc_clk is the parent of lp_apm. */
-	if (clk_get_parent(amode_parent_clk) != osc)
-		clk_set_parent(amode_parent_clk, osc);
-
-	/* Set the parent of periph_apm_clk to be lp_apm */
-	clk_set_parent(periph_apm_clk, amode_parent_clk);
-	amode_parent_clk = periph_apm_clk;
-
-	p_clk = clk_get_parent(main_bus_clk);
-	/* Set the parent of main_bus_clk to be periph_apm_clk */
-	clk_set_parent(main_bus_clk, amode_parent_clk);
 
-	clk_set_rate(ahb_clk, lp_lpm_clk);
-	/* Set the emi_internal clock to 24MHz */
-	clk_set_rate(emi_intr_clk, lp_lpm_clk);
-	if (clk_get_parent(emi_core_clk) != ahb_clk)
-		clk_set_rate(emi_core_clk, lp_lpm_clk);
-
-	if (clk_get_usecount(axi_a_clk) != 0)
-		clk_set_rate(axi_a_clk, lp_lpm_clk);
+	if (low_bus_freq_mode || (clk_get_rate(cpu_clk) != GP_LPAPM_FREQ)) {
+		spin_unlock_irqrestore(&bus_freq_lock, flags);
+		return ret;
+	}
 
-	if (clk_get_usecount(axi_b_clk) != 0)
-		clk_set_rate(axi_b_clk, lp_lpm_clk);
+	if (clk_get_rate(cpu_clk) != GP_LPAPM_FREQ)
+		return ret;
 
-	if (clk_get_usecount(axi_c_clk) != 0)
-		clk_set_rate(axi_c_clk, lp_lpm_clk);
+	lp_lpm_clk = clk_get_rate(periph_apm_clk) / 8;
 
-	amode_parent_clk = emi_core_clk;
+	/* Set the parent of peripheral_apm_clk to be lpapm */
+	clk_set_parent(periph_apm_clk, pll1);
+	/* Set the LP clocks */
+	clk_set_parent(main_bus_clk, periph_apm_clk);
 
-	p_clk = clk_get_parent(arm_axi_clk);
-	if (p_clk != amode_parent_clk)
-		clk_set_parent(arm_axi_clk, amode_parent_clk);
+	clk_set_rate(axi_a_clk, clk_round_rate(axi_a_clk, lp_lpm_clk));
+	clk_set_rate(axi_b_clk, clk_round_rate(axi_b_clk, lp_lpm_clk));
+	clk_set_rate(axi_c_clk, clk_round_rate(axi_c_clk, lp_lpm_clk));
+	clk_set_rate(emi_core_clk, clk_round_rate(emi_core_clk, lp_lpm_clk));
+	clk_set_rate(ahb_clk, clk_round_rate(ahb_clk, lp_lpm_clk));
+	/* Set the emi_intr_clk to be at 24MHz.  */
+	clk_set_rate(emi_intr_clk, clk_round_rate(emi_intr_clk, lp_lpm_clk));
 
-	p_clk = clk_get_parent(vpu_clk);
-	if (p_clk != amode_parent_clk)
-		clk_set_parent(vpu_clk, amode_parent_clk);
+	low_bus_freq_mode = 1;
+	high_bus_freq_mode = 0;
 
-	p_clk = clk_get_parent(vpu_core_clk);
-	if (p_clk != amode_parent_clk)
-		clk_set_parent(vpu_core_clk, amode_parent_clk);
 	spin_unlock_irqrestore(&bus_freq_lock, flags);
 
 	/* Set the voltage to 1.05V for the LP domain. */
@@ -143,23 +121,18 @@ int set_low_bus_freq(void)
 		return ret;
 	}
 
-	low_bus_freq_mode = 1;
-	high_bus_freq_mode = 0;
 	return ret;
 }
 
 int set_high_bus_freq(int high_bus_freq)
 {
-	struct clk *p_clk;
-	struct clk *rmode_parent_clk;
 	int ret = 0;
 	unsigned long flags;
+	unsigned long lp_lpm_clk;
 
 	if (!low_bus_freq_mode)
 		return ret;
 
-	low_bus_freq_mode = 0;
-
 	/* Set the voltage to 1.25V for the LP domain. */
 	ret = regulator_set_voltage(lp_regulator, 1250000, 1250000);
 	udelay(100);
@@ -168,28 +141,27 @@ int set_high_bus_freq(int high_bus_freq)
 		return ret;
 	}
 
-	rmode_parent_clk = pll2;
 	spin_lock_irqsave(&bus_freq_lock, flags);
 
-	/* Set the dividers before setting the parent clock. */
-	if (clk_get_usecount(axi_a_clk) != 0)
-		clk_set_rate(axi_a_clk, 4800000);
-	if (clk_get_usecount(axi_b_clk) != 0)
-		clk_set_rate(axi_b_clk, 4000000);
-	if (clk_get_usecount(axi_c_clk) != 0)
-		clk_set_rate(axi_c_clk, 6000000);
-	if (clk_get_parent(emi_core_clk) != ahb_clk)
-		clk_set_rate(emi_core_clk, 4800000);
-
-	clk_set_rate(ahb_clk, 4800000);
+	low_bus_freq_mode = 0;
+
+	/* Set the LP clocks. */
+	lp_lpm_clk = clk_get_rate(periph_apm_clk);
+	clk_set_rate(axi_a_clk, clk_round_rate(axi_a_clk, lp_lpm_clk/5));
+	clk_set_rate(axi_b_clk, clk_round_rate(axi_b_clk, lp_lpm_clk/5));
+	clk_set_rate(axi_c_clk, clk_round_rate(axi_c_clk, lp_lpm_clk/5));
+	clk_set_rate(emi_core_clk, clk_round_rate(emi_core_clk, lp_lpm_clk/5));
+	clk_set_rate(ahb_clk, clk_round_rate(ahb_clk, lp_lpm_clk/5));
 	/* Set emi_intr clock back to divide by 2. */
-	clk_set_rate(emi_intr_clk, 2400000);
+	clk_set_rate(emi_intr_clk, clk_round_rate(emi_intr_clk, lp_lpm_clk/10));
+
 	/* Set the parent of main_bus_clk to be pll2 */
-	clk_set_parent(main_bus_clk, rmode_parent_clk);
-	spin_unlock_irqrestore(&bus_freq_lock, flags);
+	clk_set_parent(main_bus_clk, pll2);
 
-	clk_enable(uart_clk);
 	high_bus_freq_mode = 1;
+
+	spin_unlock_irqrestore(&bus_freq_lock, flags);
+
 	return ret;
 }
 
@@ -240,16 +212,25 @@ void setup_pll(void)
 		/* MFN */
 		__raw_writel(p->mfn, MXC_DPLL1_BASE + MXC_PLL_DP_HFS_MFN);
 	}
-
-	/* Set PLL2_PODF to be 3 */
-	reg = __raw_readl(MXC_CCM_CCSR);
-	reg |= 2 << MXC_CCM_CCSR_PLL2_PODF_OFFSET;
-	__raw_writel(reg, MXC_CCM_CCSR);
-	/* Set the parent of STEP_CLK to be PLL2 */
-	reg = __raw_readl(MXC_CCM_CCSR);
-	reg = (reg & ~MXC_CCM_CCSR_STEP_SEL_MASK) |
-	    (2 << MXC_CCM_CCSR_STEP_SEL_OFFSET);
-	__raw_writel(reg, MXC_CCM_CCSR);
+	if (clk_get_usecount(pll2) != 0) {
+		/* Set the temporal frequency to be PLL2 */
+		/* Set PLL2_PODF to be 3. */
+		reg = __raw_readl(MXC_CCM_CCSR);
+		reg |= 2 << MXC_CCM_CCSR_PLL2_PODF_OFFSET;
+		__raw_writel(reg, MXC_CCM_CCSR);
+		/* Set the parent of STEP_CLK to be PLL2 */
+		reg = __raw_readl(MXC_CCM_CCSR);
+		reg = (reg & ~MXC_CCM_CCSR_STEP_SEL_MASK) |
+		    (2 << MXC_CCM_CCSR_STEP_SEL_OFFSET);
+		__raw_writel(reg, MXC_CCM_CCSR);
+	} else {
+		/* Set the temporal frequency to be lp-apm */
+		/* Set the parent of STEP_CLK to be lp-apm */
+		reg = __raw_readl(MXC_CCM_CCSR);
+		reg = (reg & ~MXC_CCM_CCSR_STEP_SEL_MASK) |
+		    (0 << MXC_CCM_CCSR_STEP_SEL_OFFSET);
+		__raw_writel(reg, MXC_CCM_CCSR);
+	}
 }
 
 /*!
@@ -385,7 +366,6 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 		return PTR_ERR(pll1);
 	}
 
-
 	lp_regulator = regulator_get(NULL, lp_reg_id);
 	if (IS_ERR(lp_regulator)) {
 		clk_put(ahb_clk);
@@ -394,7 +374,7 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 	}
 
 	low_bus_freq_mode = 0;
-	high_bus_freq_mode = 0;
+	high_bus_freq_mode = 1;
 
 	cpu_wp_tbl = get_cpu_wp(&cpu_wp_nr);
 
@@ -413,7 +393,6 @@ static struct platform_driver busfreq_driver = {
  *
  * @return  The function always returns 0.
  */
-
 static int __init busfreq_init(void)
 {
 	if (platform_driver_register(&busfreq_driver) != 0) {
diff --git a/arch/arm/mach-mx37/clock.c b/arch/arm/mach-mx37/clock.c
index bdf6c10bc440..c6fb7afb5568 100644
--- a/arch/arm/mach-mx37/clock.c
+++ b/arch/arm/mach-mx37/clock.c
@@ -362,6 +362,7 @@ static struct clk pll1_sw_clk = {
 static struct clk pll2_sw_clk = {
 	.name = "pll2",
 	.parent = &osc_clk,
+	.set_rate = _clk_pll_set_rate,
 	.recalc = _clk_pll_recalc,
 	.enable = _clk_pll_enable,
 	.disable = _clk_pll_disable,
@@ -493,9 +494,6 @@ static int _clk_main_bus_set_parent(struct clk *clk, struct clk *parent)
 	if (emi_intr_clk.usecount == 0)
 		emi_intr_clk.enable(&emi_intr_clk);
 
-	if (ipu_clk[0].usecount == 0)
-		ipu_clk[0].enable(&ipu_clk[0]);
-
 	if (parent == &pll2_sw_clk) {
 		reg = __raw_readl(MXC_CCM_CBCDR6) &
 		    ~MXC_CCM_CBCDR6_PERIPH_CLK_SEL;
@@ -523,9 +521,6 @@ static int _clk_main_bus_set_parent(struct clk *clk, struct clk *parent)
 	if (emi_intr_clk.usecount == 0)
 		emi_intr_clk.disable(&emi_intr_clk);
 
-	if (ipu_clk[0].usecount == 0)
-		ipu_clk[0].enable(&ipu_clk[0]);
-
 	return 0;
 }
 
@@ -761,10 +756,6 @@ static int _clk_ahb_set_rate(struct clk *clk, unsigned long rate)
 	reg |= (div - 1) << MXC_CCM_CBCDR2_AHB_PODF_OFFSET;
 	__raw_writel(reg, MXC_CCM_CBCDR2);
 
-	/* Set the Load-dividers bit in CCM */
-	reg = __raw_readl(MXC_CCM_CCDR);
-	reg |= MXC_CCM_CCDR_LOAD_DIVIDERS;
-	__raw_writel(reg, MXC_CCM_CCDR);
 	clk->rate = rate;
 
 	return 0;
@@ -917,10 +908,6 @@ static struct clk emi_core_clk = {
 	.set_rate = _clk_emi_core_set_rate,
 	.round_rate = _clk_emi_core_round_rate,
 	.flags = RATE_PROPAGATES,
-	.enable_reg = MXC_CCM_CCGR5,
-	.enable_shift = MXC_CCM_CCGR5_CG11_OFFSET,
-	.enable = _clk_enable,
-	.disable = _clk_disable_inwait,
 };
 
 static struct clk ahbmux1_clk = {
@@ -2392,12 +2379,6 @@ static int _clk_ipu_enable(struct clk *clk)
 	reg = __raw_readl(MXC_CCM_CCDR);
 	reg &= ~MXC_CCM_CCDR_IPU_HS_MASK;
 	__raw_writel(reg, MXC_CCM_CCDR);
-
-	/* Handshake with IPU when LPM is entered as its enabled. */
-	reg = __raw_readl(MXC_CCM_CLPCR);
-	reg &= ~MXC_CCM_CLPCR_BYPASS_IPU_LPM_HS;
-	__raw_writel(reg, MXC_CCM_CLPCR);
-
 	return 0;
 }
 
@@ -2410,11 +2391,6 @@ static void _clk_ipu_disable(struct clk *clk)
 	reg = __raw_readl(MXC_CCM_CCDR);
 	reg |= MXC_CCM_CCDR_IPU_HS_MASK;
 	__raw_writel(reg, MXC_CCM_CCDR);
-
-	/* No handshake with IPU when LPM is entered as its not enabled. */
-	reg = __raw_readl(MXC_CCM_CLPCR);
-	reg |= MXC_CCM_CLPCR_BYPASS_IPU_LPM_HS;
-	__raw_writel(reg, MXC_CCM_CLPCR);
 }
 
 static int _clk_ipu_set_parent(struct clk *clk, struct clk *parent)
@@ -3058,9 +3034,6 @@ int __init mxc_clocks_init(unsigned long ckil, unsigned long osc, unsigned long
 	clk_set_parent(&vpu_clk[0], &axi_a_clk);
 	clk_set_parent(&vpu_clk[1], &axi_a_clk);
 
-	clk_set_parent(&emi_core_clk, &ahb_clk);
-	clk_set_rate(&emi_core_clk, clk_round_rate(&emi_core_clk, 130000000));
-	propagate_rate(&emi_core_clk);
 	clk_set_rate(&emi_intr_clk, clk_round_rate(&emi_intr_clk, 66000000));
 	/* Change the NFC clock rate to be 1:3 ratio with emi clock. */
 	clk_set_rate(&nfc_clk, clk_round_rate(&nfc_clk,
@@ -3068,8 +3041,11 @@ int __init mxc_clocks_init(unsigned long ckil, unsigned long osc, unsigned long
 
 	clk_set_parent(&usb_phy_clk, &osc_clk);
 
+	clk_set_parent(&periph_apm_clk, &lp_apm_clk);
+
 	clk_set_parent(&cko1_clk, &ipg_perclk);
 	clk_set_rate(&cko1_clk, 8000000);
+
 	/* Set the current working point. */
 	cpu_wp_tbl = get_cpu_wp(&cpu_wp_nr);
 	for (i = 0; i < cpu_wp_nr; i++) {
diff --git a/arch/arm/mach-mx37/lpmodes.c b/arch/arm/mach-mx37/lpmodes.c
deleted file mode 100644
index e4222efe2a48..000000000000
--- a/arch/arm/mach-mx37/lpmodes.c
+++ /dev/null
@@ -1,408 +0,0 @@
-/*
- * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
- */
-
-/*
- * The code contained herein is licensed under the GNU General Public
- * License. You may obtain a copy of the GNU General Public License
- * Version 2 or later at the following locations:
- *
- * http://www.opensource.org/licenses/gpl-license.html
- * http://www.gnu.org/copyleft/gpl.html
- */
-
-/*!
- * @file mx37_lpmodes.c
- *
- * @brief Driver for the Freescale Semiconductor MXC low power modes setup.
- *
- * MX37 is designed to play and video with minimal power consumption.
- * This driver enables the platform to enter and exit audio and video low
- * power modes.
- *
- * @ingroup PM
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/fs.h>
-#include <linux/interrupt.h>
-#include <linux/jiffies.h>
-#include <linux/device.h>
-#include <linux/delay.h>
-#include <linux/clk.h>
-#include <linux/workqueue.h>
-#include <linux/platform_device.h>
-#include <mach/clock.h>
-#include <mach/hardware.h>
-#include <linux/regulator/consumer.h>
-#include "crm_regs.h"
-
-#define ARM_LP_CLK  200000000
-#define GP_LPM_VOLTAGE 850000
-#define LP_LPM_VOLTAGE 1000000
-#define GP_NORMAL_VOLTAGE 1000000
-#define LP_NORMAL_VOLTAGE 1200000
-
-static int org_cpu_rate;
-int lp_video_mode;
-int lp_audio_mode;
-static struct device *lpmode_dev;
-
-void enter_lp_video_mode(void)
-{
-	int ret = 0;
-
-	struct clk *p_clk;
-	struct clk *tclk;
-	struct clk *vmode_parent_clk;
-	struct regulator *gp_core;
-
-	tclk = clk_get(NULL, "main_bus_clk");
-	vmode_parent_clk = clk_get(NULL, "pll2");
-	p_clk = clk_get_parent(tclk);
-
-	if (p_clk != vmode_parent_clk) {
-		clk_set_parent(tclk, vmode_parent_clk);
-
-		clk_set_rate(clk_get(NULL, "axi_a_clk"), 133000000);
-		clk_set_rate(clk_get(NULL, "axi_b_clk"), 66500000);
-		clk_set_rate(clk_get(NULL, "axi_c_clk"), 166000000);
-		clk_set_rate(clk_get(NULL, "emi_core_clk"), 133000000);
-		clk_set_rate(clk_get(NULL, "nfc_clk"), 26600000);
-		clk_set_rate(clk_get(NULL, "ahb_clk"), 133000000);
-	}
-
-	/* move VPU clock to source from the emi_core_clk */
-	tclk = clk_get(NULL, "vpu_clk");
-	vmode_parent_clk = clk_get(NULL, "emi_core_clk");
-	if (clk_get_parent(tclk) != vmode_parent_clk)
-		clk_set_parent(tclk, vmode_parent_clk);
-
-	tclk = clk_get(NULL, "vpu_core_clk");
-	if (clk_get_parent(tclk) != vmode_parent_clk)
-		clk_set_parent(tclk, vmode_parent_clk);
-
-	tclk = clk_get(NULL, "arm_axi_clk");
-	if (clk_get_parent(tclk) != vmode_parent_clk)
-		clk_set_parent(tclk, vmode_parent_clk);
-
-	tclk = clk_get(NULL, "ddr_clk");
-	vmode_parent_clk = clk_get(NULL, "axi_c_clk");
-	if (clk_get_parent(tclk) != vmode_parent_clk)
-		clk_set_parent(tclk, vmode_parent_clk);
-
-	/* disable PLL3 */
-	tclk = clk_get(NULL, "pll3");
-	if (tclk->usecount == 1)
-		clk_disable(tclk);
-
-	tclk = clk_get(NULL, "cpu_clk");
-	org_cpu_rate = clk_get_rate(tclk);
-
-	ret = clk_set_rate(tclk, ARM_LP_CLK);
-	if (ret != 0)
-		printk(KERN_DEBUG "cannot set CPU clock rate\n");
-
-	/* Set the voltage to 0.8v for the GP domain. */
-
-	if (!board_is_mx37(BOARD_REV_2))
-		gp_core = regulator_get(NULL, "DCDC1");
-	else
-		gp_core = regulator_get(NULL, "SW1");
-
-	ret = regulator_set_voltage(gp_core, GP_LPM_VOLTAGE, GP_LPM_VOLTAGE);
-	if (ret < 0)
-		printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!\n");
-
-	lp_video_mode = 1;
-}
-
-void exit_lp_video_mode(void)
-{
-	int ret = 0;
-	static struct clk *tclk;
-	struct regulator *gp_core;
-
-	/*Set the voltage to 0.8v for the GP domain. */
-	if (!board_is_mx37(BOARD_REV_2))
-		gp_core = regulator_get(NULL, "DCDC1");
-	else
-		gp_core = regulator_get(NULL, "SW1");
-
-	ret = regulator_set_voltage(gp_core, GP_NORMAL_VOLTAGE, GP_NORMAL_VOLTAGE);
-	if (ret < 0)
-		printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!\n");
-
-	tclk = clk_get(NULL, "cpu_clk");
-
-	ret = clk_set_rate(tclk, org_cpu_rate);
-	if (ret != 0)
-		printk(KERN_DEBUG "cannot set CPU clock rate\n");
-
-	lp_video_mode = 0;
-}
-
-void enter_lp_audio_mode(void)
-{
-	int ret = 0;
-
-	struct clk *p_clk;
-	struct clk *tclk;
-	struct clk *amode_parent_clk;
-	struct regulator *gp_core;
-	struct regulator *lp_core;
-
-	tclk = clk_get(NULL, "ipu_clk");
-	if (clk_get_usecount(tclk) != 0) {
-		printk(KERN_INFO
-		       "Cannot enter AUDIO LPM mode - display is still active\n");
-		return;
-	}
-
-	tclk = clk_get(NULL, "periph_apm_clk");
-	amode_parent_clk = clk_get(NULL, "lp_apm");
-	p_clk = clk_get_parent(tclk);
-
-	/* Make sure osc_clk is the parent of lp_apm. */
-	clk_set_parent(amode_parent_clk, clk_get(NULL, "osc"));
-
-	/* Set the parent of periph_apm_clk to be lp_apm */
-	clk_set_parent(tclk, amode_parent_clk);
-	amode_parent_clk = tclk;
-
-	tclk = clk_get(NULL, "main_bus_clk");
-	p_clk = clk_get_parent(tclk);
-	/* Set the parent of main_bus_clk to be periph_apm_clk */
-	clk_set_parent(tclk, amode_parent_clk);
-
-	clk_set_rate(clk_get(NULL, "axi_a_clk"), 24000000);
-	clk_set_rate(clk_get(NULL, "axi_b_clk"), 24000000);
-	clk_set_rate(clk_get(NULL, "axi_c_clk"), 24000000);
-	clk_set_rate(clk_get(NULL, "emi_core_clk"), 24000000);
-	clk_set_rate(clk_get(NULL, "nfc_clk"), 4800000);
-	clk_set_rate(clk_get(NULL, "ahb_clk"), 24000000);
-
-	amode_parent_clk = clk_get(NULL, "emi_core_clk");
-
-	tclk = clk_get(NULL, "arm_axi_clk");
-	p_clk = clk_get_parent(tclk);
-	if (p_clk != amode_parent_clk) {
-		clk_set_parent(tclk, amode_parent_clk);
-	}
-
-	tclk = clk_get(NULL, "vpu_clk");
-	p_clk = clk_get_parent(tclk);
-	if (p_clk != amode_parent_clk) {
-		clk_set_parent(tclk, amode_parent_clk);
-	}
-
-	tclk = clk_get(NULL, "vpu_core_clk");
-	p_clk = clk_get_parent(tclk);
-	if (p_clk != amode_parent_clk) {
-		clk_set_parent(tclk, amode_parent_clk);
-	}
-
-	/* disable PLL3 */
-	tclk = clk_get(NULL, "pll3");
-	if (tclk->usecount == 1)
-		clk_disable(tclk);
-
-	/* disable PLL2 */
-	tclk = clk_get(NULL, "pll2");
-	if (tclk->usecount == 1)
-		clk_disable(tclk);
-
-	/* Set the voltage to 1.0v for the LP domain. */
-	if (!board_is_mx37(BOARD_REV_2))
-		lp_core = regulator_get(NULL, "DCDC4");
-	else
-		lp_core = regulator_get(NULL, "SW2");
-
-	if (lp_core != NULL) {
-		ret = regulator_set_voltage(lp_core, LP_LPM_VOLTAGE, LP_LPM_VOLTAGE);
-		if (ret < 0)
-			printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!!!\n");
-	}
-
-	tclk = clk_get(NULL, "cpu_clk");
-	org_cpu_rate = clk_get_rate(tclk);
-
-	ret = clk_set_rate(tclk, ARM_LP_CLK);
-	if (ret != 0)
-		printk(KERN_DEBUG "cannot set CPU clock rate\n");
-
-	/* Set the voltage to 0.8v for the GP domain. */
-	if (!board_is_mx37(BOARD_REV_2))
-		gp_core = regulator_get(NULL, "DCDC1");
-	else
-		gp_core = regulator_get(NULL, "SW1");
-
-	if (gp_core != NULL) {
-		ret = regulator_set_voltage(gp_core, GP_LPM_VOLTAGE, GP_LPM_VOLTAGE);
-		if (ret < 0)
-			printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!!\n");
-	}
-	lp_audio_mode = 1;
-}
-
-void exit_lp_audio_mode(void)
-{
-	struct regulator *gp_core;
-	struct regulator *lp_core;
-	struct clk *tclk;
-	struct clk *p_clk;
-	struct clk *rmode_parent_clk;
-	int ret;
-
-	lp_audio_mode = 0;
-	/* Set the voltage to 1.2v for the LP domain. */
-	if (!board_is_mx37(BOARD_REV_2))
-		lp_core = regulator_get(NULL, "DCDC4");
-	else
-		lp_core = regulator_get(NULL, "SW2");
-
-	if (lp_core != NULL) {
-		ret = regulator_set_voltage(lp_core, LP_NORMAL_VOLTAGE, LP_NORMAL_VOLTAGE);
-		if (ret < 0)
-			printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!!!\n");
-	}
-
-	/* Set the voltage to 1.0v for the GP domain. */
-	if (!board_is_mx37(BOARD_REV_2))
-		gp_core = regulator_get(NULL, "DCDC1");
-	else
-		gp_core = regulator_get(NULL, "SW1");
-
-	ret = regulator_set_voltage(gp_core, GP_NORMAL_VOLTAGE, GP_NORMAL_VOLTAGE);
-	if (ret < 0)
-		printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!\n");
-
-	tclk = clk_get(NULL, "cpu_clk");
-
-	ret = clk_set_rate(tclk, org_cpu_rate);
-	if (ret != 0)
-		printk(KERN_DEBUG "cannot set CPU clock rate\n");
-
-	rmode_parent_clk = clk_get(NULL, "pll2");
-	clk_enable(rmode_parent_clk);
-
-	tclk = clk_get(NULL, "main_bus_clk");
-	p_clk = clk_get_parent(tclk);
-
-	/* Set the dividers before setting the parent clock. */
-	clk_set_rate(clk_get(NULL, "axi_a_clk"), 4800000);
-	clk_set_rate(clk_get(NULL, "axi_b_clk"), 4000000);
-	clk_set_rate(clk_get(NULL, "axi_c_clk"), 6000000);
-	clk_set_rate(clk_get(NULL, "emi_core_clk"), 4800000);
-	clk_set_rate(clk_get(NULL, "ahb_clk"), 4800000);
-
-	/* Set the parent of main_bus_clk to be pll2 */
-	clk_set_parent(tclk, rmode_parent_clk);
-	udelay(5);
-}
-
-static ssize_t lp_curr_mode(struct device *dev,
-			    struct device_attribute *attr, char *buf)
-{
-	if (lp_video_mode)
-		return sprintf(buf, "in lp_video_mode\n");
-	else if (lp_audio_mode)
-		return sprintf(buf, "in lp_audio_mode\n");
-	else
-		return sprintf(buf, "in normal mode\n");
-}
-
-static ssize_t set_lp_mode(struct device *dev,
-			   struct device_attribute *attr,
-			   const char *buf, size_t size)
-{
-	printk(KERN_DEBUG "In set_lp_mode() \n");
-
-	if (strstr(buf, "enable_lp_video") != NULL) {
-		if (!lp_video_mode)
-			enter_lp_video_mode();
-	} else if (strstr(buf, "disable_lp_video") != NULL) {
-		if (lp_video_mode)
-			exit_lp_video_mode();
-	} else if (strstr(buf, "enable_lp_audio") != NULL) {
-		if (!lp_audio_mode)
-			enter_lp_audio_mode();
-	} else if (strstr(buf, "disable_lp_audio") != NULL) {
-		if (lp_audio_mode)
-			exit_lp_audio_mode();
-	}
-	return size;
-}
-
-static DEVICE_ATTR(lp_modes, 0644, lp_curr_mode, set_lp_mode);
-
-/*!
- * This is the probe routine for the lp_mode driver.
- *
- * @param   pdev   The platform device structure
- *
- * @return         The function returns 0 on success
- *
- */
-static int __devinit mx37_lpmode_probe(struct platform_device *pdev)
-{
-	u32 res = 0;
-	lpmode_dev = &pdev->dev;
-
-	res = sysfs_create_file(&lpmode_dev->kobj, &dev_attr_lp_modes.attr);
-	if (res) {
-		printk(KERN_ERR
-		       "lpmode_dev: Unable to register sysdev entry for lpmode_dev");
-		return res;
-	}
-
-	if (res != 0) {
-		printk(KERN_ERR "lpmode_dev: Unable to start");
-		return res;
-	}
-	lp_video_mode = 0;
-	lp_audio_mode = 0;
-
-	return 0;
-}
-
-static struct platform_driver mx37_lpmode_driver = {
-	.driver = {
-		   .name = "mx37_lpmode",
-		   },
-	.probe = mx37_lpmode_probe,
-};
-
-/*!
- * Initialise the mx37_lpmode_driver.
- *
- * @return  The function always returns 0.
- */
-
-static int __init lpmode_init(void)
-{
-	if (platform_driver_register(&mx37_lpmode_driver) != 0) {
-		printk(KERN_ERR "mx37_lpmode_driver register failed\n");
-		return -ENODEV;
-	}
-
-	printk(KERN_INFO "LPMode driver module loaded\n");
-	return 0;
-}
-
-static void __exit lpmode_cleanup(void)
-{
-	sysfs_remove_file(&lpmode_dev->kobj, &dev_attr_lp_modes.attr);
-
-	/* Unregister the device structure */
-	platform_driver_unregister(&mx37_lpmode_driver);
-}
-
-module_init(lpmode_init);
-module_exit(lpmode_cleanup);
-
-MODULE_AUTHOR("Freescale Semiconductor, Inc.");
-MODULE_DESCRIPTION("LPMode driver");
-MODULE_LICENSE("GPL");
diff --git a/arch/arm/mach-mx37/mx37_3stack.c b/arch/arm/mach-mx37/mx37_3stack.c
index fc030fc74c73..787bb76ad230 100644
--- a/arch/arm/mach-mx37/mx37_3stack.c
+++ b/arch/arm/mach-mx37/mx37_3stack.c
@@ -70,14 +70,6 @@ static struct cpu_wp cpu_wp_auto[] = {
 	 .mfn = 13,
 	 .cpu_voltage = 1050000,},
 	{
-	 .pll_rate = 400000000,
-	 .cpu_rate = 400000000,
-	 .pdf = 1,
-	 .mfi = 8,
-	 .mfd = 2,
-	 .mfn = 1,
-	 .cpu_voltage = 950000,},
-	{
 	 .pll_rate = 200000000,
 	 .cpu_rate = 200000000,
 	 .pdf = 3,
@@ -85,19 +77,11 @@ static struct cpu_wp cpu_wp_auto[] = {
 	 .mfd = 2,
 	 .mfn = 1,
 	 .cpu_voltage = 850000,},
-	{
-	 .pll_rate = 600000000,
-	 .cpu_rate = 600000000,
-	 .pdf = 0,
-	 .mfi = 6,
-	 .mfd = 3,
-	 .mfn = 1,
-	 .cpu_voltage = 1200000,},
 };
 
 struct cpu_wp *get_cpu_wp(int *wp)
 {
-	*wp = 3;
+	*wp = 2;
 	return cpu_wp_auto;
 }
 
diff --git a/arch/arm/mach-mx37/system.c b/arch/arm/mach-mx37/system.c
index d40549b29ddc..78685f625813 100644
--- a/arch/arm/mach-mx37/system.c
+++ b/arch/arm/mach-mx37/system.c
@@ -36,9 +36,6 @@
 extern int mxc_jtag_enabled;
 extern int low_bus_freq_mode;
 
-static struct clk *pll1_main;
-static struct clk *pll1_sw_clk;
-static struct clk *lp_apm_clk;
 static struct clk *gpc_dvfs_clk;
 
 /* set cpu low power mode before WFI instruction */
@@ -96,8 +93,6 @@ void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
 	if ((mxc_cpu_is_rev(CHIP_REV_1_0)) != 1)
 		__raw_writel(empgcr0, MXC_EMPGC0_ARM_EMPGCR);
 
-	__raw_writel(empgcr1, MXC_EMPGC1_ARM_EMPGCR);
-
 	flush_cache_all();
 
 	if (gpc_dvfs_clk == NULL)
@@ -105,18 +100,6 @@ void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
 
 	/* gpc clock is needed for SRPG */
 	clk_enable(gpc_dvfs_clk);
-	if (low_bus_freq_mode) {
-		if (pll1_sw_clk == NULL)
-			pll1_sw_clk = clk_get(NULL, "pll1_sw_clk");
-		if (lp_apm_clk == NULL)
-			lp_apm_clk = clk_get(NULL, "lp_apm");
-		if (pll1_main == NULL)
-			pll1_main = clk_get(NULL, "pll1_main_clk");
-
-		/* Move the ARM to run off the 24MHz clock. Shutdown the PLL1 */
-		/* Change the source of pll1_sw_clk to be the step_clk */
-		clk_set_parent(pll1_sw_clk, lp_apm_clk);
-	}
 }
 
 void mxc_pg_enable(struct platform_device *pdev)
@@ -172,11 +155,6 @@ void arch_idle(void)
 		cpu_do_idle();
 		/* gpc clock is needed for SRPG */
 		clk_disable(gpc_dvfs_clk);
-		if (low_bus_freq_mode) {
-			/* Move ARM back to PLL from step clk. */
-			/* Move the PLL1 back to the pll1_main_clk */
-			clk_set_parent(pll1_sw_clk, pll1_main);
-		}
 	}
 }
 
diff --git a/arch/arm/mach-mx51/bus_freq.c b/arch/arm/mach-mx51/bus_freq.c
index da31595832dc..69dbd59c526e 100644
--- a/arch/arm/mach-mx51/bus_freq.c
+++ b/arch/arm/mach-mx51/bus_freq.c
@@ -76,8 +76,11 @@ extern int dvfs_core_is_active;
 extern struct cpu_wp *(*get_cpu_wp)(int *wp);
 extern int cpu_wp_nr;
 
-struct dvfs_wp dvfs_core_setpoint[] = {{33, 7, 33, 20, 20, 0x10},
-					    {27, 0, 33, 20, 20, 0x10},};
+struct dvfs_wp dvfs_core_setpoint[] = {
+						{33, 8, 33, 10, 10, 0x08},
+						{26, 0, 33, 20, 10, 0x08},
+						{28, 8, 33, 20, 30, 0x08},
+						{26, 0, 33, 20, 10, 0x08},};
 
 int set_low_bus_freq(void)
 {
@@ -119,8 +122,6 @@ int set_high_bus_freq(int high_bus_freq)
 	if (bus_freq_scaling_is_active) {
 		if (clk_get_rate(main_bus_clk) == LP_APM_CLK) {
 
-			clk_enable(pll2);
-
 			/* Set the dividers before setting the parent clock. */
 			clk_set_rate(axi_a_clk,
 				     LP_APM_CLK/AXI_A_CLK_NORMAL_DIV);
@@ -140,6 +141,7 @@ int set_high_bus_freq(int high_bus_freq)
 
 			low_bus_freq_mode = 0;
 		}
+		
 		/*
 		 * If the CPU freq is 800MHz, set the bus to the high setpoint
 		 * (133MHz) and DDR to 200MHz.
@@ -155,8 +157,8 @@ int set_high_bus_freq(int high_bus_freq)
 				clk_round_rate(ahb_clk, LP_NORMAL_CLK));
 			clk_set_rate(ddr_hf_clk,
 				clk_round_rate(ddr_hf_clk, DDR_NORMAL_CLK));
-
 		}
+	
 		if (!lp_high_freq && !high_bus_freq) {
 			/* Set to the medium setpoint. */
 			high_bus_freq_mode = 0;
diff --git a/arch/arm/mach-mx51/clock.c b/arch/arm/mach-mx51/clock.c
index b0699ff4033b..94bc67687e12 100644
--- a/arch/arm/mach-mx51/clock.c
+++ b/arch/arm/mach-mx51/clock.c
@@ -421,6 +421,7 @@ static int _clk_pll1_sw_set_parent(struct clk *clk, struct clk *parent)
 		}
 	}
 	__raw_writel(reg, MXC_CCM_CCSR);
+	
 	return 0;
 }
 
diff --git a/arch/arm/plat-mxc/clock.c b/arch/arm/plat-mxc/clock.c
index d1f2f265c0ca..0fc0ce708f2a 100644
--- a/arch/arm/plat-mxc/clock.c
+++ b/arch/arm/plat-mxc/clock.c
@@ -46,12 +46,13 @@
 extern int dvfs_core_is_active;
 extern int lp_high_freq;
 extern int lp_med_freq;
-extern void dvfs_core_set_bus_freq(void);
+extern int low_bus_freq_mode;
+extern int high_bus_freq_mode;
+extern int set_high_bus_freq(int high_freq);
+extern int set_low_bus_freq();
+extern int low_freq_bus_used();
 #else
 int dvfs_core_is_active;
-void dvfs_core_set_bus_freq(void)
-{
-};
 #endif
 
 static LIST_HEAD(clocks);
@@ -173,23 +174,26 @@ int clk_enable(struct clk *clk)
 	spin_unlock_irqrestore(&clockfw_lock, flags);
 
 	if ((clk->flags & CPU_FREQ_TRIG_UPDATE)
-	    && (clk_get_usecount(clk) == 1)) {
-#if defined(CONFIG_CPU_FREQ)
-		if (dvfs_core_is_active)
-			dvfs_core_set_bus_freq();
+			&& (clk_get_usecount(clk) == 1)) {
 #if (defined(CONFIG_ARCH_MX51) || defined(CONFIG_ARCH_MX37))
-		else if ((lp_high_freq == 0 && lp_med_freq == 0) ||
-			(lp_high_freq == 1) ||
-			(lp_high_freq == 0 && lp_med_freq == 1))
-#else
-		else
-#endif
-			cpufreq_update_policy(0);
-#else
-		if (dvfs_core_is_active)
-			dvfs_core_set_bus_freq();
+		if (low_freq_bus_used() && !low_bus_freq_mode)
+			set_low_bus_freq();
+		else {
+			if (!high_bus_freq_mode) {
+				/* Currently at ow 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,
+				  * need to set to medium setpoint
+				  */
+				set_high_bus_freq(0);
+			}
+		}
 #endif
 	}
+
 	return ret;
 }
 EXPORT_SYMBOL(clk_enable);
@@ -212,19 +216,23 @@ void clk_disable(struct clk *clk)
 	spin_unlock_irqrestore(&clockfw_lock, flags);
 
 	if ((clk->flags & CPU_FREQ_TRIG_UPDATE)
-	    && (clk_get_usecount(clk) == 0)) {
-#if defined(CONFIG_CPU_FREQ)
-		if (dvfs_core_is_active)
-			dvfs_core_set_bus_freq();
+			&& (clk_get_usecount(clk) == 0)) {
 #if (defined(CONFIG_ARCH_MX51) || defined(CONFIG_ARCH_MX37))
-		else if (lp_high_freq == 0)
-#else
-		else
-#endif
-			cpufreq_update_policy(0);
-#else
-		if (dvfs_core_is_active)
-			dvfs_core_set_bus_freq();
+		if (low_freq_bus_used() && !low_bus_freq_mode)
+			set_low_bus_freq();
+		else {
+			if (!high_bus_freq_mode) {
+				/* Currently at ow 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,
+				  * need to set to medium setpoint
+				  */
+				set_high_bus_freq(0);
+			}
+		}
 #endif
 	}
 }
diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
index bc3d1c97526c..fab3986d301c 100644
--- a/arch/arm/plat-mxc/cpufreq.c
+++ b/arch/arm/plat-mxc/cpufreq.c
@@ -89,13 +89,13 @@ static int set_cpu_freq(int freq)
 			return ret;
 		}
 	}
-
+	
 	ret = clk_set_rate(cpu_clk, freq);
 	if (ret != 0) {
 		printk(KERN_DEBUG "cannot set CPU clock rate\n");
 		return ret;
 	}
-
+	
 	if (freq < org_cpu_rate) {
 		ret = regulator_set_voltage(gp_regulator, gp_volt, gp_volt);
 		if (ret < 0) {
@@ -103,7 +103,7 @@ static int set_cpu_freq(int freq)
 			return ret;
 		}
 	}
-
+	
 	return ret;
 }
 
diff --git a/arch/arm/plat-mxc/dvfs_core.c b/arch/arm/plat-mxc/dvfs_core.c
index e915f263e920..558b1ffde43b 100644
--- a/arch/arm/plat-mxc/dvfs_core.c
+++ b/arch/arm/plat-mxc/dvfs_core.c
@@ -92,7 +92,6 @@
 #define MXC_GPCVCR_VCNT_OFFSET               0
 
 extern void setup_pll(void);
-static struct delayed_work dvfs_core_work;
 static struct mxc_dvfs_platform_data *dvfs_data;
 static struct device *dvfs_dev;
 static struct cpu_wp *cpu_wp_tbl;
@@ -103,6 +102,9 @@ int dvfs_core_is_active;
 int cpufreq_trig_needed;
 struct timeval core_prev_intr;
 
+void dump_dvfs_core_regs(void);
+static struct delayed_work dvfs_core_handler;
+
 /*
  * Clock structures
  */
@@ -138,33 +140,6 @@ extern void propagate_rate(struct clk *tclk);
 
 DEFINE_SPINLOCK(mxc_dvfs_core_lock);
 
-void dvfs_core_set_bus_freq(void)
-{
-	u32 reg;
-	int low_freq_bus_ready = 0;
-
-	/* Mask DVFS irq */
-	reg = __raw_readl(dvfs_data->dvfs_cntr_reg_addr);
-	/* FSVAIM=1 */
-	reg |= MXC_DVFSCNTR_FSVAIM;
-	__raw_writel(reg, dvfs_data->dvfs_cntr_reg_addr);
-
-	low_freq_bus_ready = low_freq_bus_used();
-
-	if ((curr_wp == cpu_wp_nr - 1) && (!low_bus_freq_mode)
-	    && (low_freq_bus_ready))
-		set_low_bus_freq();
-	else if (!low_freq_bus_ready)
-		set_high_bus_freq(0);
-	/* Enable DVFS interrupt */
-	/* FSVAIM=0 */
-	reg = (reg & ~MXC_DVFSCNTR_FSVAIM);
-	/* LBFL=1 */
-	reg = (reg & ~MXC_DVFSCNTR_LBFL);
-	reg |= MXC_DVFSCNTR_LBFL;
-	__raw_writel(reg, dvfs_data->dvfs_cntr_reg_addr);
-}
-
 static void dvfs_load_config(int set_point)
 {
 	u32 reg;
@@ -181,6 +156,13 @@ static void dvfs_load_config(int set_point)
 	    MXC_DVFSCOUN_DNCNT_OFFSET;
 	reg |= dvfs_core_setpoint[set_point].upcnt << MXC_DVFSCOUN_UPCNT_OFFSET;
 	__raw_writel(reg, dvfs_data->dvfs_coun_reg_addr);
+
+	/* Set EMAC value */
+	__raw_writel((dvfs_core_setpoint[set_point].emac <<
+					MXC_DVFSEMAC_EMAC_OFFSET),
+					dvfs_data->dvfs_emac_reg_addr);
+
+
 }
 
 static int set_cpu_freq(int wp)
@@ -194,7 +176,6 @@ static int set_cpu_freq(int wp)
 	int gp_volt = 0;
 	u32 reg;
 	u32 reg1;
-
 	if (cpu_wp_tbl[wp].pll_rate != cpu_wp_tbl[old_wp].pll_rate) {
 		/* PLL_RELOCK, set ARM_FREQ_SHIFT_DIVIDER */
 		reg = __raw_readl(dvfs_data->ccm_cdcr_reg_addr);
@@ -210,7 +191,6 @@ static int set_cpu_freq(int wp)
 
 		if (gp_volt == 0)
 			return ret;
-
 		/*Set the voltage for the GP domain. */
 		if (rate > org_cpu_rate) {
 			ret = regulator_set_voltage(core_regulator, gp_volt,
@@ -221,7 +201,6 @@ static int set_cpu_freq(int wp)
 			}
 			udelay(dvfs_data->delay_time);
 		}
-
 		setup_pll();
 		/* START the GPC main control FSM */
 		/* set VINC */
@@ -244,7 +223,7 @@ static int set_cpu_freq(int wp)
 		reg |= MXC_GPCCNTR_STRT;
 		__raw_writel(reg, dvfs_data->gpc_cntr_reg_addr);
 		while (__raw_readl(dvfs_data->gpc_cntr_reg_addr) & 0x4000)
-			udelay(10);
+			udelay(50);
 
 		if (rate < org_cpu_rate) {
 			ret = regulator_set_voltage(core_regulator,
@@ -254,7 +233,7 @@ static int set_cpu_freq(int wp)
 				       "COULD NOT SET GP VOLTAGE!!!!\n");
 				return ret;
 			}
-			old_wp = wp;
+			udelay(dvfs_data->delay_time);
 		}
 
 		clk_set_rate(cpu_clk, rate);
@@ -289,10 +268,8 @@ static int set_cpu_freq(int wp)
 			}
 			udelay(dvfs_data->delay_time);
 			vinc = 1;
-			dvfs_load_config(0);
 		} else {
 			vinc = 0;
-			dvfs_load_config(1);
 		}
 
 		arm_podf = podf;
@@ -348,11 +325,11 @@ static int set_cpu_freq(int wp)
 		}
 
 		propagate_rate(pll1_sw_clk);
+	}
 #if defined(CONFIG_CPU_FREQ)
 		cpufreq_trig_needed = 1;
 #endif
-		old_wp = wp;
-	}
+	old_wp = wp;
 
 	return ret;
 }
@@ -369,11 +346,14 @@ static int start_dvfs(void)
 
 	clk_enable(dvfs_clk);
 
+	dvfs_load_config(0);
+
 	/* config reg GPC_CNTR */
 	reg = __raw_readl(dvfs_data->gpc_cntr_reg_addr);
 
 	/* GPCIRQ=1, select ARM IRQ */
 	reg |= MXC_GPCCNTR_GPCIRQ;
+	reg &= ~MXC_GPCCNTR_GPCIRQM;
 	/* ADU=1, select ARM domain */
 	reg |= MXC_GPCCNTR_ADU;
 	__raw_writel(reg, dvfs_data->gpc_cntr_reg_addr);
@@ -401,7 +381,10 @@ static int start_dvfs(void)
 	/* Set DIV3CK */
 	reg = (reg & ~(dvfs_data->div3ck_mask));
 	reg |= (dvfs_data->div3ck_val) << (dvfs_data->div3ck_offset);
+	__raw_writel(reg, dvfs_data->dvfs_cntr_reg_addr);
+
 	/* Enable DVFS */
+	reg = __raw_readl(dvfs_data->dvfs_cntr_reg_addr);
 	reg |= MXC_DVFSCNTR_DVFEN;
 	__raw_writel(reg, dvfs_data->dvfs_cntr_reg_addr);
 
@@ -430,10 +413,6 @@ static int init_dvfs_controller(void)
 	/* DVFS loading config */
 	dvfs_load_config(0);
 
-	/* Set EMAC value */
-	__raw_writel((dvfs_data->emac_val << MXC_DVFSEMAC_EMAC_OFFSET),
-		     dvfs_data->dvfs_emac_reg_addr);
-
 	return 0;
 }
 
@@ -457,12 +436,11 @@ static irqreturn_t dvfs_irq(int irq, void *dev_id)
 	reg |= MXC_GPCCNTR_GPCIRQM | 0x1000000;
 	__raw_writel(reg, dvfs_data->gpc_cntr_reg_addr);
 
-	schedule_delayed_work(&dvfs_core_work, 0);
-
+	schedule_delayed_work(&dvfs_core_handler, 0);
 	return IRQ_HANDLED;
 }
 
-static void dvfs_core_workqueue_handler(struct work_struct *work)
+static void dvfs_core_work_handler(struct work_struct *work)
 {
 	u32 fsvai;
 	u32 reg;
@@ -470,11 +448,14 @@ static void dvfs_core_workqueue_handler(struct work_struct *work)
 	int ret = 0;
 	int maxf = 0, minf = 0;
 	int low_freq_bus_ready = 0;
+	int bus_incr = 0, cpu_dcr = 0;
+
+	low_freq_bus_ready = low_freq_bus_used();
+
 
 	/* Check DVFS frequency adjustment interrupt status */
 	reg = __raw_readl(dvfs_data->dvfs_cntr_reg_addr);
 	fsvai = (reg & MXC_DVFSCNTR_FSVAI_MASK) >> MXC_DVFSCNTR_FSVAI_OFFSET;
-
 	/* Check FSVAI, FSVAI=0 is error */
 	if (fsvai == FSVAI_FREQ_NOCHANGE) {
 		/* Do nothing. Freq change is not required */
@@ -488,7 +469,8 @@ static void dvfs_core_workqueue_handler(struct work_struct *work)
 	if (fsvai == FSVAI_FREQ_DECREASE) {
 		if (curr_cpu == cpu_wp_tbl[cpu_wp_nr - 1].cpu_rate) {
 			minf = 1;
-			goto END;
+			if (low_bus_freq_mode)
+				goto END;
 		} else {
 			/* freq down */
 			curr_wp++;
@@ -497,28 +479,50 @@ static void dvfs_core_workqueue_handler(struct work_struct *work)
 				goto END;
 			}
 
-			if (curr_wp == cpu_wp_nr - 1)
+			if (curr_wp == cpu_wp_nr - 1 && !low_freq_bus_ready) {
 				minf = 1;
+				dvfs_load_config(1);
+			} else {
+				cpu_dcr = 1;
+			}
 		}
 	} else {
 		if (curr_cpu == cpu_wp_tbl[0].cpu_rate) {
 			maxf = 1;
 			goto END;
 		} else {
-			/* freq up */
-			curr_wp = 0;
-			maxf = 1;
+			if (low_bus_freq_mode) {
+				/* bump up LP freq first. */
+				bus_incr = 1;
+				dvfs_load_config(2);
+			} else {
+				/* freq up */
+				curr_wp = 0;
+				maxf = 1;
+				dvfs_load_config(0);
+			}
 		}
 	}
 
 	low_freq_bus_ready = low_freq_bus_used();
 	if ((curr_wp == cpu_wp_nr - 1) && (!low_bus_freq_mode)
-	    && (low_freq_bus_ready)) {
-		ret = set_cpu_freq(curr_wp);
-		set_low_bus_freq();
+	    && (low_freq_bus_ready) && !bus_incr) {
+		if (cpu_dcr)
+			ret = set_cpu_freq(curr_wp);
+		if (!cpu_dcr) {
+			set_low_bus_freq();
+			dvfs_load_config(3);
+		} else {
+			dvfs_load_config(2);
+			cpu_dcr = 0;
+		}
 	} else {
-		set_high_bus_freq(0);
-		ret = set_cpu_freq(curr_wp);
+		if (low_bus_freq_mode)
+			set_high_bus_freq(0);
+
+		if (!bus_incr)
+			ret = set_cpu_freq(curr_wp);
+		bus_incr = 0;
 	}
 
 #if defined(CONFIG_CPU_FREQ)
@@ -548,6 +552,7 @@ END:	/* Set MAXF, MINF */
 	__raw_writel(reg, dvfs_data->gpc_cntr_reg_addr);
 }
 
+
 /*!
  * This function disables the DVFS module.
  */
@@ -558,7 +563,6 @@ static void stop_dvfs(void)
 	u32 curr_cpu;
 
 	if (dvfs_core_is_active) {
-		spin_lock_irqsave(&mxc_dvfs_core_lock, flags);
 
 		/* Mask dvfs irq, disable DVFS */
 		reg = __raw_readl(dvfs_data->dvfs_cntr_reg_addr);
@@ -566,9 +570,6 @@ static void stop_dvfs(void)
 		reg |= MXC_DVFSCNTR_FSVAIM;
 		__raw_writel(reg, dvfs_data->dvfs_cntr_reg_addr);
 
-		dvfs_core_is_active = 0;
-		spin_unlock_irqrestore(&mxc_dvfs_core_lock, flags);
-
 		curr_wp = 0;
 		if (!high_bus_freq_mode)
 			set_high_bus_freq(1);
@@ -576,10 +577,6 @@ static void stop_dvfs(void)
 		curr_cpu = clk_get_rate(cpu_clk);
 		if (curr_cpu != cpu_wp_tbl[curr_wp].cpu_rate) {
 			set_cpu_freq(curr_wp);
-			/* disable DVFS */
-			reg = __raw_readl(dvfs_data->dvfs_cntr_reg_addr);
-			reg = (reg & ~MXC_DVFSCNTR_DVFEN);
-			__raw_writel(reg, dvfs_data->dvfs_cntr_reg_addr);
 #if defined(CONFIG_CPU_FREQ)
 			if (cpufreq_trig_needed == 1) {
 				cpufreq_trig_needed = 0;
@@ -587,12 +584,108 @@ static void stop_dvfs(void)
 			}
 #endif
 		}
+		spin_lock_irqsave(&mxc_dvfs_core_lock, flags);
+
+		reg = __raw_readl(dvfs_data->dvfs_cntr_reg_addr);
+		reg = (reg & ~MXC_DVFSCNTR_DVFEN);
+		__raw_writel(reg, dvfs_data->dvfs_cntr_reg_addr);
+
+		spin_unlock_irqrestore(&mxc_dvfs_core_lock, flags);
+
+		dvfs_core_is_active = 0;
 
 		clk_disable(dvfs_clk);
 	}
 
 	printk(KERN_DEBUG "DVFS is stopped\n");
 }
+void dump_dvfs_core_regs()
+{
+	struct timeval cur;
+	u32 diff = 0;
+	if (core_prev_intr.tv_sec == 0)
+		do_gettimeofday(&core_prev_intr);
+	else {
+		do_gettimeofday(&cur);
+		diff = (cur.tv_sec - core_prev_intr.tv_sec)*1000000
+			 + (cur.tv_usec - core_prev_intr.tv_usec);
+		core_prev_intr = cur;
+	}
+	if (diff < 90000)
+		printk(KERN_DEBUG "diff = %d\n", diff);
+
+	printk(KERN_INFO "THRS = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr));
+	printk(KERN_INFO "COUNT = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x04));
+	printk(KERN_INFO "SIG1 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x08));
+	printk(KERN_INFO "SIG0 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x0c));
+	printk(KERN_INFO "GPC0 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x10));
+	printk(KERN_INFO "GPC1 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x14));
+	printk(KERN_INFO "GPBT = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x18));
+	printk(KERN_INFO "EMAC = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x1c));
+	printk(KERN_INFO "CNTR = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x20));
+	printk(KERN_INFO "LTR0_0 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x24));
+	printk(KERN_INFO "LTR0_1 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x28));
+	printk(KERN_INFO "LTR1_0 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x2c));
+	printk(KERN_DEBUG "LTR1_1 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x30));
+	printk(KERN_INFO "PT0 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x34));
+	printk(KERN_INFO "PT1 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x38));
+	printk(KERN_INFO "PT2 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x3c));
+	printk(KERN_INFO "PT3 = 0x%08x\n",
+			__raw_readl(dvfs_data->dvfs_thrs_reg_addr+0x40));
+}
+
+static ssize_t downthreshold_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%u\n", dvfs_core_setpoint[0].downthr);
+}
+
+static ssize_t downthreshold_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	int ret = 0;
+	int val;
+	ret = sscanf(buf, "%u", &val);
+	dvfs_core_setpoint[0].downthr = val;
+
+	return size;
+}
+
+static ssize_t downcount_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%u\n", dvfs_core_setpoint[0].downcnt);
+}
+
+static ssize_t downcount_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	int ret = 0;
+	int val;
+	ret = sscanf(buf, "%u", &val);
+	dvfs_core_setpoint[0].downcnt = val;
+
+	return size;
+}
+
 
 static ssize_t dvfs_enable_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
@@ -616,7 +709,31 @@ static ssize_t dvfs_enable_store(struct device *dev,
 	return size;
 }
 
+static ssize_t dvfs_regs_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	if (dvfs_core_is_active)
+		dump_dvfs_core_regs();
+	return 0;
+}
+
+static ssize_t dvfs_regs_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	if (dvfs_core_is_active)
+		dump_dvfs_core_regs();
+	return 0;
+
+	return size;
+}
+
 static DEVICE_ATTR(enable, 0644, dvfs_enable_show, dvfs_enable_store);
+static DEVICE_ATTR(show_regs, 0644, dvfs_regs_show, dvfs_regs_store);
+
+static DEVICE_ATTR(down_threshold, 0644, downthreshold_show,
+						downthreshold_store);
+static DEVICE_ATTR(down_count, 0644, downcount_show, downcount_store);
 
 /*!
  * This is the probe routine for the DVFS driver.
@@ -635,7 +752,7 @@ static int __devinit mxc_dvfs_core_probe(struct platform_device *pdev)
 	dvfs_dev = &pdev->dev;
 	dvfs_data = pdev->dev.platform_data;
 
-	INIT_DELAYED_WORK(&dvfs_core_work, dvfs_core_workqueue_handler);
+	INIT_DELAYED_WORK(&dvfs_core_handler, dvfs_core_work_handler);
 
 	pll1_sw_clk = clk_get(NULL, "pll1_sw_clk");
 	if (IS_ERR(pll1_sw_clk)) {
@@ -700,6 +817,28 @@ static int __devinit mxc_dvfs_core_probe(struct platform_device *pdev)
 		return err;
 	}
 
+	err = sysfs_create_file(&dvfs_dev->kobj, &dev_attr_show_regs.attr);
+	if (err) {
+		printk(KERN_ERR
+		       "DVFS: Unable to register sysdev entry for DVFS");
+		return err;
+	}
+
+
+	err = sysfs_create_file(&dvfs_dev->kobj, &dev_attr_down_threshold.attr);
+	if (err) {
+		printk(KERN_ERR
+		       "DVFS: Unable to register sysdev entry for DVFS");
+		return err;
+	}
+
+	err = sysfs_create_file(&dvfs_dev->kobj, &dev_attr_down_count.attr);
+	if (err) {
+		printk(KERN_ERR
+		       "DVFS: Unable to register sysdev entry for DVFS");
+		return err;
+	}
+
 	/* Set the current working point. */
 	cpu_wp_tbl = get_cpu_wp(&cpu_wp_nr);
 	old_wp = 0;
diff --git a/sound/soc/imx/imx-pcm.c b/sound/soc/imx/imx-pcm.c
index 5f46151d2ca4..6836523a60ed 100644
--- a/sound/soc/imx/imx-pcm.c
+++ b/sound/soc/imx/imx-pcm.c
@@ -102,7 +102,7 @@ static int imx_iram_audio_playback_mmap(struct snd_pcm_substream *substream,
 	if (off + size > SND_RAM_SIZE)
 		return -EINVAL;
 
-	area->vm_page_prot = pgprot_nonshareddev(area->vm_page_prot);
+	area->vm_page_prot = pgprot_writecombine(area->vm_page_prot);
 	area->vm_flags |= VM_IO;
 	ret =
 	    remap_pfn_range(area, area->vm_start, phys >> PAGE_SHIFT,