X-Git-Url: http://git.onelab.eu/?a=blobdiff_plain;f=drivers%2Fcpufreq%2Fcpufreq.c;h=a45cc89e387a3ee14ba2afcda2cb09ac5303b1f0;hb=97bf2856c6014879bd04983a3e9dfcdac1e7fe85;hp=b30001f3161056b4c1b85a4ae00bcb03f23b0828;hpb=6a77f38946aaee1cd85eeec6cf4229b204c15071;p=linux-2.6.git

diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index b30001f31..a45cc89e3 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -4,13 +4,17 @@
  *  Copyright (C) 2001 Russell King
  *            (C) 2002 - 2003 Dominik Brodowski <linux@brodo.de>
  *
+ *  Oct 2005 - Ashok Raj <ashok.raj@intel.com>
+ *	Added handling for CPU hotplug
+ *  Feb 2006 - Jacob Shin <jacob.shin@amd.com>
+ *	Fix handling for CPU hotplug -- affected CPUs
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
  */
 
-#include <linux/config.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
@@ -23,47 +27,45 @@
 #include <linux/slab.h>
 #include <linux/cpu.h>
 #include <linux/completion.h>
+#include <linux/mutex.h>
 
-#define dprintk(msg...) cpufreq_debug_printk(CPUFREQ_DEBUG_CORE, "cpufreq-core", msg)
+#define dprintk(msg...) cpufreq_debug_printk(CPUFREQ_DEBUG_CORE, \
+						"cpufreq-core", msg)
 
 /**
- * The "cpufreq driver" - the arch- or hardware-dependend low
+ * The "cpufreq driver" - the arch- or hardware-dependent low
  * level driver of CPUFreq support, and its spinlock. This lock
  * also protects the cpufreq_cpu_data array.
  */
-static struct cpufreq_driver   	*cpufreq_driver;
-static struct cpufreq_policy	*cpufreq_cpu_data[NR_CPUS];
+static struct cpufreq_driver *cpufreq_driver;
+static struct cpufreq_policy *cpufreq_cpu_data[NR_CPUS];
 static DEFINE_SPINLOCK(cpufreq_driver_lock);
 
-
-/* we keep a copy of all ->add'ed CPU's struct sys_device here;
- * as it is only accessed in ->add and ->remove, no lock or reference
- * count is necessary.
- */
-static struct sys_device	*cpu_sys_devices[NR_CPUS];
-
-
 /* internal prototypes */
 static int __cpufreq_governor(struct cpufreq_policy *policy, unsigned int event);
-static void handle_update(void *data);
-static inline void adjust_jiffies(unsigned long val, struct cpufreq_freqs *ci);
+static void handle_update(struct work_struct *work);
 
 /**
- * Two notifier lists: the "policy" list is involved in the 
- * validation process for a new CPU frequency policy; the 
+ * Two notifier lists: the "policy" list is involved in the
+ * validation process for a new CPU frequency policy; the
  * "transition" list for kernel code that needs to handle
  * changes to devices when the CPU clock speed changes.
  * The mutex locks both lists.
  */
-static struct notifier_block    *cpufreq_policy_notifier_list;
-static struct notifier_block    *cpufreq_transition_notifier_list;
-static DECLARE_RWSEM		(cpufreq_notifier_rwsem);
+static BLOCKING_NOTIFIER_HEAD(cpufreq_policy_notifier_list);
+static struct srcu_notifier_head cpufreq_transition_notifier_list;
 
+static int __init init_cpufreq_transition_notifier_list(void)
+{
+	srcu_init_notifier_head(&cpufreq_transition_notifier_list);
+	return 0;
+}
+pure_initcall(init_cpufreq_transition_notifier_list);
 
 static LIST_HEAD(cpufreq_governor_list);
-static DECLARE_MUTEX		(cpufreq_governor_sem);
+static DEFINE_MUTEX (cpufreq_governor_mutex);
 
-struct cpufreq_policy * cpufreq_cpu_get(unsigned int cpu)
+struct cpufreq_policy *cpufreq_cpu_get(unsigned int cpu)
 {
 	struct cpufreq_policy *data;
 	unsigned long flags;
@@ -90,20 +92,19 @@ struct cpufreq_policy * cpufreq_cpu_get(unsigned int cpu)
 	if (!kobject_get(&data->kobj))
 		goto err_out_put_module;
 
-
 	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
-
 	return data;
 
- err_out_put_module:
+err_out_put_module:
 	module_put(cpufreq_driver->owner);
- err_out_unlock:
+err_out_unlock:
 	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
- err_out:
+err_out:
 	return NULL;
 }
 EXPORT_SYMBOL_GPL(cpufreq_cpu_get);
 
+
 void cpufreq_cpu_put(struct cpufreq_policy *data)
 {
 	kobject_put(&data->kobj);
@@ -132,7 +133,7 @@ static unsigned int debug_ratelimit = 1;
 static unsigned int disable_ratelimit = 1;
 static DEFINE_SPINLOCK(disable_ratelimit_lock);
 
-static inline void cpufreq_debug_enable_ratelimit(void)
+static void cpufreq_debug_enable_ratelimit(void)
 {
 	unsigned long flags;
 
@@ -142,7 +143,7 @@ static inline void cpufreq_debug_enable_ratelimit(void)
 	spin_unlock_irqrestore(&disable_ratelimit_lock, flags);
 }
 
-static inline void cpufreq_debug_disable_ratelimit(void)
+static void cpufreq_debug_disable_ratelimit(void)
 {
 	unsigned long flags;
 
@@ -151,17 +152,19 @@ static inline void cpufreq_debug_disable_ratelimit(void)
 	spin_unlock_irqrestore(&disable_ratelimit_lock, flags);
 }
 
-void cpufreq_debug_printk(unsigned int type, const char *prefix, const char *fmt, ...)
+void cpufreq_debug_printk(unsigned int type, const char *prefix,
+							const char *fmt, ...)
 {
 	char s[256];
 	va_list args;
 	unsigned int len;
 	unsigned long flags;
-	
+
 	WARN_ON(!prefix);
 	if (type & debug) {
 		spin_lock_irqsave(&disable_ratelimit_lock, flags);
-		if (!disable_ratelimit && debug_ratelimit && !printk_ratelimit()) {
+		if (!disable_ratelimit && debug_ratelimit
+					&& !printk_ratelimit()) {
 			spin_unlock_irqrestore(&disable_ratelimit_lock, flags);
 			return;
 		}
@@ -182,10 +185,12 @@ EXPORT_SYMBOL(cpufreq_debug_printk);
 
 
 module_param(debug, uint, 0644);
-MODULE_PARM_DESC(debug, "CPUfreq debugging: add 1 to debug core, 2 to debug drivers, and 4 to debug governors.");
+MODULE_PARM_DESC(debug, "CPUfreq debugging: add 1 to debug core,"
+			" 2 to debug drivers, and 4 to debug governors.");
 
 module_param(debug_ratelimit, uint, 0644);
-MODULE_PARM_DESC(debug_ratelimit, "CPUfreq debugging: set to 0 to disable ratelimiting.");
+MODULE_PARM_DESC(debug_ratelimit, "CPUfreq debugging:"
+					" set to 0 to disable ratelimiting.");
 
 #else /* !CONFIG_CPU_FREQ_DEBUG */
 
@@ -204,14 +209,14 @@ static inline void cpufreq_debug_disable_ratelimit(void) { return; }
  *
  * This function alters the system "loops_per_jiffy" for the clock
  * speed change. Note that loops_per_jiffy cannot be updated on SMP
- * systems as each CPU might be scaled differently. So, use the arch 
+ * systems as each CPU might be scaled differently. So, use the arch
  * per-CPU loops_per_jiffy value wherever possible.
  */
 #ifndef CONFIG_SMP
 static unsigned long l_p_j_ref;
 static unsigned int  l_p_j_ref_freq;
 
-static inline void adjust_jiffies(unsigned long val, struct cpufreq_freqs *ci)
+static void adjust_jiffies(unsigned long val, struct cpufreq_freqs *ci)
 {
 	if (ci->flags & CPUFREQ_CONST_LOOPS)
 		return;
@@ -219,62 +224,74 @@ static inline void adjust_jiffies(unsigned long val, struct cpufreq_freqs *ci)
 	if (!l_p_j_ref_freq) {
 		l_p_j_ref = loops_per_jiffy;
 		l_p_j_ref_freq = ci->old;
-		dprintk("saving %lu as reference value for loops_per_jiffy; freq is %u kHz\n", l_p_j_ref, l_p_j_ref_freq);
+		dprintk("saving %lu as reference value for loops_per_jiffy;"
+			"freq is %u kHz\n", l_p_j_ref, l_p_j_ref_freq);
 	}
 	if ((val == CPUFREQ_PRECHANGE  && ci->old < ci->new) ||
 	    (val == CPUFREQ_POSTCHANGE && ci->old > ci->new) ||
-	    (val == CPUFREQ_RESUMECHANGE)) {
-		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);
+	    (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);
 	}
 }
 #else
-static inline void adjust_jiffies(unsigned long val, struct cpufreq_freqs *ci) { return; }
+static inline void adjust_jiffies(unsigned long val, struct cpufreq_freqs *ci)
+{
+	return;
+}
 #endif
 
 
 /**
- * cpufreq_notify_transition - call notifier chain and adjust_jiffies on frequency transition
+ * cpufreq_notify_transition - call notifier chain and adjust_jiffies
+ * on frequency transition.
  *
- * This function calls the transition notifiers and the "adjust_jiffies" function. It is called
- * twice on all CPU frequency changes that have external effects. 
+ * This function calls the transition notifiers and the "adjust_jiffies"
+ * function. It is called twice on all CPU frequency changes that have
+ * external effects.
  */
 void cpufreq_notify_transition(struct cpufreq_freqs *freqs, unsigned int state)
 {
+	struct cpufreq_policy *policy;
+
 	BUG_ON(irqs_disabled());
 
 	freqs->flags = cpufreq_driver->flags;
-	dprintk("notification %u of frequency transition to %u kHz\n", state, freqs->new);
+	dprintk("notification %u of frequency transition to %u kHz\n",
+		state, freqs->new);
 
-	down_read(&cpufreq_notifier_rwsem);
+	policy = cpufreq_cpu_data[freqs->cpu];
 	switch (state) {
+
 	case CPUFREQ_PRECHANGE:
-		/* detect if the driver reported a value as "old frequency" which
-		 * is not equal to what the cpufreq core thinks is "old frequency".
+		/* detect if the driver reported a value as "old frequency"
+		 * which is not equal to what the cpufreq core thinks is
+		 * "old frequency".
 		 */
 		if (!(cpufreq_driver->flags & CPUFREQ_CONST_LOOPS)) {
-			if ((likely(cpufreq_cpu_data[freqs->cpu])) &&
-			    (likely(cpufreq_cpu_data[freqs->cpu]->cpu == freqs->cpu)) &&
-			    (likely(cpufreq_cpu_data[freqs->cpu]->cur)) &&
-			    (unlikely(freqs->old != cpufreq_cpu_data[freqs->cpu]->cur)))
-			{
-				printk(KERN_WARNING "Warning: CPU frequency is %u, "
-				       "cpufreq assumed %u kHz.\n", freqs->old, cpufreq_cpu_data[freqs->cpu]->cur);
-				freqs->old = cpufreq_cpu_data[freqs->cpu]->cur;
+			if ((policy) && (policy->cpu == freqs->cpu) &&
+			    (policy->cur) && (policy->cur != freqs->old)) {
+				dprintk("Warning: CPU frequency is"
+					" %u, cpufreq assumed %u kHz.\n",
+					freqs->old, policy->cur);
+				freqs->old = policy->cur;
 			}
 		}
-		notifier_call_chain(&cpufreq_transition_notifier_list, CPUFREQ_PRECHANGE, freqs);
+		srcu_notifier_call_chain(&cpufreq_transition_notifier_list,
+				CPUFREQ_PRECHANGE, freqs);
 		adjust_jiffies(CPUFREQ_PRECHANGE, freqs);
 		break;
+
 	case CPUFREQ_POSTCHANGE:
 		adjust_jiffies(CPUFREQ_POSTCHANGE, freqs);
-		notifier_call_chain(&cpufreq_transition_notifier_list, CPUFREQ_POSTCHANGE, freqs);
-		if ((likely(cpufreq_cpu_data[freqs->cpu])) && 
-		    (likely(cpufreq_cpu_data[freqs->cpu]->cpu == freqs->cpu)))
-			cpufreq_cpu_data[freqs->cpu]->cur = freqs->new;
+		srcu_notifier_call_chain(&cpufreq_transition_notifier_list,
+				CPUFREQ_POSTCHANGE, freqs);
+		if (likely(policy) && likely(policy->cpu == freqs->cpu))
+			policy->cur = freqs->new;
 		break;
 	}
-	up_read(&cpufreq_notifier_rwsem);
 }
 EXPORT_SYMBOL_GPL(cpufreq_notify_transition);
 
@@ -284,41 +301,72 @@ EXPORT_SYMBOL_GPL(cpufreq_notify_transition);
  *                          SYSFS INTERFACE                          *
  *********************************************************************/
 
+static struct cpufreq_governor *__find_governor(const char *str_governor)
+{
+	struct cpufreq_governor *t;
+
+	list_for_each_entry(t, &cpufreq_governor_list, governor_list)
+		if (!strnicmp(str_governor,t->name,CPUFREQ_NAME_LEN))
+			return t;
+
+	return NULL;
+}
+
 /**
  * cpufreq_parse_governor - parse a governor string
  */
 static int cpufreq_parse_governor (char *str_governor, unsigned int *policy,
 				struct cpufreq_governor **governor)
 {
+	int err = -EINVAL;
+
 	if (!cpufreq_driver)
-		return -EINVAL;
+		goto out;
+
 	if (cpufreq_driver->setpolicy) {
 		if (!strnicmp(str_governor, "performance", CPUFREQ_NAME_LEN)) {
 			*policy = CPUFREQ_POLICY_PERFORMANCE;
-			return 0;
-		} else if (!strnicmp(str_governor, "powersave", CPUFREQ_NAME_LEN)) {
+			err = 0;
+		} else if (!strnicmp(str_governor, "powersave",
+						CPUFREQ_NAME_LEN)) {
 			*policy = CPUFREQ_POLICY_POWERSAVE;
-			return 0;
+			err = 0;
 		}
-		return -EINVAL;
-	} else {
+	} else if (cpufreq_driver->target) {
 		struct cpufreq_governor *t;
-		down(&cpufreq_governor_sem);
-		if (!cpufreq_driver || !cpufreq_driver->target)
-			goto out;
-		list_for_each_entry(t, &cpufreq_governor_list, governor_list) {
-			if (!strnicmp(str_governor,t->name,CPUFREQ_NAME_LEN)) {
-				*governor = t;
-				up(&cpufreq_governor_sem);
-				return 0;
+
+		mutex_lock(&cpufreq_governor_mutex);
+
+		t = __find_governor(str_governor);
+
+		if (t == NULL) {
+			char *name = kasprintf(GFP_KERNEL, "cpufreq_%s",
+								str_governor);
+
+			if (name) {
+				int ret;
+
+				mutex_unlock(&cpufreq_governor_mutex);
+				ret = request_module(name);
+				mutex_lock(&cpufreq_governor_mutex);
+
+				if (ret == 0)
+					t = __find_governor(str_governor);
 			}
+
+			kfree(name);
 		}
-	out:
-		up(&cpufreq_governor_sem);
+
+		if (t != NULL) {
+			*governor = t;
+			err = 0;
+		}
+
+		mutex_unlock(&cpufreq_governor_mutex);
 	}
-	return -EINVAL;
+  out:
+	return err;
 }
-EXPORT_SYMBOL_GPL(cpufreq_parse_governor);
 
 
 /* drivers/base/cpu.c */
@@ -326,17 +374,18 @@ extern struct sysdev_class cpu_sysdev_class;
 
 
 /**
- * cpufreq_per_cpu_attr_read() / show_##file_name() - print out cpufreq information
+ * cpufreq_per_cpu_attr_read() / show_##file_name() -
+ * print out cpufreq information
  *
  * Write out information from cpufreq_driver->policy[cpu]; object must be
  * "unsigned int".
  */
 
-#define show_one(file_name, object)		 			\
-static ssize_t show_##file_name 					\
-(struct cpufreq_policy * policy, char *buf)				\
-{									\
-	return sprintf (buf, "%u\n", policy->object);			\
+#define show_one(file_name, object)			\
+static ssize_t show_##file_name				\
+(struct cpufreq_policy * policy, char *buf)		\
+{							\
+	return sprintf (buf, "%u\n", policy->object);	\
 }
 
 show_one(cpuinfo_min_freq, cpuinfo.min_freq);
@@ -345,6 +394,9 @@ show_one(scaling_min_freq, min);
 show_one(scaling_max_freq, max);
 show_one(scaling_cur_freq, cur);
 
+static int __cpufreq_set_policy(struct cpufreq_policy *data,
+				struct cpufreq_policy *policy);
+
 /**
  * cpufreq_per_cpu_attr_write() / store_##file_name() - sysfs write access
  */
@@ -363,7 +415,12 @@ static ssize_t store_##file_name					\
 	if (ret != 1)							\
 		return -EINVAL;						\
 									\
-	ret = cpufreq_set_policy(&new_policy);				\
+	lock_cpu_hotplug();						\
+	mutex_lock(&policy->lock);					\
+	ret = __cpufreq_set_policy(policy, &new_policy);		\
+	policy->user_policy.object = policy->object;			\
+	mutex_unlock(&policy->lock);					\
+	unlock_cpu_hotplug();						\
 									\
 	return ret ? ret : count;					\
 }
@@ -374,7 +431,8 @@ store_one(scaling_max_freq,max);
 /**
  * show_cpuinfo_cur_freq - current CPU frequency as detected by hardware
  */
-static ssize_t show_cpuinfo_cur_freq (struct cpufreq_policy * policy, char *buf)
+static ssize_t show_cpuinfo_cur_freq (struct cpufreq_policy * policy,
+							char *buf)
 {
 	unsigned int cur_freq = cpufreq_get(policy->cpu);
 	if (!cur_freq)
@@ -386,7 +444,8 @@ static ssize_t show_cpuinfo_cur_freq (struct cpufreq_policy * policy, char *buf)
 /**
  * show_scaling_governor - show the current policy for the specified CPU
  */
-static ssize_t show_scaling_governor (struct cpufreq_policy * policy, char *buf)
+static ssize_t show_scaling_governor (struct cpufreq_policy * policy,
+							char *buf)
 {
 	if(policy->policy == CPUFREQ_POLICY_POWERSAVE)
 		return sprintf(buf, "powersave\n");
@@ -401,8 +460,8 @@ static ssize_t show_scaling_governor (struct cpufreq_policy * policy, char *buf)
 /**
  * store_scaling_governor - store policy for the specified CPU
  */
-static ssize_t store_scaling_governor (struct cpufreq_policy * policy, 
-				       const char *buf, size_t count) 
+static ssize_t store_scaling_governor (struct cpufreq_policy * policy,
+				       const char *buf, size_t count)
 {
 	unsigned int ret = -EINVAL;
 	char	str_governor[16];
@@ -416,12 +475,27 @@ static ssize_t store_scaling_governor (struct cpufreq_policy * policy,
 	if (ret != 1)
 		return -EINVAL;
 
-	if (cpufreq_parse_governor(str_governor, &new_policy.policy, &new_policy.governor))
+	if (cpufreq_parse_governor(str_governor, &new_policy.policy,
+						&new_policy.governor))
 		return -EINVAL;
 
-	ret = cpufreq_set_policy(&new_policy);
+	lock_cpu_hotplug();
 
-	return ret ? ret : count;
+	/* Do not use cpufreq_set_policy here or the user_policy.max
+	   will be wrongly overridden */
+	mutex_lock(&policy->lock);
+	ret = __cpufreq_set_policy(policy, &new_policy);
+
+	policy->user_policy.policy = policy->policy;
+	policy->user_policy.governor = policy->governor;
+	mutex_unlock(&policy->lock);
+
+	unlock_cpu_hotplug();
+
+	if (ret)
+		return ret;
+	else
+		return count;
 }
 
 /**
@@ -435,7 +509,7 @@ static ssize_t show_scaling_driver (struct cpufreq_policy * policy, char *buf)
 /**
  * show_scaling_available_governors - show the available CPUfreq governors
  */
-static ssize_t show_scaling_available_governors (struct cpufreq_policy * policy,
+static ssize_t show_scaling_available_governors (struct cpufreq_policy *policy,
 				char *buf)
 {
 	ssize_t i = 0;
@@ -451,7 +525,7 @@ static ssize_t show_scaling_available_governors (struct cpufreq_policy * policy,
 			goto out;
 		i += scnprintf(&buf[i], CPUFREQ_NAME_LEN, "%s ", t->name);
 	}
- out:
+out:
 	i += sprintf(&buf[i], "\n");
 	return i;
 }
@@ -521,12 +595,16 @@ static ssize_t show(struct kobject * kobj, struct attribute * attr ,char * buf)
 	policy = cpufreq_cpu_get(policy->cpu);
 	if (!policy)
 		return -EINVAL;
-	ret = fattr->show ? fattr->show(policy,buf) : 0;
+	if (fattr->show)
+		ret = fattr->show(policy, buf);
+	else
+		ret = -EIO;
+
 	cpufreq_cpu_put(policy);
 	return ret;
 }
 
-static ssize_t store(struct kobject * kobj, struct attribute * attr, 
+static ssize_t store(struct kobject * kobj, struct attribute * attr,
 		     const char * buf, size_t count)
 {
 	struct cpufreq_policy * policy = to_policy(kobj);
@@ -535,7 +613,11 @@ static ssize_t store(struct kobject * kobj, struct attribute * attr,
 	policy = cpufreq_cpu_get(policy->cpu);
 	if (!policy)
 		return -EINVAL;
-	ret = fattr->store ? fattr->store(policy,buf,count) : 0;
+	if (fattr->store)
+		ret = fattr->store(policy, buf, count);
+	else
+		ret = -EIO;
+
 	cpufreq_cpu_put(policy);
 	return ret;
 }
@@ -562,7 +644,7 @@ static struct kobj_type ktype_cpufreq = {
 /**
  * cpufreq_add_dev - add a CPU device
  *
- * Adds the cpufreq interface for a CPU device. 
+ * Adds the cpufreq interface for a CPU device.
  */
 static int cpufreq_add_dev (struct sys_device * sys_dev)
 {
@@ -571,8 +653,15 @@ static int cpufreq_add_dev (struct sys_device * sys_dev)
 	struct cpufreq_policy new_policy;
 	struct cpufreq_policy *policy;
 	struct freq_attr **drv_attr;
+	struct sys_device *cpu_sys_dev;
 	unsigned long flags;
 	unsigned int j;
+#ifdef CONFIG_SMP
+	struct cpufreq_policy *managed_policy;
+#endif
+
+	if (cpu_is_offline(cpu))
+		return 0;
 
 	cpufreq_debug_disable_ratelimit();
 	dprintk("adding CPU %u\n", cpu);
@@ -582,9 +671,7 @@ static int cpufreq_add_dev (struct sys_device * sys_dev)
 	 * CPU because it is in the same boat. */
 	policy = cpufreq_cpu_get(cpu);
 	if (unlikely(policy)) {
-		cpu_sys_devices[cpu] = sys_dev;
-		dprintk("CPU already managed, adding link\n");
-		sysfs_create_link(&sys_dev->kobj, &policy->kobj, "cpufreq");
+		cpufreq_cpu_put(policy);
 		cpufreq_debug_enable_ratelimit();
 		return 0;
 	}
@@ -595,19 +682,19 @@ static int cpufreq_add_dev (struct sys_device * sys_dev)
 		goto module_out;
 	}
 
-	policy = kmalloc(sizeof(struct cpufreq_policy), GFP_KERNEL);
+	policy = kzalloc(sizeof(struct cpufreq_policy), GFP_KERNEL);
 	if (!policy) {
 		ret = -ENOMEM;
 		goto nomem_out;
 	}
-	memset(policy, 0, sizeof(struct cpufreq_policy));
 
 	policy->cpu = cpu;
 	policy->cpus = cpumask_of_cpu(cpu);
 
-	init_MUTEX_LOCKED(&policy->lock);
+	mutex_init(&policy->lock);
+	mutex_lock(&policy->lock);
 	init_completion(&policy->kobj_unregister);
-	INIT_WORK(&policy->update, handle_update, (void *)(long)cpu);
+	INIT_WORK(&policy->update, handle_update);
 
 	/* call driver. From then on the cpufreq must be able
 	 * to accept all calls to ->verify and ->setpolicy for this CPU
@@ -615,9 +702,41 @@ static int cpufreq_add_dev (struct sys_device * sys_dev)
 	ret = cpufreq_driver->init(policy);
 	if (ret) {
 		dprintk("initialization failed\n");
+		mutex_unlock(&policy->lock);
 		goto err_out;
 	}
 
+#ifdef CONFIG_SMP
+	for_each_cpu_mask(j, policy->cpus) {
+		if (cpu == j)
+			continue;
+
+		/* check for existing affected CPUs.  They may not be aware
+		 * of it due to CPU Hotplug.
+		 */
+		managed_policy = cpufreq_cpu_get(j);
+		if (unlikely(managed_policy)) {
+			spin_lock_irqsave(&cpufreq_driver_lock, flags);
+			managed_policy->cpus = policy->cpus;
+			cpufreq_cpu_data[cpu] = managed_policy;
+			spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+
+			dprintk("CPU already managed, adding link\n");
+			ret = sysfs_create_link(&sys_dev->kobj,
+						&managed_policy->kobj,
+						"cpufreq");
+			if (ret) {
+				mutex_unlock(&policy->lock);
+				goto err_out_driver_exit;
+			}
+
+			cpufreq_debug_enable_ratelimit();
+			mutex_unlock(&policy->lock);
+			ret = 0;
+			goto err_out_driver_exit; /* call driver->exit() */
+		}
+	}
+#endif
 	memcpy(&new_policy, policy, sizeof(struct cpufreq_policy));
 
 	/* prepare interface data */
@@ -626,9 +745,10 @@ static int cpufreq_add_dev (struct sys_device * sys_dev)
 	strlcpy(policy->kobj.name, "cpufreq", KOBJ_NAME_LEN);
 
 	ret = kobject_register(&policy->kobj);
-	if (ret)
-		goto err_out;
-
+	if (ret) {
+		mutex_unlock(&policy->lock);
+		goto err_out_driver_exit;
+	}
 	/* set up files for this cpu device */
 	drv_attr = cpufreq_driver->attr;
 	while ((drv_attr) && (*drv_attr)) {
@@ -644,12 +764,30 @@ static int cpufreq_add_dev (struct sys_device * sys_dev)
 	for_each_cpu_mask(j, policy->cpus)
 		cpufreq_cpu_data[j] = policy;
 	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
+
+	/* symlink affected CPUs */
+	for_each_cpu_mask(j, policy->cpus) {
+		if (j == cpu)
+			continue;
+		if (!cpu_online(j))
+			continue;
+
+		dprintk("CPU %u already managed, adding link\n", j);
+		cpufreq_cpu_get(cpu);
+		cpu_sys_dev = get_cpu_sysdev(j);
+		ret = sysfs_create_link(&cpu_sys_dev->kobj, &policy->kobj,
+					"cpufreq");
+		if (ret) {
+			mutex_unlock(&policy->lock);
+			goto err_out_unregister;
+		}
+	}
+
 	policy->governor = NULL; /* to assure that the starting sequence is
 				  * run in cpufreq_set_policy */
-	up(&policy->lock);
-	
+	mutex_unlock(&policy->lock);
+
 	/* set default policy */
-	
 	ret = cpufreq_set_policy(&new_policy);
 	if (ret) {
 		dprintk("setting policy failed\n");
@@ -657,10 +795,9 @@ static int cpufreq_add_dev (struct sys_device * sys_dev)
 	}
 
 	module_put(cpufreq_driver->owner);
-	cpu_sys_devices[cpu] = sys_dev;
 	dprintk("initialization complete\n");
 	cpufreq_debug_enable_ratelimit();
-	
+
 	return 0;
 
 
@@ -673,12 +810,16 @@ err_out_unregister:
 	kobject_unregister(&policy->kobj);
 	wait_for_completion(&policy->kobj_unregister);
 
+err_out_driver_exit:
+	if (cpufreq_driver->exit)
+		cpufreq_driver->exit(policy);
+
 err_out:
 	kfree(policy);
 
 nomem_out:
 	module_put(cpufreq_driver->owner);
- module_out:
+module_out:
 	cpufreq_debug_enable_ratelimit();
 	return ret;
 }
@@ -695,6 +836,7 @@ static int cpufreq_remove_dev (struct sys_device * sys_dev)
 	unsigned long flags;
 	struct cpufreq_policy *data;
 #ifdef CONFIG_SMP
+	struct sys_device *cpu_sys_dev;
 	unsigned int j;
 #endif
 
@@ -706,7 +848,6 @@ static int cpufreq_remove_dev (struct sys_device * sys_dev)
 
 	if (!data) {
 		spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
-		cpu_sys_devices[cpu] = NULL;
 		cpufreq_debug_enable_ratelimit();
 		return -EINVAL;
 	}
@@ -715,25 +856,24 @@ static int cpufreq_remove_dev (struct sys_device * sys_dev)
 
 #ifdef CONFIG_SMP
 	/* if this isn't the CPU which is the parent of the kobj, we
-	 * only need to unlink, put and exit 
+	 * only need to unlink, put and exit
 	 */
 	if (unlikely(cpu != data->cpu)) {
 		dprintk("removing link\n");
+		cpu_clear(cpu, data->cpus);
 		spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
 		sysfs_remove_link(&sys_dev->kobj, "cpufreq");
-		cpu_sys_devices[cpu] = NULL;
 		cpufreq_cpu_put(data);
 		cpufreq_debug_enable_ratelimit();
 		return 0;
 	}
 #endif
 
-	cpu_sys_devices[cpu] = NULL;
 
 	if (!kobject_get(&data->kobj)) {
 		spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
 		cpufreq_debug_enable_ratelimit();
- 		return -EFAULT;
+		return -EFAULT;
 	}
 
 #ifdef CONFIG_SMP
@@ -757,7 +897,8 @@ static int cpufreq_remove_dev (struct sys_device * sys_dev)
 			if (j == cpu)
 				continue;
 			dprintk("removing link for cpu %u\n", j);
-			sysfs_remove_link(&cpu_sys_devices[j]->kobj, "cpufreq");
+			cpu_sys_dev = get_cpu_sysdev(j);
+			sysfs_remove_link(&cpu_sys_dev->kobj, "cpufreq");
 			cpufreq_cpu_put(data);
 		}
 	}
@@ -765,18 +906,17 @@ static int cpufreq_remove_dev (struct sys_device * sys_dev)
 	spin_unlock_irqrestore(&cpufreq_driver_lock, flags);
 #endif
 
-	down(&data->lock);
+	mutex_lock(&data->lock);
 	if (cpufreq_driver->target)
 		__cpufreq_governor(data, CPUFREQ_GOV_STOP);
-	cpufreq_driver->target = NULL;
-	up(&data->lock);
+	mutex_unlock(&data->lock);
 
 	kobject_unregister(&data->kobj);
 
 	kobject_put(&data->kobj);
 
 	/* we need to make sure that the underlying kobj is actually
-	 * not referenced anymore by anybody before we proceed with 
+	 * not referenced anymore by anybody before we proceed with
 	 * unloading.
 	 */
 	dprintk("waiting for dropping of refcount\n");
@@ -789,14 +929,15 @@ static int cpufreq_remove_dev (struct sys_device * sys_dev)
 	kfree(data);
 
 	cpufreq_debug_enable_ratelimit();
-
 	return 0;
 }
 
 
-static void handle_update(void *data)
+static void handle_update(struct work_struct *work)
 {
-	unsigned int cpu = (unsigned int)(long)data;
+	struct cpufreq_policy *policy =
+		container_of(work, struct cpufreq_policy, update);
+	unsigned int cpu = policy->cpu;
 	dprintk("handle_update for cpu %u called\n", cpu);
 	cpufreq_update_policy(cpu);
 }
@@ -810,11 +951,12 @@ static void handle_update(void *data)
  *	We adjust to current frequency first, and need to clean up later. So either call
  *	to cpufreq_update_policy() or schedule handle_update()).
  */
-static void cpufreq_out_of_sync(unsigned int cpu, unsigned int old_freq, unsigned int new_freq)
+static void cpufreq_out_of_sync(unsigned int cpu, unsigned int old_freq,
+				unsigned int new_freq)
 {
 	struct cpufreq_freqs freqs;
 
-	printk(KERN_WARNING "Warning: CPU frequency out of sync: cpufreq and timing "
+	dprintk("Warning: CPU frequency out of sync: cpufreq and timing "
 	       "core thinks of %u, is %u kHz.\n", old_freq, new_freq);
 
 	freqs.cpu = cpu;
@@ -825,7 +967,31 @@ static void cpufreq_out_of_sync(unsigned int cpu, unsigned int old_freq, unsigne
 }
 
 
-/** 
+/**
+ * cpufreq_quick_get - get the CPU frequency (in kHz) from policy->cur
+ * @cpu: CPU number
+ *
+ * This is the last known freq, without actually getting it from the driver.
+ * Return value will be same as what is shown in scaling_cur_freq in sysfs.
+ */
+unsigned int cpufreq_quick_get(unsigned int cpu)
+{
+	struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
+	unsigned int ret_freq = 0;
+
+	if (policy) {
+		mutex_lock(&policy->lock);
+		ret_freq = policy->cur;
+		mutex_unlock(&policy->lock);
+		cpufreq_cpu_put(policy);
+	}
+
+	return (ret_freq);
+}
+EXPORT_SYMBOL(cpufreq_quick_get);
+
+
+/**
  * cpufreq_get - get the current CPU frequency (in kHz)
  * @cpu: CPU number
  *
@@ -834,7 +1000,7 @@ static void cpufreq_out_of_sync(unsigned int cpu, unsigned int old_freq, unsigne
 unsigned int cpufreq_get(unsigned int cpu)
 {
 	struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
-	unsigned int ret = 0;
+	unsigned int ret_freq = 0;
 
 	if (!policy)
 		return 0;
@@ -842,40 +1008,120 @@ unsigned int cpufreq_get(unsigned int cpu)
 	if (!cpufreq_driver->get)
 		goto out;
 
-	down(&policy->lock);
+	mutex_lock(&policy->lock);
 
-	ret = cpufreq_driver->get(cpu);
+	ret_freq = cpufreq_driver->get(cpu);
 
-	if (ret && policy->cur && !(cpufreq_driver->flags & CPUFREQ_CONST_LOOPS)) 
-	{
-		/* verify no discrepancy between actual and saved value exists */
-		if (unlikely(ret != policy->cur)) {
-			cpufreq_out_of_sync(cpu, policy->cur, ret);
+	if (ret_freq && policy->cur &&
+		!(cpufreq_driver->flags & CPUFREQ_CONST_LOOPS)) {
+		/* verify no discrepancy between actual and
+					saved value exists */
+		if (unlikely(ret_freq != policy->cur)) {
+			cpufreq_out_of_sync(cpu, policy->cur, ret_freq);
 			schedule_work(&policy->update);
 		}
 	}
 
-	up(&policy->lock);
+	mutex_unlock(&policy->lock);
 
- out:
+out:
 	cpufreq_cpu_put(policy);
 
-	return (ret);
+	return (ret_freq);
 }
 EXPORT_SYMBOL(cpufreq_get);
 
 
+/**
+ *	cpufreq_suspend - let the low level driver prepare for suspend
+ */
+
+static int cpufreq_suspend(struct sys_device * sysdev, pm_message_t pmsg)
+{
+	int cpu = sysdev->id;
+	int ret = 0;
+	unsigned int cur_freq = 0;
+	struct cpufreq_policy *cpu_policy;
+
+	dprintk("suspending 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, pmsg);
+		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))
+			dprintk("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;
+
+		srcu_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)
 {
 	int cpu = sysdev->id;
-	unsigned int ret = 0;
+	int ret = 0;
 	struct cpufreq_policy *cpu_policy;
 
 	dprintk("resuming cpu %u\n", cpu);
@@ -915,21 +1161,27 @@ 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;
 		}
 
 		if (unlikely(cur_freq != cpu_policy->cur)) {
 			struct cpufreq_freqs freqs;
 
-			printk(KERN_WARNING "Warning: CPU frequency is %u, "
-			       "cpufreq assumed %u kHz.\n", cur_freq, cpu_policy->cur);
+			if (!(cpufreq_driver->flags & CPUFREQ_PM_NO_WARN))
+				dprintk("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_RESUMECHANGE, &freqs);
+			srcu_notifier_call_chain(
+					&cpufreq_transition_notifier_list,
+					CPUFREQ_RESUMECHANGE, &freqs);
 			adjust_jiffies(CPUFREQ_RESUMECHANGE, &freqs);
 
 			cpu_policy->cur = cur_freq;
@@ -945,6 +1197,7 @@ out:
 static struct sysdev_driver cpufreq_sysdev_driver = {
 	.add		= cpufreq_add_dev,
 	.remove		= cpufreq_remove_dev,
+	.suspend	= cpufreq_suspend,
 	.resume		= cpufreq_resume,
 };
 
@@ -958,30 +1211,30 @@ static struct sysdev_driver cpufreq_sysdev_driver = {
  *	@nb: notifier function to register
  *      @list: CPUFREQ_TRANSITION_NOTIFIER or CPUFREQ_POLICY_NOTIFIER
  *
- *	Add a driver to one of two lists: either a list of drivers that 
+ *	Add a driver to one of two lists: either a list of drivers that
  *      are notified about clock rate changes (once before and once after
  *      the transition), or a list of drivers that are notified about
  *      changes in cpufreq policy.
  *
  *	This function may sleep, and has the same return conditions as
- *	notifier_chain_register.
+ *	blocking_notifier_chain_register.
  */
 int cpufreq_register_notifier(struct notifier_block *nb, unsigned int list)
 {
 	int ret;
 
-	down_write(&cpufreq_notifier_rwsem);
 	switch (list) {
 	case CPUFREQ_TRANSITION_NOTIFIER:
-		ret = notifier_chain_register(&cpufreq_transition_notifier_list, nb);
+		ret = srcu_notifier_chain_register(
+				&cpufreq_transition_notifier_list, nb);
 		break;
 	case CPUFREQ_POLICY_NOTIFIER:
-		ret = notifier_chain_register(&cpufreq_policy_notifier_list, nb);
+		ret = blocking_notifier_chain_register(
+				&cpufreq_policy_notifier_list, nb);
 		break;
 	default:
 		ret = -EINVAL;
 	}
-	up_write(&cpufreq_notifier_rwsem);
 
 	return ret;
 }
@@ -996,24 +1249,24 @@ EXPORT_SYMBOL(cpufreq_register_notifier);
  *	Remove a driver from the CPU frequency notifier list.
  *
  *	This function may sleep, and has the same return conditions as
- *	notifier_chain_unregister.
+ *	blocking_notifier_chain_unregister.
  */
 int cpufreq_unregister_notifier(struct notifier_block *nb, unsigned int list)
 {
 	int ret;
 
-	down_write(&cpufreq_notifier_rwsem);
 	switch (list) {
 	case CPUFREQ_TRANSITION_NOTIFIER:
-		ret = notifier_chain_unregister(&cpufreq_transition_notifier_list, nb);
+		ret = srcu_notifier_chain_unregister(
+				&cpufreq_transition_notifier_list, nb);
 		break;
 	case CPUFREQ_POLICY_NOTIFIER:
-		ret = notifier_chain_unregister(&cpufreq_policy_notifier_list, nb);
+		ret = blocking_notifier_chain_unregister(
+				&cpufreq_policy_notifier_list, nb);
 		break;
 	default:
 		ret = -EINVAL;
 	}
-	up_write(&cpufreq_notifier_rwsem);
 
 	return ret;
 }
@@ -1025,104 +1278,110 @@ EXPORT_SYMBOL(cpufreq_unregister_notifier);
  *********************************************************************/
 
 
+/* Must be called with lock_cpu_hotplug held */
 int __cpufreq_driver_target(struct cpufreq_policy *policy,
 			    unsigned int target_freq,
 			    unsigned int relation)
 {
 	int retval = -EINVAL;
-	lock_cpu_hotplug();
+
 	dprintk("target for CPU %u: %u kHz, relation %u\n", policy->cpu,
 		target_freq, relation);
 	if (cpu_online(policy->cpu) && cpufreq_driver->target)
 		retval = cpufreq_driver->target(policy, target_freq, relation);
-	unlock_cpu_hotplug();
+
 	return retval;
 }
 EXPORT_SYMBOL_GPL(__cpufreq_driver_target);
 
-
 int cpufreq_driver_target(struct cpufreq_policy *policy,
 			  unsigned int target_freq,
 			  unsigned int relation)
 {
-	unsigned int ret;
+	int ret;
 
 	policy = cpufreq_cpu_get(policy->cpu);
 	if (!policy)
 		return -EINVAL;
 
-	down(&policy->lock);
+	lock_cpu_hotplug();
+	mutex_lock(&policy->lock);
 
 	ret = __cpufreq_driver_target(policy, target_freq, relation);
 
-	up(&policy->lock);
+	mutex_unlock(&policy->lock);
+	unlock_cpu_hotplug();
 
 	cpufreq_cpu_put(policy);
-
 	return ret;
 }
 EXPORT_SYMBOL_GPL(cpufreq_driver_target);
 
-
-static int __cpufreq_governor(struct cpufreq_policy *policy, unsigned int event)
+int cpufreq_driver_getavg(struct cpufreq_policy *policy)
 {
-	int ret = -EINVAL;
+	int ret = 0;
 
-	if (!try_module_get(policy->governor->owner))
+	policy = cpufreq_cpu_get(policy->cpu);
+	if (!policy)
 		return -EINVAL;
 
-	dprintk("__cpufreq_governor for CPU %u, event %u\n", policy->cpu, event);
-	ret = policy->governor->governor(policy, event);
+	mutex_lock(&policy->lock);
 
-	/* we keep one module reference alive for each CPU governed by this CPU */
-	if ((event != CPUFREQ_GOV_START) || ret)
-		module_put(policy->governor->owner);
-	if ((event == CPUFREQ_GOV_STOP) && !ret)
-		module_put(policy->governor->owner);
+	if (cpu_online(policy->cpu) && cpufreq_driver->getavg)
+		ret = cpufreq_driver->getavg(policy->cpu);
+
+	mutex_unlock(&policy->lock);
 
+	cpufreq_cpu_put(policy);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(cpufreq_driver_getavg);
 
+/*
+ * Locking: Must be called with the lock_cpu_hotplug() lock held
+ * when "event" is CPUFREQ_GOV_LIMITS
+ */
 
-int cpufreq_governor(unsigned int cpu, unsigned int event)
+static int __cpufreq_governor(struct cpufreq_policy *policy,
+					unsigned int event)
 {
-	int ret = 0;
-	struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
+	int ret;
 
-	if (!policy)
+	if (!try_module_get(policy->governor->owner))
 		return -EINVAL;
 
-	down(&policy->lock);
-	ret = __cpufreq_governor(policy, event);
-	up(&policy->lock);
+	dprintk("__cpufreq_governor for CPU %u, event %u\n",
+						policy->cpu, event);
+	ret = policy->governor->governor(policy, event);
 
-	cpufreq_cpu_put(policy);
+	/* we keep one module reference alive for
+			each CPU governed by this CPU */
+	if ((event != CPUFREQ_GOV_START) || ret)
+		module_put(policy->governor->owner);
+	if ((event == CPUFREQ_GOV_STOP) && !ret)
+		module_put(policy->governor->owner);
 
 	return ret;
 }
-EXPORT_SYMBOL_GPL(cpufreq_governor);
 
 
 int cpufreq_register_governor(struct cpufreq_governor *governor)
 {
-	struct cpufreq_governor *t;
+	int err;
 
 	if (!governor)
 		return -EINVAL;
 
-	down(&cpufreq_governor_sem);
-	
-	list_for_each_entry(t, &cpufreq_governor_list, governor_list) {
-		if (!strnicmp(governor->name,t->name,CPUFREQ_NAME_LEN)) {
-			up(&cpufreq_governor_sem);
-			return -EBUSY;
-		}
-	}
-	list_add(&governor->governor_list, &cpufreq_governor_list);
+	mutex_lock(&cpufreq_governor_mutex);
 
- 	up(&cpufreq_governor_sem);
+	err = -EBUSY;
+	if (__find_governor(governor->name) == NULL) {
+		err = 0;
+		list_add(&governor->governor_list, &cpufreq_governor_list);
+	}
 
-	return 0;
+	mutex_unlock(&cpufreq_governor_mutex);
+	return err;
 }
 EXPORT_SYMBOL_GPL(cpufreq_register_governor);
 
@@ -1132,9 +1391,9 @@ void cpufreq_unregister_governor(struct cpufreq_governor *governor)
 	if (!governor)
 		return;
 
-	down(&cpufreq_governor_sem);
+	mutex_lock(&cpufreq_governor_mutex);
 	list_del(&governor->governor_list);
-	up(&cpufreq_governor_sem);
+	mutex_unlock(&cpufreq_governor_mutex);
 	return;
 }
 EXPORT_SYMBOL_GPL(cpufreq_unregister_governor);
@@ -1161,18 +1420,23 @@ int cpufreq_get_policy(struct cpufreq_policy *policy, unsigned int cpu)
 	if (!cpu_policy)
 		return -EINVAL;
 
-	down(&cpu_policy->lock);
+	mutex_lock(&cpu_policy->lock);
 	memcpy(policy, cpu_policy, sizeof(struct cpufreq_policy));
-	up(&cpu_policy->lock);
+	mutex_unlock(&cpu_policy->lock);
 
 	cpufreq_cpu_put(cpu_policy);
-
 	return 0;
 }
 EXPORT_SYMBOL(cpufreq_get_policy);
 
 
-static int __cpufreq_set_policy(struct cpufreq_policy *data, struct cpufreq_policy *policy)
+/*
+ * data   : current policy.
+ * policy : policy to be set.
+ * Locking: Must be called with the lock_cpu_hotplug() lock held
+ */
+static int __cpufreq_set_policy(struct cpufreq_policy *data,
+				struct cpufreq_policy *policy)
 {
 	int ret = 0;
 
@@ -1180,43 +1444,42 @@ static int __cpufreq_set_policy(struct cpufreq_policy *data, struct cpufreq_poli
 	dprintk("setting new policy for CPU %u: %u - %u kHz\n", policy->cpu,
 		policy->min, policy->max);
 
-	memcpy(&policy->cpuinfo, 
-	       &data->cpuinfo, 
-	       sizeof(struct cpufreq_cpuinfo));
+	memcpy(&policy->cpuinfo, &data->cpuinfo,
+				sizeof(struct cpufreq_cpuinfo));
+
+	if (policy->min > data->min && policy->min > policy->max) {
+		ret = -EINVAL;
+		goto error_out;
+	}
 
 	/* verify the cpu speed can be set within this limit */
 	ret = cpufreq_driver->verify(policy);
 	if (ret)
 		goto error_out;
 
-	down_read(&cpufreq_notifier_rwsem);
-
 	/* adjust if necessary - all reasons */
-	notifier_call_chain(&cpufreq_policy_notifier_list, CPUFREQ_ADJUST,
-			    policy);
+	blocking_notifier_call_chain(&cpufreq_policy_notifier_list,
+			CPUFREQ_ADJUST, policy);
 
 	/* adjust if necessary - hardware incompatibility*/
-	notifier_call_chain(&cpufreq_policy_notifier_list, CPUFREQ_INCOMPATIBLE,
-			    policy);
+	blocking_notifier_call_chain(&cpufreq_policy_notifier_list,
+			CPUFREQ_INCOMPATIBLE, policy);
 
 	/* verify the cpu speed can be set within this limit,
 	   which might be different to the first one */
 	ret = cpufreq_driver->verify(policy);
-	if (ret) {
-		up_read(&cpufreq_notifier_rwsem);
+	if (ret)
 		goto error_out;
-	}
 
 	/* notification of the new policy */
-	notifier_call_chain(&cpufreq_policy_notifier_list, CPUFREQ_NOTIFY,
-			    policy);
-
-	up_read(&cpufreq_notifier_rwsem);
+	blocking_notifier_call_chain(&cpufreq_policy_notifier_list,
+			CPUFREQ_NOTIFY, policy);
 
-	data->min    = policy->min;
-	data->max    = policy->max;
+	data->min = policy->min;
+	data->max = policy->max;
 
-	dprintk("new min and max freqs are %u - %u kHz\n", data->min, data->max);
+	dprintk("new min and max freqs are %u - %u kHz\n",
+					data->min, data->max);
 
 	if (cpufreq_driver->setpolicy) {
 		data->policy = policy->policy;
@@ -1237,10 +1500,12 @@ static int __cpufreq_set_policy(struct cpufreq_policy *data, struct cpufreq_poli
 			data->governor = policy->governor;
 			if (__cpufreq_governor(data, CPUFREQ_GOV_START)) {
 				/* new governor failed, so re-start old one */
-				dprintk("starting governor %s failed\n", data->governor->name);
+				dprintk("starting governor %s failed\n",
+							data->governor->name);
 				if (old_gov) {
 					data->governor = old_gov;
-					__cpufreq_governor(data, CPUFREQ_GOV_START);
+					__cpufreq_governor(data,
+							   CPUFREQ_GOV_START);
 				}
 				ret = -EINVAL;
 				goto error_out;
@@ -1251,7 +1516,7 @@ static int __cpufreq_set_policy(struct cpufreq_policy *data, struct cpufreq_poli
 		__cpufreq_governor(data, CPUFREQ_GOV_LIMITS);
 	}
 
- error_out:
+error_out:
 	cpufreq_debug_enable_ratelimit();
 	return ret;
 }
@@ -1274,8 +1539,10 @@ int cpufreq_set_policy(struct cpufreq_policy *policy)
 	if (!data)
 		return -EINVAL;
 
+	lock_cpu_hotplug();
+
 	/* lock this CPU */
-	down(&data->lock);
+	mutex_lock(&data->lock);
 
 	ret = __cpufreq_set_policy(data, policy);
 	data->user_policy.min = data->min;
@@ -1283,7 +1550,9 @@ int cpufreq_set_policy(struct cpufreq_policy *policy)
 	data->user_policy.policy = data->policy;
 	data->user_policy.governor = data->governor;
 
-	up(&data->lock);
+	mutex_unlock(&data->lock);
+
+	unlock_cpu_hotplug();
 	cpufreq_cpu_put(data);
 
 	return ret;
@@ -1307,26 +1576,78 @@ int cpufreq_update_policy(unsigned int cpu)
 	if (!data)
 		return -ENODEV;
 
-	down(&data->lock);
+	lock_cpu_hotplug();
+	mutex_lock(&data->lock);
 
 	dprintk("updating policy for CPU %u\n", cpu);
-	memcpy(&policy, 
-	       data,
-	       sizeof(struct cpufreq_policy));
+	memcpy(&policy, data, sizeof(struct cpufreq_policy));
 	policy.min = data->user_policy.min;
 	policy.max = data->user_policy.max;
 	policy.policy = data->user_policy.policy;
 	policy.governor = data->user_policy.governor;
 
-	ret = __cpufreq_set_policy(data, &policy);
+	/* BIOS might change freq behind our back
+	  -> ask driver for current freq and notify governors about a change */
+	if (cpufreq_driver->get) {
+		policy.cur = cpufreq_driver->get(cpu);
+		if (!data->cur) {
+			dprintk("Driver did not initialize current freq");
+			data->cur = policy.cur;
+		} else {
+			if (data->cur != policy.cur)
+				cpufreq_out_of_sync(cpu, data->cur,
+								policy.cur);
+		}
+	}
 
-	up(&data->lock);
+	ret = __cpufreq_set_policy(data, &policy);
 
+	mutex_unlock(&data->lock);
+	unlock_cpu_hotplug();
 	cpufreq_cpu_put(data);
 	return ret;
 }
 EXPORT_SYMBOL(cpufreq_update_policy);
 
+static int cpufreq_cpu_callback(struct notifier_block *nfb,
+					unsigned long action, void *hcpu)
+{
+	unsigned int cpu = (unsigned long)hcpu;
+	struct cpufreq_policy *policy;
+	struct sys_device *sys_dev;
+
+	sys_dev = get_cpu_sysdev(cpu);
+
+	if (sys_dev) {
+		switch (action) {
+		case CPU_ONLINE:
+			cpufreq_add_dev(sys_dev);
+			break;
+		case CPU_DOWN_PREPARE:
+			/*
+			 * We attempt to put this cpu in lowest frequency
+			 * possible before going down. This will permit
+			 * hardware-managed P-State to switch other related
+			 * threads to min or higher speeds if possible.
+			 */
+			policy = cpufreq_cpu_data[cpu];
+			if (policy) {
+				cpufreq_driver_target(policy, policy->min,
+						CPUFREQ_RELATION_H);
+			}
+			break;
+		case CPU_DEAD:
+			cpufreq_remove_dev(sys_dev);
+			break;
+		}
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block __cpuinitdata cpufreq_cpu_notifier =
+{
+    .notifier_call = cpufreq_cpu_callback,
+};
 
 /*********************************************************************
  *               REGISTER / UNREGISTER CPUFREQ DRIVER                *
@@ -1337,9 +1658,9 @@ EXPORT_SYMBOL(cpufreq_update_policy);
  * @driver_data: A struct cpufreq_driver containing the values#
  * submitted by the CPU Frequency driver.
  *
- *   Registers a CPU Frequency driver to this core code. This code 
+ *   Registers a CPU Frequency driver to this core code. This code
  * returns zero on success, -EBUSY when another driver got here first
- * (and isn't unregistered in the meantime). 
+ * (and isn't unregistered in the meantime).
  *
  */
 int cpufreq_register_driver(struct cpufreq_driver *driver_data)
@@ -1377,8 +1698,10 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 
 		/* if all ->init() calls failed, unregister */
 		if (ret) {
-			dprintk("no CPU initialized for driver %s\n", driver_data->name);
-			sysdev_driver_unregister(&cpu_sysdev_class, &cpufreq_sysdev_driver);
+			dprintk("no CPU initialized for driver %s\n",
+							driver_data->name);
+			sysdev_driver_unregister(&cpu_sysdev_class,
+						&cpufreq_sysdev_driver);
 
 			spin_lock_irqsave(&cpufreq_driver_lock, flags);
 			cpufreq_driver = NULL;
@@ -1387,6 +1710,7 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 	}
 
 	if (!ret) {
+		register_hotcpu_notifier(&cpufreq_cpu_notifier);
 		dprintk("driver %s up and running\n", driver_data->name);
 		cpufreq_debug_enable_ratelimit();
 	}
@@ -1399,7 +1723,7 @@ EXPORT_SYMBOL_GPL(cpufreq_register_driver);
 /**
  * cpufreq_unregister_driver - unregister the current CPUFreq driver
  *
- *    Unregister the current CPUFreq driver. Only call this if you have 
+ *    Unregister the current CPUFreq driver. Only call this if you have
  * the right to do so, i.e. if you have succeeded in initialising before!
  * Returns zero if successful, and -EINVAL if the cpufreq_driver is
  * currently not initialised.
@@ -1418,6 +1742,7 @@ int cpufreq_unregister_driver(struct cpufreq_driver *driver)
 	dprintk("unregistering driver %s\n", driver->name);
 
 	sysdev_driver_unregister(&cpu_sysdev_class, &cpufreq_sysdev_driver);
+	unregister_hotcpu_notifier(&cpufreq_cpu_notifier);
 
 	spin_lock_irqsave(&cpufreq_driver_lock, flags);
 	cpufreq_driver = NULL;