1 files changed, 44 insertions, 0 deletions

diff --git a/drivers/cpufreq/powernv-cpufreq.c b/drivers/cpufreq/powernv-cpufreq.c
index 379c0837f5a9..2dfd4fdb5a52 100644
--- a/drivers/cpufreq/powernv-cpufreq.c
+++ b/drivers/cpufreq/powernv-cpufreq.c
@@ -26,6 +26,7 @@
 #include <linux/cpufreq.h>
 #include <linux/smp.h>
 #include <linux/of.h>
+#include <linux/reboot.h>
 
 #include <asm/cputhreads.h>
 #include <asm/firmware.h>
@@ -35,6 +36,7 @@
 #define POWERNV_MAX_PSTATES     256
 
 static struct cpufreq_frequency_table powernv_freqs[POWERNV_MAX_PSTATES+1];
+static bool rebooting;
 
 /*
  * Note: The set of pstates consists of contiguous integers, the
@@ -284,6 +286,15 @@ static void set_pstate(void *freq_data)
 }
 
 /*
+ * get_nominal_index: Returns the index corresponding to the nominal
+ * pstate in the cpufreq table
+ */
+static inline unsigned int get_nominal_index(void)
+{
+        return powernv_pstate_info.max - powernv_pstate_info.nominal;
+}
+
+/*
  * powernv_cpufreq_target_index: Sets the frequency corresponding to
  * the cpufreq table entry indexed by new_index on the cpus in the
  * mask policy->cpus
@@ -293,6 +304,9 @@ static int powernv_cpufreq_target_index(struct cpufreq_policy *policy,
 {
         struct powernv_smp_call_data freq_data;
 
+        if (unlikely(rebooting) && new_index != get_nominal_index())
+                return 0;
+
         freq_data.pstate_id = powernv_freqs[new_index].driver_data;
 
         /*
@@ -317,6 +331,33 @@ static int powernv_cpufreq_cpu_init(struct cpufreq_policy *policy)
         return cpufreq_table_validate_and_show(policy, powernv_freqs);
 }
 
+static int powernv_cpufreq_reboot_notifier(struct notifier_block *nb,
+                                unsigned long action, void *unused)
+{
+        int cpu;
+        struct cpufreq_policy cpu_policy;
+
+        rebooting = true;
+        for_each_online_cpu(cpu) {
+                cpufreq_get_policy(&cpu_policy, cpu);
+                powernv_cpufreq_target_index(&cpu_policy, get_nominal_index());
+        }
+
+        return NOTIFY_DONE;
+}
+
+static struct notifier_block powernv_cpufreq_reboot_nb = {
+        .notifier_call = powernv_cpufreq_reboot_notifier,
+};
+
+static void powernv_cpufreq_stop_cpu(struct cpufreq_policy *policy)
+{
+        struct powernv_smp_call_data freq_data;
+
+        freq_data.pstate_id = powernv_pstate_info.min;
+        smp_call_function_single(policy->cpu, set_pstate, &freq_data, 1);
+}
+
 static struct cpufreq_driver powernv_cpufreq_driver = {
         .name           = "powernv-cpufreq",
         .flags          = CPUFREQ_CONST_LOOPS,
@@ -324,6 +365,7 @@ static struct cpufreq_driver powernv_cpufreq_driver = {
         .verify         = cpufreq_generic_frequency_table_verify,
         .target_index   = powernv_cpufreq_target_index,
         .get            = powernv_cpufreq_get,
+        .stop_cpu       = powernv_cpufreq_stop_cpu,
         .attr           = powernv_cpu_freq_attr,
 };
 
@@ -342,12 +384,14 @@ static int __init powernv_cpufreq_init(void)
                 return rc;
         }
 
+        register_reboot_notifier(&powernv_cpufreq_reboot_nb);
         return cpufreq_register_driver(&powernv_cpufreq_driver);
 }
 module_init(powernv_cpufreq_init);
 
 static void __exit powernv_cpufreq_exit(void)
 {
+        unregister_reboot_notifier(&powernv_cpufreq_reboot_nb);
         cpufreq_unregister_driver(&powernv_cpufreq_driver);
 }
 module_exit(powernv_cpufreq_exit);