1 files changed, 35 insertions, 43 deletions

diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index aed2b0cb83dc..d9fdeddcef96 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -1076,10 +1076,20 @@ static void cpufreq_policy_free(struct cpufreq_policy *policy)
         kfree(policy);
 }
 
-static void update_policy_cpu(struct cpufreq_policy *policy, unsigned int cpu)
+static int update_policy_cpu(struct cpufreq_policy *policy, unsigned int cpu,
+                             struct device *cpu_dev)
 {
+        int ret;
+
         if (WARN_ON(cpu == policy->cpu))
-                return;
+                return 0;
+
+        /* Move kobject to the new policy->cpu */
+        ret = kobject_move(&policy->kobj, &cpu_dev->kobj);
+        if (ret) {
+                pr_err("%s: Failed to move kobj: %d\n", __func__, ret);
+                return ret;
+        }
 
         down_write(&policy->rwsem);
 
@@ -1090,6 +1100,8 @@ static void update_policy_cpu(struct cpufreq_policy *policy, unsigned int cpu)
 
         blocking_notifier_call_chain(&cpufreq_policy_notifier_list,
                         CPUFREQ_UPDATE_POLICY_CPU, policy);
+
+        return 0;
 }
 
 static int __cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
@@ -1154,7 +1166,7 @@ static int __cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
          * by invoking update_policy_cpu().
          */
         if (recover_policy && cpu != policy->cpu)
-                update_policy_cpu(policy, cpu);
+                WARN_ON(update_policy_cpu(policy, cpu, dev));
         else
                 policy->cpu = cpu;
 
@@ -1307,38 +1319,11 @@ static int cpufreq_add_dev(struct device *dev, struct subsys_interface *sif)
         return __cpufreq_add_dev(dev, sif);
 }
 
-static int cpufreq_nominate_new_policy_cpu(struct cpufreq_policy *policy,
-                                           unsigned int old_cpu)
-{
-        struct device *cpu_dev;
-        int ret;
-
-        /* first sibling now owns the new sysfs dir */
-        cpu_dev = get_cpu_device(cpumask_any_but(policy->cpus, old_cpu));
-
-        sysfs_remove_link(&cpu_dev->kobj, "cpufreq");
-        ret = kobject_move(&policy->kobj, &cpu_dev->kobj);
-        if (ret) {
-                pr_err("%s: Failed to move kobj: %d\n", __func__, ret);
-
-                down_write(&policy->rwsem);
-                cpumask_set_cpu(old_cpu, policy->cpus);
-                up_write(&policy->rwsem);
-
-                ret = sysfs_create_link(&cpu_dev->kobj, &policy->kobj,
-                                        "cpufreq");
-
-                return -EINVAL;
-        }
-
-        return cpu_dev->id;
-}
-
 static int __cpufreq_remove_dev_prepare(struct device *dev,
                                         struct subsys_interface *sif)
 {
         unsigned int cpu = dev->id, cpus;
-        int new_cpu, ret;
+        int ret;
         unsigned long flags;
         struct cpufreq_policy *policy;
 
@@ -1378,14 +1363,23 @@ static int __cpufreq_remove_dev_prepare(struct device *dev,
         if (cpu != policy->cpu) {
                 sysfs_remove_link(&dev->kobj, "cpufreq");
         } else if (cpus > 1) {
-                new_cpu = cpufreq_nominate_new_policy_cpu(policy, cpu);
-                if (new_cpu >= 0) {
-                        update_policy_cpu(policy, new_cpu);
+                /* Nominate new CPU */
+                int new_cpu = cpumask_any_but(policy->cpus, cpu);
+                struct device *cpu_dev = get_cpu_device(new_cpu);
 
-                        if (!cpufreq_suspended)
-                                pr_debug("%s: policy Kobject moved to cpu: %d from: %d\n",
-                                         __func__, new_cpu, cpu);
+                sysfs_remove_link(&cpu_dev->kobj, "cpufreq");
+                ret = update_policy_cpu(policy, new_cpu, cpu_dev);
+                if (ret) {
+                        if (sysfs_create_link(&cpu_dev->kobj, &policy->kobj,
+                                              "cpufreq"))
+                                pr_err("%s: Failed to restore kobj link to cpu:%d\n",
+                                       __func__, cpu_dev->id);
+                        return ret;
                 }
+
+                if (!cpufreq_suspended)
+                        pr_debug("%s: policy Kobject moved to cpu: %d from: %d\n",
+                                 __func__, new_cpu, cpu);
         } else if (cpufreq_driver->stop_cpu && cpufreq_driver->setpolicy) {
                 cpufreq_driver->stop_cpu(policy);
         }
@@ -2242,10 +2236,8 @@ int cpufreq_update_policy(unsigned int cpu)
         struct cpufreq_policy new_policy;
         int ret;
 
-        if (!policy) {
-                ret = -ENODEV;
-                goto no_policy;
-        }
+        if (!policy)
+                return -ENODEV;
 
         down_write(&policy->rwsem);
 
@@ -2264,7 +2256,7 @@ int cpufreq_update_policy(unsigned int cpu)
                 new_policy.cur = cpufreq_driver->get(cpu);
                 if (WARN_ON(!new_policy.cur)) {
                         ret = -EIO;
-                        goto no_policy;
+                        goto unlock;
                 }
 
                 if (!policy->cur) {
@@ -2279,10 +2271,10 @@ int cpufreq_update_policy(unsigned int cpu)
 
         ret = cpufreq_set_policy(policy, &new_policy);
 
+unlock:
         up_write(&policy->rwsem);
 
         cpufreq_cpu_put(policy);
-no_policy:
         return ret;
 }
 EXPORT_SYMBOL(cpufreq_update_policy);