1 files changed, 89 insertions, 5 deletions

diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index b30001f31610..4fc0cb79f18f 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -223,7 +223,7 @@ static inline void adjust_jiffies(unsigned long val, struct cpufreq_freqs *ci)
         }
         if ((val == CPUFREQ_PRECHANGE  && ci->old < ci->new) ||
             (val == CPUFREQ_POSTCHANGE && ci->old > ci->new) ||
-            (val == CPUFREQ_RESUMECHANGE)) {
+            (val == CPUFREQ_RESUMECHANGE || val == CPUFREQ_SUSPENDCHANGE)) {
                 loops_per_jiffy = cpufreq_scale(l_p_j_ref, l_p_j_ref_freq, ci->new);
                 dprintk("scaling loops_per_jiffy to %lu for frequency %u kHz\n", loops_per_jiffy, ci->new);
         }
@@ -866,11 +866,90 @@ EXPORT_SYMBOL(cpufreq_get);
 
 
 /**
+ *      cpufreq_suspend - let the low level driver prepare for suspend
+ */
+
+static int cpufreq_suspend(struct sys_device * sysdev, u32 state)
+{
+        int cpu = sysdev->id;
+        unsigned int ret = 0;
+        unsigned int cur_freq = 0;
+        struct cpufreq_policy *cpu_policy;
+
+        dprintk("resuming cpu %u\n", cpu);
+
+        if (!cpu_online(cpu))
+                return 0;
+
+        /* we may be lax here as interrupts are off. Nonetheless
+         * we need to grab the correct cpu policy, as to check
+         * whether we really run on this CPU.
+         */
+
+        cpu_policy = cpufreq_cpu_get(cpu);
+        if (!cpu_policy)
+                return -EINVAL;
+
+        /* only handle each CPU group once */
+        if (unlikely(cpu_policy->cpu != cpu)) {
+                cpufreq_cpu_put(cpu_policy);
+                return 0;
+        }
+
+        if (cpufreq_driver->suspend) {
+                ret = cpufreq_driver->suspend(cpu_policy, state);
+                if (ret) {
+                        printk(KERN_ERR "cpufreq: suspend failed in ->suspend "
+                                        "step on CPU %u\n", cpu_policy->cpu);
+                        cpufreq_cpu_put(cpu_policy);
+                        return ret;
+                }
+        }
+
+
+        if (cpufreq_driver->flags & CPUFREQ_CONST_LOOPS)
+                goto out;
+
+        if (cpufreq_driver->get)
+                cur_freq = cpufreq_driver->get(cpu_policy->cpu);
+
+        if (!cur_freq || !cpu_policy->cur) {
+                printk(KERN_ERR "cpufreq: suspend failed to assert current "
+                       "frequency is what timing core thinks it is.\n");
+                goto out;
+        }
+
+        if (unlikely(cur_freq != cpu_policy->cur)) {
+                struct cpufreq_freqs freqs;
+
+                if (!(cpufreq_driver->flags & CPUFREQ_PM_NO_WARN))
+                        printk(KERN_DEBUG "Warning: CPU frequency is %u, "
+                               "cpufreq assumed %u kHz.\n",
+                               cur_freq, cpu_policy->cur);
+
+                freqs.cpu = cpu;
+                freqs.old = cpu_policy->cur;
+                freqs.new = cur_freq;
+
+                notifier_call_chain(&cpufreq_transition_notifier_list,
+                                    CPUFREQ_SUSPENDCHANGE, &freqs);
+                adjust_jiffies(CPUFREQ_SUSPENDCHANGE, &freqs);
+
+                cpu_policy->cur = cur_freq;
+        }
+
+ out:
+        cpufreq_cpu_put(cpu_policy);
+        return 0;
+}
+
+/**
  *      cpufreq_resume -  restore proper CPU frequency handling after resume
  *
  *      1.) resume CPUfreq hardware support (cpufreq_driver->resume())
  *      2.) if ->target and !CPUFREQ_CONST_LOOPS: verify we're in sync
- *      3.) schedule call cpufreq_update_policy() ASAP as interrupts are restored.
+ *      3.) schedule call cpufreq_update_policy() ASAP as interrupts are
+ *          restored.
  */
 static int cpufreq_resume(struct sys_device * sysdev)
 {
@@ -915,7 +994,9 @@ static int cpufreq_resume(struct sys_device * sysdev)
                         cur_freq = cpufreq_driver->get(cpu_policy->cpu);
 
                 if (!cur_freq || !cpu_policy->cur) {
-                        printk(KERN_ERR "cpufreq: resume failed to assert current frequency is what timing core thinks it is.\n");
+                        printk(KERN_ERR "cpufreq: resume failed to assert "
+                                        "current frequency is what timing core "
+                                        "thinks it is.\n");
                         goto out;
                 }
 
@@ -923,13 +1004,15 @@ static int cpufreq_resume(struct sys_device * sysdev)
                         struct cpufreq_freqs freqs;
 
                         printk(KERN_WARNING "Warning: CPU frequency is %u, "
-                               "cpufreq assumed %u kHz.\n", cur_freq, cpu_policy->cur);
+                                        "cpufreq assumed %u kHz.\n",
+                                        cur_freq, cpu_policy->cur);
 
                         freqs.cpu = cpu;
                         freqs.old = cpu_policy->cur;
                         freqs.new = cur_freq;
 
-                        notifier_call_chain(&cpufreq_transition_notifier_list, CPUFREQ_RESUMECHANGE, &freqs);
+                        notifier_call_chain(&cpufreq_transition_notifier_list,
+                                        CPUFREQ_RESUMECHANGE, &freqs);
                         adjust_jiffies(CPUFREQ_RESUMECHANGE, &freqs);
 
                         cpu_policy->cur = cur_freq;
@@ -945,6 +1028,7 @@ out:
 static struct sysdev_driver cpufreq_sysdev_driver = {
         .add            = cpufreq_add_dev,
         .remove         = cpufreq_remove_dev,
+        .suspend        = cpufreq_suspend,
         .resume         = cpufreq_resume,
 };