ENGR00117202: System cannot resume when suspended at lower CPU frequency.

Change the code such that suspend and resume always set the CPU frequency
and voltage to the highest level.

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

5 files changed, 150 insertions, 21 deletions

diff --git a/arch/arm/mach-mx37/bus_freq.c b/arch/arm/mach-mx37/bus_freq.c
index d6cde4a21487..302f59a17881 100644
--- a/arch/arm/mach-mx37/bus_freq.c
+++ b/arch/arm/mach-mx37/bus_freq.c
@@ -69,6 +69,7 @@ int high_bus_freq_mode;
 char *gp_reg_id = "SW1";
 char *lp_reg_id = "SW2";
 static struct cpu_wp *cpu_wp_tbl;
+static int busfreq_suspended;
 
 struct dvfs_wp dvfs_core_setpoint[] = {
 						{33, 8, 33, 10, 10, 0x08},
@@ -83,6 +84,9 @@ int set_low_bus_freq(void)
 	int reg;
 	unsigned long lp_lpm_clk;
 
+	if (busfreq_suspended)
+		return ret;
+
 	spin_lock_irqsave(&bus_freq_lock, flags);
 
 	if (low_bus_freq_mode || (clk_get_rate(cpu_clk) != GP_LPAPM_FREQ)) {
@@ -233,6 +237,20 @@ void setup_pll(void)
 	}
 }
 
+static int busfreq_suspend(struct platform_device *pdev, pm_message_t message)
+{
+	if (low_bus_freq_mode)
+		set_high_bus_freq(1);
+	busfreq_suspended = 1;
+	return 0;
+}
+
+static int busfreq_resume(struct platform_device *pdev)
+{
+	busfreq_suspended = 0;
+	return  0;
+}
+
 /*!
  * This is the probe routine for the bus frequency driver.
  *
@@ -386,6 +404,8 @@ static struct platform_driver busfreq_driver = {
 		   .name = "busfreq",
 		   },
 	.probe = busfreq_probe,
+	.suspend = busfreq_suspend,
+	.resume = busfreq_resume,
 };
 
 /*!
diff --git a/arch/arm/mach-mx37/pm.c b/arch/arm/mach-mx37/pm.c
index b3a7ca7adff7..ebd6607d5fdb 100644
--- a/arch/arm/mach-mx37/pm.c
+++ b/arch/arm/mach-mx37/pm.c
@@ -11,8 +11,20 @@
  * http://www.gnu.org/copyleft/gpl.html
  */
 
+#include <linux/proc_fs.h>
 #include <linux/kernel.h>
 #include <linux/suspend.h>
+#include <linux/clk.h>
+#include <linux/cpufreq.h>
+
+static struct cpu_wp *cpu_wp_tbl;
+static struct clk *cpu_clk;
+
+#if defined(CONFIG_CPU_FREQ)
+static int org_freq;
+extern int cpufreq_suspended;
+extern int set_cpu_freq(int wp);
+#endif
 
 static int mx37_suspend_enter(suspend_state_t state)
 {
@@ -40,6 +52,21 @@ static int mx37_suspend_enter(suspend_state_t state)
  */
 static int mx37_suspend_prepare(void)
 {
+#if defined(CONFIG_CPU_FREQ)
+	struct cpufreq_freqs freqs;
+	org_freq = clk_get_rate(cpu_clk);
+	freqs.old = org_freq / 1000;
+	freqs.new = cpu_wp_tbl[0].cpu_rate / 1000;
+	freqs.cpu = 0;
+	freqs.flags = 0;
+
+	cpufreq_suspended = 1;
+	if (clk_get_rate(cpu_clk) != cpu_wp_tbl[0].cpu_rate) {
+		set_cpu_freq(cpu_wp_tbl[0].cpu_rate);
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+#endif
 	return 0;
 }
 
@@ -48,6 +75,22 @@ static int mx37_suspend_prepare(void)
  */
 static void mx37_suspend_finish(void)
 {
+#if defined(CONFIG_CPU_FREQ)
+	struct cpufreq_freqs freqs;
+
+	freqs.old = clk_get_rate(cpu_clk) / 1000;
+	freqs.new = org_freq / 1000;
+	freqs.cpu = 0;
+	freqs.flags = 0;
+
+	cpufreq_suspended = 0;
+
+	if (org_freq != clk_get_rate(cpu_clk)) {
+		set_cpu_freq(org_freq);
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+#endif
 }
 
 static int mx37_pm_valid(suspend_state_t state)
@@ -64,9 +107,18 @@ struct platform_suspend_ops mx37_suspend_ops = {
 
 static int __init mx37_pm_init(void)
 {
+	int cpu_wp_nr;
+
 	pr_info("Static Power Management for Freescale i.MX37\n");
 	suspend_set_ops(&mx37_suspend_ops);
 
+	cpu_wp_tbl = get_cpu_wp(&cpu_wp_nr);
+
+	cpu_clk = clk_get(NULL, "cpu_clk");
+	if (IS_ERR(cpu_clk)) {
+		printk(KERN_DEBUG "%s: failed to get cpu_clk\n", __func__);
+		return PTR_ERR(cpu_clk);
+	}
 	return 0;
 }
 
diff --git a/arch/arm/mach-mx51/bus_freq.c b/arch/arm/mach-mx51/bus_freq.c
index c73e8cddcb06..a1c4777ede6a 100644
--- a/arch/arm/mach-mx51/bus_freq.c
+++ b/arch/arm/mach-mx51/bus_freq.c
@@ -73,6 +73,7 @@ char *lp_reg_id = "SW2";
 
 static struct cpu_wp *cpu_wp_tbl;
 static struct device *busfreq_dev;
+static int busfreq_suspended;
 int sdram_autogating_paused;
 
 extern int lp_high_freq;
@@ -98,6 +99,10 @@ int set_low_bus_freq(void)
 	u32 reg;
 
 	if (bus_freq_scaling_is_active) {
+
+		if (busfreq_suspended)
+			return 0;
+
 		if (clk_get_rate(cpu_clk) != cpu_wp_tbl[cpu_wp_nr - 1].cpu_rate)
 			return 0;
 
@@ -355,6 +360,20 @@ static ssize_t bus_freq_scaling_enable_store(struct device *dev,
 	return size;
 }
 
+static int busfreq_suspend(struct platform_device *pdev, pm_message_t message)
+{
+	if (low_bus_freq_mode)
+		set_high_bus_freq(1);
+	busfreq_suspended = 1;
+	return 0;
+}
+
+static int busfreq_resume(struct platform_device *pdev)
+{
+	busfreq_suspended = 0;
+	return  0;
+}
+
 static DEVICE_ATTR(enable, 0644, bus_freq_scaling_enable_show,
 			bus_freq_scaling_enable_store);
 
@@ -522,6 +541,8 @@ static struct platform_driver busfreq_driver = {
 		   .name = "busfreq",
 		},
 	.probe = busfreq_probe,
+	.suspend = busfreq_suspend,
+	.resume = busfreq_resume,
 };
 
 /*!
diff --git a/arch/arm/mach-mx51/pm.c b/arch/arm/mach-mx51/pm.c
index 8ec48399760c..1a6a5b1d6025 100644
--- a/arch/arm/mach-mx51/pm.c
+++ b/arch/arm/mach-mx51/pm.c
@@ -18,15 +18,29 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
 #include <linux/suspend.h>
+#include <linux/proc_fs.h>
+#include <linux/cpufreq.h>
+#include <mach/hardware.h>
 #include <asm/cacheflush.h>
 #include <asm/tlb.h>
 #include <asm/mach/map.h>
 #include "crm_regs.h"
 
+static struct cpu_wp *cpu_wp_tbl;
+static struct clk *cpu_clk;
+
+#if defined(CONFIG_CPU_FREQ)
+static int org_freq;
+extern int cpufreq_suspended;
+extern int set_cpu_freq(int wp);
+#endif
+
+
 static struct device *pm_dev;
 struct clk *gpc_dvfs_clk;
 extern void cpu_do_suspend_workaround(u32 sdclk_iomux_addr);
 extern void cpu_cortexa8_do_idle(u32);
+extern struct cpu_wp *(*get_cpu_wp)(int *wp);
 
 extern int iram_ready;
 void *suspend_iram_base;
@@ -86,6 +100,21 @@ static int mx51_suspend_enter(suspend_state_t state)
  */
 static int mx51_suspend_prepare(void)
 {
+#if defined(CONFIG_CPU_FREQ)
+	struct cpufreq_freqs freqs;
+	org_freq = clk_get_rate(cpu_clk);
+	freqs.old = org_freq / 1000;
+	freqs.new = cpu_wp_tbl[0].cpu_rate / 1000;
+	freqs.cpu = 0;
+	freqs.flags = 0;
+
+	cpufreq_suspended = 1;
+	if (clk_get_rate(cpu_clk) != cpu_wp_tbl[0].cpu_rate) {
+		set_cpu_freq(cpu_wp_tbl[0].cpu_rate);
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+#endif
 	return 0;
 }
 
@@ -94,6 +123,22 @@ static int mx51_suspend_prepare(void)
  */
 static void mx51_suspend_finish(void)
 {
+#if defined(CONFIG_CPU_FREQ)
+	struct cpufreq_freqs freqs;
+
+	freqs.old = clk_get_rate(cpu_clk) / 1000;
+	freqs.new = org_freq / 1000;
+	freqs.cpu = 0;
+	freqs.flags = 0;
+
+	cpufreq_suspended = 0;
+
+	if (org_freq != clk_get_rate(cpu_clk)) {
+		set_cpu_freq(org_freq);
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+#endif
 }
 
 /*
@@ -132,6 +177,8 @@ static struct platform_driver mx51_pm_driver = {
 
 static int __init pm_init(void)
 {
+	int cpu_wp_nr;
+
 	pr_info("Static Power Management for Freescale i.MX51\n");
 	if (platform_driver_register(&mx51_pm_driver) != 0) {
 		printk(KERN_ERR "mx51_pm_driver register failed\n");
@@ -147,6 +194,13 @@ static int __init pm_init(void)
 										MT_HIGH_VECTORS);
 	suspend_in_iram = (void *)suspend_iram_base;
 
+	cpu_wp_tbl = get_cpu_wp(&cpu_wp_nr);
+
+	cpu_clk = clk_get(NULL, "cpu_clk");
+	if (IS_ERR(cpu_clk)) {
+		printk(KERN_DEBUG "%s: failed to get cpu_clk\n", __func__);
+		return PTR_ERR(cpu_clk);
+	}
 	printk(KERN_INFO "PM driver module loaded\n");
 
 	return 0;
diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
index 485be934c912..312cb6574676 100644
--- a/arch/arm/plat-mxc/cpufreq.c
+++ b/arch/arm/plat-mxc/cpufreq.c
@@ -62,7 +62,7 @@ extern int low_freq_bus_used(void);
 extern struct cpu_wp *(*get_cpu_wp)(int *wp);
 #endif
 
-static int set_cpu_freq(int freq)
+int set_cpu_freq(int freq)
 {
 	int ret = 0;
 	int org_cpu_rate;
@@ -70,7 +70,6 @@ static int set_cpu_freq(int freq)
 	int i;
 
 	org_cpu_rate = clk_get_rate(cpu_clk);
-
 	if (org_cpu_rate == freq)
 		return ret;
 
@@ -154,10 +153,7 @@ static int mxc_set_target(struct cpufreq_policy *policy,
 	int low_freq_bus_ready = 0;
 	int ret = 0;
 
-	if (cpufreq_suspended)
-		return 0;
-
-	if (dvfs_core_is_active) {
+	if (dvfs_core_is_active || cpufreq_suspended) {
 		target_freq = clk_get_rate(cpu_clk) / 1000;
 		freq_Hz = calc_frequency_khz(target_freq, relation) * 1000;
 		if (freq_Hz == arm_lpm_clk)
@@ -281,25 +277,11 @@ static int __init mxc_cpufreq_driver_init(struct cpufreq_policy *policy)
 static int mxc_cpufreq_suspend(struct cpufreq_policy *policy,
 				     pm_message_t state)
 {
-	struct cpufreq_freqs freqs;
-	int ret = 0;
-	cpufreq_suspended = 1;
-
-	freqs.old = clk_get_rate(cpu_clk) / 1000;
-	freqs.new = arm_normal_clk / 1000;
-	freqs.cpu = 0;
-	freqs.flags = 0;
-
-	if (clk_get_rate(cpu_clk) != arm_normal_clk) {
-		set_high_bus_freq(1);
-		ret = set_cpu_freq(arm_normal_clk);
-	}
-	return ret;
+	return 0;
 }
 
 static int mxc_cpufreq_resume(struct cpufreq_policy *policy)
 {
-	cpufreq_suspended = 0;
 	return 0;
 }