diff --git a/arch/arm/mach-mx5/bus_freq.c b/arch/arm/mach-mx5/bus_freq.c
index 4fcaf95e301..2c8b111e247 100644
--- a/arch/arm/mach-mx5/bus_freq.c
+++ b/arch/arm/mach-mx5/bus_freq.c
@@ -104,7 +104,7 @@ 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;
 
 		/* currently not support on mx53 */
@@ -223,7 +223,7 @@ int set_high_bus_freq(int high_bus_freq)
 			 * If the CPU freq is 800MHz, set the bus to the high
 			 * setpoint (133MHz) and DDR to 200MHz.
 			 */
-			if (clk_get_rate(cpu_clk) !=
+			if (clk_get_rate(cpu_clk) >
 					cpu_wp_tbl[cpu_wp_nr - 1].cpu_rate)
 				high_bus_freq = 1;
 
diff --git a/arch/arm/mach-mx5/clock.c b/arch/arm/mach-mx5/clock.c
index 030887769f0..88f58893bb6 100644
--- a/arch/arm/mach-mx5/clock.c
+++ b/arch/arm/mach-mx5/clock.c
@@ -4790,9 +4790,6 @@ int __init mx53_clocks_init(unsigned long ckil, unsigned long osc, unsigned long
 	esdhc3_clk[0].recalc = _clk_esdhc3_recalc;
 	esdhc3_clk[0].set_rate = _clk_sdhc3_set_rate;
 
-#ifdef CONFIG_MXC_VPU_IRAM
-	 vpu_clk[2].secondary = &emi_intr_clk[1];
-#endif
 #if defined(CONFIG_USB_STATIC_IRAM) \
     || defined(CONFIG_USB_STATIC_IRAM_PPH)
 	usboh3_clk[1].secondary = &emi_intr_clk[1];
diff --git a/arch/arm/mach-mx5/system.c b/arch/arm/mach-mx5/system.c
index 09833a0c6e1..3eda2ac58c9 100644
--- a/arch/arm/mach-mx5/system.c
+++ b/arch/arm/mach-mx5/system.c
@@ -38,6 +38,8 @@
 
 extern int mxc_jtag_enabled;
 extern int iram_ready;
+extern int dvfs_core_is_active;
+extern void stop_dvfs(void);
 static struct clk *gpc_dvfs_clk;
 
 extern void cpu_cortexa8_do_idle(void *addr);
@@ -71,6 +73,7 @@ void mxc_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
 		if (mode == WAIT_UNCLOCKED_POWER_OFF) {
 			ccm_clpcr |= (0x1 << MXC_CCM_CLPCR_LPM_OFFSET);
 			ccm_clpcr &= ~MXC_CCM_CLPCR_VSTBY;
+			ccm_clpcr &= ~MXC_CCM_CLPCR_SBYOS;
 			stop_mode = 0;
 		} else {
 			ccm_clpcr |= (0x2 << MXC_CCM_CLPCR_LPM_OFFSET);
@@ -179,6 +182,9 @@ void arch_reset(char mode)
 	if (cpu_is_mx51() || cpu_is_mx53())
 		__raw_writel(0x20600, IO_ADDRESS(NFC_BASE_ADDR) + 0x28);
 
+	if (dvfs_core_is_active)
+		stop_dvfs();
+
 	/* Assert SRS signal */
 	mxc_wd_reset();
 #if defined(CONFIG_MACH_MX51_EFIKAMX)
diff --git a/arch/arm/plat-mxc/dvfs_core.c b/arch/arm/plat-mxc/dvfs_core.c
index c2df94c1f4c..6e408f1e004 100644
--- a/arch/arm/plat-mxc/dvfs_core.c
+++ b/arch/arm/plat-mxc/dvfs_core.c
@@ -89,6 +89,7 @@ extern int cpufreq_trig_needed;
 struct timeval core_prev_intr;
 
 void dump_dvfs_core_regs(void);
+void stop_dvfs(void);
 static struct delayed_work dvfs_core_handler;
 
 /*
@@ -268,6 +269,7 @@ static int set_cpu_freq(int wp)
 		/* Set ARM_PODF */
 		reg &= 0xFFFFFFF8;
 		reg |= arm_podf;
+		spin_lock_irqsave(&mxc_dvfs_core_lock, flags);
 
 		reg1 = __raw_readl(dvfs_data->ccm_cdhipr_reg_addr);
 		if ((reg1 & 0x00010000) == 0)
@@ -277,12 +279,6 @@ static int set_cpu_freq(int wp)
 			return 0;
 		}
 
-		/* START the GPC main control FSM */
-		reg = __raw_readl(dvfs_data->gpc_cntr_reg_addr);
-		reg |= MXC_GPCCNTR_FUPD;
-		/* ADU=1, select ARM domain */
-		reg |= MXC_GPCCNTR_ADU;
-		__raw_writel(reg, dvfs_data->gpc_cntr_reg_addr);
 		/* set VINC */
 		reg = __raw_readl(dvfs_data->gpc_vcr_reg_addr);
 		reg &=
@@ -294,8 +290,13 @@ static int set_cpu_freq(int wp)
 		__raw_writel(reg, dvfs_data->gpc_vcr_reg_addr);
 
 		reg = __raw_readl(dvfs_data->gpc_cntr_reg_addr);
-		reg &= (~(MXC_GPCCNTR_ADU | MXC_GPCCNTR_FUPD));
-		reg |= MXC_GPCCNTR_ADU | MXC_GPCCNTR_FUPD | MXC_GPCCNTR_STRT;
+		reg &= (~(MXC_GPCCNTR_ADU | MXC_GPCCNTR_FUPD | MXC_GPCCNTR_STRT));
+		__raw_writel(reg, dvfs_data->gpc_cntr_reg_addr);
+		reg = __raw_readl(dvfs_data->gpc_cntr_reg_addr);
+		reg |= MXC_GPCCNTR_ADU | MXC_GPCCNTR_FUPD;
+		__raw_writel(reg, dvfs_data->gpc_cntr_reg_addr);
+		reg = __raw_readl(dvfs_data->gpc_cntr_reg_addr);
+		reg |= MXC_GPCCNTR_STRT;
 		__raw_writel(reg, dvfs_data->gpc_cntr_reg_addr);
 
 		/* Wait for arm podf Enable */
@@ -305,6 +306,8 @@ static int set_cpu_freq(int wp)
 			udelay(10);
 		}
 
+		spin_unlock_irqrestore(&mxc_dvfs_core_lock, flags);
+
 		if (vinc == 0) {
 			ret = regulator_set_voltage(core_regulator,
 						    gp_volt, gp_volt);
@@ -485,7 +488,7 @@ static void dvfs_core_work_handler(struct work_struct *work)
 			maxf = 1;
 			goto END;
 		} else {
-			if (low_bus_freq_mode) {
+			if (!high_bus_freq_mode) {
 				/* bump up LP freq first. */
 				bus_incr = 1;
 				dvfs_load_config(2);
@@ -511,8 +514,8 @@ static void dvfs_core_work_handler(struct work_struct *work)
 			cpu_dcr = 0;
 		}
 	} else {
-		if (low_bus_freq_mode)
-			set_high_bus_freq(0);
+		if (!high_bus_freq_mode)
+			set_high_bus_freq(1);
 
 		if (!bus_incr)
 			ret = set_cpu_freq(curr_wp);
@@ -551,7 +554,7 @@ END:	/* Set MAXF, MINF */
 /*!
  * This function disables the DVFS module.
  */
-static void stop_dvfs(void)
+void stop_dvfs(void)
 {
 	u32 reg = 0;
 	unsigned long flags;
diff --git a/arch/arm/plat-mxc/system.c b/arch/arm/plat-mxc/system.c
index 79c37577c91..9d92d104761 100644
--- a/arch/arm/plat-mxc/system.c
+++ b/arch/arm/plat-mxc/system.c
@@ -38,6 +38,10 @@
 #define WDOG_WCR_ENABLE		(1 << 2)
 #endif
 
+extern int dvfs_core_is_active;
+extern void stop_dvfs(void);
+
+
 /*
  * Reset the system. It is called by machine_restart().
  */
@@ -51,6 +55,9 @@ void arch_reset(char mode, const char *cmd)
 			clk_enable(clk);
 	}
 
+	if (dvfs_core_is_active)
+		stop_dvfs();
+
 	/* Assert SRS signal */
 	__raw_writew(WDOG_WCR_ENABLE, WDOG_WCR_REG);