ENGR00220370 [MX6]Fix BUS freq suspend/resume fail in low bus mode

1. BUS freq's set low bus setpoint using delat work, which
didn't have mutex lock, so in some scenarios, set high bus
freq function can be called at the same time, we need to move
mutex lock into these two routine;

2. Using pm notify to make sure bus freq set to high setpoint
before supend and restore after resume.

3. Clear build warning.

Signed-off-by: Anson Huang <b20788@freescale.com>

5 files changed, 79 insertions, 37 deletions

diff --git a/arch/arm/mach-mx6/board-mx6q_sabresd.c b/arch/arm/mach-mx6/board-mx6q_sabresd.c
index eb0f8fc6fb1e..3a02094869e7 100644
--- a/arch/arm/mach-mx6/board-mx6q_sabresd.c
+++ b/arch/arm/mach-mx6/board-mx6q_sabresd.c
@@ -1517,11 +1517,10 @@ static struct gpio_led imx6q_gpio_leds[] = {
 /* For the latest B4 board, this GPIO_1 is connected to POR_B,
 which will reset the whole board if this pin's level is changed,
 so, for the latest board, we have to avoid using this pin as
-GPIO. */
-#if 0
+GPIO.
 	GPIO_LED(SABRESD_CHARGE_DONE, "chg_done_led", 0, 1,
 			"charger-full"),
-#endif
+*/
 };
 
 static struct gpio_led_platform_data imx6q_gpio_leds_data = {
diff --git a/arch/arm/mach-mx6/bus_freq.c b/arch/arm/mach-mx6/bus_freq.c
index 2bb2ca40022f..06454112eae3 100644
--- a/arch/arm/mach-mx6/bus_freq.c
+++ b/arch/arm/mach-mx6/bus_freq.c
@@ -44,6 +44,7 @@
 #include <asm/cacheflush.h>
 #include <asm/tlb.h>
 #include "crm_regs.h"
+#include <linux/suspend.h>
 
 #define LPAPM_CLK		24000000
 #define DDR_MED_CLK		400000000
@@ -105,13 +106,18 @@ static struct delayed_work low_bus_freq_handler;
 
 static void reduce_bus_freq_handler(struct work_struct *work)
 {
-	int ret = 0;
+	int ret;
 
-	if (low_bus_freq_mode || !low_freq_bus_used())
+	mutex_lock(&bus_freq_mutex);
+	if (low_bus_freq_mode || !low_freq_bus_used()) {
+		mutex_unlock(&bus_freq_mutex);
 		return;
+	}
 
-	if (audio_bus_freq_mode && lp_audio_freq)
+	if (audio_bus_freq_mode && lp_audio_freq) {
+		mutex_unlock(&bus_freq_mutex);
 		return;
+	}
 
 	if (!cpu_is_mx6sl()) {
 		clk_enable(pll3);
@@ -174,6 +180,7 @@ static void reduce_bus_freq_handler(struct work_struct *work)
 
 	high_bus_freq_mode = 0;
 	med_bus_freq_mode = 0;
+	mutex_unlock(&bus_freq_mutex);
 }
 /* Set the DDR, AHB to 24MHz.
   * This mode will be activated only when none of the modules that
@@ -199,31 +206,44 @@ int set_low_bus_freq(void)
  */
 int set_high_bus_freq(int high_bus_freq)
 {
-	int ret = 0;
+	int ret;
 
-	if (busfreq_suspended)
+	if (bus_freq_scaling_initialized && bus_freq_scaling_is_active)
+		cancel_delayed_work_sync(&low_bus_freq_handler);
+	mutex_lock(&bus_freq_mutex);
+	if (busfreq_suspended) {
+		mutex_unlock(&bus_freq_mutex);
 		return 0;
+	}
 
-	if (!bus_freq_scaling_initialized || !bus_freq_scaling_is_active)
+	if (!bus_freq_scaling_initialized || !bus_freq_scaling_is_active) {
+		mutex_unlock(&bus_freq_mutex);
 		return 0;
+	}
 
-	if (high_bus_freq_mode && high_bus_freq)
+	if (high_bus_freq_mode && high_bus_freq) {
+		mutex_unlock(&bus_freq_mutex);
 		return 0;
+	}
 
-	if (med_bus_freq_mode && !high_bus_freq)
+	if (med_bus_freq_mode && !high_bus_freq) {
+		mutex_unlock(&bus_freq_mutex);
 		return 0;
+	}
 
 	if (cpu_is_mx6dl() && high_bus_freq)
 		high_bus_freq = 0;
 
-	if (cpu_is_mx6dl() && med_bus_freq_mode)
+	if (cpu_is_mx6dl() && med_bus_freq_mode) {
+		mutex_unlock(&bus_freq_mutex);
 		return 0;
-
+	}
 	if ((high_bus_freq_mode && (high_bus_freq || lp_high_freq)) ||
 	    (med_bus_freq_mode && !high_bus_freq && lp_med_freq &&
-	     !lp_high_freq))
+	     !lp_high_freq)) {
+		mutex_unlock(&bus_freq_mutex);
 		return 0;
-
+	}
 	if (cpu_is_mx6sl()) {
 		/* Set the voltage of VDDSOC to 1.2V as in normal mode. */
 		ret = regulator_set_voltage(vddsoc_cap_regulator, 1200000,
@@ -294,6 +314,7 @@ int set_high_bus_freq(int high_bus_freq)
 	if (cpu_is_mx6sl())
 		arm_mem_clked_in_wait = false;
 
+	mutex_unlock(&bus_freq_mutex);
 	return 0;
 }
 
@@ -342,15 +363,21 @@ void bus_freq_update(struct clk *clk, bool flag)
 			}
 		} else {
 			if ((clk->flags & AHB_MED_SET_POINT)
-				&& !med_bus_freq_mode)
+				&& !med_bus_freq_mode) {
 				/* Set to Medium setpoint */
+				mutex_unlock(&bus_freq_mutex);
 				set_high_bus_freq(0);
+				return;
+			}
 			else if ((clk->flags & AHB_HIGH_SET_POINT)
-				&& !high_bus_freq_mode)
+				&& !high_bus_freq_mode) {
 				/* Currently at low or medium set point,
 				* need to set to high setpoint
 				*/
+				mutex_unlock(&bus_freq_mutex);
 				set_high_bus_freq(1);
+				return;
+			}
 			}
 		}
 	} else {
@@ -366,12 +393,16 @@ void bus_freq_update(struct clk *clk, bool flag)
 			&& (clk_get_usecount(clk) == 0)) {
 			if (low_freq_bus_used() && !low_bus_freq_mode)
 				set_low_bus_freq();
-			else
+			else {
 				/* Set to either high or medium setpoint. */
+				mutex_unlock(&bus_freq_mutex);
 				set_high_bus_freq(0);
+				return;
+			}
 		}
 	}
 	mutex_unlock(&bus_freq_mutex);
+	return;
 }
 void setup_pll(void)
 {
@@ -407,16 +438,28 @@ static ssize_t bus_freq_scaling_enable_store(struct device *dev,
 
 static int busfreq_suspend(struct platform_device *pdev, pm_message_t message)
 {
-	set_high_bus_freq(1);
-	busfreq_suspended = 1;
 	return 0;
 }
 
+static int bus_freq_pm_notify(struct notifier_block *nb, unsigned long event,
+	void *dummy)
+{
+	if (event == PM_SUSPEND_PREPARE) {
+		set_high_bus_freq(1);
+		busfreq_suspended = 1;
+	} else if (event == PM_POST_SUSPEND) {
+		busfreq_suspended = 0;
+	}
+
+	return NOTIFY_OK;
+}
 static int busfreq_resume(struct platform_device *pdev)
 {
-	busfreq_suspended = 0;
 	return  0;
 }
+static struct notifier_block imx_bus_freq_pm_notifier = {
+	.notifier_call = bus_freq_pm_notify,
+};
 
 static DEVICE_ATTR(enable, 0644, bus_freq_scaling_enable_show,
 			bus_freq_scaling_enable_store);
@@ -552,6 +595,7 @@ static int __devinit busfreq_probe(struct platform_device *pdev)
 	}
 
 	INIT_DELAYED_WORK(&low_bus_freq_handler, reduce_bus_freq_handler);
+	register_pm_notifier(&imx_bus_freq_pm_notifier);
 
 	if (!cpu_is_mx6sl())
 		init_mmdc_settings();
diff --git a/arch/arm/mach-mx6/clock.c b/arch/arm/mach-mx6/clock.c
index fe8b8da4fa7f..7fa93d0bb5fd 100644
--- a/arch/arm/mach-mx6/clock.c
+++ b/arch/arm/mach-mx6/clock.c
@@ -1277,11 +1277,6 @@ static int _clk_arm_set_rate(struct clk *clk, unsigned long rate)
 				else
 					pll1_sw_clk.set_parent(&pll1_sw_clk, &osc_clk);
 			}
-			if (cpu_op_tbl[i].cpu_podf) {
-				__raw_writel(cpu_op_tbl[i].cpu_podf, MXC_CCM_CACRR);
-				while (__raw_readl(MXC_CCM_CDHIPR))
-							;
-			}
 			pll1_sys_main_clk.set_rate(&pll1_sys_main_clk, cpu_op_tbl[i].pll_rate);
 		}
 		/* Make sure pll1_sw_clk is from pll1_sys_main_clk */
@@ -2002,7 +1997,7 @@ static struct clk vdoa_clk[] = {
 	},
 };
 
-static unsigned long mx6_timer_rate()
+static unsigned long mx6_timer_rate(void)
 {
 	u32 parent_rate = clk_get_rate(&osc_clk);
 
diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
index fd285cfce0bc..549f5c5c7ce0 100755
--- a/arch/arm/plat-mxc/cpufreq.c
+++ b/arch/arm/plat-mxc/cpufreq.c
@@ -96,10 +96,12 @@ int set_cpu_freq(int freq)
 	/*Set the voltage for the GP domain. */
 	if (freq > org_cpu_rate) {
 		mutex_lock(&bus_freq_mutex);
-		if (low_bus_freq_mode || audio_bus_freq_mode)
+		if (low_bus_freq_mode || audio_bus_freq_mode) {
+			mutex_unlock(&bus_freq_mutex);
 			set_high_bus_freq(0);
-		mutex_unlock(&bus_freq_mutex);
-		if (freq == cpu_op_tbl[0].cpu_rate && soc_regulator && pu_regulator) {
+		} else
+			mutex_unlock(&bus_freq_mutex);
+		if (freq == cpu_op_tbl[0].cpu_rate && !IS_ERR(soc_regulator) && !IS_ERR(pu_regulator)) {
 			ret = regulator_set_voltage(soc_regulator, soc_volt,
 							soc_volt);
 			if (ret < 0) {
diff --git a/arch/arm/plat-mxc/dvfs_core.c b/arch/arm/plat-mxc/dvfs_core.c
index 9018b49511cc..a4a15482c71f 100755
--- a/arch/arm/plat-mxc/dvfs_core.c
+++ b/arch/arm/plat-mxc/dvfs_core.c
@@ -647,22 +647,22 @@ static void dvfs_core_work_handler(struct work_struct *work)
 	mutex_lock(&bus_freq_mutex);
 	if ((curr_op == cpu_op_nr - 1) && (!low_bus_freq_mode)
 	    && (low_freq_bus_ready) && !bus_incr) {
-		mutex_unlock(&bus_freq_mutex);
 		if (!minf)
 			set_cpu_freq(curr_op);
 		/* If dvfs_core_op is greater than cpu_op_nr, it implies
 		 * we support LPAPM mode for this platform.
 		 */
 		if (dvfs_core_op > cpu_op_nr) {
-			mutex_lock(&bus_freq_mutex);
 			set_low_bus_freq();
-			mutex_unlock(&bus_freq_mutex);
 			dvfs_load_config(cpu_op_nr + 1);
 		}
+		mutex_unlock(&bus_freq_mutex);
 	} else {
-		if (!high_bus_freq_mode)
+		if (!high_bus_freq_mode) {
+			mutex_unlock(&bus_freq_mutex);
 			set_high_bus_freq(1);
-		mutex_unlock(&bus_freq_mutex);
+		} else
+			mutex_unlock(&bus_freq_mutex);
 		if (!bus_incr)
 			ret = set_cpu_freq(curr_op);
 		bus_incr = 0;
@@ -735,9 +735,11 @@ void stop_dvfs(void)
 
 		curr_op = 0;
 		mutex_lock(&bus_freq_mutex);
-		if (!high_bus_freq_mode)
+		if (!high_bus_freq_mode) {
+			mutex_unlock(&bus_freq_mutex);
 			set_high_bus_freq(1);
-		mutex_unlock(&bus_freq_mutex);
+		} else
+			mutex_unlock(&bus_freq_mutex);
 
 		curr_cpu = clk_get_rate(cpu_clk);
 		if (curr_cpu != cpu_op_tbl[curr_op].cpu_rate) {