1 files changed, 23 insertions, 11 deletions

diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 43c24aa756f6..04548f7023af 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -952,9 +952,20 @@ static void update_policy_cpu(struct cpufreq_policy *policy, unsigned int cpu)
         if (cpu == policy->cpu)
                 return;
 
+        /*
+         * Take direct locks as lock_policy_rwsem_write wouldn't work here.
+         * Also lock for last cpu is enough here as contention will happen only
+         * after policy->cpu is changed and after it is changed, other threads
+         * will try to acquire lock for new cpu. And policy is already updated
+         * by then.
+         */
+        down_write(&per_cpu(cpu_policy_rwsem, policy->cpu));
+
         policy->last_cpu = policy->cpu;
         policy->cpu = cpu;
 
+        up_write(&per_cpu(cpu_policy_rwsem, policy->last_cpu));
+
 #ifdef CONFIG_CPU_FREQ_TABLE
         cpufreq_frequency_table_update_policy_cpu(policy);
 #endif
@@ -1125,7 +1136,7 @@ static int cpufreq_nominate_new_policy_cpu(struct cpufreq_policy *policy,
         int ret;
 
         /* first sibling now owns the new sysfs dir */
-        cpu_dev = get_cpu_device(cpumask_first(policy->cpus));
+        cpu_dev = get_cpu_device(cpumask_any_but(policy->cpus, old_cpu));
 
         /* Don't touch sysfs files during light-weight tear-down */
         if (frozen)
@@ -1189,12 +1200,9 @@ static int __cpufreq_remove_dev_prepare(struct device *dev,
                         policy->governor->name, CPUFREQ_NAME_LEN);
 #endif
 
-        WARN_ON(lock_policy_rwsem_write(cpu));
+        lock_policy_rwsem_read(cpu);
         cpus = cpumask_weight(policy->cpus);
-
-        if (cpus > 1)
-                cpumask_clear_cpu(cpu, policy->cpus);
-        unlock_policy_rwsem_write(cpu);
+        unlock_policy_rwsem_read(cpu);
 
         if (cpu != policy->cpu) {
                 if (!frozen)
@@ -1203,9 +1211,7 @@ static int __cpufreq_remove_dev_prepare(struct device *dev,
 
                 new_cpu = cpufreq_nominate_new_policy_cpu(policy, cpu, frozen);
                 if (new_cpu >= 0) {
-                        WARN_ON(lock_policy_rwsem_write(cpu));
                         update_policy_cpu(policy, new_cpu);
-                        unlock_policy_rwsem_write(cpu);
 
                         if (!frozen) {
                                 pr_debug("%s: policy Kobject moved to cpu: %d "
@@ -1237,9 +1243,12 @@ static int __cpufreq_remove_dev_finish(struct device *dev,
                 return -EINVAL;
         }
 
-        lock_policy_rwsem_read(cpu);
+        WARN_ON(lock_policy_rwsem_write(cpu));
         cpus = cpumask_weight(policy->cpus);
-        unlock_policy_rwsem_read(cpu);
+
+        if (cpus > 1)
+                cpumask_clear_cpu(cpu, policy->cpus);
+        unlock_policy_rwsem_write(cpu);
 
         /* If cpu is last user of policy, free policy */
         if (cpus == 1) {
@@ -1451,6 +1460,9 @@ unsigned int cpufreq_get(unsigned int cpu)
 {
         unsigned int ret_freq = 0;
 
+        if (cpufreq_disabled() || !cpufreq_driver)
+                return -ENOENT;
+
         if (!down_read_trylock(&cpufreq_rwsem))
                 return 0;
 
@@ -2095,7 +2107,7 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
         write_lock_irqsave(&cpufreq_driver_lock, flags);
         if (cpufreq_driver) {
                 write_unlock_irqrestore(&cpufreq_driver_lock, flags);
-                return -EBUSY;
+                return -EEXIST;
         }
         cpufreq_driver = driver_data;
         write_unlock_irqrestore(&cpufreq_driver_lock, flags);