5 files changed, 57 insertions, 6 deletions

diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 1b41fca3d65a..e3219dfd736c 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -29,6 +29,7 @@
 #include <linux/async.h>
 #include <linux/suspend.h>
 #include <trace/events/power.h>
+#include <linux/cpufreq.h>
 #include <linux/cpuidle.h>
 #include <linux/timer.h>
 
@@ -540,6 +541,7 @@ static void dpm_resume_noirq(pm_message_t state)
         dpm_show_time(starttime, state, "noirq");
         resume_device_irqs();
         cpuidle_resume();
+        cpufreq_resume();
 }
 
 /**
@@ -955,6 +957,7 @@ static int dpm_suspend_noirq(pm_message_t state)
         ktime_t starttime = ktime_get();
         int error = 0;
 
+        cpufreq_suspend();
         cpuidle_pause();
         suspend_device_irqs();
         mutex_lock(&dpm_list_mtx);
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 02d534da22dd..81e9d4412db8 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -26,6 +26,7 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
+#include <linux/suspend.h>
 #include <linux/syscore_ops.h>
 #include <linux/tick.h>
 #include <trace/events/power.h>
@@ -47,6 +48,9 @@ static LIST_HEAD(cpufreq_policy_list);
 static DEFINE_PER_CPU(char[CPUFREQ_NAME_LEN], cpufreq_cpu_governor);
 #endif
 
+/* Flag to suspend/resume CPUFreq governors */
+static bool cpufreq_suspended;
+
 static inline bool has_target(void)
 {
         return cpufreq_driver->target_index || cpufreq_driver->target;
@@ -1462,6 +1466,41 @@ static struct subsys_interface cpufreq_interface = {
         .remove_dev     = cpufreq_remove_dev,
 };
 
+void cpufreq_suspend(void)
+{
+        struct cpufreq_policy *policy;
+
+        if (!has_target())
+                return;
+
+        pr_debug("%s: Suspending Governors\n", __func__);
+
+        list_for_each_entry(policy, &cpufreq_policy_list, policy_list)
+                if (__cpufreq_governor(policy, CPUFREQ_GOV_STOP))
+                        pr_err("%s: Failed to stop governor for policy: %p\n",
+                                __func__, policy);
+
+        cpufreq_suspended = true;
+}
+
+void cpufreq_resume(void)
+{
+        struct cpufreq_policy *policy;
+
+        if (!has_target())
+                return;
+
+        pr_debug("%s: Resuming Governors\n", __func__);
+
+        cpufreq_suspended = false;
+
+        list_for_each_entry(policy, &cpufreq_policy_list, policy_list)
+                if (__cpufreq_governor(policy, CPUFREQ_GOV_START)
+                    || __cpufreq_governor(policy, CPUFREQ_GOV_LIMITS))
+                        pr_err("%s: Failed to start governor for policy: %p\n",
+                                __func__, policy);
+}
+
 /**
  * cpufreq_bp_suspend - Prepare the boot CPU for system suspend.
  *
@@ -1764,6 +1803,10 @@ static int __cpufreq_governor(struct cpufreq_policy *policy,
         struct cpufreq_governor *gov = NULL;
 #endif
 
+        /* Don't start any governor operations if we are entering suspend */
+        if (cpufreq_suspended)
+                return 0;
+
         if (policy->governor->max_transition_latency &&
             policy->cpuinfo.transition_latency >
             policy->governor->max_transition_latency) {
@@ -2076,9 +2119,6 @@ static int cpufreq_cpu_callback(struct notifier_block *nfb,
         dev = get_cpu_device(cpu);
         if (dev) {
 
-                if (action & CPU_TASKS_FROZEN)
-                        frozen = true;
-
                 switch (action & ~CPU_TASKS_FROZEN) {
                 case CPU_ONLINE:
                         __cpufreq_add_dev(dev, NULL, frozen);
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 2a991e468f78..a55e68f2cfc8 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -400,7 +400,7 @@ EXPORT_SYMBOL_GPL(cpuidle_register_device);
  */
 void cpuidle_unregister_device(struct cpuidle_device *dev)
 {
-        if (dev->registered == 0)
+        if (!dev || dev->registered == 0)
                 return;
 
         cpuidle_pause_and_lock();
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
index cbd4e9abc47e..92d1206482a6 100644
--- a/drivers/idle/intel_idle.c
+++ b/drivers/idle/intel_idle.c
@@ -329,7 +329,7 @@ static struct cpuidle_state atom_cstates[] __initdata = {
         {
                 .enter = NULL }
 };
-static struct cpuidle_state avn_cstates[CPUIDLE_STATE_MAX] = {
+static struct cpuidle_state avn_cstates[] __initdata = {
         {
                 .name = "C1-AVN",
                 .desc = "MWAIT 0x00",
@@ -340,7 +340,7 @@ static struct cpuidle_state avn_cstates[CPUIDLE_STATE_MAX] = {
         {
                 .name = "C6-AVN",
                 .desc = "MWAIT 0x51",
-                .flags = MWAIT2flg(0x58) | CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
+                .flags = MWAIT2flg(0x51) | CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TLB_FLUSHED,
                 .exit_latency = 15,
                 .target_residency = 45,
                 .enter = &intel_idle },
diff --git a/include/linux/cpufreq.h b/include/linux/cpufreq.h
index dc196bbcf227..ee5fe9d77ae8 100644
--- a/include/linux/cpufreq.h
+++ b/include/linux/cpufreq.h
@@ -280,6 +280,14 @@ cpufreq_verify_within_cpu_limits(struct cpufreq_policy *policy)
                         policy->cpuinfo.max_freq);
 }
 
+#ifdef CONFIG_CPU_FREQ
+void cpufreq_suspend(void);
+void cpufreq_resume(void);
+#else
+static inline void cpufreq_suspend(void) {}
+static inline void cpufreq_resume(void) {}
+#endif
+
 /*********************************************************************
  *                     CPUFREQ NOTIFIER INTERFACE                    *
  *********************************************************************/