[SPARC64]: Use init/exit facility of cpufreq infrastructure.

arch/sparc64/kernel/us3_cpufreq.c

@@ -15,12 +15,19 @@
 #include <linux/slab.h>
 #include <linux/init.h>
 
+#include <asm/head.h>
+
 static struct cpufreq_driver *cpufreq_us3_driver;
 
-/* Index by [(CPU * 4) + INDEX] (first three indices are
- * actual us3 divisor entries, last is for CPUFREQ_TABLE_END)
- */
-static struct cpufreq_frequency_table *us3_freq_table;
+struct us3_freq_percpu_info {
+	struct cpufreq_frequency_table table[4];
+	unsigned long udelay_val_ref;
+	unsigned long clock_tick_ref;
+	unsigned int ref_freq;
+};
+
+/* Indexed by cpu number. */
+static struct us3_freq_percpu_info *us3_freq_table;
 
 /* UltraSPARC-III has three dividers: 1, 2, and 32.  These are controlled
  * in the Safari config register.
@@ -51,17 +58,66 @@ static void write_safari_cfg(unsigned long val)
 
 #ifndef CONFIG_SMP
 extern unsigned long up_clock_tick;
+unsigned long clock_tick_ref;
+unsigned int ref_freq;
 #endif
 
 static __inline__ unsigned long get_clock_tick(unsigned int cpu)
 {
 #ifdef CONFIG_SMP
+	if (us3_freq_table[cpu].clock_tick_ref)
+		return us3_freq_table[cpu].clock_tick_ref;
 	return cpu_data[cpu].clock_tick;
 #else
+	if (clock_tick_ref)
+		return clock_tick_ref;
 	return up_clock_tick;
 #endif
 }
 
+static int us3_cpufreq_notifier(struct notifier_block *nb, unsigned long val,
+				void *data)
+{
+	struct cpufreq_freqs *freq = data;
+	unsigned int cpu = freq->cpu;
+
+#ifdef CONFIG_SMP
+	if (!us3_freq_table[cpu].ref_freq) {
+		us3_freq_table[cpu].ref_freq = freq->old;
+		us3_freq_table[cpu].udelay_val_ref = cpu_data[cpu].udelay_val;
+		us3_freq_table[cpu].clock_tick_ref = cpu_data[cpu].clock_tick;
+	}
+	if ((val == CPUFREQ_PRECHANGE  && freq->old < freq->new) ||
+	    (val == CPUFREQ_POSTCHANGE && freq->old > freq->new)) {
+		cpu_data[cpu].udelay_val =
+			cpufreq_scale(us3_freq_table[cpu].udelay_val_ref,
+				      us3_freq_table[cpu].ref_freq,
+				      freq->new);
+		cpu_data[cpu].clock_tick =
+			cpufreq_scale(us3_freq_table[cpu].clock_tick_ref,
+				      us3_freq_table[cpu].ref_freq,
+				      freq->new);
+	}
+#else
+	/* In the non-SMP case, kernel/cpufreq.c takes care of adjusting
+	 * loops_per_jiffy.
+	 */
+	if (!ref_freq) {
+		ref_freq = freq->old;
+		clock_tick_ref = up_clock_tick;
+	}
+	if ((val == CPUFREQ_PRECHANGE  && freq->old < freq->new) ||
+	    (val == CPUFREQ_POSTCHANGE && freq->old > freq->new))
+		up_clock_tick = cpufreq_scale(clock_tick_ref, ref_freq, freq->new);
+#endif
+
+	return 0;
+}
+
+static struct notifier_block us3_cpufreq_notifier_block = {
+	.notifier_call	= us3_cpufreq_notifier
+};
+
 static unsigned long get_current_freq(unsigned int cpu, unsigned long safari_cfg)
 {
 	unsigned long clock_tick = get_clock_tick(cpu);
@@ -130,12 +186,12 @@ static void us3_set_cpu_divider_index(unsigned int cpu, unsigned int index)
 	set_cpus_allowed(current, cpus_allowed);
 }
 
-static int us3_setpolicy(struct cpufreq_policy *policy)
+static int us3freq_setpolicy(struct cpufreq_policy *policy)
 {
 	unsigned int new_index = 0;
 
 	if (cpufreq_frequency_table_setpolicy(policy,
-					      &us3_freq_table[(policy->cpu * 4) + 0],
+					      &us3_freq_table[policy->cpu].table[0],
 					      &new_index))
 		return -EINVAL;
 
@@ -144,83 +200,98 @@ static int us3_setpolicy(struct cpufreq_policy *policy)
 	return 0;
 }
 
-static int us3_verify(struct cpufreq_policy *policy)
+static int us3freq_verify(struct cpufreq_policy *policy)
 {
 	return cpufreq_frequency_table_verify(policy,
-					      &us3_freq_table[(policy->cpu * 4) + 0]);
+					      &us3_freq_table[policy->cpu].table[0]);
 }
 
-#ifndef CONFIG_SMP
-extern unsigned long up_clock_tick;
-#endif
-
-static void __init us3_init_freq_table(unsigned int cpu)
+static int __init us3freq_cpu_init(struct cpufreq_policy *policy)
 {
+	unsigned int cpu = policy->cpu;
 	unsigned long clock_tick = get_clock_tick(cpu);
+	struct cpufreq_frequency_table *table =
+		&us3_freq_table[cpu].table[0];
+
+	table[0].index = 0;
+	table[0].frequency = clock_tick / 1;
+	table[1].index = 1;
+	table[1].frequency = clock_tick / 2;
+	table[2].index = 2;
+	table[2].frequency = clock_tick / 32;
+	table[3].index = 0;
+	table[3].frequency = CPUFREQ_TABLE_END;
+
+	policy->policy = CPUFREQ_POLICY_PERFORMANCE;
+	policy->cpuinfo.transition_latency = 0;
+
+	return cpufreq_frequency_table_cpuinfo(policy, table);
+}
+
+static int __exit us3freq_cpu_exit(struct cpufreq_policy *policy)
+{
+	if (cpufreq_us3_driver)
+		us3_set_cpu_divider_index(policy->cpu, 0);
 
-	us3_freq_table[(cpu * 4) + 0].index = 0;
-	us3_freq_table[(cpu * 4) + 0].frequency = clock_tick / 1;
-	us3_freq_table[(cpu * 4) + 1].index = 1;
-	us3_freq_table[(cpu * 4) + 1].frequency = clock_tick / 2;
-	us3_freq_table[(cpu * 4) + 2].index = 2;
-	us3_freq_table[(cpu * 4) + 2].frequency = clock_tick / 32;
-	us3_freq_table[(cpu * 4) + 3].index = 0;
-	us3_freq_table[(cpu * 4) + 3].frequency = CPUFREQ_TABLE_END;
+	return 0;
 }
 
 static int __init us3freq_init(void)
 {
-	struct cpufreq_driver *driver;
 	unsigned long manuf, impl, ver;
-	int i, ret;
+	int ret;
 
 	__asm__("rdpr %%ver, %0" : "=r" (ver));
 	manuf = ((ver >> 48) & 0xffff);
 	impl  = ((ver >> 32) & 0xffff);
 
-	/* XXX Maybe accept cheetah+ too? */
-	if (manuf == 0x3e && impl == 0x14) {
-		driver = kmalloc(sizeof(struct cpufreq_driver) +
-				 (NR_CPUS * sizeof(struct cpufreq_policy)),
-				 GFP_KERNEL);
+	if (manuf == CHEETAH_MANUF &&
+	    (impl == CHEETAH_IMPL || impl == CHEETAH_PLUS_IMPL)) {
+		struct cpufreq_driver *driver;
+
+		cpufreq_register_notifier(&us3_cpufreq_notifier_block,
+					  CPUFREQ_TRANSITION_NOTIFIER);
+
+		ret = -ENOMEM;
+		driver = kmalloc(sizeof(struct cpufreq_driver), GFP_KERNEL);
 		if (!driver)
-			return -ENOMEM;
+			goto err_out;
+		memset(driver, 0, sizeof(*driver));
+
 		us3_freq_table = kmalloc(
-			(NR_CPUS * 4 * sizeof(struct cpufreq_frequency_table)),
+			(NR_CPUS * sizeof(struct us3_freq_percpu_info)),
 			GFP_KERNEL);
-		if (!us3_freq_table) {
-			kfree(driver);
-			return -ENOMEM;
-		}
+		if (!us3_freq_table)
+			goto err_out;
 
-		driver->policy = (struct cpufreq_policy *) (driver + 1);
-		driver->verify = us3_verify;
-		driver->setpolicy = us3_setpolicy;
+		memset(us3_freq_table, 0,
+		       (NR_CPUS * sizeof(struct us3_freq_percpu_info)));
+
+		driver->verify = us3freq_verify;
+		driver->setpolicy = us3freq_setpolicy;
+		driver->init = us3freq_cpu_init;
+		driver->exit = us3freq_cpu_exit;
+		strcpy(driver->name, "UltraSPARC-III");
 
-		for (i = 0; i < NR_CPUS; i++) {
-			driver->policy[i].cpu = i;
-			us3_init_freq_table(i);
-			ret = cpufreq_frequency_table_cpuinfo(&driver->policy[i],
-				&us3_freq_table[(i * 4) + 0]);
-			if (ret) {
-				kfree(driver);
-				kfree(us3_freq_table);
-				us3_freq_table = NULL;
-				return ret;
-			}
-			driver->policy[i].policy = CPUFREQ_POLICY_PERFORMANCE;
-			us3_set_cpu_divider_index(i, 0);
-		}
 		cpufreq_us3_driver = driver;
 		ret = cpufreq_register_driver(driver);
-		if (ret) {
+		if (ret)
+			goto err_out;
+
+		return 0;
+
+err_out:
+		if (driver) {
 			kfree(driver);
 			cpufreq_us3_driver = NULL;
+		}
+		if (us3_freq_table) {
 			kfree(us3_freq_table);
 			us3_freq_table = NULL;
-			return ret;
 		}
-		return 0;
+		cpufreq_unregister_notifier(&us3_cpufreq_notifier_block,
+					    CPUFREQ_TRANSITION_NOTIFIER);
+		return ret;
 	}
 
 	return -ENODEV;
@@ -228,12 +299,10 @@ static int __init us3freq_init(void)
 
 static void __exit us3freq_exit(void)
 {
-	int i;
-
 	if (cpufreq_us3_driver) {
-		for (i = 0; i < NR_CPUS; i++)
-			us3_set_cpu_divider_index(i, 0);
 		cpufreq_unregister_driver(cpufreq_us3_driver);
+		cpufreq_unregister_notifier(&us3_cpufreq_notifier_block,
+					    CPUFREQ_TRANSITION_NOTIFIER);
 
 		kfree(cpufreq_us3_driver);
 		cpufreq_us3_driver = NULL;