diff --git a/arch/blackfin/include/asm/dpmc.h b/arch/blackfin/include/asm/dpmc.h
index 3047120cfcff4b5ce9a7e8bca467a6df820f4aef..edf2a2ad518313883ff26cb8fc49168a1d4db669 100644
--- a/arch/blackfin/include/asm/dpmc.h
+++ b/arch/blackfin/include/asm/dpmc.h
@@ -125,6 +125,9 @@ void unset_dram_srfs(void);
 
 #define VRPAIR(vlev, freq) (((vlev) << 16) | ((freq) >> 16))
 
+#ifdef CONFIG_CPU_FREQ
+#define CPUFREQ_CPU 0
+#endif
 struct bfin_dpmc_platform_data {
 	const unsigned int *tuple_tab;
 	unsigned short tabsize;
diff --git a/arch/blackfin/include/asm/smp.h b/arch/blackfin/include/asm/smp.h
index f5b537967116d151c828eac66b2166b3d5f179eb..9dd4873752479cb9bf14cb35882a1da363786c21 100644
--- a/arch/blackfin/include/asm/smp.h
+++ b/arch/blackfin/include/asm/smp.h
@@ -34,7 +34,7 @@ extern unsigned long dcache_invld_count[NR_CPUS];
 void smp_icache_flush_range_others(unsigned long start,
 				   unsigned long end);
 #ifdef CONFIG_HOTPLUG_CPU
-void coreb_sleep(u32 sic_iwr0, u32 sic_iwr1, u32 sic_iwr2);
+void coreb_die(void);
 void cpu_die(void);
 void platform_cpu_die(void);
 int __cpu_disable(void);
diff --git a/arch/blackfin/mach-bf561/hotplug.c b/arch/blackfin/mach-bf561/hotplug.c
index 4cd3b28cd046d86d51973fe0037abdeb63279ea6..42fc085629c7693d43789f6a7c248503c81f6089 100644
--- a/arch/blackfin/mach-bf561/hotplug.c
+++ b/arch/blackfin/mach-bf561/hotplug.c
@@ -5,30 +5,27 @@
  * Licensed under the GPL-2 or later.
  */
 
+#include <linux/smp.h>
 #include <asm/blackfin.h>
-#include <asm/irq.h>
-#include <asm/smp.h>
-
-#define SIC_SYSIRQ(irq)	(irq - (IRQ_CORETMR + 1))
+#include <mach/pll.h>
 
 int hotplug_coreb;
 
 void platform_cpu_die(void)
 {
-	unsigned long iwr[2] = {0, 0};
-	unsigned long bank = SIC_SYSIRQ(IRQ_SUPPLE_0) / 32;
-	unsigned long bit = 1 << (SIC_SYSIRQ(IRQ_SUPPLE_0) % 32);
-
+	unsigned long iwr;
 	hotplug_coreb = 1;
 
-	iwr[bank] = bit;
-
 	/* disable core timer */
 	bfin_write_TCNTL(0);
 
-	/* clear ipi interrupt IRQ_SUPPLE_0 */
+	/* clear ipi interrupt IRQ_SUPPLE_0 of CoreB */
 	bfin_write_SICB_SYSCR(bfin_read_SICB_SYSCR() | (1 << (10 + 1)));
 	SSYNC();
 
-	coreb_sleep(iwr[0], iwr[1], 0);
+	/* set CoreB wakeup by ipi0, iwr will be discarded */
+	bfin_iwr_set_sup0(&iwr, &iwr, &iwr);
+	SSYNC();
+
+	coreb_die();
 }
diff --git a/arch/blackfin/mach-bf561/secondary.S b/arch/blackfin/mach-bf561/secondary.S
index ef9acf21eb8e53da5b207d3190d896e91e39da4d..148e50764555f9dcdf607d4cbecf61e529fdbe7e 100644
--- a/arch/blackfin/mach-bf561/secondary.S
+++ b/arch/blackfin/mach-bf561/secondary.S
@@ -162,39 +162,31 @@ ENTRY(_coreb_trampoline_start)
 ENDPROC(_coreb_trampoline_start)
 ENTRY(_coreb_trampoline_end)
 
+#ifdef CONFIG_HOTPLUG_CPU
 .section ".text"
-ENTRY(_set_sicb_iwr)
-	P0.H = hi(SICB_IWR0);
-	P0.L = lo(SICB_IWR0);
-	P1.H = hi(SICB_IWR1);
-	P1.L = lo(SICB_IWR1);
-	[P0] = R0;
-	[P1] = R1;
-	SSYNC;
-	RTS;
-ENDPROC(_set_sicb_iwr)
-
-ENTRY(_coreb_sleep)
+ENTRY(_coreb_die)
 	sp.l = lo(INITIAL_STACK);
 	sp.h = hi(INITIAL_STACK);
 	fp = sp;
 	usp = sp;
 
-	call _set_sicb_iwr;
-
 	CLI R2;
 	SSYNC;
 	IDLE;
 	STI R2;
 
 	R0 = IWR_DISABLE_ALL;
-	R1 = IWR_DISABLE_ALL;
-	call _set_sicb_iwr;
+	P0.H = hi(SYSMMR_BASE);
+	P0.L = lo(SYSMMR_BASE);
+	[P0 + (SICB_IWR0 - SYSMMR_BASE)] = R0;
+	[P0 + (SICB_IWR1 - SYSMMR_BASE)] = R0;
+	SSYNC;
 
 	p0.h = hi(COREB_L1_CODE_START);
 	p0.l = lo(COREB_L1_CODE_START);
 	jump (p0);
-ENDPROC(_coreb_sleep)
+ENDPROC(_coreb_die)
+#endif
 
 __INIT
 ENTRY(_coreb_start)
diff --git a/arch/blackfin/mach-common/cpufreq.c b/arch/blackfin/mach-common/cpufreq.c
index f4cf11d362e1df4f67315a68714cac5faf6b293b..c33fb61b508224badeeae1f72fe5721eae15d425 100644
--- a/arch/blackfin/mach-common/cpufreq.c
+++ b/arch/blackfin/mach-common/cpufreq.c
@@ -16,8 +16,6 @@
 #include <asm/time.h>
 #include <asm/dpmc.h>
 
-#define CPUFREQ_CPU 0
-
 /* this is the table of CCLK frequencies, in Hz */
 /* .index is the entry in the auxillary dpm_state_table[] */
 static struct cpufreq_frequency_table bfin_freq_table[] = {
diff --git a/arch/blackfin/mach-common/dpmc.c b/arch/blackfin/mach-common/dpmc.c
index 02c7efd1bcf4807939135734a5f41e159b1af224..382099fd55619be3a4f1d40c99cdf034143943a4 100644
--- a/arch/blackfin/mach-common/dpmc.c
+++ b/arch/blackfin/mach-common/dpmc.c
@@ -61,17 +61,63 @@ static unsigned int bfin_get_vlev(unsigned int freq)
 }
 
 #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;
 }