1 files changed, 48 insertions, 2 deletions

diff --git a/arch/blackfin/mach-common/dpmc.c b/arch/blackfin/mach-common/dpmc.c
index 02c7efd1bcf4..382099fd5561 100644
--- a/arch/blackfin/mach-common/dpmc.c
+++ b/arch/blackfin/mach-common/dpmc.c
@@ -61,17 +61,63 @@ 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 = cpu_online_map;
+
+        cpu_clear(this_cpu, mask);
+        for_each_cpu_mask(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;
 }