Merge branch 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/cooloney/blackfin-2.6

* 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/cooloney/blackfin-2.6: (171 commits)
  Blackfin arch: fix bug - BF527 0.2 silicon has different CPUID (DSPID) value
  Blackfin arch: Enlarge flash partition for kenel for bf533/bf537 boards
  Blackfin arch: fix bug: kernel crash when enable SDIO host driver
  Blackfin arch: Print FP at level KERN_NOTICE
  Blackfin arch: drop ad73311 test code
  Blackfin arch: update board default configs
  Blackfin arch: Set PB4 as the default irq for bf548 board v1.4+.
  Blackfin arch: fix typo in early printk bit size processing
  Blackfin arch: enable reprogram cclk and sclk for bf518f-ezbrd
  Blackfin arch: add SDIO host driver platform data
  Blackfin arch: fix bug - kernel stops at initial console
  Blackfin arch: fix bug - kernel crash after config IP for ethernet port
  Blackfin arch: add sdh support for bf518f-ezbrd
  Blackfin arch: fix bug - kernel detects BF532 incorrectly
  Blackfin arch: add () to avoid warnings from gcc
  Blackfin arch: change HWTRACE Kconfig and set it on default
  Blackfin arch: Clean oprofile build path for blackfin
  Blackfin arch: remove hardware PM code, oprofile not use it
  Blackfin arch: rewrite get_sclk()/get_vco()
  Blackfin arch: cleanup and unify the ins functions
  ...

22 files changed, 1712 insertions, 669 deletions

diff --git a/arch/blackfin/mach-bf561/Kconfig b/arch/blackfin/mach-bf561/Kconfig
index 3f4895450be..638ec38ca47 100644
--- a/arch/blackfin/mach-bf561/Kconfig
+++ b/arch/blackfin/mach-bf561/Kconfig
@@ -4,9 +4,9 @@ source "arch/blackfin/mach-bf561/boards/Kconfig"
 
 menu "BF561 Specific Configuration"
 
-comment "Core B Support"
+if (!SMP)
 
-menu "Core B Support"
+comment "Core B Support"
 
 config BF561_COREB
 	bool "Enable Core B support"
@@ -25,7 +25,7 @@ config BF561_COREB_RESET
 	  0 is set, and will reset PC to 0xff600000 when
 	  COREB_SRAM_INIT is cleared.
 
-endmenu
+endif
 
 comment "Interrupt Priority Assignment"
 
@@ -138,7 +138,7 @@ config IRQ_DMA2_11
 	default 9
 config IRQ_TIMER0
 	int "TIMER 0  Interrupt"
-	default 10
+	default 8
 config IRQ_TIMER1
 	int "TIMER 1  Interrupt"
 	default 10
diff --git a/arch/blackfin/mach-bf561/Makefile b/arch/blackfin/mach-bf561/Makefile
index f39235a5578..59e18afe28c 100644
--- a/arch/blackfin/mach-bf561/Makefile
+++ b/arch/blackfin/mach-bf561/Makefile
@@ -2,8 +2,7 @@
 # arch/blackfin/mach-bf561/Makefile
 #
 
-extra-y := head.o
-
 obj-y := ints-priority.o dma.o
 
 obj-$(CONFIG_BF561_COREB) += coreb.o
+obj-$(CONFIG_SMP)  += smp.o secondary.o atomic.o
diff --git a/arch/blackfin/mach-bf561/atomic.S b/arch/blackfin/mach-bf561/atomic.S
new file mode 100644
index 00000000000..9439bc6bd01
--- /dev/null
+++ b/arch/blackfin/mach-bf561/atomic.S
@@ -0,0 +1,919 @@
+/*
+ * File:         arch/blackfin/mach-bf561/atomic.S
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 Analog Devices Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/linkage.h>
+#include <asm/blackfin.h>
+#include <asm/cache.h>
+#include <asm/asm-offsets.h>
+#include <asm/rwlock.h>
+#include <asm/cplb.h>
+
+.text
+
+.macro coreslot_loadaddr reg:req
+	\reg\().l = _corelock;
+	\reg\().h = _corelock;
+.endm
+
+/*
+ * r0 = address of atomic data to flush and invalidate (32bit).
+ *
+ * Clear interrupts and return the old mask.
+ * We assume that no atomic data can span cachelines.
+ *
+ * Clobbers: r2:0, p0
+ */
+ENTRY(_get_core_lock)
+	r1 = -L1_CACHE_BYTES;
+	r1 = r0 & r1;
+	cli r0;
+	coreslot_loadaddr p0;
+.Lretry_corelock:
+	testset (p0);
+	if cc jump .Ldone_corelock;
+	SSYNC(r2);
+	jump .Lretry_corelock
+.Ldone_corelock:
+	p0 = r1;
+	CSYNC(r2);
+	flushinv[p0];
+	SSYNC(r2);
+	rts;
+ENDPROC(_get_core_lock)
+
+/*
+ * r0 = address of atomic data in uncacheable memory region (32bit).
+ *
+ * Clear interrupts and return the old mask.
+ *
+ * Clobbers: r0, p0
+ */
+ENTRY(_get_core_lock_noflush)
+	cli r0;
+	coreslot_loadaddr p0;
+.Lretry_corelock_noflush:
+	testset (p0);
+	if cc jump .Ldone_corelock_noflush;
+	SSYNC(r2);
+	jump .Lretry_corelock_noflush
+.Ldone_corelock_noflush:
+	rts;
+ENDPROC(_get_core_lock_noflush)
+
+/*
+ * r0 = interrupt mask to restore.
+ * r1 = address of atomic data to flush and invalidate (32bit).
+ *
+ * Interrupts are masked on entry (see _get_core_lock).
+ * Clobbers: r2:0, p0
+ */
+ENTRY(_put_core_lock)
+	/* Write-through cache assumed, so no flush needed here. */
+	coreslot_loadaddr p0;
+	r1 = 0;
+	[p0] = r1;
+	SSYNC(r2);
+	sti r0;
+	rts;
+ENDPROC(_put_core_lock)
+
+#ifdef __ARCH_SYNC_CORE_DCACHE
+
+ENTRY(___raw_smp_mark_barrier_asm)
+	[--sp] = rets;
+	[--sp] = ( r7:5 );
+	[--sp] = r0;
+	[--sp] = p1;
+	[--sp] = p0;
+	call _get_core_lock_noflush;
+
+	/*
+	 * Calculate current core mask
+	 */
+	GET_CPUID(p1, r7);
+	r6 = 1;
+	r6 <<= r7;
+
+	/*
+	 * Set bit of other cores in barrier mask. Don't change current core bit.
+	 */
+	p1.l = _barrier_mask;
+	p1.h = _barrier_mask;
+	r7 = [p1];
+	r5 = r7 & r6;
+	r7 = ~r6;
+	cc = r5 == 0;
+	if cc jump 1f;
+	r7 = r7 | r6;
+1:
+	[p1] = r7;
+	SSYNC(r2);
+
+	call _put_core_lock;
+	p0 = [sp++];
+	p1 = [sp++];
+	r0 = [sp++];
+	( r7:5 ) = [sp++];
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_smp_mark_barrier_asm)
+
+ENTRY(___raw_smp_check_barrier_asm)
+	[--sp] = rets;
+	[--sp] = ( r7:5 );
+	[--sp] = r0;
+	[--sp] = p1;
+	[--sp] = p0;
+	call _get_core_lock_noflush;
+
+	/*
+	 * Calculate current core mask
+	 */
+	GET_CPUID(p1, r7);
+	r6 = 1;
+	r6 <<= r7;
+
+	/*
+	 * Clear current core bit in barrier mask if it is set.
+	 */
+	p1.l = _barrier_mask;
+	p1.h = _barrier_mask;
+	r7 = [p1];
+	r5 = r7 & r6;
+	cc = r5 == 0;
+	if cc jump 1f;
+	r6 = ~r6;
+	r7 = r7 & r6;
+	[p1] = r7;
+	SSYNC(r2);
+
+	call _put_core_lock;
+
+	/*
+	 * Invalidate the entire D-cache of current core.
+	 */
+	sp += -12;
+	call _resync_core_dcache
+	sp += 12;
+	jump 2f;
+1:
+	call _put_core_lock;
+2:
+	p0 = [sp++];
+	p1 = [sp++];
+	r0 = [sp++];
+	( r7:5 ) = [sp++];
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_smp_check_barrier_asm)
+
+/*
+ * r0 = irqflags
+ * r1 = address of atomic data
+ *
+ * Clobbers: r2:0, p1:0
+ */
+_start_lock_coherent:
+
+	[--sp] = rets;
+	[--sp] = ( r7:6 );
+	r7 = r0;
+	p1 = r1;
+
+	/*
+	 * Determine whether the atomic data was previously
+	 * owned by another CPU (=r6).
+	 */
+	GET_CPUID(p0, r2);
+	r1 = 1;
+	r1 <<= r2;
+	r2 = ~r1;
+
+	r1 = [p1];
+	r1 >>= 28;   /* CPU fingerprints are stored in the high nibble. */
+	r6 = r1 & r2;
+	r1 = [p1];
+	r1 <<= 4;
+	r1 >>= 4;
+	[p1] = r1;
+
+	/*
+	 * Release the core lock now, but keep IRQs disabled while we are
+	 * performing the remaining housekeeping chores for the current CPU.
+	 */
+	coreslot_loadaddr p0;
+	r1 = 0;
+	[p0] = r1;
+
+	/*
+	 * If another CPU has owned the same atomic section before us,
+	 * then our D-cached copy of the shared data protected by the
+	 * current spin/write_lock may be obsolete.
+	 */
+	cc = r6 == 0;
+	if cc jump .Lcache_synced
+
+	/*
+	 * Invalidate the entire D-cache of the current core.
+	 */
+	sp += -12;
+	call _resync_core_dcache
+	sp += 12;
+
+.Lcache_synced:
+	SSYNC(r2);
+	sti r7;
+	( r7:6 ) = [sp++];
+	rets = [sp++];
+	rts
+
+/*
+ * r0 = irqflags
+ * r1 = address of atomic data
+ *
+ * Clobbers: r2:0, p1:0
+ */
+_end_lock_coherent:
+
+	p1 = r1;
+	GET_CPUID(p0, r2);
+	r2 += 28;
+	r1 = 1;
+	r1 <<= r2;
+	r2 = [p1];
+	r2 = r1 | r2;
+	[p1] = r2;
+	r1 = p1;
+	jump _put_core_lock;
+
+#endif /* __ARCH_SYNC_CORE_DCACHE */
+
+/*
+ * r0 = &spinlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_spin_is_locked_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r3 = [p1];
+	cc = bittst( r3, 0 );
+	r3 = cc;
+	r1 = p1;
+	call _put_core_lock;
+	rets = [sp++];
+	r0 = r3;
+	rts;
+ENDPROC(___raw_spin_is_locked_asm)
+
+/*
+ * r0 = &spinlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_spin_lock_asm)
+	p1 = r0;
+	[--sp] = rets;
+.Lretry_spinlock:
+	call _get_core_lock;
+	r1 = p1;
+	r2 = [p1];
+	cc = bittst( r2, 0 );
+	if cc jump .Lbusy_spinlock
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	r3 = p1;
+	bitset ( r2, 0 ); /* Raise the lock bit. */
+	[p1] = r2;
+	call _start_lock_coherent
+#else
+	r2 = 1;
+	[p1] = r2;
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+
+.Lbusy_spinlock:
+	/* We don't touch the atomic area if busy, so that flush
+	   will behave like nop in _put_core_lock. */
+	call _put_core_lock;
+	SSYNC(r2);
+	r0 = p1;
+	jump .Lretry_spinlock
+ENDPROC(___raw_spin_lock_asm)
+
+/*
+ * r0 = &spinlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_spin_trylock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = p1;
+	r3 = [p1];
+	cc = bittst( r3, 0 );
+	if cc jump .Lfailed_trylock
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	bitset ( r3, 0 ); /* Raise the lock bit. */
+	[p1] = r3;
+	call _start_lock_coherent
+#else
+	r2 = 1;
+	[p1] = r2;
+	call _put_core_lock;
+#endif
+	r0 = 1;
+	rets = [sp++];
+	rts;
+.Lfailed_trylock:
+	call _put_core_lock;
+	r0 = 0;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_spin_trylock_asm)
+
+/*
+ * r0 = &spinlock->lock
+ *
+ * Clobbers: r2:0, p1:0
+ */
+ENTRY(___raw_spin_unlock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	bitclr ( r2, 0 );
+	[p1] = r2;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _end_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_spin_unlock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r2:0, p1:0
+ */
+ENTRY(___raw_read_lock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+.Lrdlock_try:
+	r1 = [p1];
+	r1 += -1;
+	[p1] = r1;
+	cc = r1 < 0;
+	if cc jump .Lrdlock_failed
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _start_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+
+.Lrdlock_failed:
+	r1 += 1;
+	[p1] = r1;
+.Lrdlock_wait:
+	r1 = p1;
+	call _put_core_lock;
+	SSYNC(r2);
+	r0 = p1;
+	call _get_core_lock;
+	r1 = [p1];
+	cc = r1 < 2;
+	if cc jump .Lrdlock_wait;
+	jump .Lrdlock_try
+ENDPROC(___raw_read_lock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_read_trylock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = [p1];
+	cc = r1 <= 0;
+	if cc jump .Lfailed_tryrdlock;
+	r1 += -1;
+	[p1] = r1;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _start_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	r0 = 1;
+	rts;
+.Lfailed_tryrdlock:
+	r1 = p1;
+	call _put_core_lock;
+	rets = [sp++];
+	r0 = 0;
+	rts;
+ENDPROC(___raw_read_trylock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Note: Processing controlled by a reader lock should not have
+ * any side-effect on cache issues with the other core, so we
+ * just release the core lock and exit (no _end_lock_coherent).
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_read_unlock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = [p1];
+	r1 += 1;
+	[p1] = r1;
+	r1 = p1;
+	call _put_core_lock;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_read_unlock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_write_lock_asm)
+	p1 = r0;
+	r3.l = lo(RW_LOCK_BIAS);
+	r3.h = hi(RW_LOCK_BIAS);
+	[--sp] = rets;
+	call _get_core_lock;
+.Lwrlock_try:
+	r1 = [p1];
+	r1 = r1 - r3;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	r2 = r1;
+	r2 <<= 4;
+	r2 >>= 4;
+	cc = r2 == 0;
+#else
+	cc = r1 == 0;
+#endif
+	if !cc jump .Lwrlock_wait
+	[p1] = r1;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _start_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+
+.Lwrlock_wait:
+	r1 = p1;
+	call _put_core_lock;
+	SSYNC(r2);
+	r0 = p1;
+	call _get_core_lock;
+	r1 = [p1];
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	r1 <<= 4;
+	r1 >>= 4;
+#endif
+	cc = r1 == r3;
+	if !cc jump .Lwrlock_wait;
+	jump .Lwrlock_try
+ENDPROC(___raw_write_lock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_write_trylock_asm)
+	p1 = r0;
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = [p1];
+	r2.l = lo(RW_LOCK_BIAS);
+	r2.h = hi(RW_LOCK_BIAS);
+	cc = r1 == r2;
+	if !cc jump .Lfailed_trywrlock;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	r1 >>= 28;
+	r1 <<= 28;
+#else
+	r1 = 0;
+#endif
+	[p1] = r1;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _start_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	r0 = 1;
+	rts;
+
+.Lfailed_trywrlock:
+	r1 = p1;
+	call _put_core_lock;
+	rets = [sp++];
+	r0 = 0;
+	rts;
+ENDPROC(___raw_write_trylock_asm)
+
+/*
+ * r0 = &rwlock->lock
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_write_unlock_asm)
+	p1 = r0;
+	r3.l = lo(RW_LOCK_BIAS);
+	r3.h = hi(RW_LOCK_BIAS);
+	[--sp] = rets;
+	call _get_core_lock;
+	r1 = [p1];
+	r1 = r1 + r3;
+	[p1] = r1;
+	r1 = p1;
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	call _end_lock_coherent
+#else
+	call _put_core_lock;
+#endif
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_write_unlock_asm)
+
+/*
+ * r0 = ptr
+ * r1 = value
+ *
+ * Add a signed value to a 32bit word and return the new value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_update_asm)
+	p1 = r0;
+	r3 = r1;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	r3 = r3 + r2;
+	[p1] = r3;
+	r1 = p1;
+	call _put_core_lock;
+	r0 = r3;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_atomic_update_asm)
+
+/*
+ * r0 = ptr
+ * r1 = mask
+ *
+ * Clear the mask bits from a 32bit word and return the old 32bit value
+ * atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_clear_asm)
+	p1 = r0;
+	r3 = ~r1;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	r3 = r2 & r3;
+	[p1] = r3;
+	r3 = r2;
+	r1 = p1;
+	call _put_core_lock;
+	r0 = r3;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_atomic_clear_asm)
+
+/*
+ * r0 = ptr
+ * r1 = mask
+ *
+ * Set the mask bits into a 32bit word and return the old 32bit value
+ * atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_set_asm)
+	p1 = r0;
+	r3 = r1;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	r3 = r2 | r3;
+	[p1] = r3;
+	r3 = r2;
+	r1 = p1;
+	call _put_core_lock;
+	r0 = r3;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_atomic_set_asm)
+
+/*
+ * r0 = ptr
+ * r1 = mask
+ *
+ * XOR the mask bits with a 32bit word and return the old 32bit value
+ * atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_xor_asm)
+	p1 = r0;
+	r3 = r1;
+	[--sp] = rets;
+	call _get_core_lock;
+	r2 = [p1];
+	r3 = r2 ^ r3;
+	[p1] = r3;
+	r3 = r2;
+	r1 = p1;
+	call _put_core_lock;
+	r0 = r3;
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_atomic_xor_asm)
+
+/*
+ * r0 = ptr
+ * r1 = mask
+ *
+ * Perform a logical AND between the mask bits and a 32bit word, and
+ * return the masked value. We need this on this architecture in
+ * order to invalidate the local cache before testing.
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_atomic_test_asm)
+	p1 = r0;
+	r3 = r1;
+	r1 = -L1_CACHE_BYTES;
+	r1 = r0 & r1;
+	p0 = r1;
+	flushinv[p0];
+	SSYNC(r2);
+	r0 = [p1];
+	r0 = r0 & r3;
+	rts;
+ENDPROC(___raw_atomic_test_asm)
+
+/*
+ * r0 = ptr
+ * r1 = value
+ *
+ * Swap *ptr with value and return the old 32bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+#define	__do_xchg(src, dst) 		\
+	p1 = r0;			\
+	r3 = r1;			\
+	[--sp] = rets;			\
+	call _get_core_lock;		\
+	r2 = src;			\
+	dst = r3;			\
+	r3 = r2;			\
+	r1 = p1;			\
+	call _put_core_lock;		\
+	r0 = r3;			\
+	rets = [sp++];			\
+	rts;
+
+ENTRY(___raw_xchg_1_asm)
+	__do_xchg(b[p1] (z), b[p1])
+ENDPROC(___raw_xchg_1_asm)
+
+ENTRY(___raw_xchg_2_asm)
+	__do_xchg(w[p1] (z), w[p1])
+ENDPROC(___raw_xchg_2_asm)
+
+ENTRY(___raw_xchg_4_asm)
+	__do_xchg([p1], [p1])
+ENDPROC(___raw_xchg_4_asm)
+
+/*
+ * r0 = ptr
+ * r1 = new
+ * r2 = old
+ *
+ * Swap *ptr with new if *ptr == old and return the previous *ptr
+ * value atomically.
+ *
+ * Clobbers: r3:0, p1:0
+ */
+#define	__do_cmpxchg(src, dst) 		\
+	[--sp] = rets;			\
+	[--sp] = r4;			\
+	p1 = r0;			\
+	r3 = r1;			\
+	r4 = r2;			\
+	call _get_core_lock;		\
+	r2 = src;			\
+	cc = r2 == r4;			\
+	if !cc jump 1f;			\
+	dst = r3;			\
+     1: r3 = r2;			\
+	r1 = p1;			\
+	call _put_core_lock;		\
+	r0 = r3;			\
+	r4 = [sp++];			\
+	rets = [sp++];			\
+	rts;
+
+ENTRY(___raw_cmpxchg_1_asm)
+	__do_cmpxchg(b[p1] (z), b[p1])
+ENDPROC(___raw_cmpxchg_1_asm)
+
+ENTRY(___raw_cmpxchg_2_asm)
+	__do_cmpxchg(w[p1] (z), w[p1])
+ENDPROC(___raw_cmpxchg_2_asm)
+
+ENTRY(___raw_cmpxchg_4_asm)
+	__do_cmpxchg([p1], [p1])
+ENDPROC(___raw_cmpxchg_4_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Set a bit in a 32bit word and return the old 32bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_set_asm)
+	r2 = r1;
+	r1 = 1;
+	r1 <<= r2;
+	jump ___raw_atomic_set_asm
+ENDPROC(___raw_bit_set_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Clear a bit in a 32bit word and return the old 32bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_clear_asm)
+	r2 = r1;
+	r1 = 1;
+	r1 <<= r2;
+	jump ___raw_atomic_clear_asm
+ENDPROC(___raw_bit_clear_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Toggle a bit in a 32bit word and return the old 32bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_toggle_asm)
+	r2 = r1;
+	r1 = 1;
+	r1 <<= r2;
+	jump ___raw_atomic_xor_asm
+ENDPROC(___raw_bit_toggle_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Test-and-set a bit in a 32bit word and return the old bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_test_set_asm)
+	[--sp] = rets;
+	[--sp] = r1;
+	call ___raw_bit_set_asm
+	r1 = [sp++];
+	r2 = 1;
+	r2 <<= r1;
+	r0 = r0 & r2;
+	cc = r0 == 0;
+	if cc jump 1f
+	r0 = 1;
+1:
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_bit_test_set_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Test-and-clear a bit in a 32bit word and return the old bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_test_clear_asm)
+	[--sp] = rets;
+	[--sp] = r1;
+	call ___raw_bit_clear_asm
+	r1 = [sp++];
+	r2 = 1;
+	r2 <<= r1;
+	r0 = r0 & r2;
+	cc = r0 == 0;
+	if cc jump 1f
+	r0 = 1;
+1:
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_bit_test_clear_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Test-and-toggle a bit in a 32bit word,
+ * and return the old bit value atomically.
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_test_toggle_asm)
+	[--sp] = rets;
+	[--sp] = r1;
+	call ___raw_bit_toggle_asm
+	r1 = [sp++];
+	r2 = 1;
+	r2 <<= r1;
+	r0 = r0 & r2;
+	cc = r0 == 0;
+	if cc jump 1f
+	r0 = 1;
+1:
+	rets = [sp++];
+	rts;
+ENDPROC(___raw_bit_test_toggle_asm)
+
+/*
+ * r0 = ptr
+ * r1 = bitnr
+ *
+ * Test a bit in a 32bit word and return its value.
+ * We need this on this architecture in order to invalidate
+ * the local cache before testing.
+ *
+ * Clobbers: r3:0, p1:0
+ */
+ENTRY(___raw_bit_test_asm)
+	r2 = r1;
+	r1 = 1;
+	r1 <<= r2;
+	jump ___raw_atomic_test_asm
+ENDPROC(___raw_bit_test_asm)
+
+/*
+ * r0 = ptr
+ *
+ * Fetch and return an uncached 32bit value.
+ *
+ * Clobbers: r2:0, p1:0
+ */
+ENTRY(___raw_uncached_fetch_asm)
+	p1 = r0;
+	r1 = -L1_CACHE_BYTES;
+	r1 = r0 & r1;
+	p0 = r1;
+	flushinv[p0];
+	SSYNC(r2);
+	r0 = [p1];
+	rts;
+ENDPROC(___raw_uncached_fetch_asm)
diff --git a/arch/blackfin/mach-bf561/boards/cm_bf561.c b/arch/blackfin/mach-bf561/boards/cm_bf561.c
index 8f40990eea2..6880d1ebfe6 100644
--- a/arch/blackfin/mach-bf561/boards/cm_bf561.c
+++ b/arch/blackfin/mach-bf561/boards/cm_bf561.c
@@ -230,6 +230,19 @@ static struct platform_device smc91x_device = {
 };
 #endif
 
+static struct resource bfin_gpios_resources = {
+	.start = 0,
+	.end   = MAX_BLACKFIN_GPIOS - 1,
+	.flags = IORESOURCE_IRQ,
+};
+
+static struct platform_device bfin_gpios_device = {
+	.name = "simple-gpio",
+	.id = -1,
+	.num_resources = 1,
+	.resource = &bfin_gpios_resources,
+};
+
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
 static struct resource isp1362_hcd_resources[] = {
 	{
@@ -287,23 +300,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_PATA_PLATFORM) || defined(CONFIG_PATA_PLATFORM_MODULE)
 #define PATA_INT	IRQ_PF46
@@ -382,7 +405,9 @@ static struct platform_device *cm_bf561_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
@@ -400,6 +425,8 @@ static struct platform_device *cm_bf561_devices[] __initdata = {
 #if defined(CONFIG_PATA_PLATFORM) || defined(CONFIG_PATA_PLATFORM_MODULE)
 	&bfin_pata_device,
 #endif
+
+	&bfin_gpios_device,
 };
 
 static int __init cm_bf561_init(void)
diff --git a/arch/blackfin/mach-bf561/boards/ezkit.c b/arch/blackfin/mach-bf561/boards/ezkit.c
index 50b4cdceccf..0e2178a1aec 100644
--- a/arch/blackfin/mach-bf561/boards/ezkit.c
+++ b/arch/blackfin/mach-bf561/boards/ezkit.c
@@ -43,53 +43,42 @@
 /*
  * Name the Board for the /proc/cpuinfo
  */
-const char bfin_board_name[] = "ADDS-BF561-EZKIT";
-
-#define ISP1761_BASE       0x2C0F0000
-#define ISP1761_IRQ        IRQ_PF10
+const char bfin_board_name[] = "ADI BF561-EZKIT";
 
 #if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
-static struct resource bfin_isp1761_resources[] = {
-	{
-		.name	= "isp1761-regs",
-		.start  = ISP1761_BASE + 0x00000000,
-		.end    = ISP1761_BASE + 0x000fffff,
+#include <linux/usb/isp1760.h>
+static struct resource bfin_isp1760_resources[] = {
+	[0] = {
+		.start  = 0x2C0F0000,
+		.end    = 0x203C0000 + 0xfffff,
 		.flags  = IORESOURCE_MEM,
 	},
-	{
-		.start  = ISP1761_IRQ,
-		.end    = ISP1761_IRQ,
+	[1] = {
+		.start  = IRQ_PF10,
+		.end    = IRQ_PF10,
 		.flags  = IORESOURCE_IRQ,
 	},
 };
 
-static struct platform_device bfin_isp1761_device = {
-	.name           = "isp1761",
-	.id             = 0,
-	.num_resources  = ARRAY_SIZE(bfin_isp1761_resources),
-	.resource       = bfin_isp1761_resources,
+static struct isp1760_platform_data isp1760_priv = {
+	.is_isp1761 = 0,
+	.port1_disable = 0,
+	.bus_width_16 = 1,
+	.port1_otg = 0,
+	.analog_oc = 0,
+	.dack_polarity_high = 0,
+	.dreq_polarity_high = 0,
 };
 
-static struct platform_device *bfin_isp1761_devices[] = {
-	&bfin_isp1761_device,
+static struct platform_device bfin_isp1760_device = {
+	.name           = "isp1760-hcd",
+	.id             = 0,
+	.dev = {
+		.platform_data = &isp1760_priv,
+	},
+	.num_resources  = ARRAY_SIZE(bfin_isp1760_resources),
+	.resource       = bfin_isp1760_resources,
 };
-
-int __init bfin_isp1761_init(void)
-{
-	unsigned int num_devices = ARRAY_SIZE(bfin_isp1761_devices);
-
-	printk(KERN_INFO "%s(): registering device resources\n", __func__);
-	set_irq_type(ISP1761_IRQ, IRQF_TRIGGER_FALLING);
-
-	return platform_add_devices(bfin_isp1761_devices, num_devices);
-}
-
-void __exit bfin_isp1761_exit(void)
-{
-	platform_device_unregister(&bfin_isp1761_device);
-}
-
-arch_initcall(bfin_isp1761_init);
 #endif
 
 #if defined(CONFIG_USB_ISP1362_HCD) || defined(CONFIG_USB_ISP1362_HCD_MODULE)
@@ -221,23 +210,33 @@ static struct platform_device bfin_uart_device = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-static struct resource bfin_sir_resources[] = {
 #ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
 	{
 		.start = 0xFFC00400,
 		.end = 0xFFC004FF,
 		.flags = IORESOURCE_MEM,
 	},
-#endif
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
 };
 
-static struct platform_device bfin_sir_device = {
+static struct platform_device bfin_sir0_device = {
 	.name = "bfin_sir",
 	.id = 0,
-	.num_resources = ARRAY_SIZE(bfin_sir_resources),
-	.resource = bfin_sir_resources,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
 };
 #endif
+#endif
 
 #if defined(CONFIG_MTD_PHYSMAP) || defined(CONFIG_MTD_PHYSMAP_MODULE)
 static struct mtd_partition ezkit_partitions[] = {
@@ -449,6 +448,10 @@ static struct platform_device *ezkit_devices[] __initdata = {
 	&net2272_bfin_device,
 #endif
 
+#if defined(CONFIG_USB_ISP1760_HCD) || defined(CONFIG_USB_ISP1760_HCD_MODULE)
+	&bfin_isp1760_device,
+#endif
+
 #if defined(CONFIG_SPI_BFIN) || defined(CONFIG_SPI_BFIN_MODULE)
 	&bfin_spi0_device,
 #endif
@@ -458,7 +461,9 @@ static struct platform_device *ezkit_devices[] __initdata = {
 #endif
 
 #if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
-	&bfin_sir_device,
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
 #endif
 
 #if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
diff --git a/arch/blackfin/mach-bf561/boards/generic_board.c b/arch/blackfin/mach-bf561/boards/generic_board.c
index 2faa0072d61..0ba366a0e69 100644
--- a/arch/blackfin/mach-bf561/boards/generic_board.c
+++ b/arch/blackfin/mach-bf561/boards/generic_board.c
@@ -62,10 +62,45 @@ static struct platform_device smc91x_device = {
 };
 #endif
 
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
+#endif
+#endif
+
 static struct platform_device *generic_board_devices[] __initdata = {
 #if defined(CONFIG_SMC91X) || defined(CONFIG_SMC91X_MODULE)
 	&smc91x_device,
 #endif
+
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#endif
 };
 
 static int __init generic_board_init(void)
diff --git a/arch/blackfin/mach-bf561/boards/tepla.c b/arch/blackfin/mach-bf561/boards/tepla.c
index c9174b39f98..6f77dbe952f 100644
--- a/arch/blackfin/mach-bf561/boards/tepla.c
+++ b/arch/blackfin/mach-bf561/boards/tepla.c
@@ -44,8 +44,42 @@ static struct platform_device smc91x_device = {
 	.resource      = smc91x_resources,
 };
 
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+static struct resource bfin_sir0_resources[] = {
+	{
+		.start = 0xFFC00400,
+		.end = 0xFFC004FF,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = IRQ_UART0_RX,
+		.end = IRQ_UART0_RX+1,
+		.flags = IORESOURCE_IRQ,
+	},
+	{
+		.start = CH_UART0_RX,
+		.end = CH_UART0_RX+1,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
+static struct platform_device bfin_sir0_device = {
+	.name = "bfin_sir",
+	.id = 0,
+	.num_resources = ARRAY_SIZE(bfin_sir0_resources),
+	.resource = bfin_sir0_resources,
+};
+#endif
+#endif
+
 static struct platform_device *tepla_devices[] __initdata = {
 	&smc91x_device,
+#if defined(CONFIG_BFIN_SIR) || defined(CONFIG_BFIN_SIR_MODULE)
+#ifdef CONFIG_BFIN_SIR0
+	&bfin_sir0_device,
+#endif
+#endif
 };
 
 static int __init tepla_init(void)
diff --git a/arch/blackfin/mach-bf561/dma.c b/arch/blackfin/mach-bf561/dma.c
index 24415eb8269..42b0037afe6 100644
--- a/arch/blackfin/mach-bf561/dma.c
+++ b/arch/blackfin/mach-bf561/dma.c
@@ -31,7 +31,7 @@
 #include <asm/blackfin.h>
 #include <asm/dma.h>
 
-struct dma_register *dma_io_base_addr[MAX_BLACKFIN_DMA_CHANNEL] = {
+struct dma_register *dma_io_base_addr[MAX_DMA_CHANNELS] = {
 	(struct dma_register *) DMA1_0_NEXT_DESC_PTR,
 	(struct dma_register *) DMA1_1_NEXT_DESC_PTR,
 	(struct dma_register *) DMA1_2_NEXT_DESC_PTR,
diff --git a/arch/blackfin/mach-bf561/head.S b/arch/blackfin/mach-bf561/head.S
deleted file mode 100644
index 31a777a9e69..00000000000
--- a/arch/blackfin/mach-bf561/head.S
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * File:         arch/blackfin/mach-bf561/head.S
- * Based on:     arch/blackfin/mach-bf533/head.S
- * Author:
- *
- * Created:
- * Description:  BF561 startup file
- *
- * Modified:
- *               Copyright 2004-2006 Analog Devices Inc.
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, see the file COPYING, or write
- * to the Free Software Foundation, Inc.,
- * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
-
-#include <linux/linkage.h>
-#include <linux/init.h>
-#include <asm/blackfin.h>
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-#include <asm/clocks.h>
-#include <mach/mem_init.h>
-#endif
-
-.section .l1.text
-#ifdef CONFIG_BFIN_KERNEL_CLOCK
-ENTRY(_start_dma_code)
-	p0.h = hi(SICA_IWR0);
-	p0.l = lo(SICA_IWR0);
-	r0.l = 0x1;
-	[p0] = r0;
-	SSYNC;
-
-	/*
-	 *  Set PLL_CTL
-	 *   - [14:09] = MSEL[5:0] : CLKIN / VCO multiplication factors
-	 *   - [8]     = BYPASS    : BYPASS the PLL, run CLKIN into CCLK/SCLK
-	 *   - [7]     = output delay (add 200ps of delay to mem signals)
-	 *   - [6]     = input delay (add 200ps of input delay to mem signals)
-	 *   - [5]     = PDWN      : 1=All Clocks off
-	 *   - [3]     = STOPCK    : 1=Core Clock off
-	 *   - [1]     = PLL_OFF   : 1=Disable Power to PLL
-	 *   - [0]     = DF        : 1=Pass CLKIN/2 to PLL / 0=Pass CLKIN to PLL
-	 *   all other bits set to zero
-	 */
-
-	p0.h = hi(PLL_LOCKCNT);
-	p0.l = lo(PLL_LOCKCNT);
-	r0 = 0x300(Z);
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITSET (R0, 24);
-	[P2] = R0;
-	SSYNC;
-
-	r0 = CONFIG_VCO_MULT & 63;       /* Load the VCO multiplier         */
-	r0 = r0 << 9;                    /* Shift it over,                  */
-	r1 = CLKIN_HALF;                 /* Do we need to divide CLKIN by 2?*/
-	r0 = r1 | r0;
-	r1 = PLL_BYPASS;                 /* Bypass the PLL?                 */
-	r1 = r1 << 8;                    /* Shift it over                   */
-	r0 = r1 | r0;                    /* add them all together           */
-#ifdef ANOMALY_05000265
-	BITSET(r0, 15);                  /* Add 250 mV of hysteresis to SPORT input pins */
-#endif
-
-	p0.h = hi(PLL_CTL);
-	p0.l = lo(PLL_CTL);              /* Load the address                */
-	cli r2;                          /* Disable interrupts              */
-	ssync;
-	w[p0] = r0.l;                    /* Set the value                   */
-	idle;                            /* Wait for the PLL to stablize    */
-	sti r2;                          /* Enable interrupts               */
-
-.Lcheck_again:
-	p0.h = hi(PLL_STAT);
-	p0.l = lo(PLL_STAT);
-	R0 = W[P0](Z);
-	CC = BITTST(R0,5);
-	if ! CC jump .Lcheck_again;
-
-	/* Configure SCLK & CCLK Dividers */
-	r0 = (CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
-	p0.h = hi(PLL_DIV);
-	p0.l = lo(PLL_DIV);
-	w[p0] = r0.l;
-	ssync;
-
-	p0.l = lo(EBIU_SDRRC);
-	p0.h = hi(EBIU_SDRRC);
-	r0 = mem_SDRRC;
-	w[p0] = r0.l;
-	ssync;
-
-	P2.H = hi(EBIU_SDGCTL);
-	P2.L = lo(EBIU_SDGCTL);
-	R0 = [P2];
-	BITCLR (R0, 24);
-	p0.h = hi(EBIU_SDSTAT);
-	p0.l = lo(EBIU_SDSTAT);
-	r2.l = w[p0];
-	cc = bittst(r2,3);
-	if !cc jump .Lskip;
-	NOP;
-	BITSET (R0, 23);
-.Lskip:
-	[P2] = R0;
-	SSYNC;
-
-	R0.L = lo(mem_SDGCTL);
-	R0.H = hi(mem_SDGCTL);
-	R1 = [p2];
-	R1 = R1 | R0;
-	[P2] = R1;
-	SSYNC;
-
-	RTS;
-ENDPROC(_start_dma_code)
-#endif /* CONFIG_BFIN_KERNEL_CLOCK */
diff --git a/arch/blackfin/mach-bf561/include/mach/anomaly.h b/arch/blackfin/mach-bf561/include/mach/anomaly.h
index 22990df04ae..1a9e1756282 100644
--- a/arch/blackfin/mach-bf561/include/mach/anomaly.h
+++ b/arch/blackfin/mach-bf561/include/mach/anomaly.h
@@ -7,7 +7,7 @@
  */
 
 /* This file shoule be up to date with:
- *  - Revision P, 02/08/2008; ADSP-BF561 Blackfin Processor Anomaly List
+ *  - Revision Q, 11/07/2008; ADSP-BF561 Blackfin Processor Anomaly List
  */
 
 #ifndef _MACH_ANOMALY_H_
@@ -264,6 +264,18 @@
 #define ANOMALY_05000371 (1)
 /* Level-Sensitive External GPIO Wakeups May Cause Indefinite Stall */
 #define ANOMALY_05000403 (1)
+/* TESTSET Instruction Causes Data Corruption with Writeback Data Cache Enabled */
+#define ANOMALY_05000412 (1)
+/* Speculative Fetches Can Cause Undesired External FIFO Operations */
+#define ANOMALY_05000416 (1)
+/* Multichannel SPORT Channel Misalignment Under Specific Configuration */
+#define ANOMALY_05000425 (1)
+/* Speculative Fetches of Indirect-Pointer Instructions Can Cause False Hardware Errors */
+#define ANOMALY_05000426 (1)
+/* Lost/Corrupted L2/L3 Memory Write after Speculative L2 Memory Read by Core B */
+#define ANOMALY_05000428 (__SILICON_REVISION__ > 3)
+/* IFLUSH Instruction at End of Hardware Loop Causes Infinite Stall */
+#define ANOMALY_05000443 (1)
 
 /* Anomalies that don't exist on this proc */
 #define ANOMALY_05000158 (0)
@@ -272,5 +284,7 @@
 #define ANOMALY_05000311 (0)
 #define ANOMALY_05000353 (1)
 #define ANOMALY_05000386 (1)
+#define ANOMALY_05000432 (0)
+#define ANOMALY_05000435 (0)
 
 #endif
diff --git a/arch/blackfin/mach-bf561/include/mach/bf561.h b/arch/blackfin/mach-bf561/include/mach/bf561.h
index 18b1b3a223a..9968362a2ee 100644
--- a/arch/blackfin/mach-bf561/include/mach/bf561.h
+++ b/arch/blackfin/mach-bf561/include/mach/bf561.h
@@ -215,7 +215,7 @@
 #endif
 
 #ifndef CPU
-#error Unknown CPU type - This kernel doesn't seem to be configured properly
+#error "Unknown CPU type - This kernel doesn't seem to be configured properly"
 #endif
 
 #endif				/* __MACH_BF561_H__  */
diff --git a/arch/blackfin/mach-bf561/include/mach/bfin_sir.h b/arch/blackfin/mach-bf561/include/mach/bfin_sir.h
deleted file mode 100644
index 9bb87e9e2e9..00000000000
--- a/arch/blackfin/mach-bf561/include/mach/bfin_sir.h
+++ /dev/null
@@ -1,125 +0,0 @@
-/*
- * Blackfin Infra-red Driver
- *
- * Copyright 2006-2008 Analog Devices Inc.
- *
- * Enter bugs at http://blackfin.uclinux.org/
- *
- * Licensed under the GPL-2 or later.
- *
- */
-
-#include <linux/serial.h>
-#include <asm/dma.h>
-#include <asm/portmux.h>
-
-#define SIR_UART_GET_CHAR(port)   bfin_read16((port)->membase + OFFSET_RBR)
-#define SIR_UART_GET_DLL(port)    bfin_read16((port)->membase + OFFSET_DLL)
-#define SIR_UART_GET_IER(port)    bfin_read16((port)->membase + OFFSET_IER)
-#define SIR_UART_GET_DLH(port)    bfin_read16((port)->membase + OFFSET_DLH)
-#define SIR_UART_GET_IIR(port)    bfin_read16((port)->membase + OFFSET_IIR)
-#define SIR_UART_GET_LCR(port)    bfin_read16((port)->membase + OFFSET_LCR)
-#define SIR_UART_GET_GCTL(port)   bfin_read16((port)->membase + OFFSET_GCTL)
-
-#define SIR_UART_PUT_CHAR(port, v) bfin_write16(((port)->membase + OFFSET_THR), v)
-#define SIR_UART_PUT_DLL(port, v)  bfin_write16(((port)->membase + OFFSET_DLL), v)
-#define SIR_UART_PUT_IER(port, v)  bfin_write16(((port)->membase + OFFSET_IER), v)
-#define SIR_UART_PUT_DLH(port, v)  bfin_write16(((port)->membase + OFFSET_DLH), v)
-#define SIR_UART_PUT_LCR(port, v)  bfin_write16(((port)->membase + OFFSET_LCR), v)
-#define SIR_UART_PUT_GCTL(port, v) bfin_write16(((port)->membase + OFFSET_GCTL), v)
-
-#ifdef CONFIG_SIR_BFIN_DMA
-struct dma_rx_buf {
-	char *buf;
-	int head;
-	int tail;
-	};
-#endif /* CONFIG_SIR_BFIN_DMA */
-
-struct bfin_sir_port {
-	unsigned char __iomem   *membase;
-	unsigned int            irq;
-	unsigned int            lsr;
-	unsigned long           clk;
-	struct net_device       *dev;
-#ifdef CONFIG_SIR_BFIN_DMA
-	int                     tx_done;
-	struct dma_rx_buf       rx_dma_buf;
-	struct timer_list       rx_dma_timer;
-	int                     rx_dma_nrows;
-#endif /* CONFIG_SIR_BFIN_DMA */
-	unsigned int            tx_dma_channel;
-	unsigned int            rx_dma_channel;
-};
-
-struct bfin_sir_port sir_ports[BFIN_UART_NR_PORTS];
-
-struct bfin_sir_port_res {
-	unsigned long   base_addr;
-	int             irq;
-	unsigned int    rx_dma_channel;
-	unsigned int    tx_dma_channel;
-};
-
-struct bfin_sir_port_res bfin_sir_port_resource[] = {
-#ifdef CONFIG_BFIN_SIR0
-	{
-	0xFFC00400,
-	IRQ_UART_RX,
-	CH_UART_RX,
-	CH_UART_TX,
-	},
-#endif
-};
-
-int nr_sirs = ARRAY_SIZE(bfin_sir_port_resource);
-
-struct bfin_sir_self {
-	struct bfin_sir_port    *sir_port;
-	spinlock_t              lock;
-	unsigned int            open;
-	int                     speed;
-	int                     newspeed;
-
-	struct sk_buff          *txskb;
-	struct sk_buff          *rxskb;
-	struct net_device_stats stats;
-	struct device           *dev;
-	struct irlap_cb         *irlap;
-	struct qos_info         qos;
-
-	iobuff_t                tx_buff;
-	iobuff_t                rx_buff;
-
-	struct work_struct      work;
-	int                     mtt;
-};
-
-static inline unsigned int SIR_UART_GET_LSR(struct bfin_sir_port *port)
-{
-	unsigned int lsr = bfin_read16(port->membase + OFFSET_LSR);
-	port->lsr |= (lsr & (BI|FE|PE|OE));
-	return lsr | port->lsr;
-}
-
-static inline void SIR_UART_CLEAR_LSR(struct bfin_sir_port *port)
-{
-	port->lsr = 0;
-	bfin_read16(port->membase + OFFSET_LSR);
-}
-
-#define DRIVER_NAME "bfin_sir"
-
-static int bfin_sir_hw_init(void)
-{
-	int ret = -ENODEV;
-#ifdef CONFIG_BFIN_SIR0
-	ret = peripheral_request(P_UART0_TX, DRIVER_NAME);
-	if (ret)
-		return ret;
-	ret = peripheral_request(P_UART0_RX, DRIVER_NAME);
-	if (ret)
-		return ret;
-#endif
-	return ret;
-}
diff --git a/arch/blackfin/mach-bf561/include/mach/blackfin.h b/arch/blackfin/mach-bf561/include/mach/blackfin.h
index 0ea8666e676..f79f6626b7e 100644
--- a/arch/blackfin/mach-bf561/include/mach/blackfin.h
+++ b/arch/blackfin/mach-bf561/include/mach/blackfin.h
@@ -66,8 +66,12 @@
 
 #define bfin_read_SIC_IMASK(x)		bfin_read32(SICA_IMASK0 + (x << 2))
 #define bfin_write_SIC_IMASK(x, val)	bfin_write32((SICA_IMASK0 + (x << 2)), val)
+#define bfin_read_SICB_IMASK(x)		bfin_read32(SICB_IMASK0 + (x << 2))
+#define bfin_write_SICB_IMASK(x, val)	bfin_write32((SICB_IMASK0 + (x << 2)), val)
 #define bfin_read_SIC_ISR(x)		bfin_read32(SICA_ISR0 + (x << 2))
 #define bfin_write_SIC_ISR(x, val)	bfin_write32((SICA_ISR0 + (x << 2)), val)
+#define bfin_read_SICB_ISR(x)		bfin_read32(SICB_ISR0 + (x << 2))
+#define bfin_write_SICB_ISR(x, val)	bfin_write32((SICB_ISR0 + (x << 2)), val)
 
 #define BFIN_UART_NR_PORTS      1
 
diff --git a/arch/blackfin/mach-bf561/include/mach/cdefBF561.h b/arch/blackfin/mach-bf561/include/mach/cdefBF561.h
index c14d63402e7..95d609f11c9 100644
--- a/arch/blackfin/mach-bf561/include/mach/cdefBF561.h
+++ b/arch/blackfin/mach-bf561/include/mach/cdefBF561.h
@@ -39,65 +39,15 @@
 /*include core specific register pointer definitions*/
 #include <asm/cdef_LPBlackfin.h>
 
-#include <asm/system.h>
-
 /*********************************************************************************** */
 /* System MMR Register Map */
 /*********************************************************************************** */
 
 /* Clock and System Control (0xFFC00000 - 0xFFC000FF) */
 #define bfin_read_PLL_CTL()                  bfin_read16(PLL_CTL)
-/* Writing to PLL_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_PLL_CTL(unsigned int val)
-{
-	unsigned long flags, iwr0, iwr1;
-
-	if (val == bfin_read_PLL_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr0 = bfin_read32(SICA_IWR0);
-	iwr1 = bfin_read32(SICA_IWR1);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SICA_IWR0, IWR_ENABLE(0));
-	bfin_write32(SICA_IWR1, 0);
-
-	bfin_write16(PLL_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SICA_IWR0, iwr0);
-	bfin_write32(SICA_IWR1, iwr1);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_DIV()                  bfin_read16(PLL_DIV)
 #define bfin_write_PLL_DIV(val)              bfin_write16(PLL_DIV,val)
 #define bfin_read_VR_CTL()                   bfin_read16(VR_CTL)
-/* Writing to VR_CTL initiates a PLL relock sequence. */
-static __inline__ void bfin_write_VR_CTL(unsigned int val)
-{
-	unsigned long flags, iwr0, iwr1;
-
-	if (val == bfin_read_VR_CTL())
-		return;
-
-	local_irq_save(flags);
-	/* Enable the PLL Wakeup bit in SIC IWR */
-	iwr0 = bfin_read32(SICA_IWR0);
-	iwr1 = bfin_read32(SICA_IWR1);
-	/* Only allow PPL Wakeup) */
-	bfin_write32(SICA_IWR0, IWR_ENABLE(0));
-	bfin_write32(SICA_IWR1, 0);
-
-	bfin_write16(VR_CTL, val);
-	SSYNC();
-	asm("IDLE;");
-
-	bfin_write32(SICA_IWR0, iwr0);
-	bfin_write32(SICA_IWR1, iwr1);
-	local_irq_restore(flags);
-}
 #define bfin_read_PLL_STAT()                 bfin_read16(PLL_STAT)
 #define bfin_write_PLL_STAT(val)             bfin_write16(PLL_STAT,val)
 #define bfin_read_PLL_LOCKCNT()              bfin_read16(PLL_LOCKCNT)
@@ -1576,4 +1526,57 @@ static __inline__ void bfin_write_VR_CTL(unsigned int val)
 #define bfin_read_MDMA_D0_START_ADDR()  bfin_read_MDMA1_D0_START_ADDR()
 #define bfin_write_MDMA_D0_START_ADDR(val) bfin_write_MDMA1_D0_START_ADDR(val)
 
+/* These need to be last due to the cdef/linux inter-dependencies */
+#include <asm/irq.h>
+
+/* Writing to PLL_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_PLL_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_PLL_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SICA_IWR0);
+	iwr1 = bfin_read32(SICA_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SICA_IWR0, IWR_ENABLE(0));
+	bfin_write32(SICA_IWR1, 0);
+
+	bfin_write16(PLL_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SICA_IWR0, iwr0);
+	bfin_write32(SICA_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
+/* Writing to VR_CTL initiates a PLL relock sequence. */
+static __inline__ void bfin_write_VR_CTL(unsigned int val)
+{
+	unsigned long flags, iwr0, iwr1;
+
+	if (val == bfin_read_VR_CTL())
+		return;
+
+	local_irq_save_hw(flags);
+	/* Enable the PLL Wakeup bit in SIC IWR */
+	iwr0 = bfin_read32(SICA_IWR0);
+	iwr1 = bfin_read32(SICA_IWR1);
+	/* Only allow PPL Wakeup) */
+	bfin_write32(SICA_IWR0, IWR_ENABLE(0));
+	bfin_write32(SICA_IWR1, 0);
+
+	bfin_write16(VR_CTL, val);
+	SSYNC();
+	asm("IDLE;");
+
+	bfin_write32(SICA_IWR0, iwr0);
+	bfin_write32(SICA_IWR1, iwr1);
+	local_irq_restore_hw(flags);
+}
+
 #endif				/* _CDEF_BF561_H */
diff --git a/arch/blackfin/mach-bf561/include/mach/defBF561.h b/arch/blackfin/mach-bf561/include/mach/defBF561.h
index 4eca2026bb9..d7c50975965 100644
--- a/arch/blackfin/mach-bf561/include/mach/defBF561.h
+++ b/arch/blackfin/mach-bf561/include/mach/defBF561.h
@@ -912,6 +912,9 @@
 #define ACTIVE_PLLDISABLED	0x0004	/* Processor In Active Mode With PLL Disabled   */
 #define	PLL_LOCKED			0x0020	/* PLL_LOCKCNT Has Been Reached                                 */
 
+/* SICA_SYSCR Masks */
+#define COREB_SRAM_INIT		0x0020
+
 /* SWRST Mask */
 #define SYSTEM_RESET           0x0007	/* Initiates a system software reset */
 #define DOUBLE_FAULT_A         0x0008	/* Core A Double Fault Causes Reset */
diff --git a/arch/blackfin/mach-bf561/include/mach/dma.h b/arch/blackfin/mach-bf561/include/mach/dma.h
index 8bc46cd89a0..13647c71f1c 100644
--- a/arch/blackfin/mach-bf561/include/mach/dma.h
+++ b/arch/blackfin/mach-bf561/include/mach/dma.h
@@ -1,13 +1,17 @@
-/*****************************************************************************
-*
-*        BF-533/2/1 Specific Declarations
-*
-****************************************************************************/
+/* mach/dma.h - arch-specific DMA defines
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
 
 #ifndef _MACH_DMA_H_
 #define _MACH_DMA_H_
 
-#define MAX_BLACKFIN_DMA_CHANNEL 36
+#define MAX_DMA_CHANNELS 36
+
+/* [#4267] IMDMA channels have no PERIPHERAL_MAP MMR */
+#define MAX_DMA_SUSPEND_CHANNELS 32
 
 #define CH_PPI0			0
 #define CH_PPI			(CH_PPI0)
diff --git a/arch/blackfin/mach-bf561/include/mach/gpio.h b/arch/blackfin/mach-bf561/include/mach/gpio.h
new file mode 100644
index 00000000000..7882f79e1ad
--- /dev/null
+++ b/arch/blackfin/mach-bf561/include/mach/gpio.h
@@ -0,0 +1,68 @@
+/*
+ * File: arch/blackfin/mach-bf561/include/mach/gpio.h
+ * Bugs: Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Copyright (C) 2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+
+#ifndef _MACH_GPIO_H_
+#define _MACH_GPIO_H_
+
+#define MAX_BLACKFIN_GPIOS 48
+
+#define	GPIO_PF0	0
+#define	GPIO_PF1	1
+#define	GPIO_PF2	2
+#define	GPIO_PF3	3
+#define	GPIO_PF4	4
+#define	GPIO_PF5	5
+#define	GPIO_PF6	6
+#define	GPIO_PF7	7
+#define	GPIO_PF8	8
+#define	GPIO_PF9	9
+#define	GPIO_PF10	10
+#define	GPIO_PF11	11
+#define	GPIO_PF12	12
+#define	GPIO_PF13	13
+#define	GPIO_PF14	14
+#define	GPIO_PF15	15
+#define	GPIO_PF16	16
+#define	GPIO_PF17	17
+#define	GPIO_PF18	18
+#define	GPIO_PF19	19
+#define	GPIO_PF20	20
+#define	GPIO_PF21	21
+#define	GPIO_PF22	22
+#define	GPIO_PF23	23
+#define	GPIO_PF24	24
+#define	GPIO_PF25	25
+#define	GPIO_PF26	26
+#define	GPIO_PF27	27
+#define	GPIO_PF28	28
+#define	GPIO_PF29	29
+#define	GPIO_PF30	30
+#define	GPIO_PF31	31
+#define	GPIO_PF32	32
+#define	GPIO_PF33	33
+#define	GPIO_PF34	34
+#define	GPIO_PF35	35
+#define	GPIO_PF36	36
+#define	GPIO_PF37	37
+#define	GPIO_PF38	38
+#define	GPIO_PF39	39
+#define	GPIO_PF40	40
+#define	GPIO_PF41	41
+#define	GPIO_PF42	42
+#define	GPIO_PF43	43
+#define	GPIO_PF44	44
+#define	GPIO_PF45	45
+#define	GPIO_PF46	46
+#define	GPIO_PF47	47
+
+#define PORT_FIO0 GPIO_0
+#define PORT_FIO1 GPIO_16
+#define PORT_FIO2 GPIO_32
+
+#endif /* _MACH_GPIO_H_ */
diff --git a/arch/blackfin/mach-bf561/include/mach/mem_init.h b/arch/blackfin/mach-bf561/include/mach/mem_init.h
deleted file mode 100644
index e163260bca1..00000000000
--- a/arch/blackfin/mach-bf561/include/mach/mem_init.h
+++ /dev/null
@@ -1,295 +0,0 @@
-/*
- * File:         include/asm-blackfin/mach-bf561/mem_init.h
- * Based on:
- * Author:
- *
- * Created:
- * Description:
- *
- * Rev:
- *
- * Modified:
- *
- * Bugs:         Enter bugs at http://blackfin.uclinux.org/
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; see the file COPYING.
- * If not, write to the Free Software Foundation,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#if (CONFIG_MEM_MT48LC16M16A2TG_75 || CONFIG_MEM_MT48LC64M4A2FB_7E || CONFIG_MEM_GENERIC_BOARD || CONFIG_MEM_MT48LC8M32B2B5_7)
-#if (CONFIG_SCLK_HZ > 119402985)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_7
-#define SDRAM_tRAS_num  7
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 104477612) && (CONFIG_SCLK_HZ <= 119402985)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_6
-#define SDRAM_tRAS_num  6
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 89552239) && (CONFIG_SCLK_HZ <= 104477612)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_5
-#define SDRAM_tRAS_num  5
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 74626866) && (CONFIG_SCLK_HZ <= 89552239)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_4
-#define SDRAM_tRAS_num  4
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 66666667) && (CONFIG_SCLK_HZ <= 74626866)
-#define SDRAM_tRP       TRP_2
-#define SDRAM_tRP_num   2
-#define SDRAM_tRAS      TRAS_3
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_2
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 59701493) && (CONFIG_SCLK_HZ <= 66666667)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_4
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 44776119) && (CONFIG_SCLK_HZ <= 59701493)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_3
-#define SDRAM_tRAS_num  3
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ > 29850746) && (CONFIG_SCLK_HZ <= 44776119)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_2
-#define SDRAM_tRAS_num  2
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#if (CONFIG_SCLK_HZ <= 29850746)
-#define SDRAM_tRP       TRP_1
-#define SDRAM_tRP_num   1
-#define SDRAM_tRAS      TRAS_1
-#define SDRAM_tRAS_num  1
-#define SDRAM_tRCD      TRCD_1
-#define SDRAM_tWR       TWR_2
-#endif
-#endif
-
-#if (CONFIG_MEM_MT48LC16M16A2TG_75)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC64M4A2FB_7E)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_MT48LC8M32B2B5_7)
-  /*SDRAM INFORMATION: */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   4096	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-#if (CONFIG_MEM_GENERIC_BOARD)
-  /*SDRAM INFORMATION: Modify this for your board */
-#define SDRAM_Tref  64		/* Refresh period in milliseconds   */
-#define SDRAM_NRA   8192	/* Number of row addresses in SDRAM */
-#define SDRAM_CL    CL_3
-#endif
-
-/* Equation from section 17 (p17-46) of BF533 HRM */
-#define mem_SDRRC       (((CONFIG_SCLK_HZ / 1000) * SDRAM_Tref) / SDRAM_NRA) - (SDRAM_tRAS_num + SDRAM_tRP_num)
-
-/* Enable SCLK Out */
-#define mem_SDGCTL        (SCTLE | SDRAM_CL | SDRAM_tRAS | SDRAM_tRP | SDRAM_tRCD | SDRAM_tWR | PSS)
-
-#if defined CONFIG_CLKIN_HALF
-#define CLKIN_HALF       1
-#else
-#define CLKIN_HALF       0
-#endif
-
-#if defined CONFIG_PLL_BYPASS
-#define PLL_BYPASS      1
-#else
-#define PLL_BYPASS       0
-#endif
-
-/***************************************Currently Not Being Used *********************************/
-#define flash_EBIU_AMBCTL_WAT  ((CONFIG_FLASH_SPEED_BWAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_RAT  ((CONFIG_FLASH_SPEED_BRAT * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_HT   ((CONFIG_FLASH_SPEED_BHT  * 4) / (4000000000 / CONFIG_SCLK_HZ))
-#define flash_EBIU_AMBCTL_ST   ((CONFIG_FLASH_SPEED_BST  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-#define flash_EBIU_AMBCTL_TT   ((CONFIG_FLASH_SPEED_BTT  * 4) / (4000000000 / CONFIG_SCLK_HZ)) + 1
-
-#if (flash_EBIU_AMBCTL_TT > 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_4
-#endif
-#if (flash_EBIU_AMBCTL_TT == 3)
-#define flash_EBIU_AMBCTL0_TT   B0TT_3
-#endif
-#if (flash_EBIU_AMBCTL_TT == 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_2
-#endif
-#if (flash_EBIU_AMBCTL_TT < 2)
-#define flash_EBIU_AMBCTL0_TT   B0TT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_ST > 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_4
-#endif
-#if (flash_EBIU_AMBCTL_ST == 3)
-#define flash_EBIU_AMBCTL0_ST   B0ST_3
-#endif
-#if (flash_EBIU_AMBCTL_ST == 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_2
-#endif
-#if (flash_EBIU_AMBCTL_ST < 2)
-#define flash_EBIU_AMBCTL0_ST   B0ST_1
-#endif
-
-#if (flash_EBIU_AMBCTL_HT > 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_3
-#endif
-#if (flash_EBIU_AMBCTL_HT == 2)
-#define flash_EBIU_AMBCTL0_HT   B0HT_2
-#endif
-#if (flash_EBIU_AMBCTL_HT == 1)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT == 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_0
-#endif
-#if (flash_EBIU_AMBCTL_HT == 0 && CONFIG_FLASH_SPEED_BHT != 0)
-#define flash_EBIU_AMBCTL0_HT   B0HT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_WAT > 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_15
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 14)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_14
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 13)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_13
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 12)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_12
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 11)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_11
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 10)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_10
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 9)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_9
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 8)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_8
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 7)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_7
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 6)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_6
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 5)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_5
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 4)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_4
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 3)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_3
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 2)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_2
-#endif
-#if (flash_EBIU_AMBCTL_WAT == 1)
-#define flash_EBIU_AMBCTL0_WAT  B0WAT_1
-#endif
-
-#if (flash_EBIU_AMBCTL_RAT > 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_15
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 14)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_14
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 13)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_13
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 12)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_12
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 11)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_11
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 10)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_10
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 9)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_9
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 8)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_8
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 7)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_7
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 6)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_6
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 5)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_5
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 4)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_4
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 3)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_3
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 2)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_2
-#endif
-#if (flash_EBIU_AMBCTL_RAT == 1)
-#define flash_EBIU_AMBCTL0_RAT  B0RAT_1
-#endif
-
-#define flash_EBIU_AMBCTL0  \
-	(flash_EBIU_AMBCTL0_WAT | flash_EBIU_AMBCTL0_RAT | flash_EBIU_AMBCTL0_HT | \
-	 flash_EBIU_AMBCTL0_ST | flash_EBIU_AMBCTL0_TT | CONFIG_FLASH_SPEED_RDYEN)
diff --git a/arch/blackfin/mach-bf561/include/mach/mem_map.h b/arch/blackfin/mach-bf561/include/mach/mem_map.h
index f1d4c0637bd..419dffdc96e 100644
--- a/arch/blackfin/mach-bf561/include/mach/mem_map.h
+++ b/arch/blackfin/mach-bf561/include/mach/mem_map.h
@@ -85,4 +85,84 @@
 #define L1_SCRATCH_START	COREA_L1_SCRATCH_START
 #define L1_SCRATCH_LENGTH	0x1000
 
+#ifdef __ASSEMBLY__
+
+/*
+ * The following macros both return the address of the PDA for the
+ * current core.
+ *
+ * In its first safe (and hairy) form, the macro neither clobbers any
+ * register aside of the output Preg, nor uses the stack, since it
+ * could be called with an invalid stack pointer, or the current stack
+ * space being uncovered by any CPLB (e.g. early exception handling).
+ *
+ * The constraints on the second form are a bit relaxed, and the code
+ * is allowed to use the specified Dreg for determining the PDA
+ * address to be returned into Preg.
+ */
+#ifdef CONFIG_SMP
+#define GET_PDA_SAFE(preg)		\
+	preg.l = lo(DSPID);		\
+	preg.h = hi(DSPID);		\
+	preg = [preg];			\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	preg = preg << 2;		\
+	if cc jump 2f;			\
+	cc = preg == 0x0;		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;		\
+	if !cc jump 3f;			\
+1:					\
+	/* preg = 0x0; */		\
+	cc = !cc; /* restore cc to 0 */	\
+	jump 4f;			\
+2:					\
+	cc = preg == 0x0;		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;		\
+	if cc jump 4f;			\
+	/* preg = 0x1000000; */		\
+	cc = !cc; /* restore cc to 1 */	\
+3:					\
+	preg = [preg];			\
+4:
+
+#define GET_PDA(preg, dreg)		\
+	preg.l = lo(DSPID);		\
+	preg.h = hi(DSPID);		\
+	dreg = [preg];			\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;		\
+	cc = bittst(dreg, 0);		\
+	if !cc jump 1f;			\
+	preg = [preg];			\
+1:					\
+
+#define GET_CPUID(preg, dreg)		\
+	preg.l = lo(DSPID);		\
+	preg.h = hi(DSPID);		\
+	dreg = [preg];			\
+	dreg = ROT dreg BY -1;		\
+	dreg = CC;
+
+#else
+#define GET_PDA_SAFE(preg)		\
+	preg.l = _cpu_pda;		\
+	preg.h = _cpu_pda;
+
+#define GET_PDA(preg, dreg)	GET_PDA_SAFE(preg)
+#endif /* CONFIG_SMP */
+
+#endif /* __ASSEMBLY__ */
+
 #endif				/* _MEM_MAP_533_H_ */
diff --git a/arch/blackfin/mach-bf561/include/mach/smp.h b/arch/blackfin/mach-bf561/include/mach/smp.h
new file mode 100644
index 00000000000..f9e65ebe81b
--- /dev/null
+++ b/arch/blackfin/mach-bf561/include/mach/smp.h
@@ -0,0 +1,22 @@
+#ifndef _MACH_BF561_SMP
+#define _MACH_BF561_SMP
+
+struct task_struct;
+
+void platform_init_cpus(void);
+
+void platform_prepare_cpus(unsigned int max_cpus);
+
+int platform_boot_secondary(unsigned int cpu, struct task_struct *idle);
+
+void platform_secondary_init(unsigned int cpu);
+
+void platform_request_ipi(int (*handler)(int, void *));
+
+void platform_send_ipi(cpumask_t callmap);
+
+void platform_send_ipi_cpu(unsigned int cpu);
+
+void platform_clear_ipi(unsigned int cpu);
+
+#endif /* !_MACH_BF561_SMP */
diff --git a/arch/blackfin/mach-bf561/secondary.S b/arch/blackfin/mach-bf561/secondary.S
new file mode 100644
index 00000000000..35280f06b7b
--- /dev/null
+++ b/arch/blackfin/mach-bf561/secondary.S
@@ -0,0 +1,215 @@
+/*
+ * File:         arch/blackfin/mach-bf561/secondary.S
+ * Based on:     arch/blackfin/mach-bf561/head.S
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 Analog Devices Inc.
+ *
+ * Description:  BF561 coreB bootstrap file
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/blackfin.h>
+#include <asm/asm-offsets.h>
+
+__INIT
+
+/* Lay the initial stack into the L1 scratch area of Core B */
+#define INITIAL_STACK	(COREB_L1_SCRATCH_START + L1_SCRATCH_LENGTH - 12)
+
+ENTRY(_coreb_trampoline_start)
+	/* Set the SYSCFG register */
+	R0 = 0x36;
+	SYSCFG = R0; /*Enable Cycle Counter and Nesting Of Interrupts(3rd Bit)*/
+	R0 = 0;
+
+	/*Clear Out All the data and pointer  Registers*/
+	R1 = R0;
+	R2 = R0;
+	R3 = R0;
+	R4 = R0;
+	R5 = R0;
+	R6 = R0;
+	R7 = R0;
+
+	P0 = R0;
+	P1 = R0;
+	P2 = R0;
+	P3 = R0;
+	P4 = R0;
+	P5 = R0;
+
+	LC0 = r0;
+	LC1 = r0;
+	L0 = r0;
+	L1 = r0;
+	L2 = r0;
+	L3 = r0;
+
+	/* Clear Out All the DAG Registers*/
+	B0 = r0;
+	B1 = r0;
+	B2 = r0;
+	B3 = r0;
+
+	I0 = r0;
+	I1 = r0;
+	I2 = r0;
+	I3 = r0;
+
+	M0 = r0;
+	M1 = r0;
+	M2 = r0;
+	M3 = r0;
+
+	/* Turn off the icache */
+	p0.l = LO(IMEM_CONTROL);
+	p0.h = HI(IMEM_CONTROL);
+	R1 = [p0];
+	R0 = ~ENICPLB;
+	R0 = R0 & R1;
+
+	/* Anomaly 05000125 */
+#ifdef ANOMALY_05000125
+	CLI R2;
+	SSYNC;
+#endif
+	[p0] = R0;
+	SSYNC;
+#ifdef ANOMALY_05000125
+	STI R2;
+#endif
+
+	/* Turn off the dcache */
+	p0.l = LO(DMEM_CONTROL);
+	p0.h = HI(DMEM_CONTROL);
+	R1 = [p0];
+	R0 = ~ENDCPLB;
+	R0 = R0 & R1;
+
+	/* Anomaly 05000125 */
+#ifdef ANOMALY_05000125
+	CLI R2;
+	SSYNC;
+#endif
+	[p0] = R0;
+	SSYNC;
+#ifdef ANOMALY_05000125
+	STI R2;
+#endif
+
+	/* in case of double faults, save a few things */
+	p0.l = _init_retx_coreb;
+	p0.h = _init_retx_coreb;
+	R0 = RETX;
+	[P0] = R0;
+
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+	/* Only save these if we are storing them,
+	 * This happens here, since L1 gets clobbered
+	 * below
+	 */
+	GET_PDA(p0, r0);
+	r7 = [p0 + PDA_RETX];
+	p1.l = _init_saved_retx_coreb;
+	p1.h = _init_saved_retx_coreb;
+	[p1] = r7;
+
+	r7 = [p0 + PDA_DCPLB];
+	p1.l = _init_saved_dcplb_fault_addr_coreb;
+	p1.h = _init_saved_dcplb_fault_addr_coreb;
+	[p1] = r7;
+
+	r7 = [p0 + PDA_ICPLB];
+	p1.l = _init_saved_icplb_fault_addr_coreb;
+	p1.h = _init_saved_icplb_fault_addr_coreb;
+	[p1] = r7;
+
+	r7 = [p0 + PDA_SEQSTAT];
+	p1.l = _init_saved_seqstat_coreb;
+	p1.h = _init_saved_seqstat_coreb;
+	[p1] = r7;
+#endif
+
+	/* Initialize stack pointer */
+	sp.l = lo(INITIAL_STACK);
+	sp.h = hi(INITIAL_STACK);
+	fp = sp;
+	usp = sp;
+
+	/* This section keeps the processor in supervisor mode
+	 * during core B startup.  Branches to the idle task.
+	 */
+
+	/* EVT15 = _real_start */
+
+	p0.l = lo(EVT15);
+	p0.h = hi(EVT15);
+	p1.l = _coreb_start;
+	p1.h = _coreb_start;
+	[p0] = p1;
+	csync;
+
+	p0.l = lo(IMASK);
+	p0.h = hi(IMASK);
+	p1.l = IMASK_IVG15;
+	p1.h = 0x0;
+	[p0] = p1;
+	csync;
+
+	raise 15;
+	p0.l = .LWAIT_HERE;
+	p0.h = .LWAIT_HERE;
+	reti = p0;
+#if defined(ANOMALY_05000281)
+	nop; nop; nop;
+#endif
+	rti;
+
+.LWAIT_HERE:
+	jump .LWAIT_HERE;
+ENDPROC(_coreb_trampoline_start)
+ENTRY(_coreb_trampoline_end)
+
+ENTRY(_coreb_start)
+	[--sp] = reti;
+
+	p0.l = lo(WDOGB_CTL);
+	p0.h = hi(WDOGB_CTL);
+	r0 = 0xAD6(z);
+	w[p0] = r0;	/* Clear the watchdog. */
+	ssync;
+
+	/*
+	 * switch to IDLE stack.
+	 */
+	p0.l = _secondary_stack;
+	p0.h = _secondary_stack;
+	sp = [p0];
+	usp = sp;
+	fp = sp;
+	sp += -12;
+	call _init_pda
+	sp += 12;
+	call _secondary_start_kernel;
+.L_exit:
+	jump.s	.L_exit;
+ENDPROC(_coreb_start)
+
+__FINIT
diff --git a/arch/blackfin/mach-bf561/smp.c b/arch/blackfin/mach-bf561/smp.c
new file mode 100644
index 00000000000..9b27e698c0b
--- /dev/null
+++ b/arch/blackfin/mach-bf561/smp.c
@@ -0,0 +1,167 @@
+/*
+ * File:         arch/blackfin/mach-bf561/smp.c
+ * Author:       Philippe Gerum <rpm@xenomai.org>
+ *
+ *               Copyright 2007 Analog Devices Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see the file COPYING, or write
+ * to the Free Software Foundation, Inc.,
+ * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+#include <asm/smp.h>
+#include <asm/dma.h>
+
+static DEFINE_SPINLOCK(boot_lock);
+
+static cpumask_t cpu_callin_map;
+
+/*
+ * platform_init_cpus() - Tell the world about how many cores we
+ * have. This is called while setting up the architecture support
+ * (setup_arch()), so don't be too demanding here with respect to
+ * available kernel services.
+ */
+
+void __init platform_init_cpus(void)
+{
+	cpu_set(0, cpu_possible_map); /* CoreA */
+	cpu_set(1, cpu_possible_map); /* CoreB */
+}
+
+void __init platform_prepare_cpus(unsigned int max_cpus)
+{
+	int len;
+
+	len = &coreb_trampoline_end - &coreb_trampoline_start + 1;
+	BUG_ON(len > L1_CODE_LENGTH);
+
+	dma_memcpy((void *)COREB_L1_CODE_START, &coreb_trampoline_start, len);
+
+	/* Both cores ought to be present on a bf561! */
+	cpu_set(0, cpu_present_map); /* CoreA */
+	cpu_set(1, cpu_present_map); /* CoreB */
+
+	printk(KERN_INFO "CoreB bootstrap code to SRAM %p via DMA.\n", (void *)COREB_L1_CODE_START);
+}
+
+int __init setup_profiling_timer(unsigned int multiplier) /* not supported */
+{
+	return -EINVAL;
+}
+
+void __cpuinit platform_secondary_init(unsigned int cpu)
+{
+	local_irq_disable();
+
+	/* Clone setup for peripheral interrupt sources from CoreA. */
+	bfin_write_SICB_IMASK0(bfin_read_SICA_IMASK0());
+	bfin_write_SICB_IMASK1(bfin_read_SICA_IMASK1());
+	SSYNC();
+
+	/* Clone setup for IARs from CoreA. */
+	bfin_write_SICB_IAR0(bfin_read_SICA_IAR0());
+	bfin_write_SICB_IAR1(bfin_read_SICA_IAR1());
+	bfin_write_SICB_IAR2(bfin_read_SICA_IAR2());
+	bfin_write_SICB_IAR3(bfin_read_SICA_IAR3());
+	bfin_write_SICB_IAR4(bfin_read_SICA_IAR4());
+	bfin_write_SICB_IAR5(bfin_read_SICA_IAR5());
+	bfin_write_SICB_IAR6(bfin_read_SICA_IAR6());
+	bfin_write_SICB_IAR7(bfin_read_SICA_IAR7());
+	SSYNC();
+
+	local_irq_enable();
+
+	/* Calibrate loops per jiffy value. */
+	calibrate_delay();
+
+	/* Store CPU-private information to the cpu_data array. */
+	bfin_setup_cpudata(cpu);
+
+	/* We are done with local CPU inits, unblock the boot CPU. */
+	cpu_set(cpu, cpu_callin_map);
+	spin_lock(&boot_lock);
+	spin_unlock(&boot_lock);
+}
+
+int __cpuinit platform_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	unsigned long timeout;
+
+	/* CoreB already running?! */
+	BUG_ON((bfin_read_SICA_SYSCR() & COREB_SRAM_INIT) == 0);
+
+	printk(KERN_INFO "Booting Core B.\n");
+
+	spin_lock(&boot_lock);
+
+	/* Kick CoreB, which should start execution from CORE_SRAM_BASE. */
+	SSYNC();
+	bfin_write_SICA_SYSCR(bfin_read_SICA_SYSCR() & ~COREB_SRAM_INIT);
+	SSYNC();
+
+	timeout = jiffies + 1 * HZ;
+	while (time_before(jiffies, timeout)) {
+		if (cpu_isset(cpu, cpu_callin_map))
+			break;
+		udelay(100);
+		barrier();
+	}
+
+	spin_unlock(&boot_lock);
+
+	return cpu_isset(cpu, cpu_callin_map) ? 0 : -ENOSYS;
+}
+
+void __init platform_request_ipi(irq_handler_t handler)
+{
+	int ret;
+
+	ret = request_irq(IRQ_SUPPLE_0, handler, IRQF_DISABLED,
+			  "SMP interrupt", handler);
+	if (ret)
+		panic("Cannot request supplemental interrupt 0 for IPI service\n");
+}
+
+void platform_send_ipi(cpumask_t callmap)
+{
+	unsigned int cpu;
+
+	for_each_cpu_mask(cpu, callmap) {
+		BUG_ON(cpu >= 2);
+		SSYNC();
+		bfin_write_SICB_SYSCR(bfin_read_SICB_SYSCR() | (1 << (6 + cpu)));
+		SSYNC();
+	}
+}
+
+void platform_send_ipi_cpu(unsigned int cpu)
+{
+	BUG_ON(cpu >= 2);
+	SSYNC();
+	bfin_write_SICB_SYSCR(bfin_read_SICB_SYSCR() | (1 << (6 + cpu)));
+	SSYNC();
+}
+
+void platform_clear_ipi(unsigned int cpu)
+{
+	BUG_ON(cpu >= 2);
+	SSYNC();
+	bfin_write_SICB_SYSCR(bfin_read_SICB_SYSCR() | (1 << (10 + cpu)));
+	SSYNC();
+}