Merge git://git.kernel.org/pub/scm/linux/kernel/git/davem/net-2.6

[mirror_ubuntu-artful-kernel.git] / arch / blackfin / mach-common / dpmc.c

diff --git a/arch/blackfin/mach-common/dpmc.c b/arch/blackfin/mach-common/dpmc.c
index 02c7efd1bcf4807939135734a5f41e159b1af224..382099fd55619be3a4f1d40c99cdf034143943a4 100644 (file)
--- 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;
 }