1 files changed, 53 insertions, 26 deletions

diff --git a/arch/blackfin/mach-common/dpmc.c b/arch/blackfin/mach-common/dpmc.c
index 02c7efd1bcf..724a8c5f557 100644
--- a/arch/blackfin/mach-common/dpmc.c
+++ b/arch/blackfin/mach-common/dpmc.c
@@ -19,9 +19,6 @@
 
 #define DRIVER_NAME "bfin dpmc"
 
-#define dprintk(msg...) \
-	cpufreq_debug_printk(CPUFREQ_DEBUG_DRIVER, DRIVER_NAME, msg)
-
 struct bfin_dpmc_platform_data *pdata;
 
 /**
@@ -61,17 +58,64 @@ err_out:
 }
 
 #ifdef CONFIG_CPU_FREQ
+# ifdef CONFIG_SMP
+static void bfin_idle_this_cpu(void *info)
+{
+	unsigned long flags = 0;
+	unsigned long iwr0, iwr1, iwr2;
+	unsigned int cpu = smp_processor_id();
+
+	local_irq_save_hw(flags);
+	bfin_iwr_set_sup0(&iwr0, &iwr1, &iwr2);
+
+	platform_clear_ipi(cpu, IRQ_SUPPLE_0);
+	SSYNC();
+	asm("IDLE;");
+	bfin_iwr_restore(iwr0, iwr1, iwr2);
+
+	local_irq_restore_hw(flags);
+}
+
+static void bfin_idle_cpu(void)
+{
+	smp_call_function(bfin_idle_this_cpu, NULL, 0);
+}
+
+static void bfin_wakeup_cpu(void)
+{
+	unsigned int cpu;
+	unsigned int this_cpu = smp_processor_id();
+	cpumask_t mask;
+
+	cpumask_copy(&mask, cpu_online_mask);
+	cpumask_clear_cpu(this_cpu, &mask);
+	for_each_cpu(cpu, &mask)
+		platform_send_ipi_cpu(cpu, IRQ_SUPPLE_0);
+}
+
+# else
+static void bfin_idle_cpu(void) {}
+static void bfin_wakeup_cpu(void) {}
+# endif
+
 static int
 vreg_cpufreq_notifier(struct notifier_block *nb, unsigned long val, void *data)
 {
 	struct cpufreq_freqs *freq = data;
 
+	if (freq->cpu != CPUFREQ_CPU)
+		return 0;
+
 	if (val == CPUFREQ_PRECHANGE && freq->old < freq->new) {
+		bfin_idle_cpu();
 		bfin_set_vlev(bfin_get_vlev(freq->new));
 		udelay(pdata->vr_settling_time); /* Wait until Volatge settled */
-
-	} else if (val == CPUFREQ_POSTCHANGE && freq->old > freq->new)
+		bfin_wakeup_cpu();
+	} else if (val == CPUFREQ_POSTCHANGE && freq->old > freq->new) {
+		bfin_idle_cpu();
 		bfin_set_vlev(bfin_get_vlev(freq->new));
+		bfin_wakeup_cpu();
+	}
 
 	return 0;
 }
@@ -85,7 +129,7 @@ static struct notifier_block vreg_cpufreq_notifier_block = {
  *	bfin_dpmc_probe -
  *
  */
-static int __devinit bfin_dpmc_probe(struct platform_device *pdev)
+static int bfin_dpmc_probe(struct platform_device *pdev)
 {
 	if (pdev->dev.platform_data)
 		pdata = pdev->dev.platform_data;
@@ -99,7 +143,7 @@ static int __devinit bfin_dpmc_probe(struct platform_device *pdev)
 /**
  *	bfin_dpmc_remove -
  */
-static int __devexit bfin_dpmc_remove(struct platform_device *pdev)
+static int bfin_dpmc_remove(struct platform_device *pdev)
 {
 	pdata = NULL;
 	return cpufreq_unregister_notifier(&vreg_cpufreq_notifier_block,
@@ -108,29 +152,12 @@ static int __devexit bfin_dpmc_remove(struct platform_device *pdev)
 
 struct platform_driver bfin_dpmc_device_driver = {
 	.probe   = bfin_dpmc_probe,
-	.remove  = __devexit_p(bfin_dpmc_remove),
+	.remove  = bfin_dpmc_remove,
 	.driver  = {
 		.name = DRIVER_NAME,
 	}
 };
-
-/**
- *	bfin_dpmc_init - Init driver
- */
-static int __init bfin_dpmc_init(void)
-{
-	return platform_driver_register(&bfin_dpmc_device_driver);
-}
-module_init(bfin_dpmc_init);
-
-/**
- *	bfin_dpmc_exit - break down driver
- */
-static void __exit bfin_dpmc_exit(void)
-{
-	platform_driver_unregister(&bfin_dpmc_device_driver);
-}
-module_exit(bfin_dpmc_exit);
+module_platform_driver(bfin_dpmc_device_driver);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("cpu power management driver for Blackfin");