cpufreq: intel_pstate: Fix HWP on boot CPU after system resume

Commit 41cfd64cf49fc "Update frequencies of policy->cpus only from
->set_policy()" changed the way the intel_pstate driver's ->set_policy
callback updates the HWP (hardware-managed P-states) settings.
A side effect of it is that if those settings are modified on the
boot CPU during system suspend and wakeup, they will never be
restored during subsequent system resume.

To address this problem, allow cpufreq drivers that don't provide
->target or ->target_index callbacks to use ->suspend and ->resume
callbacks and add a ->resume callback to intel_pstate to restore
the HWP settings on the CPUs that belong to the given policy.

Fixes: 41cfd64cf49fc "Update frequencies of policy->cpus only from ->set_policy()"
Tested-by: Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>
Signed-off-by: Rafael J. Wysocki <rafael.j.wysocki@intel.com>
Acked-by: Viresh Kumar <viresh.kumar@linaro.org>

2 files changed, 25 insertions, 13 deletions

diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index e93405f0eac4..c4acfc5273b3 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -1557,21 +1557,25 @@ void cpufreq_suspend(void)
         if (!cpufreq_driver)
                 return;
 
-        if (!has_target())
+        if (!has_target() && !cpufreq_driver->suspend)
                 goto suspend;
 
         pr_debug("%s: Suspending Governors\n", __func__);
 
         for_each_active_policy(policy) {
-                down_write(&policy->rwsem);
-                ret = cpufreq_governor(policy, CPUFREQ_GOV_STOP);
-                up_write(&policy->rwsem);
+                if (has_target()) {
+                        down_write(&policy->rwsem);
+                        ret = cpufreq_governor(policy, CPUFREQ_GOV_STOP);
+                        up_write(&policy->rwsem);
 
-                if (ret)
-                        pr_err("%s: Failed to stop governor for policy: %p\n",
-                                __func__, policy);
-                else if (cpufreq_driver->suspend
-                    && cpufreq_driver->suspend(policy))
+                        if (ret) {
+                                pr_err("%s: Failed to stop governor for policy: %p\n",
+                                        __func__, policy);
+                                continue;
+                        }
+                }
+
+                if (cpufreq_driver->suspend && cpufreq_driver->suspend(policy))
                         pr_err("%s: Failed to suspend driver: %p\n", __func__,
                                 policy);
         }
@@ -1596,7 +1600,7 @@ void cpufreq_resume(void)
 
         cpufreq_suspended = false;
 
-        if (!has_target())
+        if (!has_target() && !cpufreq_driver->resume)
                 return;
 
         pr_debug("%s: Resuming Governors\n", __func__);
@@ -1605,7 +1609,7 @@ void cpufreq_resume(void)
                 if (cpufreq_driver->resume && cpufreq_driver->resume(policy)) {
                         pr_err("%s: Failed to resume driver: %p\n", __func__,
                                 policy);
-                } else {
+                } else if (has_target()) {
                         down_write(&policy->rwsem);
                         ret = cpufreq_start_governor(policy);
                         up_write(&policy->rwsem);
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index f502d5b90c25..66f7f0071cab 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -453,6 +453,14 @@ static void intel_pstate_hwp_set(const struct cpumask *cpumask)
         }
 }
 
+static int intel_pstate_hwp_set_policy(struct cpufreq_policy *policy)
+{
+        if (hwp_active)
+                intel_pstate_hwp_set(policy->cpus);
+
+        return 0;
+}
+
 static void intel_pstate_hwp_set_online_cpus(void)
 {
         get_online_cpus();
@@ -1346,8 +1354,7 @@ static int intel_pstate_set_policy(struct cpufreq_policy *policy)
  out:
         intel_pstate_set_update_util_hook(policy->cpu);
 
-        if (hwp_active)
-                intel_pstate_hwp_set(policy->cpus);
+        intel_pstate_hwp_set_policy(policy);
 
         return 0;
 }
@@ -1411,6 +1418,7 @@ static struct cpufreq_driver intel_pstate_driver = {
         .flags          = CPUFREQ_CONST_LOOPS,
         .verify         = intel_pstate_verify_policy,
         .setpolicy      = intel_pstate_set_policy,
+        .resume         = intel_pstate_hwp_set_policy,
         .get            = intel_pstate_get,
         .init           = intel_pstate_cpu_init,
         .stop_cpu       = intel_pstate_stop_cpu,