Merge branch 'master' of master.kernel.org:/pub/scm/linux/kernel/git/torvalds/linux-2.6

34 files changed, 2298 insertions, 3019 deletions

diff --git a/arch/blackfin/kernel/Makefile b/arch/blackfin/kernel/Makefile
index 606adc78aa8..38a233374f0 100644
--- a/arch/blackfin/kernel/Makefile
+++ b/arch/blackfin/kernel/Makefile
@@ -7,7 +7,7 @@ extra-y := init_task.o vmlinux.lds
 obj-y := \
 	entry.o process.o bfin_ksyms.o ptrace.o setup.o signal.o \
 	sys_bfin.o traps.o irqchip.o dma-mapping.o flat.o \
-	fixed_code.o reboot.o bfin_gpio.o
+	fixed_code.o reboot.o bfin_gpio.o bfin_dma_5xx.o
 
 ifeq ($(CONFIG_GENERIC_CLOCKEVENTS),y)
     obj-y += time-ts.o
@@ -15,8 +15,11 @@ else
     obj-y += time.o
 endif
 
+obj-$(CONFIG_IPIPE)                  += ipipe.o
+obj-$(CONFIG_IPIPE_TRACE_MCOUNT)     += mcount.o
 obj-$(CONFIG_BFIN_GPTIMERS)          += gptimers.o
+obj-$(CONFIG_CPLB_INFO)              += cplbinfo.o
 obj-$(CONFIG_MODULES)                += module.o
-obj-$(CONFIG_BFIN_DMA_5XX)           += bfin_dma_5xx.o
 obj-$(CONFIG_KGDB)                   += kgdb.o
+obj-$(CONFIG_KGDB_TESTCASE)          += kgdb_test.o
 obj-$(CONFIG_EARLY_PRINTK)           += early_printk.o
diff --git a/arch/blackfin/kernel/asm-offsets.c b/arch/blackfin/kernel/asm-offsets.c
index 9bb85dd5ccb..b5df9459d6d 100644
--- a/arch/blackfin/kernel/asm-offsets.c
+++ b/arch/blackfin/kernel/asm-offsets.c
@@ -56,6 +56,9 @@ int main(void)
 	/* offsets into the thread struct */
 	DEFINE(THREAD_KSP, offsetof(struct thread_struct, ksp));
 	DEFINE(THREAD_USP, offsetof(struct thread_struct, usp));
+	DEFINE(THREAD_SR, offsetof(struct thread_struct, seqstat));
+	DEFINE(PT_SR, offsetof(struct thread_struct, seqstat));
+	DEFINE(THREAD_ESP0, offsetof(struct thread_struct, esp0));
 	DEFINE(THREAD_PC, offsetof(struct thread_struct, pc));
 	DEFINE(KERNEL_STACK_SIZE, THREAD_SIZE);
 
@@ -128,5 +131,31 @@ int main(void)
 	DEFINE(SIGSEGV, SIGSEGV);
 	DEFINE(SIGTRAP, SIGTRAP);
 
+	/* PDA management (in L1 scratchpad) */
+	DEFINE(PDA_SYSCFG, offsetof(struct blackfin_pda, syscfg));
+#ifdef CONFIG_SMP
+	DEFINE(PDA_IRQFLAGS, offsetof(struct blackfin_pda, imask));
+#endif
+	DEFINE(PDA_IPDT, offsetof(struct blackfin_pda, ipdt));
+	DEFINE(PDA_IPDT_SWAPCOUNT, offsetof(struct blackfin_pda, ipdt_swapcount));
+	DEFINE(PDA_DPDT, offsetof(struct blackfin_pda, dpdt));
+	DEFINE(PDA_DPDT_SWAPCOUNT, offsetof(struct blackfin_pda, dpdt_swapcount));
+	DEFINE(PDA_EXIPTR, offsetof(struct blackfin_pda, ex_iptr));
+	DEFINE(PDA_EXOPTR, offsetof(struct blackfin_pda, ex_optr));
+	DEFINE(PDA_EXBUF, offsetof(struct blackfin_pda, ex_buf));
+	DEFINE(PDA_EXIMASK, offsetof(struct blackfin_pda, ex_imask));
+	DEFINE(PDA_EXSTACK, offsetof(struct blackfin_pda, ex_stack));
+#ifdef ANOMALY_05000261
+	DEFINE(PDA_LFRETX, offsetof(struct blackfin_pda, last_cplb_fault_retx));
+#endif
+	DEFINE(PDA_DCPLB, offsetof(struct blackfin_pda, dcplb_fault_addr));
+	DEFINE(PDA_ICPLB, offsetof(struct blackfin_pda, icplb_fault_addr));
+	DEFINE(PDA_RETX, offsetof(struct blackfin_pda, retx));
+	DEFINE(PDA_SEQSTAT, offsetof(struct blackfin_pda, seqstat));
+#ifdef CONFIG_SMP
+	/* Inter-core lock (in L2 SRAM) */
+	DEFINE(SIZEOF_CORELOCK, sizeof(struct corelock_slot));
+#endif
+
 	return 0;
 }
diff --git a/arch/blackfin/kernel/bfin_dma_5xx.c b/arch/blackfin/kernel/bfin_dma_5xx.c
index 339293d677c..07e02c0d1c0 100644
--- a/arch/blackfin/kernel/bfin_dma_5xx.c
+++ b/arch/blackfin/kernel/bfin_dma_5xx.c
@@ -1,63 +1,27 @@
 /*
- * File:         arch/blackfin/kernel/bfin_dma_5xx.c
- * Based on:
- * Author:
+ * bfin_dma_5xx.c - Blackfin DMA implementation
  *
- * Created:
- * Description:  This file contains the simple DMA Implementation for Blackfin
- *
- * 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
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/errno.h>
-#include <linux/module.h>
-#include <linux/sched.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/param.h>
+#include <linux/proc_fs.h>
+#include <linux/sched.h>
+#include <linux/seq_file.h>
+#include <linux/spinlock.h>
 
 #include <asm/blackfin.h>
-#include <asm/dma.h>
 #include <asm/cacheflush.h>
+#include <asm/dma.h>
+#include <asm/uaccess.h>
 
-/* Remove unused code not exported by symbol or internally called */
-#define REMOVE_DEAD_CODE
-
-/**************************************************************************
- * Global Variables
-***************************************************************************/
-
-static struct dma_channel dma_ch[MAX_BLACKFIN_DMA_CHANNEL];
-
-/*------------------------------------------------------------------------------
- *       Set the Buffer Clear bit in the Configuration register of specific DMA
- *       channel. This will stop the descriptor based DMA operation.
- *-----------------------------------------------------------------------------*/
-static void clear_dma_buffer(unsigned int channel)
-{
-	dma_ch[channel].regs->cfg |= RESTART;
-	SSYNC();
-	dma_ch[channel].regs->cfg &= ~RESTART;
-	SSYNC();
-}
+struct dma_channel dma_ch[MAX_DMA_CHANNELS];
+EXPORT_SYMBOL(dma_ch);
 
 static int __init blackfin_dma_init(void)
 {
@@ -65,32 +29,67 @@ static int __init blackfin_dma_init(void)
 
 	printk(KERN_INFO "Blackfin DMA Controller\n");
 
-	for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++) {
+	for (i = 0; i < MAX_DMA_CHANNELS; i++) {
 		dma_ch[i].chan_status = DMA_CHANNEL_FREE;
 		dma_ch[i].regs = dma_io_base_addr[i];
 		mutex_init(&(dma_ch[i].dmalock));
 	}
 	/* Mark MEMDMA Channel 0 as requested since we're using it internally */
-	dma_ch[CH_MEM_STREAM0_DEST].chan_status = DMA_CHANNEL_REQUESTED;
-	dma_ch[CH_MEM_STREAM0_SRC].chan_status = DMA_CHANNEL_REQUESTED;
+	request_dma(CH_MEM_STREAM0_DEST, "Blackfin dma_memcpy");
+	request_dma(CH_MEM_STREAM0_SRC, "Blackfin dma_memcpy");
 
 #if defined(CONFIG_DEB_DMA_URGENT)
 	bfin_write_EBIU_DDRQUE(bfin_read_EBIU_DDRQUE()
 			 | DEB1_URGENT | DEB2_URGENT | DEB3_URGENT);
 #endif
+
 	return 0;
 }
-
 arch_initcall(blackfin_dma_init);
 
-/*------------------------------------------------------------------------------
- *	Request the specific DMA channel from the system.
- *-----------------------------------------------------------------------------*/
-int request_dma(unsigned int channel, char *device_id)
+#ifdef CONFIG_PROC_FS
+static int proc_dma_show(struct seq_file *m, void *v)
 {
+	int i;
+
+	for (i = 0; i < MAX_DMA_CHANNELS; ++i)
+		if (dma_ch[i].chan_status != DMA_CHANNEL_FREE)
+			seq_printf(m, "%2d: %s\n", i, dma_ch[i].device_id);
+
+	return 0;
+}
 
+static int proc_dma_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, proc_dma_show, NULL);
+}
+
+static const struct file_operations proc_dma_operations = {
+	.open		= proc_dma_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int __init proc_dma_init(void)
+{
+	return proc_create("dma", 0, NULL, &proc_dma_operations) != NULL;
+}
+late_initcall(proc_dma_init);
+#endif
+
+/**
+ *	request_dma - request a DMA channel
+ *
+ * Request the specific DMA channel from the system if it's available.
+ */
+int request_dma(unsigned int channel, const char *device_id)
+{
 	pr_debug("request_dma() : BEGIN \n");
 
+	if (device_id == NULL)
+		printk(KERN_WARNING "request_dma(%u): no device_id given\n", channel);
+
 #if defined(CONFIG_BF561) && ANOMALY_05000182
 	if (channel >= CH_IMEM_STREAM0_DEST && channel <= CH_IMEM_STREAM1_DEST) {
 		if (get_cclk() > 500000000) {
@@ -129,60 +128,63 @@ int request_dma(unsigned int channel, char *device_id)
 #endif
 
 	dma_ch[channel].device_id = device_id;
-	dma_ch[channel].irq_callback = NULL;
+	dma_ch[channel].irq = 0;
 
 	/* This is to be enabled by putting a restriction -
 	 * you have to request DMA, before doing any operations on
 	 * descriptor/channel
 	 */
 	pr_debug("request_dma() : END  \n");
-	return channel;
+	return 0;
 }
 EXPORT_SYMBOL(request_dma);
 
-int set_dma_callback(unsigned int channel, dma_interrupt_t callback, void *data)
+int set_dma_callback(unsigned int channel, irq_handler_t callback, void *data)
 {
-	int ret_irq = 0;
-
 	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
+	       && channel < MAX_DMA_CHANNELS));
 
 	if (callback != NULL) {
-		int ret_val;
-		ret_irq = channel2irq(channel);
+		int ret;
+		unsigned int irq = channel2irq(channel);
 
-		dma_ch[channel].data = data;
+		ret = request_irq(irq, callback, IRQF_DISABLED,
+			dma_ch[channel].device_id, data);
+		if (ret)
+			return ret;
 
-		ret_val =
-		    request_irq(ret_irq, (void *)callback, IRQF_DISABLED,
-				dma_ch[channel].device_id, data);
-		if (ret_val) {
-			printk(KERN_NOTICE
-			       "Request irq in DMA engine failed.\n");
-			return -EPERM;
-		}
-		dma_ch[channel].irq_callback = callback;
+		dma_ch[channel].irq = irq;
+		dma_ch[channel].data = data;
 	}
 	return 0;
 }
 EXPORT_SYMBOL(set_dma_callback);
 
-void free_dma(unsigned int channel)
+/**
+ *	clear_dma_buffer - clear DMA fifos for specified channel
+ *
+ * Set the Buffer Clear bit in the Configuration register of specific DMA
+ * channel. This will stop the descriptor based DMA operation.
+ */
+static void clear_dma_buffer(unsigned int channel)
 {
-	int ret_irq;
+	dma_ch[channel].regs->cfg |= RESTART;
+	SSYNC();
+	dma_ch[channel].regs->cfg &= ~RESTART;
+}
 
+void free_dma(unsigned int channel)
+{
 	pr_debug("freedma() : BEGIN \n");
 	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
+	       && channel < MAX_DMA_CHANNELS));
 
 	/* Halt the DMA */
 	disable_dma(channel);
 	clear_dma_buffer(channel);
 
-	if (dma_ch[channel].irq_callback != NULL) {
-		ret_irq = channel2irq(channel);
-		free_irq(ret_irq, dma_ch[channel].data);
-	}
+	if (dma_ch[channel].irq)
+		free_irq(dma_ch[channel].irq, dma_ch[channel].data);
 
 	/* Clear the DMA Variable in the Channel */
 	mutex_lock(&(dma_ch[channel].dmalock));
@@ -193,294 +195,15 @@ void free_dma(unsigned int channel)
 }
 EXPORT_SYMBOL(free_dma);
 
-void dma_enable_irq(unsigned int channel)
-{
-	int ret_irq;
-
-	pr_debug("dma_enable_irq() : BEGIN \n");
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	ret_irq = channel2irq(channel);
-	enable_irq(ret_irq);
-}
-EXPORT_SYMBOL(dma_enable_irq);
-
-void dma_disable_irq(unsigned int channel)
-{
-	int ret_irq;
-
-	pr_debug("dma_disable_irq() : BEGIN \n");
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	ret_irq = channel2irq(channel);
-	disable_irq(ret_irq);
-}
-EXPORT_SYMBOL(dma_disable_irq);
-
-int dma_channel_active(unsigned int channel)
-{
-	if (dma_ch[channel].chan_status == DMA_CHANNEL_FREE) {
-		return 0;
-	} else {
-		return 1;
-	}
-}
-EXPORT_SYMBOL(dma_channel_active);
-
-/*------------------------------------------------------------------------------
-*	stop the specific DMA channel.
-*-----------------------------------------------------------------------------*/
-void disable_dma(unsigned int channel)
-{
-	pr_debug("stop_dma() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->cfg &= ~DMAEN;	/* Clean the enable bit */
-	SSYNC();
-	dma_ch[channel].chan_status = DMA_CHANNEL_REQUESTED;
-	/* Needs to be enabled Later */
-	pr_debug("stop_dma() : END \n");
-	return;
-}
-EXPORT_SYMBOL(disable_dma);
-
-void enable_dma(unsigned int channel)
-{
-	pr_debug("enable_dma() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].chan_status = DMA_CHANNEL_ENABLED;
-	dma_ch[channel].regs->curr_x_count = 0;
-	dma_ch[channel].regs->curr_y_count = 0;
-
-	dma_ch[channel].regs->cfg |= DMAEN;	/* Set the enable bit */
-	SSYNC();
-	pr_debug("enable_dma() : END \n");
-	return;
-}
-EXPORT_SYMBOL(enable_dma);
-
-/*------------------------------------------------------------------------------
-*		Set the Start Address register for the specific DMA channel
-* 		This function can be used for register based DMA,
-*		to setup the start address
-*		addr:		Starting address of the DMA Data to be transferred.
-*-----------------------------------------------------------------------------*/
-void set_dma_start_addr(unsigned int channel, unsigned long addr)
-{
-	pr_debug("set_dma_start_addr() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->start_addr = addr;
-	SSYNC();
-	pr_debug("set_dma_start_addr() : END\n");
-}
-EXPORT_SYMBOL(set_dma_start_addr);
-
-void set_dma_next_desc_addr(unsigned int channel, unsigned long addr)
-{
-	pr_debug("set_dma_next_desc_addr() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->next_desc_ptr = addr;
-	SSYNC();
-	pr_debug("set_dma_next_desc_addr() : END\n");
-}
-EXPORT_SYMBOL(set_dma_next_desc_addr);
-
-void set_dma_curr_desc_addr(unsigned int channel, unsigned long addr)
-{
-	pr_debug("set_dma_curr_desc_addr() : BEGIN \n");
-
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->curr_desc_ptr = addr;
-	SSYNC();
-	pr_debug("set_dma_curr_desc_addr() : END\n");
-}
-EXPORT_SYMBOL(set_dma_curr_desc_addr);
-
-void set_dma_x_count(unsigned int channel, unsigned short x_count)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->x_count = x_count;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_x_count);
-
-void set_dma_y_count(unsigned int channel, unsigned short y_count)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->y_count = y_count;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_y_count);
-
-void set_dma_x_modify(unsigned int channel, short x_modify)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->x_modify = x_modify;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_x_modify);
-
-void set_dma_y_modify(unsigned int channel, short y_modify)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->y_modify = y_modify;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_y_modify);
-
-void set_dma_config(unsigned int channel, unsigned short config)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->cfg = config;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_config);
-
-unsigned short
-set_bfin_dma_config(char direction, char flow_mode,
-		    char intr_mode, char dma_mode, char width, char syncmode)
-{
-	unsigned short config;
-
-	config =
-	    ((direction << 1) | (width << 2) | (dma_mode << 4) |
-	     (intr_mode << 6) | (flow_mode << 12) | (syncmode << 5));
-	return config;
-}
-EXPORT_SYMBOL(set_bfin_dma_config);
-
-void set_dma_sg(unsigned int channel, struct dmasg *sg, int nr_sg)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->cfg |= ((nr_sg & 0x0F) << 8);
-
-	dma_ch[channel].regs->next_desc_ptr = (unsigned int)sg;
-
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_sg);
-
-void set_dma_curr_addr(unsigned int channel, unsigned long addr)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	dma_ch[channel].regs->curr_addr_ptr = addr;
-	SSYNC();
-}
-EXPORT_SYMBOL(set_dma_curr_addr);
-
-/*------------------------------------------------------------------------------
- *	Get the DMA status of a specific DMA channel from the system.
- *-----------------------------------------------------------------------------*/
-unsigned short get_dma_curr_irqstat(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->irq_status;
-}
-EXPORT_SYMBOL(get_dma_curr_irqstat);
-
-/*------------------------------------------------------------------------------
- *	Clear the DMA_DONE bit in DMA status. Stop the DMA completion interrupt.
- *-----------------------------------------------------------------------------*/
-void clear_dma_irqstat(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-	dma_ch[channel].regs->irq_status |= 3;
-}
-EXPORT_SYMBOL(clear_dma_irqstat);
-
-/*------------------------------------------------------------------------------
- *	Get current DMA xcount of a specific DMA channel from the system.
- *-----------------------------------------------------------------------------*/
-unsigned short get_dma_curr_xcount(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->curr_x_count;
-}
-EXPORT_SYMBOL(get_dma_curr_xcount);
-
-/*------------------------------------------------------------------------------
- *	Get current DMA ycount of a specific DMA channel from the system.
- *-----------------------------------------------------------------------------*/
-unsigned short get_dma_curr_ycount(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	       && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->curr_y_count;
-}
-EXPORT_SYMBOL(get_dma_curr_ycount);
-
-unsigned long get_dma_next_desc_ptr(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	      && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->next_desc_ptr;
-}
-EXPORT_SYMBOL(get_dma_next_desc_ptr);
-
-unsigned long get_dma_curr_desc_ptr(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	      && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->curr_desc_ptr;
-}
-EXPORT_SYMBOL(get_dma_curr_desc_ptr);
-
-unsigned long get_dma_curr_addr(unsigned int channel)
-{
-	BUG_ON(!(dma_ch[channel].chan_status != DMA_CHANNEL_FREE
-	      && channel < MAX_BLACKFIN_DMA_CHANNEL));
-
-	return dma_ch[channel].regs->curr_addr_ptr;
-}
-EXPORT_SYMBOL(get_dma_curr_addr);
-
 #ifdef CONFIG_PM
+# ifndef MAX_DMA_SUSPEND_CHANNELS
+#  define MAX_DMA_SUSPEND_CHANNELS MAX_DMA_CHANNELS
+# endif
 int blackfin_dma_suspend(void)
 {
 	int i;
 
-#ifdef CONFIG_BF561	/* IMDMA channels doesn't have a PERIPHERAL_MAP */
-	for (i = 0; i <= CH_MEM_STREAM3_SRC; i++) {
-#else
-	for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++) {
-#endif
+	for (i = 0; i < MAX_DMA_SUSPEND_CHANNELS; ++i) {
 		if (dma_ch[i].chan_status == DMA_CHANNEL_ENABLED) {
 			printk(KERN_ERR "DMA Channel %d failed to suspend\n", i);
 			return -EBUSY;
@@ -495,388 +218,201 @@ int blackfin_dma_suspend(void)
 void blackfin_dma_resume(void)
 {
 	int i;
-
-#ifdef CONFIG_BF561	/* IMDMA channels doesn't have a PERIPHERAL_MAP */
-	for (i = 0; i <= CH_MEM_STREAM3_SRC; i++)
-#else
-	for (i = 0; i < MAX_BLACKFIN_DMA_CHANNEL; i++)
-#endif
+	for (i = 0; i < MAX_DMA_SUSPEND_CHANNELS; ++i)
 		dma_ch[i].regs->peripheral_map = dma_ch[i].saved_peripheral_map;
 }
 #endif
 
-static void *__dma_memcpy(void *dest, const void *src, size_t size)
+/**
+ *	blackfin_dma_early_init - minimal DMA init
+ *
+ * Setup a few DMA registers so we can safely do DMA transfers early on in
+ * the kernel booting process.  Really this just means using dma_memcpy().
+ */
+void __init blackfin_dma_early_init(void)
 {
-	int direction;	/* 1 - address decrease, 0 - address increase */
-	int flag_align;	/* 1 - address aligned,  0 - address unaligned */
-	int flag_2D;	/* 1 - 2D DMA needed,	 0 - 1D DMA needed */
-	unsigned long flags;
-
-	if (size <= 0)
-		return NULL;
-
-	local_irq_save(flags);
-
-	if ((unsigned long)src < memory_end)
-		blackfin_dcache_flush_range((unsigned int)src,
-					    (unsigned int)(src + size));
-
-	if ((unsigned long)dest < memory_end)
-		blackfin_dcache_invalidate_range((unsigned int)dest,
-						 (unsigned int)(dest + size));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	if ((unsigned long)src < (unsigned long)dest)
-		direction = 1;
-	else
-		direction = 0;
-
-	if ((((unsigned long)dest % 2) == 0) && (((unsigned long)src % 2) == 0)
-	    && ((size % 2) == 0))
-		flag_align = 1;
-	else
-		flag_align = 0;
-
-	if (size > 0x10000)	/* size > 64K */
-		flag_2D = 1;
-	else
-		flag_2D = 0;
-
-	/* Setup destination and source start address */
-	if (direction) {
-		if (flag_align) {
-			bfin_write_MDMA_D0_START_ADDR(dest + size - 2);
-			bfin_write_MDMA_S0_START_ADDR(src + size - 2);
-		} else {
-			bfin_write_MDMA_D0_START_ADDR(dest + size - 1);
-			bfin_write_MDMA_S0_START_ADDR(src + size - 1);
-		}
-	} else {
-		bfin_write_MDMA_D0_START_ADDR(dest);
-		bfin_write_MDMA_S0_START_ADDR(src);
-	}
-
-	/* Setup destination and source xcount */
-	if (flag_2D) {
-		if (flag_align) {
-			bfin_write_MDMA_D0_X_COUNT(1024 / 2);
-			bfin_write_MDMA_S0_X_COUNT(1024 / 2);
-		} else {
-			bfin_write_MDMA_D0_X_COUNT(1024);
-			bfin_write_MDMA_S0_X_COUNT(1024);
-		}
-		bfin_write_MDMA_D0_Y_COUNT(size >> 10);
-		bfin_write_MDMA_S0_Y_COUNT(size >> 10);
-	} else {
-		if (flag_align) {
-			bfin_write_MDMA_D0_X_COUNT(size / 2);
-			bfin_write_MDMA_S0_X_COUNT(size / 2);
-		} else {
-			bfin_write_MDMA_D0_X_COUNT(size);
-			bfin_write_MDMA_S0_X_COUNT(size);
-		}
-	}
-
-	/* Setup destination and source xmodify and ymodify */
-	if (direction) {
-		if (flag_align) {
-			bfin_write_MDMA_D0_X_MODIFY(-2);
-			bfin_write_MDMA_S0_X_MODIFY(-2);
-			if (flag_2D) {
-				bfin_write_MDMA_D0_Y_MODIFY(-2);
-				bfin_write_MDMA_S0_Y_MODIFY(-2);
-			}
-		} else {
-			bfin_write_MDMA_D0_X_MODIFY(-1);
-			bfin_write_MDMA_S0_X_MODIFY(-1);
-			if (flag_2D) {
-				bfin_write_MDMA_D0_Y_MODIFY(-1);
-				bfin_write_MDMA_S0_Y_MODIFY(-1);
-			}
-		}
-	} else {
-		if (flag_align) {
-			bfin_write_MDMA_D0_X_MODIFY(2);
-			bfin_write_MDMA_S0_X_MODIFY(2);
-			if (flag_2D) {
-				bfin_write_MDMA_D0_Y_MODIFY(2);
-				bfin_write_MDMA_S0_Y_MODIFY(2);
-			}
-		} else {
-			bfin_write_MDMA_D0_X_MODIFY(1);
-			bfin_write_MDMA_S0_X_MODIFY(1);
-			if (flag_2D) {
-				bfin_write_MDMA_D0_Y_MODIFY(1);
-				bfin_write_MDMA_S0_Y_MODIFY(1);
-			}
-		}
-	}
-
-	/* Enable source DMA */
-	if (flag_2D) {
-		if (flag_align) {
-			bfin_write_MDMA_S0_CONFIG(DMAEN | DMA2D | WDSIZE_16);
-			bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | DMA2D | WDSIZE_16);
-		} else {
-			bfin_write_MDMA_S0_CONFIG(DMAEN | DMA2D);
-			bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | DMA2D);
-		}
-	} else {
-		if (flag_align) {
-			bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_16);
-			bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_16);
-		} else {
-			bfin_write_MDMA_S0_CONFIG(DMAEN);
-			bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN);
-		}
-	}
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE))
-		;
-
-	bfin_write_MDMA_D0_IRQ_STATUS(bfin_read_MDMA_D0_IRQ_STATUS() |
-				      (DMA_DONE | DMA_ERR));
-
 	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-
-	local_irq_restore(flags);
-
-	return dest;
 }
 
-void *dma_memcpy(void *dest, const void *src, size_t size)
-{
-	size_t bulk;
-	size_t rest;
-	void * addr;
-
-	bulk = (size >> 16) << 16;
-	rest = size - bulk;
-	if (bulk)
-		__dma_memcpy(dest, src, bulk);
-	addr = __dma_memcpy(dest+bulk, src+bulk, rest);
-	return addr;
-}
-EXPORT_SYMBOL(dma_memcpy);
-
-void *safe_dma_memcpy(void *dest, const void *src, size_t size)
-{
-	void *addr;
-	addr = dma_memcpy(dest, src, size);
-	return addr;
-}
-EXPORT_SYMBOL(safe_dma_memcpy);
-
-void dma_outsb(unsigned long addr, const void *buf, unsigned short len)
+/**
+ *	__dma_memcpy - program the MDMA registers
+ *
+ * Actually program MDMA0 and wait for the transfer to finish.  Disable IRQs
+ * while programming registers so that everything is fully configured.  Wait
+ * for DMA to finish with IRQs enabled.  If interrupted, the initial DMA_DONE
+ * check will make sure we don't clobber any existing transfer.
+ */
+static void __dma_memcpy(u32 daddr, s16 dmod, u32 saddr, s16 smod, size_t cnt, u32 conf)
 {
+	static DEFINE_SPINLOCK(mdma_lock);
 	unsigned long flags;
 
-	local_irq_save(flags);
-
-	blackfin_dcache_flush_range((unsigned int)buf,
-			 (unsigned int)(buf) + len);
+	spin_lock_irqsave(&mdma_lock, flags);
+
+	if (bfin_read_MDMA_S0_CONFIG())
+		while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE))
+			continue;
+
+	if (conf & DMA2D) {
+		/* For larger bit sizes, we've already divided down cnt so it
+		 * is no longer a multiple of 64k.  So we have to break down
+		 * the limit here so it is a multiple of the incoming size.
+		 * There is no limitation here in terms of total size other
+		 * than the hardware though as the bits lost in the shift are
+		 * made up by MODIFY (== we can hit the whole address space).
+		 * X: (2^(16 - 0)) * 1 == (2^(16 - 1)) * 2 == (2^(16 - 2)) * 4
+		 */
+		u32 shift = abs(dmod) >> 1;
+		size_t ycnt = cnt >> (16 - shift);
+		cnt = 1 << (16 - shift);
+		bfin_write_MDMA_D0_Y_COUNT(ycnt);
+		bfin_write_MDMA_S0_Y_COUNT(ycnt);
+		bfin_write_MDMA_D0_Y_MODIFY(dmod);
+		bfin_write_MDMA_S0_Y_MODIFY(smod);
+	}
 
-	bfin_write_MDMA_D0_START_ADDR(addr);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(0);
+	bfin_write_MDMA_D0_START_ADDR(daddr);
+	bfin_write_MDMA_D0_X_COUNT(cnt);
+	bfin_write_MDMA_D0_X_MODIFY(dmod);
 	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
 
-	bfin_write_MDMA_S0_START_ADDR(buf);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(1);
+	bfin_write_MDMA_S0_START_ADDR(saddr);
+	bfin_write_MDMA_S0_X_COUNT(cnt);
+	bfin_write_MDMA_S0_X_MODIFY(smod);
 	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
 
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_8);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_8);
+	bfin_write_MDMA_S0_CONFIG(DMAEN | conf);
+	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | conf);
+
+	spin_unlock_irqrestore(&mdma_lock, flags);
 
 	SSYNC();
 
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
+	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE))
+		if (bfin_read_MDMA_S0_CONFIG())
+			continue;
+		else
+			return;
 
 	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
 
 	bfin_write_MDMA_S0_CONFIG(0);
 	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
-
 }
-EXPORT_SYMBOL(dma_outsb);
-
 
-void dma_insb(unsigned long addr, void *buf, unsigned short len)
+/**
+ *	_dma_memcpy - translate C memcpy settings into MDMA settings
+ *
+ * Handle all the high level steps before we touch the MDMA registers.  So
+ * handle direction, tweaking of sizes, and formatting of addresses.
+ */
+static void *_dma_memcpy(void *pdst, const void *psrc, size_t size)
 {
-	unsigned long flags;
-
-	blackfin_dcache_invalidate_range((unsigned int)buf,
-			 (unsigned int)(buf) + len);
-
-	local_irq_save(flags);
-	bfin_write_MDMA_D0_START_ADDR(buf);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(1);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(addr);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(0);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
+	u32 conf, shift;
+	s16 mod;
+	unsigned long dst = (unsigned long)pdst;
+	unsigned long src = (unsigned long)psrc;
 
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_8);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_8);
+	if (size == 0)
+		return NULL;
 
-	SSYNC();
+	if (dst % 4 == 0 && src % 4 == 0 && size % 4 == 0) {
+		conf = WDSIZE_32;
+		shift = 2;
+	} else if (dst % 2 == 0 && src % 2 == 0 && size % 2 == 0) {
+		conf = WDSIZE_16;
+		shift = 1;
+	} else {
+		conf = WDSIZE_8;
+		shift = 0;
+	}
 
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
+	/* If the two memory regions have a chance of overlapping, make
+	 * sure the memcpy still works as expected.  Do this by having the
+	 * copy run backwards instead.
+	 */
+	mod = 1 << shift;
+	if (src < dst) {
+		mod *= -1;
+		dst += size + mod;
+		src += size + mod;
+	}
+	size >>= shift;
 
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
+	if (size > 0x10000)
+		conf |= DMA2D;
 
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
+	__dma_memcpy(dst, mod, src, mod, size, conf);
 
+	return pdst;
 }
-EXPORT_SYMBOL(dma_insb);
 
-void dma_outsw(unsigned long addr, const void  *buf, unsigned short len)
+/**
+ *	dma_memcpy - DMA memcpy under mutex lock
+ *
+ * Do not check arguments before starting the DMA memcpy.  Break the transfer
+ * up into two pieces.  The first transfer is in multiples of 64k and the
+ * second transfer is the piece smaller than 64k.
+ */
+void *dma_memcpy(void *pdst, const void *psrc, size_t size)
 {
-	unsigned long flags;
-
-	local_irq_save(flags);
+	unsigned long dst = (unsigned long)pdst;
+	unsigned long src = (unsigned long)psrc;
+	size_t bulk, rest;
 
-	blackfin_dcache_flush_range((unsigned int)buf,
-			 (unsigned int)(buf) + len * sizeof(short));
+	if (bfin_addr_dcachable(src))
+		blackfin_dcache_flush_range(src, src + size);
 
-	bfin_write_MDMA_D0_START_ADDR(addr);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(0);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(buf);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(2);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_16);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_16);
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
+	if (bfin_addr_dcachable(dst))
+		blackfin_dcache_invalidate_range(dst, dst + size);
 
+	bulk = size & ~0xffff;
+	rest = size - bulk;
+	if (bulk)
+		_dma_memcpy(pdst, psrc, bulk);
+	_dma_memcpy(pdst + bulk, psrc + bulk, rest);
+	return pdst;
 }
-EXPORT_SYMBOL(dma_outsw);
+EXPORT_SYMBOL(dma_memcpy);
 
-void dma_insw(unsigned long addr, void *buf, unsigned short len)
+/**
+ *	safe_dma_memcpy - DMA memcpy w/argument checking
+ *
+ * Verify arguments are safe before heading to dma_memcpy().
+ */
+void *safe_dma_memcpy(void *dst, const void *src, size_t size)
 {
-	unsigned long flags;
-
-	blackfin_dcache_invalidate_range((unsigned int)buf,
-			 (unsigned int)(buf) + len * sizeof(short));
-
-	local_irq_save(flags);
-
-	bfin_write_MDMA_D0_START_ADDR(buf);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(2);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(addr);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(0);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_16);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_16);
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
-
+	if (!access_ok(VERIFY_WRITE, dst, size))
+		return NULL;
+	if (!access_ok(VERIFY_READ, src, size))
+		return NULL;
+	return dma_memcpy(dst, src, size);
 }
-EXPORT_SYMBOL(dma_insw);
+EXPORT_SYMBOL(safe_dma_memcpy);
 
-void dma_outsl(unsigned long addr, const void *buf, unsigned short len)
+static void _dma_out(unsigned long addr, unsigned long buf, unsigned short len,
+                     u16 size, u16 dma_size)
 {
-	unsigned long flags;
-
-	local_irq_save(flags);
-
-	blackfin_dcache_flush_range((unsigned int)buf,
-			 (unsigned int)(buf) + len * sizeof(long));
-
-	bfin_write_MDMA_D0_START_ADDR(addr);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(0);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(buf);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(4);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_32);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_32);
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
-
+	blackfin_dcache_flush_range(buf, buf + len * size);
+	__dma_memcpy(addr, 0, buf, size, len, dma_size);
 }
-EXPORT_SYMBOL(dma_outsl);
 
-void dma_insl(unsigned long addr, void *buf, unsigned short len)
+static void _dma_in(unsigned long addr, unsigned long buf, unsigned short len,
+                    u16 size, u16 dma_size)
 {
-	unsigned long flags;
-
-	blackfin_dcache_invalidate_range((unsigned int)buf,
-			 (unsigned int)(buf) + len * sizeof(long));
-
-	local_irq_save(flags);
-
-	bfin_write_MDMA_D0_START_ADDR(buf);
-	bfin_write_MDMA_D0_X_COUNT(len);
-	bfin_write_MDMA_D0_X_MODIFY(4);
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_START_ADDR(addr);
-	bfin_write_MDMA_S0_X_COUNT(len);
-	bfin_write_MDMA_S0_X_MODIFY(0);
-	bfin_write_MDMA_S0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(DMAEN | WDSIZE_32);
-	bfin_write_MDMA_D0_CONFIG(WNR | DI_EN | DMAEN | WDSIZE_32);
-
-	SSYNC();
-
-	while (!(bfin_read_MDMA_D0_IRQ_STATUS() & DMA_DONE));
-
-	bfin_write_MDMA_D0_IRQ_STATUS(DMA_DONE | DMA_ERR);
-
-	bfin_write_MDMA_S0_CONFIG(0);
-	bfin_write_MDMA_D0_CONFIG(0);
-	local_irq_restore(flags);
-
+	blackfin_dcache_invalidate_range(buf, buf + len * size);
+	__dma_memcpy(buf, size, addr, 0, len, dma_size);
 }
-EXPORT_SYMBOL(dma_insl);
+
+#define MAKE_DMA_IO(io, bwl, isize, dmasize, cnst) \
+void dma_##io##s##bwl(unsigned long addr, cnst void *buf, unsigned short len) \
+{ \
+	_dma_##io(addr, (unsigned long)buf, len, isize, WDSIZE_##dmasize); \
+} \
+EXPORT_SYMBOL(dma_##io##s##bwl)
+MAKE_DMA_IO(out, b, 1,  8, const);
+MAKE_DMA_IO(in,  b, 1,  8, );
+MAKE_DMA_IO(out, w, 2, 16, const);
+MAKE_DMA_IO(in,  w, 2, 16, );
+MAKE_DMA_IO(out, l, 4, 32, const);
+MAKE_DMA_IO(in,  l, 4, 32, );
diff --git a/arch/blackfin/kernel/bfin_gpio.c b/arch/blackfin/kernel/bfin_gpio.c
index 5c0800adb4d..4c14331978f 100644
--- a/arch/blackfin/kernel/bfin_gpio.c
+++ b/arch/blackfin/kernel/bfin_gpio.c
@@ -119,28 +119,28 @@ enum {
 #define AWA_DUMMY_READ(...)  do { } while (0)
 #endif
 
-#ifdef BF533_FAMILY
-static struct gpio_port_t *gpio_bankb[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+#if defined(BF533_FAMILY) || defined(BF538_FAMILY)
+static struct gpio_port_t *gpio_bankb[] = {
 	(struct gpio_port_t *) FIO_FLAG_D,
 };
 #endif
 
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
-static struct gpio_port_t *gpio_bankb[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
+static struct gpio_port_t *gpio_bankb[] = {
 	(struct gpio_port_t *) PORTFIO,
 	(struct gpio_port_t *) PORTGIO,
 	(struct gpio_port_t *) PORTHIO,
 };
 
-static unsigned short *port_fer[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+static unsigned short *port_fer[] = {
 	(unsigned short *) PORTF_FER,
 	(unsigned short *) PORTG_FER,
 	(unsigned short *) PORTH_FER,
 };
 #endif
 
-#ifdef BF527_FAMILY
-static unsigned short *port_mux[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+#if defined(BF527_FAMILY) || defined(BF518_FAMILY)
+static unsigned short *port_mux[] = {
 	(unsigned short *) PORTF_MUX,
 	(unsigned short *) PORTG_MUX,
 	(unsigned short *) PORTH_MUX,
@@ -155,7 +155,7 @@ u8 pmux_offset[][16] =
 #endif
 
 #ifdef BF561_FAMILY
-static struct gpio_port_t *gpio_bankb[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+static struct gpio_port_t *gpio_bankb[] = {
 	(struct gpio_port_t *) FIO0_FLAG_D,
 	(struct gpio_port_t *) FIO1_FLAG_D,
 	(struct gpio_port_t *) FIO2_FLAG_D,
@@ -163,7 +163,7 @@ static struct gpio_port_t *gpio_bankb[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
 #endif
 
 #ifdef BF548_FAMILY
-static struct gpio_port_t *gpio_array[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
+static struct gpio_port_t *gpio_array[] = {
 	(struct gpio_port_t *)PORTA_FER,
 	(struct gpio_port_t *)PORTB_FER,
 	(struct gpio_port_t *)PORTC_FER,
@@ -177,8 +177,9 @@ static struct gpio_port_t *gpio_array[gpio_bank(MAX_BLACKFIN_GPIOS)] = {
 };
 #endif
 
-static unsigned short reserved_gpio_map[gpio_bank(MAX_BLACKFIN_GPIOS)];
+static unsigned short reserved_gpio_map[GPIO_BANK_NUM];
 static unsigned short reserved_peri_map[gpio_bank(MAX_RESOURCES)];
+static unsigned short reserved_gpio_irq_map[GPIO_BANK_NUM];
 
 #define RESOURCE_LABEL_SIZE 	16
 
@@ -188,48 +189,46 @@ static struct str_ident {
 
 #if defined(CONFIG_PM)
 #if defined(CONFIG_BF54x)
-static struct gpio_port_s gpio_bank_saved[gpio_bank(MAX_BLACKFIN_GPIOS)];
+static struct gpio_port_s gpio_bank_saved[GPIO_BANK_NUM];
 #else
-static unsigned short wakeup_map[gpio_bank(MAX_BLACKFIN_GPIOS)];
+static unsigned short wakeup_map[GPIO_BANK_NUM];
 static unsigned char wakeup_flags_map[MAX_BLACKFIN_GPIOS];
-static struct gpio_port_s gpio_bank_saved[gpio_bank(MAX_BLACKFIN_GPIOS)];
+static struct gpio_port_s gpio_bank_saved[GPIO_BANK_NUM];
 
 #ifdef BF533_FAMILY
-static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PROG_INTB};
+static unsigned int sic_iwr_irqs[] = {IRQ_PROG_INTB};
 #endif
 
 #ifdef BF537_FAMILY
-static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PROG_INTB, IRQ_PORTG_INTB, IRQ_MAC_TX};
+static unsigned int sic_iwr_irqs[] = {IRQ_PROG_INTB, IRQ_PORTG_INTB, IRQ_MAC_TX};
+#endif
+
+#ifdef BF538_FAMILY
+static unsigned int sic_iwr_irqs[] = {IRQ_PORTF_INTB};
 #endif
 
-#ifdef BF527_FAMILY
-static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PORTF_INTB, IRQ_PORTG_INTB, IRQ_PORTH_INTB};
+#if defined(BF527_FAMILY) || defined(BF518_FAMILY)
+static unsigned int sic_iwr_irqs[] = {IRQ_PORTF_INTB, IRQ_PORTG_INTB, IRQ_PORTH_INTB};
 #endif
 
 #ifdef BF561_FAMILY
-static unsigned int sic_iwr_irqs[gpio_bank(MAX_BLACKFIN_GPIOS)] = {IRQ_PROG0_INTB, IRQ_PROG1_INTB, IRQ_PROG2_INTB};
+static unsigned int sic_iwr_irqs[] = {IRQ_PROG0_INTB, IRQ_PROG1_INTB, IRQ_PROG2_INTB};
 #endif
 #endif
 #endif /* CONFIG_PM */
 
-#if defined(BF548_FAMILY)
 inline int check_gpio(unsigned gpio)
 {
+#if defined(BF548_FAMILY)
 	if (gpio == GPIO_PB15 || gpio == GPIO_PC14 || gpio == GPIO_PC15
 	    || gpio == GPIO_PH14 || gpio == GPIO_PH15
-	    || gpio == GPIO_PJ14 || gpio == GPIO_PJ15
-	    || gpio >= MAX_BLACKFIN_GPIOS)
+	    || gpio == GPIO_PJ14 || gpio == GPIO_PJ15)
 		return -EINVAL;
-	return 0;
-}
-#else
-inline int check_gpio(unsigned gpio)
-{
+#endif
 	if (gpio >= MAX_BLACKFIN_GPIOS)
 		return -EINVAL;
 	return 0;
 }
-#endif
 
 static void gpio_error(unsigned gpio)
 {
@@ -258,35 +257,30 @@ static int cmp_label(unsigned short ident, const char *label)
 	}
 
 	if (label)
-		return strncmp(str_ident[ident].name,
-				 label, strlen(label));
+		return strcmp(str_ident[ident].name, label);
 	else
 		return -EINVAL;
 }
 
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
 static void port_setup(unsigned gpio, unsigned short usage)
 {
-	if (!check_gpio(gpio)) {
-		if (usage == GPIO_USAGE)
-			*port_fer[gpio_bank(gpio)] &= ~gpio_bit(gpio);
-		else
-			*port_fer[gpio_bank(gpio)] |= gpio_bit(gpio);
-		SSYNC();
-	}
-}
+	if (check_gpio(gpio))
+		return;
+
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
+	if (usage == GPIO_USAGE)
+		*port_fer[gpio_bank(gpio)] &= ~gpio_bit(gpio);
+	else
+		*port_fer[gpio_bank(gpio)] |= gpio_bit(gpio);
+	SSYNC();
 #elif defined(BF548_FAMILY)
-static void port_setup(unsigned gpio, unsigned short usage)
-{
 	if (usage == GPIO_USAGE)
 		gpio_array[gpio_bank(gpio)]->port_fer &= ~gpio_bit(gpio);
 	else
 		gpio_array[gpio_bank(gpio)]->port_fer |= gpio_bit(gpio);
 	SSYNC();
-}
-#else
-# define port_setup(...)  do { } while (0)
 #endif
+}
 
 #ifdef BF537_FAMILY
 static struct {
@@ -379,7 +373,7 @@ inline u16 get_portmux(unsigned short portno)
 
 	return (pmux >> (2 * gpio_sub_n(portno)) & 0x3);
 }
-#elif defined(BF527_FAMILY)
+#elif defined(BF527_FAMILY) || defined(BF518_FAMILY)
 inline void portmux_setup(unsigned short portno, unsigned short function)
 {
 	u16 pmux, ident = P_IDENT(portno);
@@ -428,13 +422,13 @@ arch_initcall(bfin_gpio_init);
 void set_gpio_ ## name(unsigned gpio, unsigned short arg) \
 { \
 	unsigned long flags; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	if (arg) \
 		gpio_bankb[gpio_bank(gpio)]->name |= gpio_bit(gpio); \
 	else \
 		gpio_bankb[gpio_bank(gpio)]->name &= ~gpio_bit(gpio); \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 } \
 EXPORT_SYMBOL(set_gpio_ ## name);
 
@@ -450,13 +444,13 @@ SET_GPIO(both)
 void set_gpio_ ## name(unsigned gpio, unsigned short arg) \
 { \
 	unsigned long flags; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	if (arg) \
 		gpio_bankb[gpio_bank(gpio)]->name ## _set = gpio_bit(gpio); \
 	else \
 		gpio_bankb[gpio_bank(gpio)]->name ## _clear = gpio_bit(gpio); \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 } \
 EXPORT_SYMBOL(set_gpio_ ## name);
 #else
@@ -479,10 +473,10 @@ SET_GPIO_SC(data)
 void set_gpio_toggle(unsigned gpio)
 {
 	unsigned long flags;
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_bankb[gpio_bank(gpio)]->toggle = gpio_bit(gpio);
 	AWA_DUMMY_READ(toggle);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 #else
 void set_gpio_toggle(unsigned gpio)
@@ -500,10 +494,10 @@ EXPORT_SYMBOL(set_gpio_toggle);
 void set_gpiop_ ## name(unsigned gpio, unsigned short arg) \
 { \
 	unsigned long flags; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	gpio_bankb[gpio_bank(gpio)]->name = arg; \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 } \
 EXPORT_SYMBOL(set_gpiop_ ## name);
 #else
@@ -531,10 +525,10 @@ unsigned short get_gpio_ ## name(unsigned gpio) \
 { \
 	unsigned long flags; \
 	unsigned short ret; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	ret = 0x01 & (gpio_bankb[gpio_bank(gpio)]->name >> gpio_sub_n(gpio)); \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 	return ret; \
 } \
 EXPORT_SYMBOL(get_gpio_ ## name);
@@ -564,10 +558,10 @@ unsigned short get_gpiop_ ## name(unsigned gpio) \
 { \
 	unsigned long flags; \
 	unsigned short ret; \
-	local_irq_save(flags); \
+	local_irq_save_hw(flags); \
 	ret = (gpio_bankb[gpio_bank(gpio)]->name); \
 	AWA_DUMMY_READ(name); \
-	local_irq_restore(flags); \
+	local_irq_restore_hw(flags); \
 	return ret; \
 } \
 EXPORT_SYMBOL(get_gpiop_ ## name);
@@ -617,10 +611,10 @@ int gpio_pm_wakeup_request(unsigned gpio, unsigned char type)
 	if ((check_gpio(gpio) < 0) || !type)
 		return -EINVAL;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	wakeup_map[gpio_bank(gpio)] |= gpio_bit(gpio);
 	wakeup_flags_map[gpio] = type;
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
@@ -633,11 +627,11 @@ void gpio_pm_wakeup_free(unsigned gpio)
 	if (check_gpio(gpio) < 0)
 		return;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	wakeup_map[gpio_bank(gpio)] &= ~gpio_bit(gpio);
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 EXPORT_SYMBOL(gpio_pm_wakeup_free);
 
@@ -679,7 +673,7 @@ u32 bfin_pm_standby_setup(void)
 		gpio_bankb[bank]->maskb = 0;
 
 		if (mask) {
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
 			gpio_bank_saved[bank].fer   = *port_fer[bank];
 #endif
 			gpio_bank_saved[bank].inen  = gpio_bankb[bank]->inen;
@@ -724,7 +718,7 @@ void bfin_pm_standby_restore(void)
 		bank = gpio_bank(i);
 
 		if (mask) {
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
 			*port_fer[bank]   	= gpio_bank_saved[bank].fer;
 #endif
 			gpio_bankb[bank]->inen  = gpio_bank_saved[bank].inen;
@@ -750,9 +744,9 @@ void bfin_gpio_pm_hibernate_suspend(void)
 	for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
 		bank = gpio_bank(i);
 
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
 			gpio_bank_saved[bank].fer   = *port_fer[bank];
-#ifdef BF527_FAMILY
+#if defined(BF527_FAMILY) || defined(BF518_FAMILY)
 			gpio_bank_saved[bank].mux   = *port_mux[bank];
 #else
 			if (bank == 0)
@@ -778,8 +772,8 @@ void bfin_gpio_pm_hibernate_restore(void)
 	for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
 			bank = gpio_bank(i);
 
-#if defined(BF527_FAMILY) || defined(BF537_FAMILY)
-#ifdef BF527_FAMILY
+#if defined(BF527_FAMILY) || defined(BF537_FAMILY) || defined(BF518_FAMILY)
+#if defined(BF527_FAMILY) || defined(BF518_FAMILY)
 			*port_mux[bank] = gpio_bank_saved[bank].mux;
 #else
 			if (bank == 0)
@@ -873,7 +867,6 @@ EXPORT_SYMBOL(get_gpio_dir);
 * MODIFICATION HISTORY :
 **************************************************************/
 
-#ifdef BF548_FAMILY
 int peripheral_request(unsigned short per, const char *label)
 {
 	unsigned long flags;
@@ -889,31 +882,35 @@ int peripheral_request(unsigned short per, const char *label)
 	if (!(per & P_DEFINED))
 		return -ENODEV;
 
-	if (check_gpio(ident) < 0)
-		return -EINVAL;
+	local_irq_save_hw(flags);
 
-	local_irq_save(flags);
-
-	if (unlikely(reserved_gpio_map[gpio_bank(ident)] & gpio_bit(ident))) {
+	/* If a pin can be muxed as either GPIO or peripheral, make
+	 * sure it is not already a GPIO pin when we request it.
+	 */
+	if (unlikely(!check_gpio(ident) &&
+	    reserved_gpio_map[gpio_bank(ident)] & gpio_bit(ident))) {
 		dump_stack();
 		printk(KERN_ERR
-		    "%s: Peripheral %d is already reserved as GPIO by %s !\n",
+		       "%s: Peripheral %d is already reserved as GPIO by %s !\n",
 		       __func__, ident, get_label(ident));
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return -EBUSY;
 	}
 
 	if (unlikely(reserved_peri_map[gpio_bank(ident)] & gpio_bit(ident))) {
 
-		u16 funct = get_portmux(ident);
-
 		/*
 		 * Pin functions like AMC address strobes my
 		 * be requested and used by several drivers
 		 */
 
-		if (!((per & P_MAYSHARE) && (funct == P_FUNCT2MUX(per)))) {
+#ifdef BF548_FAMILY
+		u16 funct = get_portmux(ident);
 
+		if (!((per & P_MAYSHARE) && (funct == P_FUNCT2MUX(per)))) {
+#else
+		if (!(per & P_MAYSHARE)) {
+#endif
 			/*
 			 * Allow that the identical pin function can
 			 * be requested from the same driver twice
@@ -926,7 +923,7 @@ int peripheral_request(unsigned short per, const char *label)
 			printk(KERN_ERR
 			       "%s: Peripheral %d function %d is already reserved by %s !\n",
 			       __func__, ident, P_FUNCT2MUX(per), get_label(ident));
-			local_irq_restore(flags);
+			local_irq_restore_hw(flags);
 			return -EBUSY;
 		}
 	}
@@ -934,89 +931,19 @@ int peripheral_request(unsigned short per, const char *label)
  anyway:
 	reserved_peri_map[gpio_bank(ident)] |= gpio_bit(ident);
 
+#ifdef BF548_FAMILY
 	portmux_setup(ident, P_FUNCT2MUX(per));
-	port_setup(ident, PERIPHERAL_USAGE);
-
-	local_irq_restore(flags);
-	set_label(ident, label);
-
-	return 0;
-}
-EXPORT_SYMBOL(peripheral_request);
 #else
-
-int peripheral_request(unsigned short per, const char *label)
-{
-	unsigned long flags;
-	unsigned short ident = P_IDENT(per);
-
-	/*
-	 * Don't cares are pins with only one dedicated function
-	 */
-
-	if (per & P_DONTCARE)
-		return 0;
-
-	if (!(per & P_DEFINED))
-		return -ENODEV;
-
-	local_irq_save(flags);
-
-	if (!check_gpio(ident)) {
-
-		if (unlikely(reserved_gpio_map[gpio_bank(ident)] & gpio_bit(ident))) {
-			dump_stack();
-			printk(KERN_ERR
-			       "%s: Peripheral %d is already reserved as GPIO by %s !\n",
-			       __func__, ident, get_label(ident));
-			local_irq_restore(flags);
-			return -EBUSY;
-		}
-
-	}
-
-	if (unlikely(reserved_peri_map[gpio_bank(ident)] & gpio_bit(ident))) {
-
-		/*
-		 * Pin functions like AMC address strobes my
-		 * be requested and used by several drivers
-		 */
-
-		if (!(per & P_MAYSHARE)) {
-
-			/*
-			 * Allow that the identical pin function can
-			 * be requested from the same driver twice
-			 */
-
-			if (cmp_label(ident, label) == 0)
-				goto anyway;
-
-			dump_stack();
-			printk(KERN_ERR
-			       "%s: Peripheral %d function %d is already"
-			       " reserved by %s !\n",
-			       __func__, ident, P_FUNCT2MUX(per),
-				get_label(ident));
-			local_irq_restore(flags);
-			return -EBUSY;
-		}
-
-	}
-
- anyway:
 	portmux_setup(per, P_FUNCT2MUX(per));
-
+#endif
 	port_setup(ident, PERIPHERAL_USAGE);
 
-	reserved_peri_map[gpio_bank(ident)] |= gpio_bit(ident);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 	set_label(ident, label);
 
 	return 0;
 }
 EXPORT_SYMBOL(peripheral_request);
-#endif
 
 int peripheral_request_list(const unsigned short per[], const char *label)
 {
@@ -1053,10 +980,10 @@ void peripheral_free(unsigned short per)
 	if (check_gpio(ident) < 0)
 		return;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	if (unlikely(!(reserved_peri_map[gpio_bank(ident)] & gpio_bit(ident)))) {
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return;
 	}
 
@@ -1067,7 +994,7 @@ void peripheral_free(unsigned short per)
 
 	set_label(ident, "free");
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 EXPORT_SYMBOL(peripheral_free);
 
@@ -1094,14 +1021,14 @@ EXPORT_SYMBOL(peripheral_free_list);
 * MODIFICATION HISTORY :
 **************************************************************/
 
-int gpio_request(unsigned gpio, const char *label)
+int bfin_gpio_request(unsigned gpio, const char *label)
 {
 	unsigned long flags;
 
 	if (check_gpio(gpio) < 0)
 		return -EINVAL;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	/*
 	 * Allow that the identical GPIO can
@@ -1110,15 +1037,15 @@ int gpio_request(unsigned gpio, const char *label)
 	 */
 
 	if (cmp_label(gpio, label) == 0) {
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return 0;
 	}
 
 	if (unlikely(reserved_gpio_map[gpio_bank(gpio)] & gpio_bit(gpio))) {
 		dump_stack();
 		printk(KERN_ERR "bfin-gpio: GPIO %d is already reserved by %s !\n",
-			 gpio, get_label(gpio));
-		local_irq_restore(flags);
+		       gpio, get_label(gpio));
+		local_irq_restore_hw(flags);
 		return -EBUSY;
 	}
 	if (unlikely(reserved_peri_map[gpio_bank(gpio)] & gpio_bit(gpio))) {
@@ -1126,34 +1053,37 @@ int gpio_request(unsigned gpio, const char *label)
 		printk(KERN_ERR
 		       "bfin-gpio: GPIO %d is already reserved as Peripheral by %s !\n",
 		       gpio, get_label(gpio));
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return -EBUSY;
 	}
+	if (unlikely(reserved_gpio_irq_map[gpio_bank(gpio)] & gpio_bit(gpio)))
+		printk(KERN_NOTICE "bfin-gpio: GPIO %d is already reserved as gpio-irq!"
+		       " (Documentation/blackfin/bfin-gpio-notes.txt)\n", gpio);
 
 	reserved_gpio_map[gpio_bank(gpio)] |= gpio_bit(gpio);
+	set_label(gpio, label);
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	port_setup(gpio, GPIO_USAGE);
-	set_label(gpio, label);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_request);
+EXPORT_SYMBOL(bfin_gpio_request);
 
-void gpio_free(unsigned gpio)
+void bfin_gpio_free(unsigned gpio)
 {
 	unsigned long flags;
 
 	if (check_gpio(gpio) < 0)
 		return;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 
 	if (unlikely(!(reserved_gpio_map[gpio_bank(gpio)] & gpio_bit(gpio)))) {
 		dump_stack();
 		gpio_error(gpio);
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 		return;
 	}
 
@@ -1161,13 +1091,76 @@ void gpio_free(unsigned gpio)
 
 	set_label(gpio, "free");
 
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
+}
+EXPORT_SYMBOL(bfin_gpio_free);
+
+int bfin_gpio_irq_request(unsigned gpio, const char *label)
+{
+	unsigned long flags;
+
+	if (check_gpio(gpio) < 0)
+		return -EINVAL;
+
+	local_irq_save_hw(flags);
+
+	if (unlikely(reserved_gpio_irq_map[gpio_bank(gpio)] & gpio_bit(gpio))) {
+		dump_stack();
+		printk(KERN_ERR
+		       "bfin-gpio: GPIO %d is already reserved as gpio-irq !\n",
+		       gpio);
+		local_irq_restore_hw(flags);
+		return -EBUSY;
+	}
+	if (unlikely(reserved_peri_map[gpio_bank(gpio)] & gpio_bit(gpio))) {
+		dump_stack();
+		printk(KERN_ERR
+		       "bfin-gpio: GPIO %d is already reserved as Peripheral by %s !\n",
+		       gpio, get_label(gpio));
+		local_irq_restore_hw(flags);
+		return -EBUSY;
+	}
+	if (unlikely(reserved_gpio_map[gpio_bank(gpio)] & gpio_bit(gpio)))
+		printk(KERN_NOTICE "bfin-gpio: GPIO %d is already reserved by %s! "
+		       "(Documentation/blackfin/bfin-gpio-notes.txt)\n",
+		       gpio, get_label(gpio));
+
+	reserved_gpio_irq_map[gpio_bank(gpio)] |= gpio_bit(gpio);
+	set_label(gpio, label);
+
+	local_irq_restore_hw(flags);
+
+	port_setup(gpio, GPIO_USAGE);
+
+	return 0;
+}
+
+void bfin_gpio_irq_free(unsigned gpio)
+{
+	unsigned long flags;
+
+	if (check_gpio(gpio) < 0)
+		return;
+
+	local_irq_save_hw(flags);
+
+	if (unlikely(!(reserved_gpio_irq_map[gpio_bank(gpio)] & gpio_bit(gpio)))) {
+		dump_stack();
+		gpio_error(gpio);
+		local_irq_restore_hw(flags);
+		return;
+	}
+
+	reserved_gpio_irq_map[gpio_bank(gpio)] &= ~gpio_bit(gpio);
+
+	set_label(gpio, "free");
+
+	local_irq_restore_hw(flags);
 }
-EXPORT_SYMBOL(gpio_free);
 
 
 #ifdef BF548_FAMILY
-int gpio_direction_input(unsigned gpio)
+int bfin_gpio_direction_input(unsigned gpio)
 {
 	unsigned long flags;
 
@@ -1176,16 +1169,16 @@ int gpio_direction_input(unsigned gpio)
 		return -EINVAL;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_array[gpio_bank(gpio)]->port_dir_clear = gpio_bit(gpio);
 	gpio_array[gpio_bank(gpio)]->port_inen |= gpio_bit(gpio);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_direction_input);
+EXPORT_SYMBOL(bfin_gpio_direction_input);
 
-int gpio_direction_output(unsigned gpio, int value)
+int bfin_gpio_direction_output(unsigned gpio, int value)
 {
 	unsigned long flags;
 
@@ -1194,30 +1187,30 @@ int gpio_direction_output(unsigned gpio, int value)
 		return -EINVAL;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_array[gpio_bank(gpio)]->port_inen &= ~gpio_bit(gpio);
 	gpio_set_value(gpio, value);
 	gpio_array[gpio_bank(gpio)]->port_dir_set = gpio_bit(gpio);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_direction_output);
+EXPORT_SYMBOL(bfin_gpio_direction_output);
 
-void gpio_set_value(unsigned gpio, int arg)
+void bfin_gpio_set_value(unsigned gpio, int arg)
 {
 	if (arg)
 		gpio_array[gpio_bank(gpio)]->port_set = gpio_bit(gpio);
 	else
 		gpio_array[gpio_bank(gpio)]->port_clear = gpio_bit(gpio);
 }
-EXPORT_SYMBOL(gpio_set_value);
+EXPORT_SYMBOL(bfin_gpio_set_value);
 
-int gpio_get_value(unsigned gpio)
+int bfin_gpio_get_value(unsigned gpio)
 {
 	return (1 & (gpio_array[gpio_bank(gpio)]->port_data >> gpio_sub_n(gpio)));
 }
-EXPORT_SYMBOL(gpio_get_value);
+EXPORT_SYMBOL(bfin_gpio_get_value);
 
 void bfin_gpio_irq_prepare(unsigned gpio)
 {
@@ -1225,34 +1218,34 @@ void bfin_gpio_irq_prepare(unsigned gpio)
 
 	port_setup(gpio, GPIO_USAGE);
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_array[gpio_bank(gpio)]->port_dir_clear = gpio_bit(gpio);
 	gpio_array[gpio_bank(gpio)]->port_inen |= gpio_bit(gpio);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
 
 #else
 
-int gpio_get_value(unsigned gpio)
+int bfin_gpio_get_value(unsigned gpio)
 {
 	unsigned long flags;
 	int ret;
 
 	if (unlikely(get_gpio_edge(gpio))) {
-		local_irq_save(flags);
+		local_irq_save_hw(flags);
 		set_gpio_edge(gpio, 0);
 		ret = get_gpio_data(gpio);
 		set_gpio_edge(gpio, 1);
-		local_irq_restore(flags);
+		local_irq_restore_hw(flags);
 
 		return ret;
 	} else
 		return get_gpio_data(gpio);
 }
-EXPORT_SYMBOL(gpio_get_value);
+EXPORT_SYMBOL(bfin_gpio_get_value);
 
 
-int gpio_direction_input(unsigned gpio)
+int bfin_gpio_direction_input(unsigned gpio)
 {
 	unsigned long flags;
 
@@ -1261,17 +1254,17 @@ int gpio_direction_input(unsigned gpio)
 		return -EINVAL;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_bankb[gpio_bank(gpio)]->dir &= ~gpio_bit(gpio);
 	gpio_bankb[gpio_bank(gpio)]->inen |= gpio_bit(gpio);
 	AWA_DUMMY_READ(inen);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_direction_input);
+EXPORT_SYMBOL(bfin_gpio_direction_input);
 
-int gpio_direction_output(unsigned gpio, int value)
+int bfin_gpio_direction_output(unsigned gpio, int value)
 {
 	unsigned long flags;
 
@@ -1280,7 +1273,7 @@ int gpio_direction_output(unsigned gpio, int value)
 		return -EINVAL;
 	}
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	gpio_bankb[gpio_bank(gpio)]->inen &= ~gpio_bit(gpio);
 
 	if (value)
@@ -1290,11 +1283,11 @@ int gpio_direction_output(unsigned gpio, int value)
 
 	gpio_bankb[gpio_bank(gpio)]->dir |= gpio_bit(gpio);
 	AWA_DUMMY_READ(dir);
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 	return 0;
 }
-EXPORT_SYMBOL(gpio_direction_output);
+EXPORT_SYMBOL(bfin_gpio_direction_output);
 
 /* If we are booting from SPI and our board lacks a strong enough pull up,
  * the core can reset and execute the bootrom faster than the resistor can
@@ -1327,14 +1320,17 @@ void bfin_gpio_irq_prepare(unsigned gpio)
 static int gpio_proc_read(char *buf, char **start, off_t offset,
 			  int len, int *unused_i, void *unused_v)
 {
-	int c, outlen = 0;
+	int c, irq, gpio, outlen = 0;
 
 	for (c = 0; c < MAX_RESOURCES; c++) {
-		if (!check_gpio(c) && (reserved_gpio_map[gpio_bank(c)] & gpio_bit(c)))
-			len = sprintf(buf, "GPIO_%d: %s \t\tGPIO %s\n", c,
-				 get_label(c), get_gpio_dir(c) ? "OUTPUT" : "INPUT");
+		irq = reserved_gpio_irq_map[gpio_bank(c)] & gpio_bit(c);
+		gpio = reserved_gpio_map[gpio_bank(c)] & gpio_bit(c);
+		if (!check_gpio(c) && (gpio || irq))
+			len = sprintf(buf, "GPIO_%d: \t%s%s \t\tGPIO %s\n", c,
+				 get_label(c), (gpio && irq) ? " *" : "",
+				 get_gpio_dir(c) ? "OUTPUT" : "INPUT");
 		else if (reserved_peri_map[gpio_bank(c)] & gpio_bit(c))
-			len = sprintf(buf, "GPIO_%d: %s \t\tPeripheral\n", c, get_label(c));
+			len = sprintf(buf, "GPIO_%d: \t%s \t\tPeripheral\n", c, get_label(c));
 		else
 			continue;
 		buf += len;
@@ -1354,3 +1350,57 @@ static __init int gpio_register_proc(void)
 }
 __initcall(gpio_register_proc);
 #endif
+
+#ifdef CONFIG_GPIOLIB
+int bfin_gpiolib_direction_input(struct gpio_chip *chip, unsigned gpio)
+{
+	return bfin_gpio_direction_input(gpio);
+}
+
+int bfin_gpiolib_direction_output(struct gpio_chip *chip, unsigned gpio, int level)
+{
+	return bfin_gpio_direction_output(gpio, level);
+}
+
+int bfin_gpiolib_get_value(struct gpio_chip *chip, unsigned gpio)
+{
+	return bfin_gpio_get_value(gpio);
+}
+
+void bfin_gpiolib_set_value(struct gpio_chip *chip, unsigned gpio, int value)
+{
+#ifdef BF548_FAMILY
+	return bfin_gpio_set_value(gpio, value);
+#else
+	return set_gpio_data(gpio, value);
+#endif
+}
+
+int bfin_gpiolib_gpio_request(struct gpio_chip *chip, unsigned gpio)
+{
+	return bfin_gpio_request(gpio, chip->label);
+}
+
+void bfin_gpiolib_gpio_free(struct gpio_chip *chip, unsigned gpio)
+{
+	return bfin_gpio_free(gpio);
+}
+
+static struct gpio_chip bfin_chip = {
+	.label			= "Blackfin-GPIOlib",
+	.direction_input	= bfin_gpiolib_direction_input,
+	.get			= bfin_gpiolib_get_value,
+	.direction_output	= bfin_gpiolib_direction_output,
+	.set			= bfin_gpiolib_set_value,
+	.request		= bfin_gpiolib_gpio_request,
+	.free			= bfin_gpiolib_gpio_free,
+	.base			= 0,
+	.ngpio			= MAX_BLACKFIN_GPIOS,
+};
+
+static int __init bfin_gpiolib_setup(void)
+{
+	return gpiochip_add(&bfin_chip);
+}
+arch_initcall(bfin_gpiolib_setup);
+#endif
diff --git a/arch/blackfin/kernel/bfin_ksyms.c b/arch/blackfin/kernel/bfin_ksyms.c
index 4367330909b..01f917d58b5 100644
--- a/arch/blackfin/kernel/bfin_ksyms.c
+++ b/arch/blackfin/kernel/bfin_ksyms.c
@@ -1,52 +1,25 @@
 /*
- * File:         arch/blackfin/kernel/bfin_ksyms.c
- * Based on:     none - original work
- * Author:
+ * arch/blackfin/kernel/bfin_ksyms.c - exports for random symbols
  *
- * Created:
- * Description:
+ * Copyright 2004-2008 Analog Devices Inc.
  *
- * 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
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/module.h>
-#include <linux/irq.h>
 #include <linux/uaccess.h>
 
-#include <asm/checksum.h>
 #include <asm/cacheflush.h>
 
-/* platform dependent support */
-
-EXPORT_SYMBOL(__ioremap);
-
-EXPORT_SYMBOL(ip_fast_csum);
-
-EXPORT_SYMBOL(kernel_thread);
-
-EXPORT_SYMBOL(is_in_rom);
+/* Allow people to have their own Blackfin exception handler in a module */
 EXPORT_SYMBOL(bfin_return_from_exception);
 
-/* Networking helper routines. */
-EXPORT_SYMBOL(csum_partial_copy);
+/* All the Blackfin cache functions: mach-common/cache.S */
+EXPORT_SYMBOL(blackfin_dcache_invalidate_range);
+EXPORT_SYMBOL(blackfin_icache_dcache_flush_range);
+EXPORT_SYMBOL(blackfin_icache_flush_range);
+EXPORT_SYMBOL(blackfin_dcache_flush_range);
+EXPORT_SYMBOL(blackfin_dflush_page);
 
 /* The following are special because they're not called
  * explicitly (the C compiler generates them).  Fortunately,
@@ -74,8 +47,6 @@ extern void __modsi3(void);
 extern void __muldi3(void);
 extern void __udivsi3(void);
 extern void __umodsi3(void);
-
-/* gcc lib functions */
 EXPORT_SYMBOL(__ashldi3);
 EXPORT_SYMBOL(__ashrdi3);
 EXPORT_SYMBOL(__umulsi3_highpart);
@@ -87,6 +58,7 @@ EXPORT_SYMBOL(__muldi3);
 EXPORT_SYMBOL(__udivsi3);
 EXPORT_SYMBOL(__umodsi3);
 
+/* Input/output symbols: lib/{in,out}s.S */
 EXPORT_SYMBOL(outsb);
 EXPORT_SYMBOL(insb);
 EXPORT_SYMBOL(outsw);
@@ -96,20 +68,39 @@ EXPORT_SYMBOL(insw_8);
 EXPORT_SYMBOL(outsl);
 EXPORT_SYMBOL(insl);
 EXPORT_SYMBOL(insl_16);
-EXPORT_SYMBOL(irq_flags);
-EXPORT_SYMBOL(iounmap);
-EXPORT_SYMBOL(blackfin_dcache_invalidate_range);
-EXPORT_SYMBOL(blackfin_icache_dcache_flush_range);
-EXPORT_SYMBOL(blackfin_icache_flush_range);
-EXPORT_SYMBOL(blackfin_dcache_flush_range);
-EXPORT_SYMBOL(blackfin_dflush_page);
 
-EXPORT_SYMBOL(csum_partial);
-EXPORT_SYMBOL(__init_begin);
-EXPORT_SYMBOL(__init_end);
-EXPORT_SYMBOL(_ebss_l1);
-EXPORT_SYMBOL(_stext_l1);
-EXPORT_SYMBOL(_etext_l1);
-EXPORT_SYMBOL(_sdata_l1);
-EXPORT_SYMBOL(_ebss_b_l1);
-EXPORT_SYMBOL(_sdata_b_l1);
+#ifdef CONFIG_SMP
+EXPORT_SYMBOL(__raw_atomic_update_asm);
+EXPORT_SYMBOL(__raw_atomic_clear_asm);
+EXPORT_SYMBOL(__raw_atomic_set_asm);
+EXPORT_SYMBOL(__raw_atomic_xor_asm);
+EXPORT_SYMBOL(__raw_atomic_test_asm);
+EXPORT_SYMBOL(__raw_xchg_1_asm);
+EXPORT_SYMBOL(__raw_xchg_2_asm);
+EXPORT_SYMBOL(__raw_xchg_4_asm);
+EXPORT_SYMBOL(__raw_cmpxchg_1_asm);
+EXPORT_SYMBOL(__raw_cmpxchg_2_asm);
+EXPORT_SYMBOL(__raw_cmpxchg_4_asm);
+EXPORT_SYMBOL(__raw_spin_is_locked_asm);
+EXPORT_SYMBOL(__raw_spin_lock_asm);
+EXPORT_SYMBOL(__raw_spin_trylock_asm);
+EXPORT_SYMBOL(__raw_spin_unlock_asm);
+EXPORT_SYMBOL(__raw_read_lock_asm);
+EXPORT_SYMBOL(__raw_read_trylock_asm);
+EXPORT_SYMBOL(__raw_read_unlock_asm);
+EXPORT_SYMBOL(__raw_write_lock_asm);
+EXPORT_SYMBOL(__raw_write_trylock_asm);
+EXPORT_SYMBOL(__raw_write_unlock_asm);
+EXPORT_SYMBOL(__raw_bit_set_asm);
+EXPORT_SYMBOL(__raw_bit_clear_asm);
+EXPORT_SYMBOL(__raw_bit_toggle_asm);
+EXPORT_SYMBOL(__raw_bit_test_asm);
+EXPORT_SYMBOL(__raw_bit_test_set_asm);
+EXPORT_SYMBOL(__raw_bit_test_clear_asm);
+EXPORT_SYMBOL(__raw_bit_test_toggle_asm);
+EXPORT_SYMBOL(__raw_uncached_fetch_asm);
+#ifdef __ARCH_SYNC_CORE_DCACHE
+EXPORT_SYMBOL(__raw_smp_mark_barrier_asm);
+EXPORT_SYMBOL(__raw_smp_check_barrier_asm);
+#endif
+#endif
diff --git a/arch/blackfin/kernel/cplb-mpu/Makefile b/arch/blackfin/kernel/cplb-mpu/Makefile
index 286b69357f9..7d70d3bf321 100644
--- a/arch/blackfin/kernel/cplb-mpu/Makefile
+++ b/arch/blackfin/kernel/cplb-mpu/Makefile
@@ -4,5 +4,7 @@
 
 obj-y := cplbinit.o cacheinit.o cplbmgr.o
 
-obj-$(CONFIG_CPLB_INFO) += cplbinfo.o
-
+CFLAGS_cplbmgr.o := -ffixed-I0 -ffixed-I1 -ffixed-I2 -ffixed-I3 \
+		    -ffixed-L0 -ffixed-L1 -ffixed-L2 -ffixed-L3 \
+		    -ffixed-M0 -ffixed-M1 -ffixed-M2 -ffixed-M3 \
+		    -ffixed-B0 -ffixed-B1 -ffixed-B2 -ffixed-B3
diff --git a/arch/blackfin/kernel/cplb-mpu/cacheinit.c b/arch/blackfin/kernel/cplb-mpu/cacheinit.c
index a8b712a24c5..c6ff947f9d3 100644
--- a/arch/blackfin/kernel/cplb-mpu/cacheinit.c
+++ b/arch/blackfin/kernel/cplb-mpu/cacheinit.c
@@ -25,7 +25,7 @@
 #include <asm/cplbinit.h>
 
 #if defined(CONFIG_BFIN_ICACHE)
-void __init bfin_icache_init(void)
+void __cpuinit bfin_icache_init(struct cplb_entry *icplb_tbl)
 {
 	unsigned long ctrl;
 	int i;
@@ -43,7 +43,7 @@ void __init bfin_icache_init(void)
 #endif
 
 #if defined(CONFIG_BFIN_DCACHE)
-void __init bfin_dcache_init(void)
+void __cpuinit bfin_dcache_init(struct cplb_entry *dcplb_tbl)
 {
 	unsigned long ctrl;
 	int i;
diff --git a/arch/blackfin/kernel/cplb-mpu/cplbinfo.c b/arch/blackfin/kernel/cplb-mpu/cplbinfo.c
deleted file mode 100644
index 822beefa3a4..00000000000
--- a/arch/blackfin/kernel/cplb-mpu/cplbinfo.c
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * File:         arch/blackfin/mach-common/cplbinfo.c
- * Based on:
- * Author:       Sonic Zhang <sonic.zhang@analog.com>
- *
- * Created:      Jan. 2005
- * Description:  Display CPLB status
- *
- * 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/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/proc_fs.h>
-#include <linux/uaccess.h>
-
-#include <asm/current.h>
-#include <asm/system.h>
-#include <asm/cplb.h>
-#include <asm/cplbinit.h>
-#include <asm/blackfin.h>
-
-static char page_size_string_table[][4] = { "1K", "4K", "1M", "4M" };
-
-static char *cplb_print_entry(char *buf, struct cplb_entry *tbl, int switched)
-{
-	int i;
-	buf += sprintf(buf, "Index\tAddress\t\tData\tSize\tU/RD\tU/WR\tS/WR\tSwitch\n");
-	for (i = 0; i < MAX_CPLBS; i++) {
-		unsigned long data = tbl[i].data;
-		unsigned long addr = tbl[i].addr;
-		if (!(data & CPLB_VALID))
-			continue;
-
-		buf +=
-		    sprintf(buf,
-			    "%d\t0x%08lx\t%06lx\t%s\t%c\t%c\t%c\t%c\n",
-			    i, addr, data,
-			    page_size_string_table[(data & 0x30000) >> 16],
-			    (data & CPLB_USER_RD) ? 'Y' : 'N',
-			    (data & CPLB_USER_WR) ? 'Y' : 'N',
-			    (data & CPLB_SUPV_WR) ? 'Y' : 'N',
-			    i < switched ? 'N' : 'Y');
-	}
-	buf += sprintf(buf, "\n");
-
-	return buf;
-}
-
-int cplbinfo_proc_output(char *buf)
-{
-	char *p;
-
-	p = buf;
-
-	p += sprintf(p, "------------------ CPLB Information ------------------\n\n");
-
-	if (bfin_read_IMEM_CONTROL() & ENICPLB) {
-		p += sprintf(p, "Instruction CPLB entry:\n");
-		p = cplb_print_entry(p, icplb_tbl, first_switched_icplb);
-	} else
-		p += sprintf(p, "Instruction CPLB is disabled.\n\n");
-
-	if (1 || bfin_read_DMEM_CONTROL() & ENDCPLB) {
-		p += sprintf(p, "Data CPLB entry:\n");
-		p = cplb_print_entry(p, dcplb_tbl, first_switched_dcplb);
-	} else
-		p += sprintf(p, "Data CPLB is disabled.\n");
-
-	p += sprintf(p, "ICPLB miss: %d\nICPLB supervisor miss: %d\n",
-		     nr_icplb_miss, nr_icplb_supv_miss);
-	p += sprintf(p, "DCPLB miss: %d\nDCPLB protection fault:%d\n",
-		     nr_dcplb_miss, nr_dcplb_prot);
-	p += sprintf(p, "CPLB flushes: %d\n",
-		     nr_cplb_flush);
-
-	return p - buf;
-}
-
-static int cplbinfo_read_proc(char *page, char **start, off_t off,
-			      int count, int *eof, void *data)
-{
-	int len;
-
-	len = cplbinfo_proc_output(page);
-	if (len <= off + count)
-		*eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len > count)
-		len = count;
-	if (len < 0)
-		len = 0;
-	return len;
-}
-
-static int __init cplbinfo_init(void)
-{
-	struct proc_dir_entry *entry;
-
-	entry = create_proc_entry("cplbinfo", 0, NULL);
-	if (!entry)
-		return -ENOMEM;
-
-	entry->read_proc = cplbinfo_read_proc;
-	entry->data = NULL;
-
-	return 0;
-}
-
-static void __exit cplbinfo_exit(void)
-{
-	remove_proc_entry("cplbinfo", NULL);
-}
-
-module_init(cplbinfo_init);
-module_exit(cplbinfo_exit);
diff --git a/arch/blackfin/kernel/cplb-mpu/cplbinit.c b/arch/blackfin/kernel/cplb-mpu/cplbinit.c
index 55af729f849..bdb958486e7 100644
--- a/arch/blackfin/kernel/cplb-mpu/cplbinit.c
+++ b/arch/blackfin/kernel/cplb-mpu/cplbinit.c
@@ -25,18 +25,19 @@
 #include <asm/blackfin.h>
 #include <asm/cplb.h>
 #include <asm/cplbinit.h>
+#include <asm/mem_map.h>
 
 #if ANOMALY_05000263
 # error the MPU will not function safely while Anomaly 05000263 applies
 #endif
 
-struct cplb_entry icplb_tbl[MAX_CPLBS];
-struct cplb_entry dcplb_tbl[MAX_CPLBS];
+struct cplb_entry icplb_tbl[NR_CPUS][MAX_CPLBS];
+struct cplb_entry dcplb_tbl[NR_CPUS][MAX_CPLBS];
 
 int first_switched_icplb, first_switched_dcplb;
 int first_mask_dcplb;
 
-void __init generate_cplb_tables(void)
+void __init generate_cplb_tables_cpu(unsigned int cpu)
 {
 	int i_d, i_i;
 	unsigned long addr;
@@ -55,15 +56,16 @@ void __init generate_cplb_tables(void)
 	d_cache |= CPLB_L1_AOW | CPLB_WT;
 #endif
 #endif
+
 	i_d = i_i = 0;
 
 	/* Set up the zero page.  */
-	dcplb_tbl[i_d].addr = 0;
-	dcplb_tbl[i_d++].data = SDRAM_OOPS | PAGE_SIZE_1KB;
+	dcplb_tbl[cpu][i_d].addr = 0;
+	dcplb_tbl[cpu][i_d++].data = SDRAM_OOPS | PAGE_SIZE_1KB;
 
 #if 0
-	icplb_tbl[i_i].addr = 0;
-	icplb_tbl[i_i++].data = i_cache | CPLB_USER_RD | PAGE_SIZE_4KB;
+	icplb_tbl[cpu][i_i].addr = 0;
+	icplb_tbl[cpu][i_i++].data = i_cache | CPLB_USER_RD | PAGE_SIZE_4KB;
 #endif
 
 	/* Cover kernel memory with 4M pages.  */
@@ -72,28 +74,28 @@ void __init generate_cplb_tables(void)
 	i_data = i_cache | CPLB_VALID | CPLB_PORTPRIO | PAGE_SIZE_4MB;
 
 	for (; addr < memory_start; addr += 4 * 1024 * 1024) {
-		dcplb_tbl[i_d].addr = addr;
-		dcplb_tbl[i_d++].data = d_data;
-		icplb_tbl[i_i].addr = addr;
-		icplb_tbl[i_i++].data = i_data | (addr == 0 ? CPLB_USER_RD : 0);
+		dcplb_tbl[cpu][i_d].addr = addr;
+		dcplb_tbl[cpu][i_d++].data = d_data;
+		icplb_tbl[cpu][i_i].addr = addr;
+		icplb_tbl[cpu][i_i++].data = i_data | (addr == 0 ? CPLB_USER_RD : 0);
 	}
 
 	/* Cover L1 memory.  One 4M area for code and data each is enough.  */
 #if L1_DATA_A_LENGTH > 0 || L1_DATA_B_LENGTH > 0
-	dcplb_tbl[i_d].addr = L1_DATA_A_START;
-	dcplb_tbl[i_d++].data = L1_DMEMORY | PAGE_SIZE_4MB;
+	dcplb_tbl[cpu][i_d].addr = get_l1_data_a_start_cpu(cpu);
+	dcplb_tbl[cpu][i_d++].data = L1_DMEMORY | PAGE_SIZE_4MB;
 #endif
 #if L1_CODE_LENGTH > 0
-	icplb_tbl[i_i].addr = L1_CODE_START;
-	icplb_tbl[i_i++].data = L1_IMEMORY | PAGE_SIZE_4MB;
+	icplb_tbl[cpu][i_i].addr = get_l1_code_start_cpu(cpu);
+	icplb_tbl[cpu][i_i++].data = L1_IMEMORY | PAGE_SIZE_4MB;
 #endif
 
 	/* Cover L2 memory */
 #if L2_LENGTH > 0
-	dcplb_tbl[i_d].addr = L2_START;
-	dcplb_tbl[i_d++].data = L2_DMEMORY | PAGE_SIZE_1MB;
-	icplb_tbl[i_i].addr = L2_START;
-	icplb_tbl[i_i++].data = L2_IMEMORY | PAGE_SIZE_1MB;
+	dcplb_tbl[cpu][i_d].addr = L2_START;
+	dcplb_tbl[cpu][i_d++].data = L2_DMEMORY | PAGE_SIZE_1MB;
+	icplb_tbl[cpu][i_i].addr = L2_START;
+	icplb_tbl[cpu][i_i++].data = L2_IMEMORY | PAGE_SIZE_1MB;
 #endif
 
 	first_mask_dcplb = i_d;
@@ -101,7 +103,11 @@ void __init generate_cplb_tables(void)
 	first_switched_icplb = i_i;
 
 	while (i_d < MAX_CPLBS)
-		dcplb_tbl[i_d++].data = 0;
+		dcplb_tbl[cpu][i_d++].data = 0;
 	while (i_i < MAX_CPLBS)
-		icplb_tbl[i_i++].data = 0;
+		icplb_tbl[cpu][i_i++].data = 0;
+}
+
+void generate_cplb_tables_all(void)
+{
 }
diff --git a/arch/blackfin/kernel/cplb-mpu/cplbmgr.c b/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
index baa52e261f0..87463ce87f5 100644
--- a/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
+++ b/arch/blackfin/kernel/cplb-mpu/cplbmgr.c
@@ -25,15 +25,21 @@
 #include <asm/cplbinit.h>
 #include <asm/mmu_context.h>
 
-#define FAULT_RW	(1 << 16)
-#define FAULT_USERSUPV	(1 << 17)
+/*
+ * WARNING
+ *
+ * This file is compiled with certain -ffixed-reg options.  We have to
+ * make sure not to call any functions here that could clobber these
+ * registers.
+ */
 
 int page_mask_nelts;
 int page_mask_order;
-unsigned long *current_rwx_mask;
+unsigned long *current_rwx_mask[NR_CPUS];
 
-int nr_dcplb_miss, nr_icplb_miss, nr_icplb_supv_miss, nr_dcplb_prot;
-int nr_cplb_flush;
+int nr_dcplb_miss[NR_CPUS], nr_icplb_miss[NR_CPUS];
+int nr_icplb_supv_miss[NR_CPUS], nr_dcplb_prot[NR_CPUS];
+int nr_cplb_flush[NR_CPUS];
 
 static inline void disable_dcplb(void)
 {
@@ -98,42 +104,42 @@ static inline int write_permitted(int status, unsigned long data)
 }
 
 /* Counters to implement round-robin replacement.  */
-static int icplb_rr_index, dcplb_rr_index;
+static int icplb_rr_index[NR_CPUS], dcplb_rr_index[NR_CPUS];
 
 /*
  * Find an ICPLB entry to be evicted and return its index.
  */
-static int evict_one_icplb(void)
+static int evict_one_icplb(unsigned int cpu)
 {
 	int i;
 	for (i = first_switched_icplb; i < MAX_CPLBS; i++)
-		if ((icplb_tbl[i].data & CPLB_VALID) == 0)
+		if ((icplb_tbl[cpu][i].data & CPLB_VALID) == 0)
 			return i;
-	i = first_switched_icplb + icplb_rr_index;
+	i = first_switched_icplb + icplb_rr_index[cpu];
 	if (i >= MAX_CPLBS) {
 		i -= MAX_CPLBS - first_switched_icplb;
-		icplb_rr_index -= MAX_CPLBS - first_switched_icplb;
+		icplb_rr_index[cpu] -= MAX_CPLBS - first_switched_icplb;
 	}
-	icplb_rr_index++;
+	icplb_rr_index[cpu]++;
 	return i;
 }
 
-static int evict_one_dcplb(void)
+static int evict_one_dcplb(unsigned int cpu)
 {
 	int i;
 	for (i = first_switched_dcplb; i < MAX_CPLBS; i++)
-		if ((dcplb_tbl[i].data & CPLB_VALID) == 0)
+		if ((dcplb_tbl[cpu][i].data & CPLB_VALID) == 0)
 			return i;
-	i = first_switched_dcplb + dcplb_rr_index;
+	i = first_switched_dcplb + dcplb_rr_index[cpu];
 	if (i >= MAX_CPLBS) {
 		i -= MAX_CPLBS - first_switched_dcplb;
-		dcplb_rr_index -= MAX_CPLBS - first_switched_dcplb;
+		dcplb_rr_index[cpu] -= MAX_CPLBS - first_switched_dcplb;
 	}
-	dcplb_rr_index++;
+	dcplb_rr_index[cpu]++;
 	return i;
 }
 
-static noinline int dcplb_miss(void)
+static noinline int dcplb_miss(unsigned int cpu)
 {
 	unsigned long addr = bfin_read_DCPLB_FAULT_ADDR();
 	int status = bfin_read_DCPLB_STATUS();
@@ -141,7 +147,7 @@ static noinline int dcplb_miss(void)
 	int idx;
 	unsigned long d_data;
 
-	nr_dcplb_miss++;
+	nr_dcplb_miss[cpu]++;
 
 	d_data = CPLB_SUPV_WR | CPLB_VALID | CPLB_DIRTY | PAGE_SIZE_4KB;
 #ifdef CONFIG_BFIN_DCACHE
@@ -168,25 +174,25 @@ static noinline int dcplb_miss(void)
 	} else if (addr >= _ramend) {
 	    d_data |= CPLB_USER_RD | CPLB_USER_WR;
 	} else {
-		mask = current_rwx_mask;
+		mask = current_rwx_mask[cpu];
 		if (mask) {
 			int page = addr >> PAGE_SHIFT;
-			int offs = page >> 5;
+			int idx = page >> 5;
 			int bit = 1 << (page & 31);
 
-			if (mask[offs] & bit)
+			if (mask[idx] & bit)
 				d_data |= CPLB_USER_RD;
 
 			mask += page_mask_nelts;
-			if (mask[offs] & bit)
+			if (mask[idx] & bit)
 				d_data |= CPLB_USER_WR;
 		}
 	}
-	idx = evict_one_dcplb();
+	idx = evict_one_dcplb(cpu);
 
 	addr &= PAGE_MASK;
-	dcplb_tbl[idx].addr = addr;
-	dcplb_tbl[idx].data = d_data;
+	dcplb_tbl[cpu][idx].addr = addr;
+	dcplb_tbl[cpu][idx].data = d_data;
 
 	disable_dcplb();
 	bfin_write32(DCPLB_DATA0 + idx * 4, d_data);
@@ -196,21 +202,21 @@ static noinline int dcplb_miss(void)
 	return 0;
 }
 
-static noinline int icplb_miss(void)
+static noinline int icplb_miss(unsigned int cpu)
 {
 	unsigned long addr = bfin_read_ICPLB_FAULT_ADDR();
 	int status = bfin_read_ICPLB_STATUS();
 	int idx;
 	unsigned long i_data;
 
-	nr_icplb_miss++;
+	nr_icplb_miss[cpu]++;
 
 	/* If inside the uncached DMA region, fault.  */
 	if (addr >= _ramend - DMA_UNCACHED_REGION && addr < _ramend)
 		return CPLB_PROT_VIOL;
 
 	if (status & FAULT_USERSUPV)
-		nr_icplb_supv_miss++;
+		nr_icplb_supv_miss[cpu]++;
 
 	/*
 	 * First, try to find a CPLB that matches this address.  If we
@@ -218,8 +224,8 @@ static noinline int icplb_miss(void)
 	 * that the instruction crosses a page boundary.
 	 */
 	for (idx = first_switched_icplb; idx < MAX_CPLBS; idx++) {
-		if (icplb_tbl[idx].data & CPLB_VALID) {
-			unsigned long this_addr = icplb_tbl[idx].addr;
+		if (icplb_tbl[cpu][idx].data & CPLB_VALID) {
+			unsigned long this_addr = icplb_tbl[cpu][idx].addr;
 			if (this_addr <= addr && this_addr + PAGE_SIZE > addr) {
 				addr += PAGE_SIZE;
 				break;
@@ -257,23 +263,23 @@ static noinline int icplb_miss(void)
 		 * Otherwise, check the x bitmap of the current process.
 		 */
 		if (!(status & FAULT_USERSUPV)) {
-			unsigned long *mask = current_rwx_mask;
+			unsigned long *mask = current_rwx_mask[cpu];
 
 			if (mask) {
 				int page = addr >> PAGE_SHIFT;
-				int offs = page >> 5;
+				int idx = page >> 5;
 				int bit = 1 << (page & 31);
 
 				mask += 2 * page_mask_nelts;
-				if (mask[offs] & bit)
+				if (mask[idx] & bit)
 					i_data |= CPLB_USER_RD;
 			}
 		}
 	}
-	idx = evict_one_icplb();
+	idx = evict_one_icplb(cpu);
 	addr &= PAGE_MASK;
-	icplb_tbl[idx].addr = addr;
-	icplb_tbl[idx].data = i_data;
+	icplb_tbl[cpu][idx].addr = addr;
+	icplb_tbl[cpu][idx].data = i_data;
 
 	disable_icplb();
 	bfin_write32(ICPLB_DATA0 + idx * 4, i_data);
@@ -283,19 +289,19 @@ static noinline int icplb_miss(void)
 	return 0;
 }
 
-static noinline int dcplb_protection_fault(void)
+static noinline int dcplb_protection_fault(unsigned int cpu)
 {
 	int status = bfin_read_DCPLB_STATUS();
 
-	nr_dcplb_prot++;
+	nr_dcplb_prot[cpu]++;
 
 	if (status & FAULT_RW) {
 		int idx = faulting_cplb_index(status);
-		unsigned long data = dcplb_tbl[idx].data;
+		unsigned long data = dcplb_tbl[cpu][idx].data;
 		if (!(data & CPLB_WT) && !(data & CPLB_DIRTY) &&
 		    write_permitted(status, data)) {
 			data |= CPLB_DIRTY;
-			dcplb_tbl[idx].data = data;
+			dcplb_tbl[cpu][idx].data = data;
 			bfin_write32(DCPLB_DATA0 + idx * 4, data);
 			return 0;
 		}
@@ -306,44 +312,45 @@ static noinline int dcplb_protection_fault(void)
 int cplb_hdr(int seqstat, struct pt_regs *regs)
 {
 	int cause = seqstat & 0x3f;
+	unsigned int cpu = smp_processor_id();
 	switch (cause) {
 	case 0x23:
-		return dcplb_protection_fault();
+		return dcplb_protection_fault(cpu);
 	case 0x2C:
-		return icplb_miss();
+		return icplb_miss(cpu);
 	case 0x26:
-		return dcplb_miss();
+		return dcplb_miss(cpu);
 	default:
 		return 1;
 	}
 }
 
-void flush_switched_cplbs(void)
+void flush_switched_cplbs(unsigned int cpu)
 {
 	int i;
 	unsigned long flags;
 
-	nr_cplb_flush++;
+	nr_cplb_flush[cpu]++;
 
-	local_irq_save(flags);
+	local_irq_save_hw(flags);
 	disable_icplb();
 	for (i = first_switched_icplb; i < MAX_CPLBS; i++) {
-		icplb_tbl[i].data = 0;
+		icplb_tbl[cpu][i].data = 0;
 		bfin_write32(ICPLB_DATA0 + i * 4, 0);
 	}
 	enable_icplb();
 
 	disable_dcplb();
 	for (i = first_switched_dcplb; i < MAX_CPLBS; i++) {
-		dcplb_tbl[i].data = 0;
+		dcplb_tbl[cpu][i].data = 0;
 		bfin_write32(DCPLB_DATA0 + i * 4, 0);
 	}
 	enable_dcplb();
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 
 }
 
-void set_mask_dcplbs(unsigned long *masks)
+void set_mask_dcplbs(unsigned long *masks, unsigned int cpu)
 {
 	int i;
 	unsigned long addr = (unsigned long)masks;
@@ -351,12 +358,12 @@ void set_mask_dcplbs(unsigned long *masks)
 	unsigned long flags;
 
 	if (!masks) {
-		current_rwx_mask = masks;
+		current_rwx_mask[cpu] = masks;
 		return;
 	}
 
-	local_irq_save(flags);
-	current_rwx_mask = masks;
+	local_irq_save_hw(flags);
+	current_rwx_mask[cpu] = masks;
 
 	d_data = CPLB_SUPV_WR | CPLB_VALID | CPLB_DIRTY | PAGE_SIZE_4KB;
 #ifdef CONFIG_BFIN_DCACHE
@@ -368,12 +375,12 @@ void set_mask_dcplbs(unsigned long *masks)
 
 	disable_dcplb();
 	for (i = first_mask_dcplb; i < first_switched_dcplb; i++) {
-		dcplb_tbl[i].addr = addr;
-		dcplb_tbl[i].data = d_data;
+		dcplb_tbl[cpu][i].addr = addr;
+		dcplb_tbl[cpu][i].data = d_data;
 		bfin_write32(DCPLB_DATA0 + i * 4, d_data);
 		bfin_write32(DCPLB_ADDR0 + i * 4, addr);
 		addr += PAGE_SIZE;
 	}
 	enable_dcplb();
-	local_irq_restore(flags);
+	local_irq_restore_hw(flags);
 }
diff --git a/arch/blackfin/kernel/cplb-nompu/Makefile b/arch/blackfin/kernel/cplb-nompu/Makefile
index d36ea9b5382..7d70d3bf321 100644
--- a/arch/blackfin/kernel/cplb-nompu/Makefile
+++ b/arch/blackfin/kernel/cplb-nompu/Makefile
@@ -2,7 +2,9 @@
 # arch/blackfin/kernel/cplb-nompu/Makefile
 #
 
-obj-y := cplbinit.o cacheinit.o cplbhdlr.o cplbmgr.o
-
-obj-$(CONFIG_CPLB_INFO) += cplbinfo.o
+obj-y := cplbinit.o cacheinit.o cplbmgr.o
 
+CFLAGS_cplbmgr.o := -ffixed-I0 -ffixed-I1 -ffixed-I2 -ffixed-I3 \
+		    -ffixed-L0 -ffixed-L1 -ffixed-L2 -ffixed-L3 \
+		    -ffixed-M0 -ffixed-M1 -ffixed-M2 -ffixed-M3 \
+		    -ffixed-B0 -ffixed-B1 -ffixed-B2 -ffixed-B3
diff --git a/arch/blackfin/kernel/cplb-nompu/cacheinit.c b/arch/blackfin/kernel/cplb-nompu/cacheinit.c
index bd0831592c2..c6ff947f9d3 100644
--- a/arch/blackfin/kernel/cplb-nompu/cacheinit.c
+++ b/arch/blackfin/kernel/cplb-nompu/cacheinit.c
@@ -25,19 +25,15 @@
 #include <asm/cplbinit.h>
 
 #if defined(CONFIG_BFIN_ICACHE)
-void __init bfin_icache_init(void)
+void __cpuinit bfin_icache_init(struct cplb_entry *icplb_tbl)
 {
-	unsigned long *table = icplb_table;
 	unsigned long ctrl;
 	int i;
 
+	SSYNC();
 	for (i = 0; i < MAX_CPLBS; i++) {
-		unsigned long addr = *table++;
-		unsigned long data = *table++;
-		if (addr == (unsigned long)-1)
-			break;
-		bfin_write32(ICPLB_ADDR0 + i * 4, addr);
-		bfin_write32(ICPLB_DATA0 + i * 4, data);
+		bfin_write32(ICPLB_ADDR0 + i * 4, icplb_tbl[i].addr);
+		bfin_write32(ICPLB_DATA0 + i * 4, icplb_tbl[i].data);
 	}
 	ctrl = bfin_read_IMEM_CONTROL();
 	ctrl |= IMC | ENICPLB;
@@ -47,20 +43,17 @@ void __init bfin_icache_init(void)
 #endif
 
 #if defined(CONFIG_BFIN_DCACHE)
-void __init bfin_dcache_init(void)
+void __cpuinit bfin_dcache_init(struct cplb_entry *dcplb_tbl)
 {
-	unsigned long *table = dcplb_table;
 	unsigned long ctrl;
 	int i;
 
+	SSYNC();
 	for (i = 0; i < MAX_CPLBS; i++) {
-		unsigned long addr = *table++;
-		unsigned long data = *table++;
-		if (addr == (unsigned long)-1)
-			break;
-		bfin_write32(DCPLB_ADDR0 + i * 4, addr);
-		bfin_write32(DCPLB_DATA0 + i * 4, data);
+		bfin_write32(DCPLB_ADDR0 + i * 4, dcplb_tbl[i].addr);
+		bfin_write32(DCPLB_DATA0 + i * 4, dcplb_tbl[i].data);
 	}
+
 	ctrl = bfin_read_DMEM_CONTROL();
 	ctrl |= DMEM_CNTR;
 	bfin_write_DMEM_CONTROL(ctrl);
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S b/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S
deleted file mode 100644
index ecbabc0a1fe..00000000000
--- a/arch/blackfin/kernel/cplb-nompu/cplbhdlr.S
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * File:         arch/blackfin/mach-common/cplbhdlr.S
- * Based on:
- * Author:       LG Soft India
- *
- * Created:      ?
- * Description:  CPLB exception handler
- *
- * 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 <asm/cplb.h>
-#include <asm/entry.h>
-
-#ifdef CONFIG_EXCPT_IRQ_SYSC_L1
-.section .l1.text
-#else
-.text
-#endif
-
-.type _cplb_mgr, STT_FUNC;
-.type _panic_cplb_error, STT_FUNC;
-
-.align 2
-
-ENTRY(__cplb_hdr)
-	R2 = SEQSTAT;
-
-	/* Mask the contents of SEQSTAT and leave only EXCAUSE in R2 */
-	R2 <<= 26;
-	R2 >>= 26;
-
-	R1 = 0x23; /* Data access CPLB protection violation */
-	CC = R2 == R1;
-	IF !CC JUMP .Lnot_data_write;
-	R0 = 2;		/* is a write to data space*/
-	JUMP .Lis_icplb_miss;
-
-.Lnot_data_write:
-	R1 = 0x2C; /* CPLB miss on an instruction fetch */
-	CC = R2 == R1;
-	R0 = 0;		/* is_data_miss == False*/
-	IF CC JUMP .Lis_icplb_miss;
-
-	R1 = 0x26;
-	CC = R2 == R1;
-	IF !CC JUMP .Lunknown;
-
-	R0 = 1;		/* is_data_miss == True*/
-
-.Lis_icplb_miss:
-
-#if defined(CONFIG_BFIN_ICACHE) || defined(CONFIG_BFIN_DCACHE)
-# if defined(CONFIG_BFIN_ICACHE) && !defined(CONFIG_BFIN_DCACHE)
-	R1 = CPLB_ENABLE_ICACHE;
-# endif
-# if !defined(CONFIG_BFIN_ICACHE) && defined(CONFIG_BFIN_DCACHE)
-	R1 = CPLB_ENABLE_DCACHE;
-# endif
-# if defined(CONFIG_BFIN_ICACHE) && defined(CONFIG_BFIN_DCACHE)
-	R1 = CPLB_ENABLE_DCACHE | CPLB_ENABLE_ICACHE;
-# endif
-#else
-	R1 = 0;
-#endif
-
-	[--SP] = RETS;
-	CALL _cplb_mgr;
-	RETS = [SP++];
-	CC = R0 == 0;
-	IF !CC JUMP .Lnot_replaced;
-	RTS;
-
-/*
- * Diagnostic exception handlers
- */
-.Lunknown:
-	R0 = CPLB_UNKNOWN_ERR;
-	JUMP .Lcplb_error;
-
-.Lnot_replaced:
-	CC = R0 == CPLB_NO_UNLOCKED;
-	IF !CC JUMP .Lnext_check;
-	R0 = CPLB_NO_UNLOCKED;
-	JUMP .Lcplb_error;
-
-.Lnext_check:
-	CC = R0 == CPLB_NO_ADDR_MATCH;
-	IF !CC JUMP .Lnext_check2;
-	R0 = CPLB_NO_ADDR_MATCH;
-	JUMP .Lcplb_error;
-
-.Lnext_check2:
-	CC = R0 == CPLB_PROT_VIOL;
-	IF !CC JUMP .Lstrange_return_from_cplb_mgr;
-	R0 = CPLB_PROT_VIOL;
-	JUMP .Lcplb_error;
-
-.Lstrange_return_from_cplb_mgr:
-	IDLE;
-	CSYNC;
-	JUMP .Lstrange_return_from_cplb_mgr;
-
-.Lcplb_error:
-	R1 = sp;
-	SP += -12;
-	call _panic_cplb_error;
-	SP += 12;
-	JUMP.L _handle_bad_cplb;
-
-ENDPROC(__cplb_hdr)
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbinfo.c b/arch/blackfin/kernel/cplb-nompu/cplbinfo.c
deleted file mode 100644
index 1e74f0b9799..00000000000
--- a/arch/blackfin/kernel/cplb-nompu/cplbinfo.c
+++ /dev/null
@@ -1,195 +0,0 @@
-/*
- * File:         arch/blackfin/mach-common/cplbinfo.c
- * Based on:
- * Author:       Sonic Zhang <sonic.zhang@analog.com>
- *
- * Created:      Jan. 2005
- * Description:  Display CPLB status
- *
- * 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/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/proc_fs.h>
-#include <linux/uaccess.h>
-
-#include <asm/cplbinit.h>
-#include <asm/blackfin.h>
-
-#define CPLB_I 1
-#define CPLB_D 2
-
-#define SYNC_SYS    SSYNC()
-#define SYNC_CORE   CSYNC()
-
-#define CPLB_BIT_PAGESIZE 0x30000
-
-static int page_size_table[4] = {
-	0x00000400,		/* 1K */
-	0x00001000,		/* 4K */
-	0x00100000,		/* 1M */
-	0x00400000		/* 4M */
-};
-
-static char page_size_string_table[][4] = { "1K", "4K", "1M", "4M" };
-
-static int cplb_find_entry(unsigned long *cplb_addr,
-			   unsigned long *cplb_data, unsigned long addr,
-			   unsigned long data)
-{
-	int ii;
-
-	for (ii = 0; ii < 16; ii++)
-		if (addr >= cplb_addr[ii] && addr < cplb_addr[ii] +
-		    page_size_table[(cplb_data[ii] & CPLB_BIT_PAGESIZE) >> 16]
-			&& (cplb_data[ii] == data))
-			return ii;
-
-	return -1;
-}
-
-static char *cplb_print_entry(char *buf, int type)
-{
-	unsigned long *p_addr = dpdt_table;
-	unsigned long *p_data = dpdt_table + 1;
-	unsigned long *p_icount = dpdt_swapcount_table;
-	unsigned long *p_ocount = dpdt_swapcount_table + 1;
-	unsigned long *cplb_addr = (unsigned long *)DCPLB_ADDR0;
-	unsigned long *cplb_data = (unsigned long *)DCPLB_DATA0;
-	int entry = 0, used_cplb = 0;
-
-	if (type == CPLB_I) {
-		buf += sprintf(buf, "Instruction CPLB entry:\n");
-		p_addr = ipdt_table;
-		p_data = ipdt_table + 1;
-		p_icount = ipdt_swapcount_table;
-		p_ocount = ipdt_swapcount_table + 1;
-		cplb_addr = (unsigned long *)ICPLB_ADDR0;
-		cplb_data = (unsigned long *)ICPLB_DATA0;
-	} else
-		buf += sprintf(buf, "Data CPLB entry:\n");
-
-	buf += sprintf(buf, "Address\t\tData\tSize\tValid\tLocked\tSwapin\tiCount\toCount\n");
-
-	while (*p_addr != 0xffffffff) {
-		entry = cplb_find_entry(cplb_addr, cplb_data, *p_addr, *p_data);
-		if (entry >= 0)
-			used_cplb |= 1 << entry;
-
-		buf +=
-		    sprintf(buf,
-			    "0x%08lx\t0x%05lx\t%s\t%c\t%c\t%2d\t%ld\t%ld\n",
-			    *p_addr, *p_data,
-			    page_size_string_table[(*p_data & 0x30000) >> 16],
-			    (*p_data & CPLB_VALID) ? 'Y' : 'N',
-			    (*p_data & CPLB_LOCK) ? 'Y' : 'N', entry, *p_icount,
-			    *p_ocount);
-
-		p_addr += 2;
-		p_data += 2;
-		p_icount += 2;
-		p_ocount += 2;
-	}
-
-	if (used_cplb != 0xffff) {
-		buf += sprintf(buf, "Unused/mismatched CPLBs:\n");
-
-		for (entry = 0; entry < 16; entry++)
-			if (0 == ((1 << entry) & used_cplb)) {
-				int flags = cplb_data[entry];
-				buf +=
-				    sprintf(buf,
-					    "%2d: 0x%08lx\t0x%05x\t%s\t%c\t%c\n",
-					    entry, cplb_addr[entry], flags,
-					    page_size_string_table[(flags &
-								    0x30000) >>
-								   16],
-					    (flags & CPLB_VALID) ? 'Y' : 'N',
-					    (flags & CPLB_LOCK) ? 'Y' : 'N');
-			}
-	}
-
-	buf += sprintf(buf, "\n");
-
-	return buf;
-}
-
-static int cplbinfo_proc_output(char *buf)
-{
-	char *p;
-
-	p = buf;
-
-	p += sprintf(p, "------------------ CPLB Information ------------------\n\n");
-
-	if (bfin_read_IMEM_CONTROL() & ENICPLB)
-		p = cplb_print_entry(p, CPLB_I);
-	else
-		p += sprintf(p, "Instruction CPLB is disabled.\n\n");
-
-	if (bfin_read_DMEM_CONTROL() & ENDCPLB)
-		p = cplb_print_entry(p, CPLB_D);
-	else
-		p += sprintf(p, "Data CPLB is disabled.\n");
-
-	return p - buf;
-}
-
-static int cplbinfo_read_proc(char *page, char **start, off_t off,
-			      int count, int *eof, void *data)
-{
-	int len;
-
-	len = cplbinfo_proc_output(page);
-	if (len <= off + count)
-		*eof = 1;
-	*start = page + off;
-	len -= off;
-	if (len > count)
-		len = count;
-	if (len < 0)
-		len = 0;
-	return len;
-}
-
-static int __init cplbinfo_init(void)
-{
-	struct proc_dir_entry *entry;
-
-	entry = create_proc_entry("cplbinfo", 0, NULL);
-	if (!entry)
-		return -ENOMEM;
-
-	entry->read_proc = cplbinfo_read_proc;
-	entry->data = NULL;
-
-	return 0;
-}
-
-static void __exit cplbinfo_exit(void)
-{
-	remove_proc_entry("cplbinfo", NULL);
-}
-
-module_init(cplbinfo_init);
-module_exit(cplbinfo_exit);
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbinit.c b/arch/blackfin/kernel/cplb-nompu/cplbinit.c
index 2debc900e24..0e28f759573 100644
--- a/arch/blackfin/kernel/cplb-nompu/cplbinit.c
+++ b/arch/blackfin/kernel/cplb-nompu/cplbinit.c
@@ -20,445 +20,152 @@
  * to the Free Software Foundation, Inc.,
  * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  */
+
 #include <linux/module.h>
 
 #include <asm/blackfin.h>
 #include <asm/cacheflush.h>
 #include <asm/cplb.h>
 #include <asm/cplbinit.h>
+#include <asm/mem_map.h>
 
-#define CPLB_MEM CONFIG_MAX_MEM_SIZE
-
-/*
-* Number of required data CPLB switchtable entries
-* MEMSIZE / 4 (we mostly install 4M page size CPLBs
-* approx 16 for smaller 1MB page size CPLBs for allignment purposes
-* 1 for L1 Data Memory
-* possibly 1 for L2 Data Memory
-* 1 for CONFIG_DEBUG_HUNT_FOR_ZERO
-* 1 for ASYNC Memory
-*/
-#define MAX_SWITCH_D_CPLBS (((CPLB_MEM / 4) + 16 + 1 + 1 + 1 \
-				 + ASYNC_MEMORY_CPLB_COVERAGE) * 2)
-
-/*
-* Number of required instruction CPLB switchtable entries
-* MEMSIZE / 4 (we mostly install 4M page size CPLBs
-* approx 12 for smaller 1MB page size CPLBs for allignment purposes
-* 1 for L1 Instruction Memory
-* possibly 1 for L2 Instruction Memory
-* 1 for CONFIG_DEBUG_HUNT_FOR_ZERO
-*/
-#define MAX_SWITCH_I_CPLBS (((CPLB_MEM / 4) + 12 + 1 + 1 + 1) * 2)
-
-
-u_long icplb_table[MAX_CPLBS + 1];
-u_long dcplb_table[MAX_CPLBS + 1];
-
-#ifdef CONFIG_CPLB_SWITCH_TAB_L1
-# define PDT_ATTR __attribute__((l1_data))
-#else
-# define PDT_ATTR
-#endif
-
-u_long ipdt_table[MAX_SWITCH_I_CPLBS + 1] PDT_ATTR;
-u_long dpdt_table[MAX_SWITCH_D_CPLBS + 1] PDT_ATTR;
+struct cplb_entry icplb_tbl[NR_CPUS][MAX_CPLBS] PDT_ATTR;
+struct cplb_entry dcplb_tbl[NR_CPUS][MAX_CPLBS] PDT_ATTR;
 
-#ifdef CONFIG_CPLB_INFO
-u_long ipdt_swapcount_table[MAX_SWITCH_I_CPLBS] PDT_ATTR;
-u_long dpdt_swapcount_table[MAX_SWITCH_D_CPLBS] PDT_ATTR;
-#endif
+int first_switched_icplb PDT_ATTR;
+int first_switched_dcplb PDT_ATTR;
 
-struct s_cplb {
-	struct cplb_tab init_i;
-	struct cplb_tab init_d;
-	struct cplb_tab switch_i;
-	struct cplb_tab switch_d;
-};
+struct cplb_boundary dcplb_bounds[9] PDT_ATTR;
+struct cplb_boundary icplb_bounds[7] PDT_ATTR;
 
-#if defined(CONFIG_BFIN_DCACHE) || defined(CONFIG_BFIN_ICACHE)
-static struct cplb_desc cplb_data[] = {
-	{
-		.start = 0,
-		.end = SIZE_1K,
-		.psize = SIZE_1K,
-		.attr = INITIAL_T | SWITCH_T | I_CPLB | D_CPLB,
-		.i_conf = SDRAM_OOPS,
-		.d_conf = SDRAM_OOPS,
-#if defined(CONFIG_DEBUG_HUNT_FOR_ZERO)
-		.valid = 1,
-#else
-		.valid = 0,
-#endif
-		.name = "Zero Pointer Guard Page",
-	},
-	{
-		.start = L1_CODE_START,
-		.end = L1_CODE_START + L1_CODE_LENGTH,
-		.psize = SIZE_4M,
-		.attr = INITIAL_T | SWITCH_T | I_CPLB,
-		.i_conf = L1_IMEMORY,
-		.d_conf = 0,
-		.valid = 1,
-		.name = "L1 I-Memory",
-	},
-	{
-		.start = L1_DATA_A_START,
-		.end = L1_DATA_B_START + L1_DATA_B_LENGTH,
-		.psize = SIZE_4M,
-		.attr = INITIAL_T | SWITCH_T | D_CPLB,
-		.i_conf = 0,
-		.d_conf = L1_DMEMORY,
-#if ((L1_DATA_A_LENGTH > 0) || (L1_DATA_B_LENGTH > 0))
-		.valid = 1,
-#else
-		.valid = 0,
-#endif
-		.name = "L1 D-Memory",
-	},
-	{
-		.start = 0,
-		.end = 0,  /* dynamic */
-		.psize = 0,
-		.attr = INITIAL_T | SWITCH_T | I_CPLB | D_CPLB,
-		.i_conf = SDRAM_IGENERIC,
-		.d_conf = SDRAM_DGENERIC,
-		.valid = 1,
-		.name = "Kernel Memory",
-	},
-	{
-		.start = 0, /* dynamic */
-		.end = 0, /* dynamic */
-		.psize = 0,
-		.attr = INITIAL_T | SWITCH_T | D_CPLB,
-		.i_conf = SDRAM_IGENERIC,
-		.d_conf = SDRAM_DNON_CHBL,
-		.valid = 1,
-		.name = "uClinux MTD Memory",
-	},
-	{
-		.start = 0, /* dynamic */
-		.end = 0,   /* dynamic */
-		.psize = SIZE_1M,
-		.attr = INITIAL_T | SWITCH_T | D_CPLB,
-		.d_conf = SDRAM_DNON_CHBL,
-		.valid = 1,
-		.name = "Uncached DMA Zone",
-	},
-	{
-		.start = 0, /* dynamic */
-		.end = 0, /* dynamic */
-		.psize = 0,
-		.attr = SWITCH_T | D_CPLB,
-		.i_conf = 0, /* dynamic */
-		.d_conf = 0, /* dynamic */
-		.valid = 1,
-		.name = "Reserved Memory",
-	},
-	{
-		.start = ASYNC_BANK0_BASE,
-		.end = ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE,
-		.psize = 0,
-		.attr = SWITCH_T | D_CPLB,
-		.d_conf = SDRAM_EBIU,
-		.valid = 1,
-		.name = "Asynchronous Memory Banks",
-	},
-	{
-		.start = L2_START,
-		.end = L2_START + L2_LENGTH,
-		.psize = SIZE_1M,
-		.attr = SWITCH_T | I_CPLB | D_CPLB,
-		.i_conf = L2_IMEMORY,
-		.d_conf = L2_DMEMORY,
-		.valid = (L2_LENGTH > 0),
-		.name = "L2 Memory",
-	},
-	{
-		.start = BOOT_ROM_START,
-		.end = BOOT_ROM_START + BOOT_ROM_LENGTH,
-		.psize = SIZE_1M,
-		.attr = SWITCH_T | I_CPLB | D_CPLB,
-		.i_conf = SDRAM_IGENERIC,
-		.d_conf = SDRAM_DGENERIC,
-		.valid = 1,
-		.name = "On-Chip BootROM",
-	},
-};
+int icplb_nr_bounds PDT_ATTR;
+int dcplb_nr_bounds PDT_ATTR;
 
-static u16 __init lock_kernel_check(u32 start, u32 end)
+void __init generate_cplb_tables_cpu(unsigned int cpu)
 {
-	if (start >= (u32)_end || end <= (u32)_stext)
-		return 0;
+	int i_d, i_i;
+	unsigned long addr;
 
-	/* This cplb block overlapped with kernel area. */
-	return IN_KERNEL;
-}
+	struct cplb_entry *d_tbl = dcplb_tbl[cpu];
+	struct cplb_entry *i_tbl = icplb_tbl[cpu];
 
-static unsigned short __init
-fill_cplbtab(struct cplb_tab *table,
-	     unsigned long start, unsigned long end,
-	     unsigned long block_size, unsigned long cplb_data)
-{
-	int i;
+	printk(KERN_INFO "NOMPU: setting up cplb tables\n");
 
-	switch (block_size) {
-	case SIZE_4M:
-		i = 3;
-		break;
-	case SIZE_1M:
-		i = 2;
-		break;
-	case SIZE_4K:
-		i = 1;
-		break;
-	case SIZE_1K:
-	default:
-		i = 0;
-		break;
-	}
-
-	cplb_data = (cplb_data & ~(3 << 16)) | (i << 16);
-
-	while ((start < end) && (table->pos < table->size)) {
+	i_d = i_i = 0;
 
-		table->tab[table->pos++] = start;
+	/* Set up the zero page.  */
+	d_tbl[i_d].addr = 0;
+	d_tbl[i_d++].data = SDRAM_OOPS | PAGE_SIZE_1KB;
 
-		if (lock_kernel_check(start, start + block_size) == IN_KERNEL)
-			table->tab[table->pos++] =
-			    cplb_data | CPLB_LOCK | CPLB_DIRTY;
-		else
-			table->tab[table->pos++] = cplb_data;
+	/* Cover kernel memory with 4M pages.  */
+	addr = 0;
 
-		start += block_size;
+	for (; addr < memory_start; addr += 4 * 1024 * 1024) {
+		d_tbl[i_d].addr = addr;
+		d_tbl[i_d++].data = SDRAM_DGENERIC | PAGE_SIZE_4MB;
+		i_tbl[i_i].addr = addr;
+		i_tbl[i_i++].data = SDRAM_IGENERIC | PAGE_SIZE_4MB;
 	}
-	return 0;
-}
 
-static unsigned short __init
-close_cplbtab(struct cplb_tab *table)
-{
-
-	while (table->pos < table->size) {
-
-		table->tab[table->pos++] = 0;
-		table->tab[table->pos++] = 0; /* !CPLB_VALID */
+	/* Cover L1 memory.  One 4M area for code and data each is enough.  */
+	if (L1_DATA_A_LENGTH || L1_DATA_B_LENGTH) {
+		d_tbl[i_d].addr = L1_DATA_A_START;
+		d_tbl[i_d++].data = L1_DMEMORY | PAGE_SIZE_4MB;
 	}
-	return 0;
-}
+	i_tbl[i_i].addr = L1_CODE_START;
+	i_tbl[i_i++].data = L1_IMEMORY | PAGE_SIZE_4MB;
 
-/* helper function */
-static void __init
-__fill_code_cplbtab(struct cplb_tab *t, int i, u32 a_start, u32 a_end)
-{
-	if (cplb_data[i].psize) {
-		fill_cplbtab(t,
-				cplb_data[i].start,
-				cplb_data[i].end,
-				cplb_data[i].psize,
-				cplb_data[i].i_conf);
-	} else {
-#if defined(CONFIG_BFIN_ICACHE)
-		if (ANOMALY_05000263 && i == SDRAM_KERN) {
-			fill_cplbtab(t,
-					cplb_data[i].start,
-					cplb_data[i].end,
-					SIZE_4M,
-					cplb_data[i].i_conf);
-		} else
-#endif
-		{
-			fill_cplbtab(t,
-					cplb_data[i].start,
-					a_start,
-					SIZE_1M,
-					cplb_data[i].i_conf);
-			fill_cplbtab(t,
-					a_start,
-					a_end,
-					SIZE_4M,
-					cplb_data[i].i_conf);
-			fill_cplbtab(t, a_end,
-					cplb_data[i].end,
-					SIZE_1M,
-					cplb_data[i].i_conf);
-		}
-	}
-}
+	first_switched_dcplb = i_d;
+	first_switched_icplb = i_i;
 
-static void __init
-__fill_data_cplbtab(struct cplb_tab *t, int i, u32 a_start, u32 a_end)
-{
-	if (cplb_data[i].psize) {
-		fill_cplbtab(t,
-				cplb_data[i].start,
-				cplb_data[i].end,
-				cplb_data[i].psize,
-				cplb_data[i].d_conf);
-	} else {
-		fill_cplbtab(t,
-				cplb_data[i].start,
-				a_start, SIZE_1M,
-				cplb_data[i].d_conf);
-		fill_cplbtab(t, a_start,
-				a_end, SIZE_4M,
-				cplb_data[i].d_conf);
-		fill_cplbtab(t, a_end,
-				cplb_data[i].end,
-				SIZE_1M,
-				cplb_data[i].d_conf);
-	}
+	BUG_ON(first_switched_dcplb > MAX_CPLBS);
+	BUG_ON(first_switched_icplb > MAX_CPLBS);
+
+	while (i_d < MAX_CPLBS)
+		d_tbl[i_d++].data = 0;
+	while (i_i < MAX_CPLBS)
+		i_tbl[i_i++].data = 0;
 }
 
-void __init generate_cplb_tables(void)
+void __init generate_cplb_tables_all(void)
 {
+	int i_d, i_i;
 
-	u16 i, j, process;
-	u32 a_start, a_end, as, ae, as_1m;
-
-	struct cplb_tab *t_i = NULL;
-	struct cplb_tab *t_d = NULL;
-	struct s_cplb cplb;
-
-	printk(KERN_INFO "NOMPU: setting up cplb tables for global access\n");
-
-	cplb.init_i.size = MAX_CPLBS;
-	cplb.init_d.size = MAX_CPLBS;
-	cplb.switch_i.size = MAX_SWITCH_I_CPLBS;
-	cplb.switch_d.size = MAX_SWITCH_D_CPLBS;
-
-	cplb.init_i.pos = 0;
-	cplb.init_d.pos = 0;
-	cplb.switch_i.pos = 0;
-	cplb.switch_d.pos = 0;
-
-	cplb.init_i.tab = icplb_table;
-	cplb.init_d.tab = dcplb_table;
-	cplb.switch_i.tab = ipdt_table;
-	cplb.switch_d.tab = dpdt_table;
-
-	cplb_data[SDRAM_KERN].end = memory_end;
-
+	i_d = 0;
+	/* Normal RAM, including MTD FS.  */
 #ifdef CONFIG_MTD_UCLINUX
-	cplb_data[SDRAM_RAM_MTD].start = memory_mtd_start;
-	cplb_data[SDRAM_RAM_MTD].end = memory_mtd_start + mtd_size;
-	cplb_data[SDRAM_RAM_MTD].valid = mtd_size > 0;
-# if defined(CONFIG_ROMFS_FS)
-	cplb_data[SDRAM_RAM_MTD].attr |= I_CPLB;
-
-	/*
-	 * The ROMFS_FS size is often not multiple of 1MB.
-	 * This can cause multiple CPLB sets covering the same memory area.
-	 * This will then cause multiple CPLB hit exceptions.
-	 * Workaround: We ensure a contiguous memory area by extending the kernel
-	 * memory section over the mtd section.
-	 * For ROMFS_FS memory must be covered with ICPLBs anyways.
-	 * So there is no difference between kernel and mtd memory setup.
-	 */
-
-	cplb_data[SDRAM_KERN].end = memory_mtd_start + mtd_size;;
-	cplb_data[SDRAM_RAM_MTD].valid = 0;
-
-# endif
+	dcplb_bounds[i_d].eaddr = memory_mtd_start + mtd_size;
 #else
-	cplb_data[SDRAM_RAM_MTD].valid = 0;
+	dcplb_bounds[i_d].eaddr = memory_end;
 #endif
+	dcplb_bounds[i_d++].data = SDRAM_DGENERIC;
+	/* DMA uncached region.  */
+	if (DMA_UNCACHED_REGION) {
+		dcplb_bounds[i_d].eaddr = _ramend;
+		dcplb_bounds[i_d++].data = SDRAM_DNON_CHBL;
+	}
+	if (_ramend != physical_mem_end) {
+		/* Reserved memory.  */
+		dcplb_bounds[i_d].eaddr = physical_mem_end;
+		dcplb_bounds[i_d++].data = (reserved_mem_dcache_on ?
+					    SDRAM_DGENERIC : SDRAM_DNON_CHBL);
+	}
+	/* Addressing hole up to the async bank.  */
+	dcplb_bounds[i_d].eaddr = ASYNC_BANK0_BASE;
+	dcplb_bounds[i_d++].data = 0;
+	/* ASYNC banks.  */
+	dcplb_bounds[i_d].eaddr = ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE;
+	dcplb_bounds[i_d++].data = SDRAM_EBIU;
+	/* Addressing hole up to BootROM.  */
+	dcplb_bounds[i_d].eaddr = BOOT_ROM_START;
+	dcplb_bounds[i_d++].data = 0;
+	/* BootROM -- largest one should be less than 1 meg.  */
+	dcplb_bounds[i_d].eaddr = BOOT_ROM_START + (1 * 1024 * 1024);
+	dcplb_bounds[i_d++].data = SDRAM_DGENERIC;
+	if (L2_LENGTH) {
+		/* Addressing hole up to L2 SRAM.  */
+		dcplb_bounds[i_d].eaddr = L2_START;
+		dcplb_bounds[i_d++].data = 0;
+		/* L2 SRAM.  */
+		dcplb_bounds[i_d].eaddr = L2_START + L2_LENGTH;
+		dcplb_bounds[i_d++].data = L2_DMEMORY;
+	}
+	dcplb_nr_bounds = i_d;
+	BUG_ON(dcplb_nr_bounds > ARRAY_SIZE(dcplb_bounds));
 
-	cplb_data[SDRAM_DMAZ].start = _ramend - DMA_UNCACHED_REGION;
-	cplb_data[SDRAM_DMAZ].end = _ramend;
-
-	cplb_data[RES_MEM].start = _ramend;
-	cplb_data[RES_MEM].end = physical_mem_end;
-
-	if (reserved_mem_dcache_on)
-		cplb_data[RES_MEM].d_conf = SDRAM_DGENERIC;
-	else
-		cplb_data[RES_MEM].d_conf = SDRAM_DNON_CHBL;
-
-	if (reserved_mem_icache_on)
-		cplb_data[RES_MEM].i_conf = SDRAM_IGENERIC;
-	else
-		cplb_data[RES_MEM].i_conf = SDRAM_INON_CHBL;
-
-	for (i = ZERO_P; i < ARRAY_SIZE(cplb_data); ++i) {
-		if (!cplb_data[i].valid)
-			continue;
-
-		as_1m = cplb_data[i].start % SIZE_1M;
-
-		/* We need to make sure all sections are properly 1M aligned
-		 * However between Kernel Memory and the Kernel mtd section, depending on the
-		 * rootfs size, there can be overlapping memory areas.
-		 */
-
-		if (as_1m && i != L1I_MEM && i != L1D_MEM) {
+	i_i = 0;
+	/* Normal RAM, including MTD FS.  */
 #ifdef CONFIG_MTD_UCLINUX
-			if (i == SDRAM_RAM_MTD) {
-				if ((cplb_data[SDRAM_KERN].end + 1) > cplb_data[SDRAM_RAM_MTD].start)
-					cplb_data[SDRAM_RAM_MTD].start = (cplb_data[i].start & (-2*SIZE_1M)) + SIZE_1M;
-				else
-					cplb_data[SDRAM_RAM_MTD].start = (cplb_data[i].start & (-2*SIZE_1M));
-			} else
+	icplb_bounds[i_i].eaddr = memory_mtd_start + mtd_size;
+#else
+	icplb_bounds[i_i].eaddr = memory_end;
 #endif
-				printk(KERN_WARNING "Unaligned Start of %s at 0x%X\n",
-				       cplb_data[i].name, cplb_data[i].start);
-		}
-
-		as = cplb_data[i].start % SIZE_4M;
-		ae = cplb_data[i].end % SIZE_4M;
-
-		if (as)
-			a_start = cplb_data[i].start + (SIZE_4M - (as));
-		else
-			a_start = cplb_data[i].start;
-
-		a_end = cplb_data[i].end - ae;
-
-		for (j = INITIAL_T; j <= SWITCH_T; j++) {
-
-			switch (j) {
-			case INITIAL_T:
-				if (cplb_data[i].attr & INITIAL_T) {
-					t_i = &cplb.init_i;
-					t_d = &cplb.init_d;
-					process = 1;
-				} else
-					process = 0;
-				break;
-			case SWITCH_T:
-				if (cplb_data[i].attr & SWITCH_T) {
-					t_i = &cplb.switch_i;
-					t_d = &cplb.switch_d;
-					process = 1;
-				} else
-					process = 0;
-				break;
-			default:
-					process = 0;
-				break;
-			}
-
-			if (!process)
-				continue;
-			if (cplb_data[i].attr & I_CPLB)
-				__fill_code_cplbtab(t_i, i, a_start, a_end);
-
-			if (cplb_data[i].attr & D_CPLB)
-				__fill_data_cplbtab(t_d, i, a_start, a_end);
-		}
+	icplb_bounds[i_i++].data = SDRAM_IGENERIC;
+	/* DMA uncached region.  */
+	if (DMA_UNCACHED_REGION) {
+		icplb_bounds[i_i].eaddr = _ramend;
+		icplb_bounds[i_i++].data = 0;
 	}
-
-/* close tables */
-
-	close_cplbtab(&cplb.init_i);
-	close_cplbtab(&cplb.init_d);
-
-	cplb.init_i.tab[cplb.init_i.pos] = -1;
-	cplb.init_d.tab[cplb.init_d.pos] = -1;
-	cplb.switch_i.tab[cplb.switch_i.pos] = -1;
-	cplb.switch_d.tab[cplb.switch_d.pos] = -1;
-
+	if (_ramend != physical_mem_end) {
+		/* Reserved memory.  */
+		icplb_bounds[i_i].eaddr = physical_mem_end;
+		icplb_bounds[i_i++].data = (reserved_mem_icache_on ?
+					    SDRAM_IGENERIC : SDRAM_INON_CHBL);
+	}
+	/* Addressing hole up to BootROM.  */
+	icplb_bounds[i_i].eaddr = BOOT_ROM_START;
+	icplb_bounds[i_i++].data = 0;
+	/* BootROM -- largest one should be less than 1 meg.  */
+	icplb_bounds[i_i].eaddr = BOOT_ROM_START + (1 * 1024 * 1024);
+	icplb_bounds[i_i++].data = SDRAM_IGENERIC;
+	if (L2_LENGTH) {
+		/* Addressing hole up to L2 SRAM, including the async bank.  */
+		icplb_bounds[i_i].eaddr = L2_START;
+		icplb_bounds[i_i++].data = 0;
+		/* L2 SRAM.  */
+		icplb_bounds[i_i].eaddr = L2_START + L2_LENGTH;
+		icplb_bounds[i_i++].data = L2_IMEMORY;
+	}
+	icplb_nr_bounds = i_i;
+	BUG_ON(icplb_nr_bounds > ARRAY_SIZE(icplb_bounds));
 }
-
-#endif
-
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbmgr.S b/arch/blackfin/kernel/cplb-nompu/cplbmgr.S
deleted file mode 100644
index f5cf3accef3..00000000000
--- a/arch/blackfin/kernel/cplb-nompu/cplbmgr.S
+++ /dev/null
@@ -1,646 +0,0 @@
-/*
- * File:         arch/blackfin/mach-common/cplbmgtr.S
- * Based on:
- * Author:       LG Soft India
- *
- * Created:      ?
- * Description:  CPLB replacement routine for CPLB mismatch
- *
- * 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
- */
-
-/* Usage: int _cplb_mgr(is_data_miss,int enable_cache)
- * is_data_miss==2 => Mark as Dirty, write to the clean data page
- * is_data_miss==1 => Replace a data CPLB.
- * is_data_miss==0 => Replace an instruction CPLB.
- *
- * Returns:
- * CPLB_RELOADED	=> Successfully updated CPLB table.
- * CPLB_NO_UNLOCKED	=> All CPLBs are locked, so cannot be evicted.
- *			   This indicates that the CPLBs in the configuration
- *			   tablei are badly configured, as this should never
- *			   occur.
- * CPLB_NO_ADDR_MATCH	=> The address being accessed, that triggered the
- *			   exception, is not covered by any of the CPLBs in
- *			   the configuration table. The application is
- *			   presumably misbehaving.
- * CPLB_PROT_VIOL	=> The address being accessed, that triggered the
- *			   exception, was not a first-write to a clean Write
- *			   Back Data page, and so presumably is a genuine
- *			   violation of the page's protection attributes.
- *			   The application is misbehaving.
- */
-
-#include <linux/linkage.h>
-#include <asm/blackfin.h>
-#include <asm/cplb.h>
-
-#ifdef CONFIG_EXCPT_IRQ_SYSC_L1
-.section .l1.text
-#else
-.text
-#endif
-
-.align 2;
-ENTRY(_cplb_mgr)
-
-	[--SP]=( R7:4,P5:3 );
-
-	CC = R0 == 2;
-	IF CC JUMP .Ldcplb_write;
-
-	CC = R0 == 0;
-	IF !CC JUMP .Ldcplb_miss_compare;
-
-	/* ICPLB Miss Exception. We need to choose one of the
-	* currently-installed CPLBs, and replace it with one
-	* from the configuration table.
-	*/
-
-	/* A multi-word instruction can cross a page boundary. This means the
-	 * first part of the instruction can be in a valid page, but the
-	 * second part is not, and hence generates the instruction miss.
-	 * However, the fault address is for the start of the instruction,
-	 * not the part that's in the bad page. Therefore, we have to check
-	 * whether the fault address applies to a page that is already present
-	 * in the table.
-	 */
-
-	P4.L = LO(ICPLB_FAULT_ADDR);
-	P4.H = HI(ICPLB_FAULT_ADDR);
-
-	P1 = 16;
-	P5.L = _page_size_table;
-	P5.H = _page_size_table;
-
-	P0.L = LO(ICPLB_DATA0);
-	P0.H = HI(ICPLB_DATA0);
-	R4 = [P4];		/* Get faulting address*/
-	R6 = 64;		/* Advance past the fault address, which*/
-	R6 = R6 + R4;		/* we'll use if we find a match*/
-	R3 = ((16 << 8) | 2);	/* Extract mask, two bits at posn 16 */
-
-	R5 = 0;
-.Lisearch:
-
-	R1 = [P0-0x100];	/* Address for this CPLB */
-
-	R0 = [P0++];		/* Info for this CPLB*/
-	CC = BITTST(R0,0);	/* Is the CPLB valid?*/
-	IF !CC JUMP .Lnomatch;	/* Skip it, if not.*/
-	CC = R4 < R1(IU);	/* If fault address less than page start*/
-	IF CC JUMP .Lnomatch;	/* then skip this one.*/
-	R2 = EXTRACT(R0,R3.L) (Z);	/* Get page size*/
-	P1 = R2;
-	P1 = P5 + (P1<<2);	/* index into page-size table*/
-	R2 = [P1];		/* Get the page size*/
-	R1 = R1 + R2;		/* and add to page start, to get page end*/
-	CC = R4 < R1(IU);	/* and see whether fault addr is in page.*/
-	IF !CC R4 = R6;		/* If so, advance the address and finish loop.*/
-	IF !CC JUMP .Lisearch_done;
-.Lnomatch:
-	/* Go around again*/
-	R5 += 1;
-	CC = BITTST(R5, 4);	/* i.e CC = R5 >= 16*/
-	IF !CC JUMP .Lisearch;
-
-.Lisearch_done:
-	I0 = R4;		/* Fault address we'll search for*/
-
-	/* set up pointers */
-	P0.L = LO(ICPLB_DATA0);
-	P0.H = HI(ICPLB_DATA0);
-
-	/* The replacement procedure for ICPLBs */
-
-	P4.L = LO(IMEM_CONTROL);
-	P4.H = HI(IMEM_CONTROL);
-
-	/* Turn off CPLBs while we work, necessary according to HRM before
-	 * modifying CPLB descriptors
-	 */
-	R5 = [P4];		/* Control Register*/
-	BITCLR(R5,ENICPLB_P);
-	CLI R1;
-	SSYNC;		/* SSYNC required before writing to IMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R1;
-
-	R1 = -1;		/* end point comparison */
-	R3 = 16;		/* counter */
-
-	/* Search through CPLBs for first non-locked entry */
-	/* Overwrite it by moving everyone else up by 1 */
-.Licheck_lock:
-	R0 = [P0++];
-	R3 = R3 + R1;
-	CC = R3 == R1;
-	IF CC JUMP .Lall_locked;
-	CC = BITTST(R0, 0);		/* an invalid entry is good */
-	IF !CC JUMP .Lifound_victim;
-	CC = BITTST(R0,1);		/* but a locked entry isn't */
-	IF CC JUMP .Licheck_lock;
-
-.Lifound_victim:
-#ifdef CONFIG_CPLB_INFO
-	R7 = [P0 - 0x104];
-	P2.L = _ipdt_table;
-	P2.H = _ipdt_table;
-	P3.L = _ipdt_swapcount_table;
-	P3.H = _ipdt_swapcount_table;
-	P3 += -4;
-.Licount:
-	R2 = [P2];	/* address from config table */
-	P2 += 8;
-	P3 += 8;
-	CC = R2==-1;
-	IF CC JUMP .Licount_done;
-	CC = R7==R2;
-	IF !CC JUMP .Licount;
-	R7 = [P3];
-	R7 += 1;
-	[P3] = R7;
-	CSYNC;
-.Licount_done:
-#endif
-	LC0=R3;
-	LSETUP(.Lis_move,.Lie_move) LC0;
-.Lis_move:
-	R0 = [P0];
-	[P0 - 4] = R0;
-	R0 = [P0 - 0x100];
-	[P0-0x104] = R0;
-.Lie_move:
-	P0+=4;
-
-	/* Clear ICPLB_DATA15, in case we don't find a replacement
-	 * otherwise, we would have a duplicate entry, and will crash
-	 */
-	R0 = 0;
-	[P0 - 4] = R0;
-
-	/* We've made space in the ICPLB table, so that ICPLB15
-	 * is now free to be overwritten. Next, we have to determine
-	 * which CPLB we need to install, from the configuration
-	 * table. This is a matter of getting the start-of-page
-	 * addresses and page-lengths from the config table, and
-	 * determining whether the fault address falls within that
-	 * range.
- 	 */
-
-	P2.L = _ipdt_table;
-	P2.H = _ipdt_table;
-#ifdef	CONFIG_CPLB_INFO
-	P3.L = _ipdt_swapcount_table;
-	P3.H = _ipdt_swapcount_table;
-	P3 += -8;
-#endif
-	P0.L = _page_size_table;
-	P0.H = _page_size_table;
-
-	/* Retrieve our fault address (which may have been advanced
-	 * because the faulting instruction crossed a page boundary).
-	 */
-
-	R0 = I0;
-
-	/* An extraction pattern, to get the page-size bits from
-	 * the CPLB data entry. Bits 16-17, so two bits at posn 16.
-	 */
-
-	R1 = ((16<<8)|2);
-.Linext:	R4 = [P2++];	/* address from config table */
-	R2 = [P2++];	/* data from config table */
-#ifdef	CONFIG_CPLB_INFO
-	P3 += 8;
-#endif
-
-	CC = R4 == -1;	/* End of config table*/
-	IF CC JUMP .Lno_page_in_table;
-
-	/* See if failed address > start address */
-	CC = R4 <= R0(IU);
-	IF !CC JUMP .Linext;
-
-	/* extract page size (17:16)*/
-	R3 = EXTRACT(R2, R1.L) (Z);
-
-	/* add page size to addr to get range */
-
-	P5 = R3;
-	P5 = P0 + (P5 << 2);	/* scaled, for int access*/
-	R3 = [P5];
-	R3 = R3 + R4;
-
-	/* See if failed address < (start address + page size) */
-	CC = R0 < R3(IU);
-	IF !CC JUMP .Linext;
-
-	/* We've found a CPLB in the config table that covers
-	 * the faulting address, so install this CPLB into the
-	 * last entry of the table.
-	 */
-
-	P1.L = LO(ICPLB_DATA15);		/* ICPLB_DATA15 */
-	P1.H = HI(ICPLB_DATA15);
-	[P1] = R2;
-	[P1-0x100] = R4;
-#ifdef	CONFIG_CPLB_INFO
-	R3 = [P3];
-	R3 += 1;
-	[P3] = R3;
-#endif
-
-	/* P4 points to IMEM_CONTROL, and R5 contains its old
-	 * value, after we disabled ICPLBS. Re-enable them.
-	 */
-
-	BITSET(R5,ENICPLB_P);
-	CLI R2;
-	SSYNC;		/* SSYNC required before writing to IMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R2;
-
-	( R7:4,P5:3 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-
-/* FAILED CASES*/
-.Lno_page_in_table:
-	R0 = CPLB_NO_ADDR_MATCH;
-	JUMP .Lfail_ret;
-
-.Lall_locked:
-	R0 = CPLB_NO_UNLOCKED;
-	JUMP .Lfail_ret;
-
-.Lprot_violation:
-	R0 = CPLB_PROT_VIOL;
-
-.Lfail_ret:
-	/* Make sure we turn protection/cache back on, even in the failing case */
-	BITSET(R5,ENICPLB_P);
-	CLI R2;
-	SSYNC;          /* SSYNC required before writing to IMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R2;
-
-	( R7:4,P5:3 ) = [SP++];
-	RTS;
-
-.Ldcplb_write:
-
-	/* if a DCPLB is marked as write-back (CPLB_WT==0), and
-	 * it is clean (CPLB_DIRTY==0), then a write to the
-	 * CPLB's page triggers a protection violation. We have to
-	 * mark the CPLB as dirty, to indicate that there are
-	 * pending writes associated with the CPLB.
-	 */
-
-	P4.L = LO(DCPLB_STATUS);
-	P4.H = HI(DCPLB_STATUS);
-	P3.L = LO(DCPLB_DATA0);
-	P3.H = HI(DCPLB_DATA0);
-	R5 = [P4];
-
-	/* A protection violation can be caused by more than just writes
-	 * to a clean WB page, so we have to ensure that:
-	 * - It's a write
-	 * - to a clean WB page
-	 * - and is allowed in the mode the access occurred.
-	 */
-
-	CC = BITTST(R5, 16);	/* ensure it was a write*/
-	IF !CC JUMP .Lprot_violation;
-
-	/* to check the rest, we have to retrieve the DCPLB.*/
-
-	/* The low half of DCPLB_STATUS is a bit mask*/
-
-	R2 = R5.L (Z);	/* indicating which CPLB triggered the event.*/
-	R3 = 30;	/* so we can use this to determine the offset*/
-	R2.L = SIGNBITS R2;
-	R2 = R2.L (Z);	/* into the DCPLB table.*/
-	R3 = R3 - R2;
-	P4 = R3;
-	P3 = P3 + (P4<<2);
-	R3 = [P3];	/* Retrieve the CPLB*/
-
-	/* Now we can check whether it's a clean WB page*/
-
-	CC = BITTST(R3, 14);	/* 0==WB, 1==WT*/
-	IF CC JUMP .Lprot_violation;
-	CC = BITTST(R3, 7);	/* 0 == clean, 1 == dirty*/
-	IF CC JUMP .Lprot_violation;
-
-	/* Check whether the write is allowed in the mode that was active.*/
-
-	R2 = 1<<3;		/* checking write in user mode*/
-	CC = BITTST(R5, 17);	/* 0==was user, 1==was super*/
-	R5 = CC;
-	R2 <<= R5;		/* if was super, check write in super mode*/
-	R2 = R3 & R2;
-	CC = R2 == 0;
-	IF CC JUMP .Lprot_violation;
-
-	/* It's a genuine write-to-clean-page.*/
-
-	BITSET(R3, 7);		/* mark as dirty*/
-	[P3] = R3;		/* and write back.*/
-	NOP;
-	CSYNC;
-	( R7:4,P5:3 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-
-.Ldcplb_miss_compare:
-
-	/* Data CPLB Miss event. We need to choose a CPLB to
-	 * evict, and then locate a new CPLB to install from the
-	 * config table, that covers the faulting address.
-	 */
-
-	P1.L = LO(DCPLB_DATA15);
-	P1.H = HI(DCPLB_DATA15);
-
-	P4.L = LO(DCPLB_FAULT_ADDR);
-	P4.H = HI(DCPLB_FAULT_ADDR);
-	R4 = [P4];
-	I0 = R4;
-
-	/* The replacement procedure for DCPLBs*/
-
-	R6 = R1;	/* Save for later*/
-
-	/* Turn off CPLBs while we work.*/
-	P4.L = LO(DMEM_CONTROL);
-	P4.H = HI(DMEM_CONTROL);
-	R5 = [P4];
-	BITCLR(R5,ENDCPLB_P);
-	CLI R0;
-	SSYNC;		/* SSYNC required before writing to DMEM_CONTROL. */
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R0;
-
-	/* Start looking for a CPLB to evict. Our order of preference
-	 * is: invalid CPLBs, clean CPLBs, dirty CPLBs. Locked CPLBs
-	 * are no good.
-	 */
-
-	I1.L = LO(DCPLB_DATA0);
-	I1.H = HI(DCPLB_DATA0);
-	P1 = 2;
-	P2 = 16;
-	I2.L = _dcplb_preference;
-	I2.H = _dcplb_preference;
-	LSETUP(.Lsdsearch1, .Ledsearch1) LC0 = P1;
-.Lsdsearch1:
-	R0 = [I2++];		/* Get the bits we're interested in*/
-	P0 = I1;		/* Go back to start of table*/
-	LSETUP (.Lsdsearch2, .Ledsearch2) LC1 = P2;
-.Lsdsearch2:
-	R1 = [P0++];		/* Fetch each installed CPLB in turn*/
-	R2 = R1 & R0;		/* and test for interesting bits.*/
-	CC = R2 == 0;		/* If none are set, it'll do.*/
-	IF !CC JUMP .Lskip_stack_check;
-
-	R2 = [P0 - 0x104]; 	/* R2 - PageStart */
-	P3.L = _page_size_table; /* retrieve end address */
-	P3.H = _page_size_table; /* retrieve end address */
-	R3 = 0x1002;		/* 16th - position, 2 bits -length */
-#if ANOMALY_05000209
-	nop;			/* Anomaly 05000209 */
-#endif
-	R7 = EXTRACT(R1,R3.l);
-	R7 = R7 << 2;		/* Page size index offset */
-	P5 = R7;
-	P3 = P3 + P5;
-	R7 = [P3];		/* page size in bytes */
-
-	R7 = R2 + R7;		/* R7 - PageEnd */
-	R4 = SP; 		/* Test SP is in range */
-
-	CC = R7 < R4;		/* if PageEnd < SP */
-	IF CC JUMP .Ldfound_victim;
-	R3 = 0x284;		/* stack length from start of trap till
-				 * the point.
-				 * 20 stack locations for future modifications
-				 */
-	R4 = R4 + R3;
-	CC = R4 < R2;		/* if SP + stacklen < PageStart */
-	IF CC JUMP .Ldfound_victim;
-.Lskip_stack_check:
-
-.Ledsearch2: NOP;
-.Ledsearch1: NOP;
-
-	/* If we got here, we didn't find a DCPLB we considered
-	 * replacable, which means all of them were locked.
-	 */
-
-	JUMP .Lall_locked;
-.Ldfound_victim:
-
-#ifdef CONFIG_CPLB_INFO
-	R7 = [P0 - 0x104];
-	P2.L = _dpdt_table;
-	P2.H = _dpdt_table;
-	P3.L = _dpdt_swapcount_table;
-	P3.H = _dpdt_swapcount_table;
-	P3 += -4;
-.Ldicount:
-	R2 = [P2];
-	P2 += 8;
-	P3 += 8;
-	CC = R2==-1;
-	IF CC JUMP .Ldicount_done;
-	CC = R7==R2;
-	IF !CC JUMP .Ldicount;
-	R7 = [P3];
-	R7 += 1;
-	[P3] = R7;
-.Ldicount_done:
-#endif
-
-	/* Clean down the hardware loops*/
-	R2 = 0;
-	LC1 = R2;
-	LC0 = R2;
-
-	/* There's a suitable victim in [P0-4] (because we've
-	 * advanced already).
-	 */
-
-.LDdoverwrite:
-
-	/* [P0-4] is a suitable victim CPLB, so we want to
-	 * overwrite it by moving all the following CPLBs
-	 * one space closer to the start.
-	 */
-
-	R1.L = LO(DCPLB_DATA16);		/* DCPLB_DATA15 + 4 */
-	R1.H = HI(DCPLB_DATA16);
-	R0 = P0;
-
-	/* If the victim happens to be in DCPLB15,
-	 * we don't need to move anything.
-	 */
-
-	CC = R1 == R0;
-	IF CC JUMP .Lde_moved;
-	R1 = R1 - R0;
-	R1 >>= 2;
-	P1 = R1;
-	LSETUP(.Lds_move, .Lde_move) LC0=P1;
-.Lds_move:
-	R0 = [P0++];	/* move data */
-	[P0 - 8] = R0;
-	R0 = [P0-0x104]	/* move address */
-.Lde_move:
-	 [P0-0x108] = R0;
-
-.Lde_moved:
-	NOP;
-
-	/* Clear DCPLB_DATA15, in case we don't find a replacement
-	 * otherwise, we would have a duplicate entry, and will crash
-	 */
-	R0 = 0;
-	[P0 - 0x4] = R0;
-
-	/* We've now made space in DCPLB15 for the new CPLB to be
-	 * installed. The next stage is to locate a CPLB in the
-	 * config table that covers the faulting address.
-	 */
-
-	R0 = I0;		/* Our faulting address */
-
-	P2.L = _dpdt_table;
-	P2.H = _dpdt_table;
-#ifdef	CONFIG_CPLB_INFO
-	P3.L = _dpdt_swapcount_table;
-	P3.H = _dpdt_swapcount_table;
-	P3 += -8;
-#endif
-
-	P1.L = _page_size_table;
-	P1.H = _page_size_table;
-
-	/* An extraction pattern, to retrieve bits 17:16.*/
-
-	R1 = (16<<8)|2;
-.Ldnext:	R4 = [P2++];	/* address */
-	R2 = [P2++];	/* data */
-#ifdef	CONFIG_CPLB_INFO
-	P3 += 8;
-#endif
-
-	CC = R4 == -1;
-	IF CC JUMP .Lno_page_in_table;
-
-	/* See if failed address > start address */
-	CC = R4 <= R0(IU);
-	IF !CC JUMP .Ldnext;
-
-	/* extract page size (17:16)*/
-	R3 = EXTRACT(R2, R1.L) (Z);
-
-	/* add page size to addr to get range */
-
-	P5 = R3;
-	P5 = P1 + (P5 << 2);
-	R3 = [P5];
-	R3 = R3 + R4;
-
-	/* See if failed address < (start address + page size) */
-	CC = R0 < R3(IU);
-	IF !CC JUMP .Ldnext;
-
-	/* We've found the CPLB that should be installed, so
-	 * write it into CPLB15, masking off any caching bits
-	 * if necessary.
-	 */
-
-	P1.L = LO(DCPLB_DATA15);
-	P1.H = HI(DCPLB_DATA15);
-
-	/* If the DCPLB has cache bits set, but caching hasn't
-	 * been enabled, then we want to mask off the cache-in-L1
-	 * bit before installing. Moreover, if caching is off, we
-	 * also want to ensure that the DCPLB has WT mode set, rather
-	 * than WB, since WB pages still trigger first-write exceptions
-	 * even when not caching is off, and the page isn't marked as
-	 * cachable. Finally, we could mark the page as clean, not dirty,
-	 * but we choose to leave that decision to the user; if the user
-	 * chooses to have a CPLB pre-defined as dirty, then they always
-	 * pay the cost of flushing during eviction, but don't pay the
-	 * cost of first-write exceptions to mark the page as dirty.
-	 */
-
-#ifdef CONFIG_BFIN_WT
-	BITSET(R6, 14);		/* Set WT*/
-#endif
-
-	[P1] = R2;
-	[P1-0x100] = R4;
-#ifdef	CONFIG_CPLB_INFO
-	R3 = [P3];
-	R3 += 1;
-	[P3] = R3;
-#endif
-
-	/* We've installed the CPLB, so re-enable CPLBs. P4
-	 * points to DMEM_CONTROL, and R5 is the value we
-	 * last wrote to it, when we were disabling CPLBs.
-	 */
-
-	BITSET(R5,ENDCPLB_P);
-	CLI R2;
-	.align 8;
-	[P4] = R5;
-	SSYNC;
-	STI R2;
-
-	( R7:4,P5:3 ) = [SP++];
-	R0 = CPLB_RELOADED;
-	RTS;
-ENDPROC(_cplb_mgr)
-
-.data
-.align 4;
-_page_size_table:
-.byte4	0x00000400;	/* 1K */
-.byte4	0x00001000;	/* 4K */
-.byte4	0x00100000;	/* 1M */
-.byte4	0x00400000;	/* 4M */
-
-.align 4;
-_dcplb_preference:
-.byte4	0x00000001;	/* valid bit */
-.byte4	0x00000002;	/* lock bit */
diff --git a/arch/blackfin/kernel/cplb-nompu/cplbmgr.c b/arch/blackfin/kernel/cplb-nompu/cplbmgr.c
new file mode 100644
index 00000000000..376249ab269
--- /dev/null
+++ b/arch/blackfin/kernel/cplb-nompu/cplbmgr.c
@@ -0,0 +1,283 @@
+/*
+ * File:         arch/blackfin/kernel/cplb-nompu-c/cplbmgr.c
+ * Based on:     arch/blackfin/kernel/cplb-mpu/cplbmgr.c
+ * Author:       Michael McTernan <mmcternan@airvana.com>
+ *
+ * Created:      01Nov2008
+ * Description:  CPLB miss handler.
+ *
+ * Modified:
+ *               Copyright 2008 Airvana Inc.
+ *               Copyright 2004-2007 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.
+ */
+
+#include <linux/kernel.h>
+#include <asm/blackfin.h>
+#include <asm/cplbinit.h>
+#include <asm/cplb.h>
+#include <asm/mmu_context.h>
+
+/*
+ * WARNING
+ *
+ * This file is compiled with certain -ffixed-reg options.  We have to
+ * make sure not to call any functions here that could clobber these
+ * registers.
+ */
+
+int nr_dcplb_miss[NR_CPUS], nr_icplb_miss[NR_CPUS];
+int nr_dcplb_supv_miss[NR_CPUS], nr_icplb_supv_miss[NR_CPUS];
+int nr_cplb_flush[NR_CPUS], nr_dcplb_prot[NR_CPUS];
+
+#ifdef CONFIG_EXCPT_IRQ_SYSC_L1
+#define MGR_ATTR __attribute__((l1_text))
+#else
+#define MGR_ATTR
+#endif
+
+/*
+ * We're in an exception handler.  The normal cli nop nop workaround
+ * isn't going to do very much, as the only thing that can interrupt
+ * us is an NMI, and the cli isn't going to stop that.
+ */
+#define NOWA_SSYNC __asm__ __volatile__ ("ssync;")
+
+/* Anomaly handlers provide SSYNCs, so avoid extra if anomaly is present */
+#if ANOMALY_05000125
+
+#define bfin_write_DMEM_CONTROL_SSYNC(v)    bfin_write_DMEM_CONTROL(v)
+#define bfin_write_IMEM_CONTROL_SSYNC(v)    bfin_write_IMEM_CONTROL(v)
+
+#else
+
+#define bfin_write_DMEM_CONTROL_SSYNC(v) \
+    do { NOWA_SSYNC; bfin_write_DMEM_CONTROL(v); NOWA_SSYNC; } while (0)
+#define bfin_write_IMEM_CONTROL_SSYNC(v) \
+    do { NOWA_SSYNC; bfin_write_IMEM_CONTROL(v); NOWA_SSYNC; } while (0)
+
+#endif
+
+static inline void write_dcplb_data(int cpu, int idx, unsigned long data,
+				    unsigned long addr)
+{
+	unsigned long ctrl = bfin_read_DMEM_CONTROL();
+	bfin_write_DMEM_CONTROL_SSYNC(ctrl & ~ENDCPLB);
+	bfin_write32(DCPLB_DATA0 + idx * 4, data);
+	bfin_write32(DCPLB_ADDR0 + idx * 4, addr);
+	bfin_write_DMEM_CONTROL_SSYNC(ctrl);
+
+#ifdef CONFIG_CPLB_INFO
+	dcplb_tbl[cpu][idx].addr = addr;
+	dcplb_tbl[cpu][idx].data = data;
+#endif
+}
+
+static inline void write_icplb_data(int cpu, int idx, unsigned long data,
+				    unsigned long addr)
+{
+	unsigned long ctrl = bfin_read_IMEM_CONTROL();
+
+	bfin_write_IMEM_CONTROL_SSYNC(ctrl & ~ENICPLB);
+	bfin_write32(ICPLB_DATA0 + idx * 4, data);
+	bfin_write32(ICPLB_ADDR0 + idx * 4, addr);
+	bfin_write_IMEM_CONTROL_SSYNC(ctrl);
+
+#ifdef CONFIG_CPLB_INFO
+	icplb_tbl[cpu][idx].addr = addr;
+	icplb_tbl[cpu][idx].data = data;
+#endif
+}
+
+/*
+ * Given the contents of the status register, return the index of the
+ * CPLB that caused the fault.
+ */
+static inline int faulting_cplb_index(int status)
+{
+	int signbits = __builtin_bfin_norm_fr1x32(status & 0xFFFF);
+	return 30 - signbits;
+}
+
+/*
+ * Given the contents of the status register and the DCPLB_DATA contents,
+ * return true if a write access should be permitted.
+ */
+static inline int write_permitted(int status, unsigned long data)
+{
+	if (status & FAULT_USERSUPV)
+		return !!(data & CPLB_SUPV_WR);
+	else
+		return !!(data & CPLB_USER_WR);
+}
+
+/* Counters to implement round-robin replacement.  */
+static int icplb_rr_index[NR_CPUS] PDT_ATTR;
+static int dcplb_rr_index[NR_CPUS] PDT_ATTR;
+
+/*
+ * Find an ICPLB entry to be evicted and return its index.
+ */
+static int evict_one_icplb(int cpu)
+{
+	int i = first_switched_icplb + icplb_rr_index[cpu];
+	if (i >= MAX_CPLBS) {
+		i -= MAX_CPLBS - first_switched_icplb;
+		icplb_rr_index[cpu] -= MAX_CPLBS - first_switched_icplb;
+	}
+	icplb_rr_index[cpu]++;
+	return i;
+}
+
+static int evict_one_dcplb(int cpu)
+{
+	int i = first_switched_dcplb + dcplb_rr_index[cpu];
+	if (i >= MAX_CPLBS) {
+		i -= MAX_CPLBS - first_switched_dcplb;
+		dcplb_rr_index[cpu] -= MAX_CPLBS - first_switched_dcplb;
+	}
+	dcplb_rr_index[cpu]++;
+	return i;
+}
+
+MGR_ATTR static int icplb_miss(int cpu)
+{
+	unsigned long addr = bfin_read_ICPLB_FAULT_ADDR();
+	int status = bfin_read_ICPLB_STATUS();
+	int idx;
+	unsigned long i_data, base, addr1, eaddr;
+
+	nr_icplb_miss[cpu]++;
+	if (unlikely(status & FAULT_USERSUPV))
+		nr_icplb_supv_miss[cpu]++;
+
+	base = 0;
+	for (idx = 0; idx < icplb_nr_bounds; idx++) {
+		eaddr = icplb_bounds[idx].eaddr;
+		if (addr < eaddr)
+			break;
+		base = eaddr;
+	}
+	if (unlikely(idx == icplb_nr_bounds))
+		return CPLB_NO_ADDR_MATCH;
+
+	i_data = icplb_bounds[idx].data;
+	if (unlikely(i_data == 0))
+		return CPLB_NO_ADDR_MATCH;
+
+	addr1 = addr & ~(SIZE_4M - 1);
+	addr &= ~(SIZE_1M - 1);
+	i_data |= PAGE_SIZE_1MB;
+	if (addr1 >= base && (addr1 + SIZE_4M) <= eaddr) {
+		/*
+		 * This works because
+		 * (PAGE_SIZE_4MB & PAGE_SIZE_1MB) == PAGE_SIZE_1MB.
+		 */
+		i_data |= PAGE_SIZE_4MB;
+		addr = addr1;
+	}
+
+	/* Pick entry to evict */
+	idx = evict_one_icplb(cpu);
+
+	write_icplb_data(cpu, idx, i_data, addr);
+
+	return CPLB_RELOADED;
+}
+
+MGR_ATTR static int dcplb_miss(int cpu)
+{
+	unsigned long addr = bfin_read_DCPLB_FAULT_ADDR();
+	int status = bfin_read_DCPLB_STATUS();
+	int idx;
+	unsigned long d_data, base, addr1, eaddr;
+
+	nr_dcplb_miss[cpu]++;
+	if (unlikely(status & FAULT_USERSUPV))
+		nr_dcplb_supv_miss[cpu]++;
+
+	base = 0;
+	for (idx = 0; idx < dcplb_nr_bounds; idx++) {
+		eaddr = dcplb_bounds[idx].eaddr;
+		if (addr < eaddr)
+			break;
+		base = eaddr;
+	}
+	if (unlikely(idx == dcplb_nr_bounds))
+		return CPLB_NO_ADDR_MATCH;
+
+	d_data = dcplb_bounds[idx].data;
+	if (unlikely(d_data == 0))
+		return CPLB_NO_ADDR_MATCH;
+
+	addr1 = addr & ~(SIZE_4M - 1);
+	addr &= ~(SIZE_1M - 1);
+	d_data |= PAGE_SIZE_1MB;
+	if (addr1 >= base && (addr1 + SIZE_4M) <= eaddr) {
+		/*
+		 * This works because
+		 * (PAGE_SIZE_4MB & PAGE_SIZE_1MB) == PAGE_SIZE_1MB.
+		 */
+		d_data |= PAGE_SIZE_4MB;
+		addr = addr1;
+	}
+
+	/* Pick entry to evict */
+	idx = evict_one_dcplb(cpu);
+
+	write_dcplb_data(cpu, idx, d_data, addr);
+
+	return CPLB_RELOADED;
+}
+
+MGR_ATTR static noinline int dcplb_protection_fault(int cpu)
+{
+	int status = bfin_read_DCPLB_STATUS();
+
+	nr_dcplb_prot[cpu]++;
+
+	if (likely(status & FAULT_RW)) {
+		int idx = faulting_cplb_index(status);
+		unsigned long regaddr = DCPLB_DATA0 + idx * 4;
+		unsigned long data = bfin_read32(regaddr);
+
+		/* Check if fault is to dirty a clean page */
+		if (!(data & CPLB_WT) && !(data & CPLB_DIRTY) &&
+		    write_permitted(status, data)) {
+
+			dcplb_tbl[cpu][idx].data = data;
+			bfin_write32(regaddr, data);
+			return CPLB_RELOADED;
+		}
+	}
+
+	return CPLB_PROT_VIOL;
+}
+
+MGR_ATTR int cplb_hdr(int seqstat, struct pt_regs *regs)
+{
+	int cause = seqstat & 0x3f;
+	unsigned int cpu = smp_processor_id();
+	switch (cause) {
+	case 0x2C:
+		return icplb_miss(cpu);
+	case 0x26:
+		return dcplb_miss(cpu);
+	default:
+		if (unlikely(cause == 0x23))
+			return dcplb_protection_fault(cpu);
+
+		return CPLB_UNKNOWN_ERR;
+	}
+}
diff --git a/arch/blackfin/kernel/cplbinfo.c b/arch/blackfin/kernel/cplbinfo.c
new file mode 100644
index 00000000000..64d78300dd0
--- /dev/null
+++ b/arch/blackfin/kernel/cplbinfo.c
@@ -0,0 +1,177 @@
+/*
+ * arch/blackfin/kernel/cplbinfo.c - display CPLB status
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/ctype.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+
+#include <asm/cplbinit.h>
+#include <asm/blackfin.h>
+
+static char const page_strtbl[][3] = { "1K", "4K", "1M", "4M" };
+#define page(flags)    (((flags) & 0x30000) >> 16)
+#define strpage(flags) page_strtbl[page(flags)]
+
+struct cplbinfo_data {
+	loff_t pos;
+	char cplb_type;
+	u32 mem_control;
+	struct cplb_entry *tbl;
+	int switched;
+};
+
+static void cplbinfo_print_header(struct seq_file *m)
+{
+	seq_printf(m, "Index\tAddress\t\tData\tSize\tU/RD\tU/WR\tS/WR\tSwitch\n");
+}
+
+static int cplbinfo_nomore(struct cplbinfo_data *cdata)
+{
+	return cdata->pos >= MAX_CPLBS;
+}
+
+static int cplbinfo_show(struct seq_file *m, void *p)
+{
+	struct cplbinfo_data *cdata;
+	unsigned long data, addr;
+	loff_t pos;
+
+	cdata = p;
+	pos = cdata->pos;
+	addr = cdata->tbl[pos].addr;
+	data = cdata->tbl[pos].data;
+
+	seq_printf(m,
+		"%d\t0x%08lx\t%05lx\t%s\t%c\t%c\t%c\t%c\n",
+		(int)pos, addr, data, strpage(data),
+		(data & CPLB_USER_RD) ? 'Y' : 'N',
+		(data & CPLB_USER_WR) ? 'Y' : 'N',
+		(data & CPLB_SUPV_WR) ? 'Y' : 'N',
+		pos < cdata->switched ? 'N' : 'Y');
+
+	return 0;
+}
+
+static void cplbinfo_seq_init(struct cplbinfo_data *cdata, unsigned int cpu)
+{
+	if (cdata->cplb_type == 'I') {
+		cdata->mem_control = bfin_read_IMEM_CONTROL();
+		cdata->tbl = icplb_tbl[cpu];
+		cdata->switched = first_switched_icplb;
+	} else {
+		cdata->mem_control = bfin_read_DMEM_CONTROL();
+		cdata->tbl = dcplb_tbl[cpu];
+		cdata->switched = first_switched_dcplb;
+	}
+}
+
+static void *cplbinfo_start(struct seq_file *m, loff_t *pos)
+{
+	struct cplbinfo_data *cdata = m->private;
+
+	if (!*pos) {
+		seq_printf(m, "%cCPLBs are %sabled: 0x%x\n", cdata->cplb_type,
+			(cdata->mem_control & ENDCPLB ? "en" : "dis"),
+			cdata->mem_control);
+		cplbinfo_print_header(m);
+	} else if (cplbinfo_nomore(cdata))
+		return NULL;
+
+	get_cpu();
+	return cdata;
+}
+
+static void *cplbinfo_next(struct seq_file *m, void *p, loff_t *pos)
+{
+	struct cplbinfo_data *cdata = p;
+	cdata->pos = ++(*pos);
+	if (cplbinfo_nomore(cdata))
+		return NULL;
+	else
+		return cdata;
+}
+
+static void cplbinfo_stop(struct seq_file *m, void *p)
+{
+	put_cpu();
+}
+
+static const struct seq_operations cplbinfo_sops = {
+	.start = cplbinfo_start,
+	.next  = cplbinfo_next,
+	.stop  = cplbinfo_stop,
+	.show  = cplbinfo_show,
+};
+
+static int cplbinfo_open(struct inode *inode, struct file *file)
+{
+	char buf[256], *path, *p;
+	unsigned int cpu;
+	char *s_cpu, *s_cplb;
+	int ret;
+	struct seq_file *m;
+	struct cplbinfo_data *cdata;
+
+	path = d_path(&file->f_path, buf, sizeof(buf));
+	if (IS_ERR(path))
+		return PTR_ERR(path);
+	s_cpu = strstr(path, "/cpu");
+	s_cplb = strrchr(path, '/');
+	if (!s_cpu || !s_cplb)
+		return -EINVAL;
+
+	cpu = simple_strtoul(s_cpu + 4, &p, 10);
+	if (!cpu_online(cpu))
+		return -ENODEV;
+
+	ret = seq_open_private(file, &cplbinfo_sops, sizeof(*cdata));
+	if (ret)
+		return ret;
+	m = file->private_data;
+	cdata = m->private;
+
+	cdata->pos = 0;
+	cdata->cplb_type = toupper(s_cplb[1]);
+	cplbinfo_seq_init(cdata, cpu);
+
+	return 0;
+}
+
+static const struct file_operations cplbinfo_fops = {
+	.open    = cplbinfo_open,
+	.read    = seq_read,
+	.llseek  = seq_lseek,
+	.release = seq_release_private,
+};
+
+static int __init cplbinfo_init(void)
+{
+	struct proc_dir_entry *cplb_dir, *cpu_dir;
+	char buf[10];
+	unsigned int cpu;
+
+	cplb_dir = proc_mkdir("cplbinfo", NULL);
+	if (!cplb_dir)
+		return -ENOMEM;
+
+	for_each_possible_cpu(cpu) {
+		sprintf(buf, "cpu%i", cpu);
+		cpu_dir = proc_mkdir(buf, cplb_dir);
+		if (!cpu_dir)
+			return -ENOMEM;
+
+		proc_create("icplb", S_IRUGO, cpu_dir, &cplbinfo_fops);
+		proc_create("dcplb", S_IRUGO, cpu_dir, &cplbinfo_fops);
+	}
+
+	return 0;
+}
+late_initcall(cplbinfo_init);
diff --git a/arch/blackfin/kernel/early_printk.c b/arch/blackfin/kernel/early_printk.c
index 1f4e3d2e090..c8ad051742e 100644
--- a/arch/blackfin/kernel/early_printk.c
+++ b/arch/blackfin/kernel/early_printk.c
@@ -105,10 +105,10 @@ static struct console * __init earlyserial_init(char *buf)
 		cflag |= CS5;
 		break;
 	case 6:
-		cflag |= CS5;
+		cflag |= CS6;
 		break;
 	case 7:
-		cflag |= CS5;
+		cflag |= CS7;
 		break;
 	default:
 		cflag |= CS8;
diff --git a/arch/blackfin/kernel/entry.S b/arch/blackfin/kernel/entry.S
index faea88ebb2e..a9cfba9946b 100644
--- a/arch/blackfin/kernel/entry.S
+++ b/arch/blackfin/kernel/entry.S
@@ -30,6 +30,7 @@
 #include <linux/linkage.h>
 #include <asm/thread_info.h>
 #include <asm/errno.h>
+#include <asm/blackfin.h>
 #include <asm/asm-offsets.h>
 
 #include <asm/context.S>
@@ -41,6 +42,10 @@
 #endif
 
 ENTRY(_ret_from_fork)
+#ifdef CONFIG_IPIPE
+	[--sp] = reti; 		/* IRQs on. */
+	SP += 4;
+#endif /* CONFIG_IPIPE */
 	SP += -12;
 	call _schedule_tail;
 	SP += 12;
diff --git a/arch/blackfin/kernel/fixed_code.S b/arch/blackfin/kernel/fixed_code.S
index 4b03ba02548..0d2d9e0968c 100644
--- a/arch/blackfin/kernel/fixed_code.S
+++ b/arch/blackfin/kernel/fixed_code.S
@@ -8,10 +8,12 @@
  * BF561 SMP).
  */
 #include <linux/linkage.h>
+#include <linux/init.h>
 #include <linux/unistd.h>
 #include <asm/entry.h>
 
-.text
+__INIT
+
 ENTRY(_fixed_code_start)
 
 .align 16
@@ -144,3 +146,5 @@ ENTRY(_safe_user_instruction)
 ENDPROC(_safe_user_instruction)
 
 ENTRY(_fixed_code_end)
+
+__FINIT
diff --git a/arch/blackfin/kernel/ipipe.c b/arch/blackfin/kernel/ipipe.c
new file mode 100644
index 00000000000..339be5a3ae6
--- /dev/null
+++ b/arch/blackfin/kernel/ipipe.c
@@ -0,0 +1,428 @@
+/* -*- linux-c -*-
+ * linux/arch/blackfin/kernel/ipipe.c
+ *
+ * Copyright (C) 2005-2007 Philippe Gerum.
+ *
+ * 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, Inc., 675 Mass Ave, Cambridge MA 02139,
+ * USA; 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, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * Architecture-dependent I-pipe support for the Blackfin.
+ */
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/percpu.h>
+#include <linux/bitops.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/kthread.h>
+#include <asm/unistd.h>
+#include <asm/system.h>
+#include <asm/atomic.h>
+#include <asm/io.h>
+
+static int create_irq_threads;
+
+DEFINE_PER_CPU(struct pt_regs, __ipipe_tick_regs);
+
+static DEFINE_PER_CPU(unsigned long, pending_irqthread_mask);
+
+static DEFINE_PER_CPU(int [IVG13 + 1], pending_irq_count);
+
+asmlinkage void asm_do_IRQ(unsigned int irq, struct pt_regs *regs);
+
+static void __ipipe_no_irqtail(void);
+
+unsigned long __ipipe_irq_tail_hook = (unsigned long)&__ipipe_no_irqtail;
+EXPORT_SYMBOL(__ipipe_irq_tail_hook);
+
+unsigned long __ipipe_core_clock;
+EXPORT_SYMBOL(__ipipe_core_clock);
+
+unsigned long __ipipe_freq_scale;
+EXPORT_SYMBOL(__ipipe_freq_scale);
+
+atomic_t __ipipe_irq_lvdepth[IVG15 + 1];
+
+unsigned long __ipipe_irq_lvmask = __all_masked_irq_flags;
+EXPORT_SYMBOL(__ipipe_irq_lvmask);
+
+static void __ipipe_ack_irq(unsigned irq, struct irq_desc *desc)
+{
+	desc->ipipe_ack(irq, desc);
+}
+
+/*
+ * __ipipe_enable_pipeline() -- We are running on the boot CPU, hw
+ * interrupts are off, and secondary CPUs are still lost in space.
+ */
+void __ipipe_enable_pipeline(void)
+{
+	unsigned irq;
+
+	__ipipe_core_clock = get_cclk(); /* Fetch this once. */
+	__ipipe_freq_scale = 1000000000UL / __ipipe_core_clock;
+
+	for (irq = 0; irq < NR_IRQS; ++irq)
+		ipipe_virtualize_irq(ipipe_root_domain,
+				     irq,
+				     (ipipe_irq_handler_t)&asm_do_IRQ,
+				     NULL,
+				     &__ipipe_ack_irq,
+				     IPIPE_HANDLE_MASK | IPIPE_PASS_MASK);
+}
+
+/*
+ * __ipipe_handle_irq() -- IPIPE's generic IRQ handler. An optimistic
+ * interrupt protection log is maintained here for each domain. Hw
+ * interrupts are masked on entry.
+ */
+void __ipipe_handle_irq(unsigned irq, struct pt_regs *regs)
+{
+	struct ipipe_domain *this_domain, *next_domain;
+	struct list_head *head, *pos;
+	int m_ack, s = -1;
+
+	/*
+	 * Software-triggered IRQs do not need any ack.  The contents
+	 * of the register frame should only be used when processing
+	 * the timer interrupt, but not for handling any other
+	 * interrupt.
+	 */
+	m_ack = (regs == NULL || irq == IRQ_SYSTMR || irq == IRQ_CORETMR);
+
+	this_domain = ipipe_current_domain;
+
+	if (unlikely(test_bit(IPIPE_STICKY_FLAG, &this_domain->irqs[irq].control)))
+		head = &this_domain->p_link;
+	else {
+		head = __ipipe_pipeline.next;
+		next_domain = list_entry(head, struct ipipe_domain, p_link);
+		if (likely(test_bit(IPIPE_WIRED_FLAG, &next_domain->irqs[irq].control))) {
+			if (!m_ack && next_domain->irqs[irq].acknowledge != NULL)
+				next_domain->irqs[irq].acknowledge(irq, irq_desc + irq);
+			if (test_bit(IPIPE_ROOTLOCK_FLAG, &ipipe_root_domain->flags))
+				s = __test_and_set_bit(IPIPE_STALL_FLAG,
+						       &ipipe_root_cpudom_var(status));
+			__ipipe_dispatch_wired(next_domain, irq);
+				goto finalize;
+			return;
+		}
+	}
+
+	/* Ack the interrupt. */
+
+	pos = head;
+
+	while (pos != &__ipipe_pipeline) {
+		next_domain = list_entry(pos, struct ipipe_domain, p_link);
+		/*
+		 * For each domain handling the incoming IRQ, mark it
+		 * as pending in its log.
+		 */
+		if (test_bit(IPIPE_HANDLE_FLAG, &next_domain->irqs[irq].control)) {
+			/*
+			 * Domains that handle this IRQ are polled for
+			 * acknowledging it by decreasing priority
+			 * order. The interrupt must be made pending
+			 * _first_ in the domain's status flags before
+			 * the PIC is unlocked.
+			 */
+			__ipipe_set_irq_pending(next_domain, irq);
+
+			if (!m_ack && next_domain->irqs[irq].acknowledge != NULL) {
+				next_domain->irqs[irq].acknowledge(irq, irq_desc + irq);
+				m_ack = 1;
+			}
+		}
+
+		/*
+		 * If the domain does not want the IRQ to be passed
+		 * down the interrupt pipe, exit the loop now.
+		 */
+		if (!test_bit(IPIPE_PASS_FLAG, &next_domain->irqs[irq].control))
+			break;
+
+		pos = next_domain->p_link.next;
+	}
+
+	/*
+	 * Now walk the pipeline, yielding control to the highest
+	 * priority domain that has pending interrupt(s) or
+	 * immediately to the current domain if the interrupt has been
+	 * marked as 'sticky'. This search does not go beyond the
+	 * current domain in the pipeline. We also enforce the
+	 * additional root stage lock (blackfin-specific). */
+
+	if (test_bit(IPIPE_ROOTLOCK_FLAG, &ipipe_root_domain->flags))
+		s = __test_and_set_bit(IPIPE_STALL_FLAG,
+				       &ipipe_root_cpudom_var(status));
+finalize:
+
+	__ipipe_walk_pipeline(head);
+
+	if (!s)
+		__clear_bit(IPIPE_STALL_FLAG,
+			    &ipipe_root_cpudom_var(status));
+}
+
+int __ipipe_check_root(void)
+{
+	return ipipe_root_domain_p;
+}
+
+void __ipipe_enable_irqdesc(struct ipipe_domain *ipd, unsigned irq)
+{
+	struct irq_desc *desc = irq_desc + irq;
+	int prio = desc->ic_prio;
+
+	desc->depth = 0;
+	if (ipd != &ipipe_root &&
+	    atomic_inc_return(&__ipipe_irq_lvdepth[prio]) == 1)
+		__set_bit(prio, &__ipipe_irq_lvmask);
+}
+EXPORT_SYMBOL(__ipipe_enable_irqdesc);
+
+void __ipipe_disable_irqdesc(struct ipipe_domain *ipd, unsigned irq)
+{
+	struct irq_desc *desc = irq_desc + irq;
+	int prio = desc->ic_prio;
+
+	if (ipd != &ipipe_root &&
+	    atomic_dec_and_test(&__ipipe_irq_lvdepth[prio]))
+		__clear_bit(prio, &__ipipe_irq_lvmask);
+}
+EXPORT_SYMBOL(__ipipe_disable_irqdesc);
+
+void __ipipe_stall_root_raw(void)
+{
+	/*
+	 * This code is called by the ins{bwl} routines (see
+	 * arch/blackfin/lib/ins.S), which are heavily used by the
+	 * network stack. It masks all interrupts but those handled by
+	 * non-root domains, so that we keep decent network transfer
+	 * rates for Linux without inducing pathological jitter for
+	 * the real-time domain.
+	 */
+	__asm__ __volatile__ ("sti %0;" : : "d"(__ipipe_irq_lvmask));
+
+	__set_bit(IPIPE_STALL_FLAG,
+		  &ipipe_root_cpudom_var(status));
+}
+
+void __ipipe_unstall_root_raw(void)
+{
+	__clear_bit(IPIPE_STALL_FLAG,
+		    &ipipe_root_cpudom_var(status));
+
+	__asm__ __volatile__ ("sti %0;" : : "d"(bfin_irq_flags));
+}
+
+int __ipipe_syscall_root(struct pt_regs *regs)
+{
+	unsigned long flags;
+
+	/* We need to run the IRQ tail hook whenever we don't
+	 * propagate a syscall to higher domains, because we know that
+	 * important operations might be pending there (e.g. Xenomai
+	 * deferred rescheduling). */
+
+	if (!__ipipe_syscall_watched_p(current, regs->orig_p0)) {
+		void (*hook)(void) = (void (*)(void))__ipipe_irq_tail_hook;
+		hook();
+		return 0;
+	}
+
+	/*
+	 * This routine either returns:
+	 * 0 -- if the syscall is to be passed to Linux;
+	 * 1 -- if the syscall should not be passed to Linux, and no
+	 * tail work should be performed;
+	 * -1 -- if the syscall should not be passed to Linux but the
+	 * tail work has to be performed (for handling signals etc).
+	 */
+
+	if (__ipipe_event_monitored_p(IPIPE_EVENT_SYSCALL) &&
+	    __ipipe_dispatch_event(IPIPE_EVENT_SYSCALL, regs) > 0) {
+		if (ipipe_root_domain_p && !in_atomic()) {
+			/*
+			 * Sync pending VIRQs before _TIF_NEED_RESCHED
+			 * is tested.
+			 */
+			local_irq_save_hw(flags);
+			if ((ipipe_root_cpudom_var(irqpend_himask) & IPIPE_IRQMASK_VIRT) != 0)
+				__ipipe_sync_pipeline(IPIPE_IRQMASK_VIRT);
+			local_irq_restore_hw(flags);
+			return -1;
+		}
+		return 1;
+	}
+
+	return 0;
+}
+
+unsigned long ipipe_critical_enter(void (*syncfn) (void))
+{
+	unsigned long flags;
+
+	local_irq_save_hw(flags);
+
+	return flags;
+}
+
+void ipipe_critical_exit(unsigned long flags)
+{
+	local_irq_restore_hw(flags);
+}
+
+static void __ipipe_no_irqtail(void)
+{
+}
+
+int ipipe_get_sysinfo(struct ipipe_sysinfo *info)
+{
+	info->ncpus = num_online_cpus();
+	info->cpufreq = ipipe_cpu_freq();
+	info->archdep.tmirq = IPIPE_TIMER_IRQ;
+	info->archdep.tmfreq = info->cpufreq;
+
+	return 0;
+}
+
+/*
+ * ipipe_trigger_irq() -- Push the interrupt at front of the pipeline
+ * just like if it has been actually received from a hw source. Also
+ * works for virtual interrupts.
+ */
+int ipipe_trigger_irq(unsigned irq)
+{
+	unsigned long flags;
+
+	if (irq >= IPIPE_NR_IRQS ||
+	    (ipipe_virtual_irq_p(irq)
+	     && !test_bit(irq - IPIPE_VIRQ_BASE, &__ipipe_virtual_irq_map)))
+		return -EINVAL;
+
+	local_irq_save_hw(flags);
+
+	__ipipe_handle_irq(irq, NULL);
+
+	local_irq_restore_hw(flags);
+
+	return 1;
+}
+
+/* Move Linux IRQ to threads. */
+
+static int do_irqd(void *__desc)
+{
+	struct irq_desc *desc = __desc;
+	unsigned irq = desc - irq_desc;
+	int thrprio = desc->thr_prio;
+	int thrmask = 1 << thrprio;
+	int cpu = smp_processor_id();
+	cpumask_t cpumask;
+
+	sigfillset(&current->blocked);
+	current->flags |= PF_NOFREEZE;
+	cpumask = cpumask_of_cpu(cpu);
+	set_cpus_allowed(current, cpumask);
+	ipipe_setscheduler_root(current, SCHED_FIFO, 50 + thrprio);
+
+	while (!kthread_should_stop()) {
+		local_irq_disable();
+		if (!(desc->status & IRQ_SCHEDULED)) {
+			set_current_state(TASK_INTERRUPTIBLE);
+resched:
+			local_irq_enable();
+			schedule();
+			local_irq_disable();
+		}
+		__set_current_state(TASK_RUNNING);
+		/*
+		 * If higher priority interrupt servers are ready to
+		 * run, reschedule immediately. We need this for the
+		 * GPIO demux IRQ handler to unmask the interrupt line
+		 * _last_, after all GPIO IRQs have run.
+		 */
+		if (per_cpu(pending_irqthread_mask, cpu) & ~(thrmask|(thrmask-1)))
+			goto resched;
+		if (--per_cpu(pending_irq_count[thrprio], cpu) == 0)
+			per_cpu(pending_irqthread_mask, cpu) &= ~thrmask;
+		desc->status &= ~IRQ_SCHEDULED;
+		desc->thr_handler(irq, &__raw_get_cpu_var(__ipipe_tick_regs));
+		local_irq_enable();
+	}
+	__set_current_state(TASK_RUNNING);
+	return 0;
+}
+
+static void kick_irqd(unsigned irq, void *cookie)
+{
+	struct irq_desc *desc = irq_desc + irq;
+	int thrprio = desc->thr_prio;
+	int thrmask = 1 << thrprio;
+	int cpu = smp_processor_id();
+
+	if (!(desc->status & IRQ_SCHEDULED)) {
+		desc->status |= IRQ_SCHEDULED;
+		per_cpu(pending_irqthread_mask, cpu) |= thrmask;
+		++per_cpu(pending_irq_count[thrprio], cpu);
+		wake_up_process(desc->thread);
+	}
+}
+
+int ipipe_start_irq_thread(unsigned irq, struct irq_desc *desc)
+{
+	if (desc->thread || !create_irq_threads)
+		return 0;
+
+	desc->thread = kthread_create(do_irqd, desc, "IRQ %d", irq);
+	if (desc->thread == NULL) {
+		printk(KERN_ERR "irqd: could not create IRQ thread %d!\n", irq);
+		return -ENOMEM;
+	}
+
+	wake_up_process(desc->thread);
+
+	desc->thr_handler = ipipe_root_domain->irqs[irq].handler;
+	ipipe_root_domain->irqs[irq].handler = &kick_irqd;
+
+	return 0;
+}
+
+void __init ipipe_init_irq_threads(void)
+{
+	unsigned irq;
+	struct irq_desc *desc;
+
+	create_irq_threads = 1;
+
+	for (irq = 0; irq < NR_IRQS; irq++) {
+		desc = irq_desc + irq;
+		if (desc->action != NULL ||
+			(desc->status & IRQ_NOREQUEST) != 0)
+			ipipe_start_irq_thread(irq, desc);
+	}
+}
+
+EXPORT_SYMBOL(show_stack);
+
+#ifdef CONFIG_IPIPE_TRACE_MCOUNT
+void notrace _mcount(void);
+EXPORT_SYMBOL(_mcount);
+#endif /* CONFIG_IPIPE_TRACE_MCOUNT */
diff --git a/arch/blackfin/kernel/irqchip.c b/arch/blackfin/kernel/irqchip.c
index 07402f57c9d..ab8209cbbad 100644
--- a/arch/blackfin/kernel/irqchip.c
+++ b/arch/blackfin/kernel/irqchip.c
@@ -36,7 +36,7 @@
 #include <linux/irq.h>
 #include <asm/trace.h>
 
-static unsigned long irq_err_count;
+static atomic_t irq_err_count;
 static spinlock_t irq_controller_lock;
 
 /*
@@ -48,10 +48,9 @@ void dummy_mask_unmask_irq(unsigned int irq)
 
 void ack_bad_irq(unsigned int irq)
 {
-	irq_err_count += 1;
+	atomic_inc(&irq_err_count);
 	printk(KERN_ERR "IRQ: spurious interrupt %d\n", irq);
 }
-EXPORT_SYMBOL(ack_bad_irq);
 
 static struct irq_chip bad_chip = {
 	.ack = dummy_mask_unmask_irq,
@@ -72,7 +71,7 @@ static struct irq_desc bad_irq_desc = {
 
 int show_interrupts(struct seq_file *p, void *v)
 {
-	int i = *(loff_t *) v;
+	int i = *(loff_t *) v, j;
 	struct irqaction *action;
 	unsigned long flags;
 
@@ -80,19 +79,20 @@ int show_interrupts(struct seq_file *p, void *v)
 		spin_lock_irqsave(&irq_desc[i].lock, flags);
 		action = irq_desc[i].action;
 		if (!action)
-			goto unlock;
-
-		seq_printf(p, "%3d: %10u ", i, kstat_irqs(i));
+			goto skip;
+		seq_printf(p, "%3d: ", i);
+		for_each_online_cpu(j)
+			seq_printf(p, "%10u ", kstat_cpu(j).irqs[i]);
+		seq_printf(p, " %8s", irq_desc[i].chip->name);
 		seq_printf(p, "  %s", action->name);
 		for (action = action->next; action; action = action->next)
-			seq_printf(p, ", %s", action->name);
+			seq_printf(p, "  %s", action->name);
 
 		seq_putc(p, '\n');
- unlock:
+ skip:
 		spin_unlock_irqrestore(&irq_desc[i].lock, flags);
-	} else if (i == NR_IRQS) {
-		seq_printf(p, "Err: %10lu\n", irq_err_count);
-	}
+	} else if (i == NR_IRQS)
+		seq_printf(p, "Err: %10u\n",  atomic_read(&irq_err_count));
 	return 0;
 }
 
@@ -101,7 +101,6 @@ int show_interrupts(struct seq_file *p, void *v)
  * come via this function.  Instead, they should provide their
  * own 'handler'
  */
-
 #ifdef CONFIG_DO_IRQ_L1
 __attribute__((l1_text))
 #endif
@@ -109,8 +108,9 @@ asmlinkage void asm_do_IRQ(unsigned int irq, struct pt_regs *regs)
 {
 	struct pt_regs *old_regs;
 	struct irq_desc *desc = irq_desc + irq;
+#ifndef CONFIG_IPIPE
 	unsigned short pending, other_ints;
-
+#endif
 	old_regs = set_irq_regs(regs);
 
 	/*
@@ -121,9 +121,24 @@ asmlinkage void asm_do_IRQ(unsigned int irq, struct pt_regs *regs)
 		desc = &bad_irq_desc;
 
 	irq_enter();
-
+#ifdef CONFIG_DEBUG_STACKOVERFLOW
+	/* Debugging check for stack overflow: is there less than STACK_WARN free? */
+	{
+		long sp;
+
+		sp = __get_SP() & (THREAD_SIZE-1);
+
+		if (unlikely(sp < (sizeof(struct thread_info) + STACK_WARN))) {
+			dump_stack();
+			printk(KERN_EMERG "%s: possible stack overflow while handling irq %i "
+					" only %ld bytes free\n",
+				__func__, irq, sp - sizeof(struct thread_info));
+		}
+	}
+#endif
 	generic_handle_irq(irq);
 
+#ifndef CONFIG_IPIPE	/* Useless and bugous over the I-pipe: IRQs are threaded. */
 	/* If we're the only interrupt running (ignoring IRQ15 which is for
 	   syscalls), lower our priority to IRQ14 so that softirqs run at
 	   that level.  If there's another, lower-level interrupt, irq_exit
@@ -133,6 +148,7 @@ asmlinkage void asm_do_IRQ(unsigned int irq, struct pt_regs *regs)
 	other_ints = pending & (pending - 1);
 	if (other_ints == 0)
 		lower_to_irq14();
+#endif /* !CONFIG_IPIPE */
 	irq_exit();
 
 	set_irq_regs(old_regs);
diff --git a/arch/blackfin/kernel/kgdb.c b/arch/blackfin/kernel/kgdb.c
index b795a207742..b163f6d3330 100644
--- a/arch/blackfin/kernel/kgdb.c
+++ b/arch/blackfin/kernel/kgdb.c
@@ -34,9 +34,14 @@ int gdb_bfin_vector = -1;
 #error change the definition of slavecpulocks
 #endif
 
-#ifdef CONFIG_BFIN_WDT
-# error "Please unselect blackfin watchdog driver before build KGDB."
-#endif
+#define IN_MEM(addr, size, l1_addr, l1_size) \
+({ \
+	unsigned long __addr = (unsigned long)(addr); \
+	(l1_size && __addr >= l1_addr && __addr + (size) <= l1_addr + l1_size); \
+})
+#define ASYNC_BANK_SIZE \
+	(ASYNC_BANK0_SIZE + ASYNC_BANK1_SIZE + \
+	 ASYNC_BANK2_SIZE + ASYNC_BANK3_SIZE)
 
 void pt_regs_to_gdb_regs(unsigned long *gdb_regs, struct pt_regs *regs)
 {
@@ -105,7 +110,7 @@ void pt_regs_to_gdb_regs(unsigned long *gdb_regs, struct pt_regs *regs)
  * Extracts ebp, esp and eip values understandable by gdb from the values
  * saved by switch_to.
  * thread.esp points to ebp. flags and ebp are pushed in switch_to hence esp
- * prior to entering switch_to is 8 greater then the value that is saved.
+ * prior to entering switch_to is 8 greater than the value that is saved.
  * If switch_to changes, change following code appropriately.
  */
 void sleeping_thread_to_gdb_regs(unsigned long *gdb_regs, struct task_struct *p)
@@ -219,6 +224,7 @@ int bfin_set_hw_break(unsigned long addr, int len, enum kgdb_bptype type)
 		if (bfin_type == breakinfo[breakno].type
 			&& !breakinfo[breakno].occupied) {
 			breakinfo[breakno].occupied = 1;
+			breakinfo[breakno].skip = 0;
 			breakinfo[breakno].enabled = 1;
 			breakinfo[breakno].addr = addr;
 			breakinfo[breakno].dataacc = dataacc;
@@ -363,12 +369,12 @@ void kgdb_passive_cpu_callback(void *info)
 
 void kgdb_roundup_cpus(unsigned long flags)
 {
-	smp_call_function(kgdb_passive_cpu_callback, NULL, 0, 0);
+	smp_call_function(kgdb_passive_cpu_callback, NULL, 0);
 }
 
 void kgdb_roundup_cpu(int cpu, unsigned long flags)
 {
-	smp_call_function_single(cpu, kgdb_passive_cpu_callback, NULL, 0, 0);
+	smp_call_function_single(cpu, kgdb_passive_cpu_callback, NULL, 0);
 }
 #endif
 
@@ -385,10 +391,8 @@ int kgdb_arch_handle_exception(int vector, int signo,
 			       struct pt_regs *regs)
 {
 	long addr;
-	long breakno;
 	char *ptr;
 	int newPC;
-	int wp_status;
 	int i;
 
 	switch (remcom_in_buffer[0]) {
@@ -426,17 +430,6 @@ int kgdb_arch_handle_exception(int vector, int signo,
 			kgdb_single_step = i + 1;
 		}
 
-		if (vector == VEC_WATCH) {
-			wp_status = bfin_read_WPSTAT();
-			for (breakno = 0; breakno < HW_WATCHPOINT_NUM; breakno++) {
-				if (wp_status & (1 << breakno)) {
-					breakinfo->skip = 1;
-					break;
-				}
-			}
-			bfin_write_WPSTAT(0);
-		}
-
 		bfin_correct_hw_break();
 
 		return 0;
@@ -478,57 +471,32 @@ static int validate_memory_access_address(unsigned long addr, int size)
 		return 0;
 	if (addr >= SYSMMR_BASE)
 		return 0;
-	if (addr >= ASYNC_BANK0_BASE
-	   && addr + size <= ASYNC_BANK3_BASE + ASYNC_BANK3_SIZE)
+	if (IN_MEM(addr, size, ASYNC_BANK0_BASE, ASYNC_BANK_SIZE))
 		return 0;
 	if (cpu == 0) {
-		if (addr >= L1_SCRATCH_START
-		   && (addr + size <= L1_SCRATCH_START + L1_SCRATCH_LENGTH))
+		if (IN_MEM(addr, size, L1_SCRATCH_START, L1_SCRATCH_LENGTH))
 			return 0;
-#if L1_CODE_LENGTH != 0
-		if (addr >= L1_CODE_START
-		   && (addr + size <= L1_CODE_START + L1_CODE_LENGTH))
+		if (IN_MEM(addr, size, L1_CODE_START, L1_CODE_LENGTH))
 			return 0;
-#endif
-#if L1_DATA_A_LENGTH != 0
-		if (addr >= L1_DATA_A_START
-		   && (addr + size <= L1_DATA_A_START + L1_DATA_A_LENGTH))
+		if (IN_MEM(addr, size, L1_DATA_A_START, L1_DATA_A_LENGTH))
 			return 0;
-#endif
-#if L1_DATA_B_LENGTH != 0
-		if (addr >= L1_DATA_B_START
-		   && (addr + size <= L1_DATA_B_START + L1_DATA_B_LENGTH))
+		if (IN_MEM(addr, size, L1_DATA_B_START, L1_DATA_B_LENGTH))
 			return 0;
-#endif
 #ifdef CONFIG_SMP
 	} else if (cpu == 1) {
-		if (addr >= COREB_L1_SCRATCH_START
-		   && (addr + size <= COREB_L1_SCRATCH_START
-		   + L1_SCRATCH_LENGTH))
+		if (IN_MEM(addr, size, COREB_L1_SCRATCH_START, L1_SCRATCH_LENGTH))
 			return 0;
-# if L1_CODE_LENGTH != 0
-		if (addr >= COREB_L1_CODE_START
-		   && (addr + size <= COREB_L1_CODE_START + L1_CODE_LENGTH))
+		if (IN_MEM(addr, size, COREB_L1_CODE_START, L1_CODE_LENGTH))
 			return 0;
-# endif
-# if L1_DATA_A_LENGTH != 0
-		if (addr >= COREB_L1_DATA_A_START
-		   && (addr + size <= COREB_L1_DATA_A_START + L1_DATA_A_LENGTH))
+		if (IN_MEM(addr, size, COREB_L1_DATA_A_START, L1_DATA_A_LENGTH))
 			return 0;
-# endif
-# if L1_DATA_B_LENGTH != 0
-		if (addr >= COREB_L1_DATA_B_START
-		   && (addr + size <= COREB_L1_DATA_B_START + L1_DATA_B_LENGTH))
+		if (IN_MEM(addr, size, COREB_L1_DATA_B_START, L1_DATA_B_LENGTH))
 			return 0;
-# endif
 #endif
 	}
 
-#if L2_LENGTH != 0
-	if (addr >= L2_START
-	   && addr + size <= L2_START + L2_LENGTH)
+	if (IN_MEM(addr, size, L2_START, L2_LENGTH))
 		return 0;
-#endif
 
 	return EFAULT;
 }
@@ -582,12 +550,9 @@ int kgdb_mem2hex(char *mem, char *buf, int count)
 		default:
 			err = EFAULT;
 		}
-	} else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
-		(unsigned int)(mem + count) <= L1_CODE_START + L1_CODE_LENGTH
+	} else if ((cpu == 0 && IN_MEM(mem, count, L1_CODE_START, L1_CODE_LENGTH))
 #ifdef CONFIG_SMP
-		|| cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
-		(unsigned int)(mem + count) <=
-		COREB_L1_CODE_START + L1_CODE_LENGTH
+		|| (cpu == 1 && IN_MEM(mem, count, COREB_L1_CODE_START, L1_CODE_LENGTH))
 #endif
 		) {
 		/* access L1 instruction SRAM*/
@@ -658,12 +623,9 @@ int kgdb_ebin2mem(char *buf, char *mem, int count)
 		default:
 			return EFAULT;
 		}
-	} else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
-		(unsigned int)(mem + count) < L1_CODE_START + L1_CODE_LENGTH
+	} else if ((cpu == 0 && IN_MEM(mem, count, L1_CODE_START, L1_CODE_LENGTH))
 #ifdef CONFIG_SMP
-		|| cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
-		(unsigned int)(mem + count) <=
-		COREB_L1_CODE_START + L1_CODE_LENGTH
+		|| (cpu == 1 && IN_MEM(mem, count, COREB_L1_CODE_START, L1_CODE_LENGTH))
 #endif
 		) {
 		/* access L1 instruction SRAM */
@@ -723,12 +685,9 @@ int kgdb_hex2mem(char *buf, char *mem, int count)
 		default:
 			return EFAULT;
 		}
-	} else if (cpu == 0 && (unsigned int)mem >= L1_CODE_START &&
-		(unsigned int)(mem + count) <= L1_CODE_START + L1_CODE_LENGTH
+	} else if ((cpu == 0 && IN_MEM(mem, count, L1_CODE_START, L1_CODE_LENGTH))
 #ifdef CONFIG_SMP
-		|| cpu == 1 && (unsigned int)mem >= COREB_L1_CODE_START &&
-		(unsigned int)(mem + count) <=
-		COREB_L1_CODE_START + L1_CODE_LENGTH
+		|| (cpu == 1 && IN_MEM(mem, count, COREB_L1_CODE_START, L1_CODE_LENGTH))
 #endif
 		) {
 		/* access L1 instruction SRAM */
@@ -745,24 +704,16 @@ int kgdb_validate_break_address(unsigned long addr)
 
 	if (addr >= 0x1000 && (addr + BREAK_INSTR_SIZE) <= physical_mem_end)
 		return 0;
-	if (addr >= ASYNC_BANK0_BASE
-	   && addr + BREAK_INSTR_SIZE <= ASYNC_BANK3_BASE + ASYNC_BANK3_BASE)
+	if (IN_MEM(addr, BREAK_INSTR_SIZE, ASYNC_BANK0_BASE, ASYNC_BANK_SIZE))
 		return 0;
-#if L1_CODE_LENGTH != 0
-	if (cpu == 0 && addr >= L1_CODE_START
-	   && addr + BREAK_INSTR_SIZE <= L1_CODE_START + L1_CODE_LENGTH)
+	if (cpu == 0 && IN_MEM(addr, BREAK_INSTR_SIZE, L1_CODE_START, L1_CODE_LENGTH))
 		return 0;
-# ifdef CONFIG_SMP
-	else if (cpu == 1 && addr >= COREB_L1_CODE_START
-	   && addr + BREAK_INSTR_SIZE <= COREB_L1_CODE_START + L1_CODE_LENGTH)
+#ifdef CONFIG_SMP
+	else if (cpu == 1 && IN_MEM(addr, BREAK_INSTR_SIZE, COREB_L1_CODE_START, L1_CODE_LENGTH))
 		return 0;
-# endif
 #endif
-#if L2_LENGTH != 0
-	if (addr >= L2_START
-	   && addr + BREAK_INSTR_SIZE <= L2_START + L2_LENGTH)
+	if (IN_MEM(addr, BREAK_INSTR_SIZE, L2_START, L2_LENGTH))
 		return 0;
-#endif
 
 	return EFAULT;
 }
@@ -772,13 +723,9 @@ int kgdb_arch_set_breakpoint(unsigned long addr, char *saved_instr)
 	int err;
 	int cpu = raw_smp_processor_id();
 
-	if ((cpu == 0 && (unsigned int)addr >= L1_CODE_START
-		&& (unsigned int)(addr + BREAK_INSTR_SIZE)
-		< L1_CODE_START + L1_CODE_LENGTH)
+	if ((cpu == 0 && IN_MEM(addr, BREAK_INSTR_SIZE, L1_CODE_START, L1_CODE_LENGTH))
 #ifdef CONFIG_SMP
-		|| (cpu == 1 && (unsigned int)addr >= COREB_L1_CODE_START
-		&& (unsigned int)(addr + BREAK_INSTR_SIZE)
-		< COREB_L1_CODE_START + L1_CODE_LENGTH)
+		|| (cpu == 1 && IN_MEM(addr, BREAK_INSTR_SIZE, COREB_L1_CODE_START, L1_CODE_LENGTH))
 #endif
 		) {
 		/* access L1 instruction SRAM */
@@ -804,9 +751,7 @@ int kgdb_arch_set_breakpoint(unsigned long addr, char *saved_instr)
 
 int kgdb_arch_remove_breakpoint(unsigned long addr, char *bundle)
 {
-	if ((unsigned int)addr >= L1_CODE_START &&
-		(unsigned int)(addr + BREAK_INSTR_SIZE) <
-			L1_CODE_START + L1_CODE_LENGTH) {
+	if (IN_MEM(addr, BREAK_INSTR_SIZE, L1_CODE_START, L1_CODE_LENGTH)) {
 		/* access L1 instruction SRAM */
 		if (dma_memcpy((void *)addr, bundle, BREAK_INSTR_SIZE) == NULL)
 			return -EFAULT;
diff --git a/arch/blackfin/kernel/kgdb_test.c b/arch/blackfin/kernel/kgdb_test.c
new file mode 100644
index 00000000000..3dba9c17304
--- /dev/null
+++ b/arch/blackfin/kernel/kgdb_test.c
@@ -0,0 +1,123 @@
+/*
+ * arch/blackfin/kernel/kgdb_test.c - Blackfin kgdb tests
+ *
+ * Copyright 2005-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/proc_fs.h>
+
+#include <asm/current.h>
+#include <asm/uaccess.h>
+#include <asm/system.h>
+
+#include <asm/blackfin.h>
+
+static char cmdline[256];
+static unsigned long len;
+
+static int num1 __attribute__((l1_data));
+
+void kgdb_l1_test(void) __attribute__((l1_text));
+
+void kgdb_l1_test(void)
+{
+	printk(KERN_ALERT "L1(before change) : data variable addr = 0x%p, data value is %d\n", &num1, num1);
+	printk(KERN_ALERT "L1 : code function addr = 0x%p\n", kgdb_l1_test);
+	num1 = num1 + 10 ;
+	printk(KERN_ALERT "L1(after change) : data variable addr = 0x%p, data value is %d\n", &num1, num1);
+	return ;
+}
+#if L2_LENGTH
+
+static int num2 __attribute__((l2));
+void kgdb_l2_test(void) __attribute__((l2));
+
+void kgdb_l2_test(void)
+{
+	printk(KERN_ALERT "L2(before change) : data variable addr = 0x%p, data value is %d\n", &num2, num2);
+	printk(KERN_ALERT "L2 : code function addr = 0x%p\n", kgdb_l2_test);
+	num2 = num2 + 20 ;
+	printk(KERN_ALERT "L2(after change) : data variable addr = 0x%p, data value is %d\n", &num2, num2);
+	return ;
+}
+
+#endif
+
+
+int kgdb_test(char *name, int len, int count, int z)
+{
+	printk(KERN_DEBUG "kgdb name(%d): %s, %d, %d\n", len, name, count, z);
+	count = z;
+	return count;
+}
+
+static int test_proc_output(char *buf)
+{
+	kgdb_test("hello world!", 12, 0x55, 0x10);
+	kgdb_l1_test();
+	#if L2_LENGTH
+	kgdb_l2_test();
+	#endif
+
+	return 0;
+}
+
+static int test_read_proc(char *page, char **start, off_t off,
+				 int count, int *eof, void *data)
+{
+	int len;
+
+	len = test_proc_output(page);
+	if (len <= off+count)
+		*eof = 1;
+	*start = page + off;
+	len -= off;
+	if (len > count)
+		len = count;
+	if (len < 0)
+		len = 0;
+	return len;
+}
+
+static int test_write_proc(struct file *file, const char *buffer,
+				 unsigned long count, void *data)
+{
+	if (count >= 256)
+		len = 255;
+	else
+		len = count;
+
+	memcpy(cmdline, buffer, count);
+	cmdline[len] = 0;
+
+	return len;
+}
+
+static int __init kgdbtest_init(void)
+{
+	struct proc_dir_entry *entry;
+
+	entry = create_proc_entry("kgdbtest", 0, NULL);
+	if (entry == NULL)
+		return -ENOMEM;
+
+	entry->read_proc = test_read_proc;
+	entry->write_proc = test_write_proc;
+	entry->data = NULL;
+
+	return 0;
+}
+
+static void __exit kgdbtest_exit(void)
+{
+	remove_proc_entry("kgdbtest", NULL);
+}
+
+module_init(kgdbtest_init);
+module_exit(kgdbtest_exit);
+MODULE_LICENSE("GPL");
diff --git a/arch/blackfin/kernel/mcount.S b/arch/blackfin/kernel/mcount.S
new file mode 100644
index 00000000000..edcfb3865f4
--- /dev/null
+++ b/arch/blackfin/kernel/mcount.S
@@ -0,0 +1,70 @@
+/*
+ * linux/arch/blackfin/mcount.S
+ *
+ * Copyright (C) 2006 Analog Devices Inc.
+ *
+ * 2007/04/12 Save index, length, modify and base registers. --rpm
+ */
+
+#include <linux/linkage.h>
+#include <asm/blackfin.h>
+
+.text
+
+.align 4 	/* just in case */
+
+ENTRY(__mcount)
+	[--sp] = i0;
+	[--sp] = i1;
+	[--sp] = i2;
+	[--sp] = i3;
+	[--sp] = l0;
+	[--sp] = l1;
+	[--sp] = l2;
+	[--sp] = l3;
+	[--sp] = m0;
+	[--sp] = m1;
+	[--sp] = m2;
+	[--sp] = m3;
+	[--sp] = b0;
+	[--sp] = b1;
+	[--sp] = b2;
+	[--sp] = b3;
+	[--sp] = ( r7:0, p5:0 );
+	[--sp] = ASTAT;
+
+	p1.L = _ipipe_trace_enable;
+	p1.H = _ipipe_trace_enable;
+	r7 = [p1];
+	CC = r7 == 0;
+	if CC jump out;
+	link 0x10;
+	r0 = 0x0;
+	[sp + 0xc] = r0; /* v */
+	r0 = 0x0;	/* type: IPIPE_TRACE_FN */
+	r1 = rets;
+	p0 = [fp];	/* p0: Prior FP */
+	r2 = [p0 + 4];	/* r2: Prior RETS */
+	call ___ipipe_trace;
+	unlink;
+out:
+	ASTAT = [sp++];
+	( r7:0, p5:0 ) = [sp++];
+	b3 = [sp++];
+	b2 = [sp++];
+	b1 = [sp++];
+	b0 = [sp++];
+	m3 = [sp++];
+	m2 = [sp++];
+	m1 = [sp++];
+	m0 = [sp++];
+	l3 = [sp++];
+	l2 = [sp++];
+	l1 = [sp++];
+	l0 = [sp++];
+	i3 = [sp++];
+	i2 = [sp++];
+	i1 = [sp++];
+	i0 = [sp++];
+	rts;
+ENDPROC(__mcount)
diff --git a/arch/blackfin/kernel/module.c b/arch/blackfin/kernel/module.c
index e1bebc80a5b..1bd7f2d018a 100644
--- a/arch/blackfin/kernel/module.c
+++ b/arch/blackfin/kernel/module.c
@@ -37,111 +37,6 @@
 #include <asm/dma.h>
 #include <asm/cacheflush.h>
 
-/*
- * handle arithmetic relocations.
- * See binutils/bfd/elf32-bfin.c for more details
- */
-#define RELOC_STACK_SIZE 100
-static uint32_t reloc_stack[RELOC_STACK_SIZE];
-static unsigned int reloc_stack_tos;
-
-#define is_reloc_stack_empty() ((reloc_stack_tos > 0)?0:1)
-
-static void reloc_stack_push(uint32_t value)
-{
-	reloc_stack[reloc_stack_tos++] = value;
-}
-
-static uint32_t reloc_stack_pop(void)
-{
-	return reloc_stack[--reloc_stack_tos];
-}
-
-static uint32_t reloc_stack_operate(unsigned int oper, struct module *mod)
-{
-	uint32_t value;
-
-	switch (oper) {
-	case R_add:
-		value = reloc_stack[reloc_stack_tos - 2] +
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_sub:
-		value = reloc_stack[reloc_stack_tos - 2] -
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_mult:
-		value = reloc_stack[reloc_stack_tos - 2] *
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_div:
-		value = reloc_stack[reloc_stack_tos - 2] /
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_mod:
-		value = reloc_stack[reloc_stack_tos - 2] %
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_lshift:
-		value = reloc_stack[reloc_stack_tos - 2] <<
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_rshift:
-		value = reloc_stack[reloc_stack_tos - 2] >>
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_and:
-		value = reloc_stack[reloc_stack_tos - 2] &
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_or:
-		value = reloc_stack[reloc_stack_tos - 2] |
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_xor:
-		value = reloc_stack[reloc_stack_tos - 2] ^
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_land:
-		value = reloc_stack[reloc_stack_tos - 2] &&
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_lor:
-		value = reloc_stack[reloc_stack_tos - 2] ||
-			reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 2;
-		break;
-	case R_neg:
-		value = -reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos--;
-		break;
-	case R_comp:
-		value = ~reloc_stack[reloc_stack_tos - 1];
-		reloc_stack_tos -= 1;
-		break;
-	default:
-		printk(KERN_WARNING "module %s: unhandled reloction\n",
-				mod->name);
-		return 0;
-	}
-
-	/* now push the new value back on stack */
-	reloc_stack_push(value);
-
-	return value;
-}
-
 void *module_alloc(unsigned long size)
 {
 	if (size == 0)
@@ -334,16 +229,18 @@ apply_relocate_add(Elf_Shdr * sechdrs, const char *strtab,
 		   undefined symbols have been resolved. */
 		sym = (Elf32_Sym *) sechdrs[symindex].sh_addr
 		    + ELF32_R_SYM(rel[i].r_info);
-		if (is_reloc_stack_empty()) {
-			value = sym->st_value;
-		} else {
-			value = reloc_stack_pop();
-		}
+		value = sym->st_value;
 		value += rel[i].r_addend;
 		pr_debug("location is %x, value is %x type is %d \n",
 			 (unsigned int) location32, value,
 			 ELF32_R_TYPE(rel[i].r_info));
-
+#ifdef CONFIG_SMP
+		if ((unsigned long)location16 >= COREB_L1_DATA_A_START) {
+			printk(KERN_ERR "module %s: cannot relocate in L1: %u (SMP kernel)",
+				       mod->name, ELF32_R_TYPE(rel[i].r_info));
+			return -ENOEXEC;
+		}
+#endif
 		switch (ELF32_R_TYPE(rel[i].r_info)) {
 
 		case R_pcrel24:
@@ -355,6 +252,12 @@ apply_relocate_add(Elf_Shdr * sechdrs, const char *strtab,
 			location32 = (uint32_t *) location16;
 			value -= (uint32_t) location32;
 			value >>= 1;
+			if ((value & 0xFF000000) != 0 &&
+			    (value & 0xFF000000) != 0xFF000000) {
+				printk(KERN_ERR "module %s: relocation overflow\n",
+				       mod->name);
+				return -ENOEXEC;
+			}
 			pr_debug("value is %x, before %x-%x after %x-%x\n", value,
 			       *location16, *(location16 + 1),
 			       (*location16 & 0xff00) | (value >> 16 & 0x00ff),
@@ -399,28 +302,6 @@ apply_relocate_add(Elf_Shdr * sechdrs, const char *strtab,
 			pr_debug("before %x after %x\n", *location32, value);
 			*location32 = value;
 			break;
-		case R_push:
-			reloc_stack_push(value);
-			break;
-		case R_const:
-			reloc_stack_push(rel[i].r_addend);
-			break;
-		case R_add:
-		case R_sub:
-		case R_mult:
-		case R_div:
-		case R_mod:
-		case R_lshift:
-		case R_rshift:
-		case R_and:
-		case R_or:
-		case R_xor:
-		case R_land:
-		case R_lor:
-		case R_neg:
-		case R_comp:
-			reloc_stack_operate(ELF32_R_TYPE(rel[i].r_info), mod);
-			break;
 		default:
 			printk(KERN_ERR "module %s: Unknown relocation: %u\n",
 			       mod->name, ELF32_R_TYPE(rel[i].r_info));
@@ -436,6 +317,7 @@ module_finalize(const Elf_Ehdr * hdr,
 {
 	unsigned int i, strindex = 0, symindex = 0;
 	char *secstrings;
+	long err = 0;
 
 	secstrings = (void *)hdr + sechdrs[hdr->e_shstrndx].sh_offset;
 
@@ -460,8 +342,10 @@ module_finalize(const Elf_Ehdr * hdr,
 		    (strcmp(".rela.l1.text", secstrings + sechdrs[i].sh_name) == 0) ||
 		    ((strcmp(".rela.text", secstrings + sechdrs[i].sh_name) == 0) &&
 			(hdr->e_flags & (EF_BFIN_CODE_IN_L1|EF_BFIN_CODE_IN_L2))))) {
-			apply_relocate_add((Elf_Shdr *) sechdrs, strtab,
+			err = apply_relocate_add((Elf_Shdr *) sechdrs, strtab,
 					   symindex, i, mod);
+			if (err < 0)
+				return -ENOEXEC;
 		}
 	}
 	return 0;
diff --git a/arch/blackfin/kernel/process.c b/arch/blackfin/kernel/process.c
index 0c3ea118b65..33e2e8993f7 100644
--- a/arch/blackfin/kernel/process.c
+++ b/arch/blackfin/kernel/process.c
@@ -39,6 +39,7 @@
 
 #include <asm/blackfin.h>
 #include <asm/fixed_code.h>
+#include <asm/mem_map.h>
 
 asmlinkage void ret_from_fork(void);
 
@@ -81,11 +82,14 @@ void cpu_idle(void)__attribute__((l1_text));
  */
 static void default_idle(void)
 {
-	local_irq_disable();
+#ifdef CONFIG_IPIPE
+	ipipe_suspend_domain();
+#endif
+	local_irq_disable_hw();
 	if (!need_resched())
 		idle_with_irq_disabled();
 
-	local_irq_enable();
+	local_irq_enable_hw();
 }
 
 /*
@@ -154,6 +158,7 @@ pid_t kernel_thread(int (*fn) (void *), void *arg, unsigned long flags)
 	return do_fork(flags | CLONE_VM | CLONE_UNTRACED, 0, &regs, 0, NULL,
 		       NULL);
 }
+EXPORT_SYMBOL(kernel_thread);
 
 void flush_thread(void)
 {
@@ -170,6 +175,13 @@ asmlinkage int bfin_clone(struct pt_regs *regs)
 	unsigned long clone_flags;
 	unsigned long newsp;
 
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	if (current->rt.nr_cpus_allowed == num_possible_cpus()) {
+		current->cpus_allowed = cpumask_of_cpu(smp_processor_id());
+		current->rt.nr_cpus_allowed = 1;
+	}
+#endif
+
 	/* syscall2 puts clone_flags in r0 and usp in r1 */
 	clone_flags = regs->r0;
 	newsp = regs->r1;
@@ -337,22 +349,22 @@ int _access_ok(unsigned long addr, unsigned long size)
 	if (addr >= (unsigned long)__init_begin &&
 	    addr + size <= (unsigned long)__init_end)
 		return 1;
-	if (addr >= L1_SCRATCH_START
-	    && addr + size <= L1_SCRATCH_START + L1_SCRATCH_LENGTH)
+	if (addr >= get_l1_scratch_start()
+	    && addr + size <= get_l1_scratch_start() + L1_SCRATCH_LENGTH)
 		return 1;
 #if L1_CODE_LENGTH != 0
-	if (addr >= L1_CODE_START + (_etext_l1 - _stext_l1)
-	    && addr + size <= L1_CODE_START + L1_CODE_LENGTH)
+	if (addr >= get_l1_code_start() + (_etext_l1 - _stext_l1)
+	    && addr + size <= get_l1_code_start() + L1_CODE_LENGTH)
 		return 1;
 #endif
 #if L1_DATA_A_LENGTH != 0
-	if (addr >= L1_DATA_A_START + (_ebss_l1 - _sdata_l1)
-	    && addr + size <= L1_DATA_A_START + L1_DATA_A_LENGTH)
+	if (addr >= get_l1_data_a_start() + (_ebss_l1 - _sdata_l1)
+	    && addr + size <= get_l1_data_a_start() + L1_DATA_A_LENGTH)
 		return 1;
 #endif
 #if L1_DATA_B_LENGTH != 0
-	if (addr >= L1_DATA_B_START + (_ebss_b_l1 - _sdata_b_l1)
-	    && addr + size <= L1_DATA_B_START + L1_DATA_B_LENGTH)
+	if (addr >= get_l1_data_b_start() + (_ebss_b_l1 - _sdata_b_l1)
+	    && addr + size <= get_l1_data_b_start() + L1_DATA_B_LENGTH)
 		return 1;
 #endif
 #if L2_LENGTH != 0
diff --git a/arch/blackfin/kernel/ptrace.c b/arch/blackfin/kernel/ptrace.c
index 140bf00e997..d2d38853663 100644
--- a/arch/blackfin/kernel/ptrace.c
+++ b/arch/blackfin/kernel/ptrace.c
@@ -45,6 +45,7 @@
 #include <asm/asm-offsets.h>
 #include <asm/dma.h>
 #include <asm/fixed_code.h>
+#include <asm/mem_map.h>
 
 #define TEXT_OFFSET 0
 /*
@@ -80,10 +81,12 @@ static inline struct pt_regs *get_user_regs(struct task_struct *task)
 /*
  * Get all user integer registers.
  */
-static inline int ptrace_getregs(struct task_struct *tsk, void __user * uregs)
+static inline int ptrace_getregs(struct task_struct *tsk, void __user *uregs)
 {
-	struct pt_regs *regs = get_user_regs(tsk);
-	return copy_to_user(uregs, regs, sizeof(struct pt_regs)) ? -EFAULT : 0;
+	struct pt_regs regs;
+	memcpy(&regs, get_user_regs(tsk), sizeof(regs));
+	regs.usp = tsk->thread.usp;
+	return copy_to_user(uregs, &regs, sizeof(struct pt_regs)) ? -EFAULT : 0;
 }
 
 /* Mapping from PT_xxx to the stack offset at which the register is
@@ -220,8 +223,8 @@ long arch_ptrace(struct task_struct *child, long request, long addr, long data)
 				break;
 			pr_debug("ptrace: user address is valid\n");
 
-			if (L1_CODE_LENGTH != 0 && addr >= L1_CODE_START
-			    && addr + sizeof(tmp) <= L1_CODE_START + L1_CODE_LENGTH) {
+			if (L1_CODE_LENGTH != 0 && addr >= get_l1_code_start()
+			    && addr + sizeof(tmp) <= get_l1_code_start() + L1_CODE_LENGTH) {
 				safe_dma_memcpy (&tmp, (const void *)(addr), sizeof(tmp));
 				copied = sizeof(tmp);
 
@@ -300,8 +303,8 @@ long arch_ptrace(struct task_struct *child, long request, long addr, long data)
 				break;
 			pr_debug("ptrace: user address is valid\n");
 
-			if (L1_CODE_LENGTH != 0 && addr >= L1_CODE_START
-			    && addr + sizeof(data) <= L1_CODE_START + L1_CODE_LENGTH) {
+			if (L1_CODE_LENGTH != 0 && addr >= get_l1_code_start()
+			    && addr + sizeof(data) <= get_l1_code_start() + L1_CODE_LENGTH) {
 				safe_dma_memcpy ((void *)(addr), &data, sizeof(data));
 				copied = sizeof(data);
 
diff --git a/arch/blackfin/kernel/reboot.c b/arch/blackfin/kernel/reboot.c
index ae97ca407b0..eeee8cb4336 100644
--- a/arch/blackfin/kernel/reboot.c
+++ b/arch/blackfin/kernel/reboot.c
@@ -21,7 +21,7 @@
  * the core reset.
  */
 __attribute__((l1_text))
-static void bfin_reset(void)
+static void _bfin_reset(void)
 {
 	/* Wait for completion of "system" events such as cache line
 	 * line fills so that we avoid infinite stalls later on as
@@ -66,6 +66,18 @@ static void bfin_reset(void)
 	}
 }
 
+static void bfin_reset(void)
+{
+	if (ANOMALY_05000353 || ANOMALY_05000386)
+		_bfin_reset();
+	else
+		/* the bootrom checks to see how it was reset and will
+		 * automatically perform a software reset for us when
+		 * it starts executing boot
+		 */
+		asm("raise 1;");
+}
+
 __attribute__((weak))
 void native_machine_restart(char *cmd)
 {
@@ -75,14 +87,10 @@ void machine_restart(char *cmd)
 {
 	native_machine_restart(cmd);
 	local_irq_disable();
-	if (ANOMALY_05000353 || ANOMALY_05000386)
-		bfin_reset();
+	if (smp_processor_id())
+		smp_call_function((void *)bfin_reset, 0, 1);
 	else
-		/* the bootrom checks to see how it was reset and will
-		 * automatically perform a software reset for us when
-		 * it starts executing boot
-		 */
-		asm("raise 1;");
+		bfin_reset();
 }
 
 __attribute__((weak))
diff --git a/arch/blackfin/kernel/setup.c b/arch/blackfin/kernel/setup.c
index 71a9a8c53ce..b2a811347b6 100644
--- a/arch/blackfin/kernel/setup.c
+++ b/arch/blackfin/kernel/setup.c
@@ -13,6 +13,7 @@
 #include <linux/bootmem.h>
 #include <linux/seq_file.h>
 #include <linux/cpu.h>
+#include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/tty.h>
 #include <linux/pfn.h>
@@ -26,11 +27,10 @@
 #include <asm/blackfin.h>
 #include <asm/cplbinit.h>
 #include <asm/div64.h>
+#include <asm/cpu.h>
 #include <asm/fixed_code.h>
 #include <asm/early_printk.h>
 
-static DEFINE_PER_CPU(struct cpu, cpu_devices);
-
 u16 _bfin_swrst;
 EXPORT_SYMBOL(_bfin_swrst);
 
@@ -79,27 +79,68 @@ static struct change_member *change_point[2*BFIN_MEMMAP_MAX] __initdata;
 static struct bfin_memmap_entry *overlap_list[BFIN_MEMMAP_MAX] __initdata;
 static struct bfin_memmap_entry new_map[BFIN_MEMMAP_MAX] __initdata;
 
-void __init bfin_cache_init(void)
-{
+DEFINE_PER_CPU(struct blackfin_cpudata, cpu_data);
+
+static int early_init_clkin_hz(char *buf);
+
 #if defined(CONFIG_BFIN_DCACHE) || defined(CONFIG_BFIN_ICACHE)
-	generate_cplb_tables();
+void __init generate_cplb_tables(void)
+{
+	unsigned int cpu;
+
+	generate_cplb_tables_all();
+	/* Generate per-CPU I&D CPLB tables */
+	for (cpu = 0; cpu < num_possible_cpus(); ++cpu)
+		generate_cplb_tables_cpu(cpu);
+}
 #endif
 
+void __cpuinit bfin_setup_caches(unsigned int cpu)
+{
 #ifdef CONFIG_BFIN_ICACHE
-	bfin_icache_init();
-	printk(KERN_INFO "Instruction Cache Enabled\n");
+	bfin_icache_init(icplb_tbl[cpu]);
 #endif
 
 #ifdef CONFIG_BFIN_DCACHE
-	bfin_dcache_init();
-	printk(KERN_INFO "Data Cache Enabled"
+	bfin_dcache_init(dcplb_tbl[cpu]);
+#endif
+
+	/*
+	 * In cache coherence emulation mode, we need to have the
+	 * D-cache enabled before running any atomic operation which
+	 * might invove cache invalidation (i.e. spinlock, rwlock).
+	 * So printk's are deferred until then.
+	 */
+#ifdef CONFIG_BFIN_ICACHE
+	printk(KERN_INFO "Instruction Cache Enabled for CPU%u\n", cpu);
+#endif
+#ifdef CONFIG_BFIN_DCACHE
+	printk(KERN_INFO "Data Cache Enabled for CPU%u"
 # if defined CONFIG_BFIN_WB
 		" (write-back)"
 # elif defined CONFIG_BFIN_WT
 		" (write-through)"
 # endif
-		"\n");
+		"\n", cpu);
+#endif
+}
+
+void __cpuinit bfin_setup_cpudata(unsigned int cpu)
+{
+	struct blackfin_cpudata *cpudata = &per_cpu(cpu_data, cpu);
+
+	cpudata->idle = current;
+	cpudata->loops_per_jiffy = loops_per_jiffy;
+	cpudata->imemctl = bfin_read_IMEM_CONTROL();
+	cpudata->dmemctl = bfin_read_DMEM_CONTROL();
+}
+
+void __init bfin_cache_init(void)
+{
+#if defined(CONFIG_BFIN_DCACHE) || defined(CONFIG_BFIN_ICACHE)
+	generate_cplb_tables();
 #endif
+	bfin_setup_caches(0);
 }
 
 void __init bfin_relocate_l1_mem(void)
@@ -109,6 +150,8 @@ void __init bfin_relocate_l1_mem(void)
 	unsigned long l1_data_b_length;
 	unsigned long l2_length;
 
+	blackfin_dma_early_init();
+
 	l1_code_length = _etext_l1 - _stext_l1;
 	if (l1_code_length > L1_CODE_LENGTH)
 		panic("L1 Instruction SRAM Overflow\n");
@@ -230,7 +273,7 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 	/* record all known change-points (starting and ending addresses),
 	   omitting those that are for empty memory regions */
 	chgidx = 0;
-	for (i = 0; i < old_nr; i++)	{
+	for (i = 0; i < old_nr; i++) {
 		if (map[i].size != 0) {
 			change_point[chgidx]->addr = map[i].addr;
 			change_point[chgidx++]->pentry = &map[i];
@@ -238,13 +281,13 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 			change_point[chgidx++]->pentry = &map[i];
 		}
 	}
-	chg_nr = chgidx;    	/* true number of change-points */
+	chg_nr = chgidx;	/* true number of change-points */
 
 	/* sort change-point list by memory addresses (low -> high) */
 	still_changing = 1;
-	while (still_changing)	{
+	while (still_changing) {
 		still_changing = 0;
-		for (i = 1; i < chg_nr; i++)  {
+		for (i = 1; i < chg_nr; i++) {
 			/* if <current_addr> > <last_addr>, swap */
 			/* or, if current=<start_addr> & last=<end_addr>, swap */
 			if ((change_point[i]->addr < change_point[i-1]->addr) ||
@@ -261,10 +304,10 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 	}
 
 	/* create a new memmap, removing overlaps */
-	overlap_entries = 0;	 /* number of entries in the overlap table */
-	new_entry = 0;	 /* index for creating new memmap entries */
-	last_type = 0;		 /* start with undefined memory type */
-	last_addr = 0;		 /* start with 0 as last starting address */
+	overlap_entries = 0;	/* number of entries in the overlap table */
+	new_entry = 0;		/* index for creating new memmap entries */
+	last_type = 0;		/* start with undefined memory type */
+	last_addr = 0;		/* start with 0 as last starting address */
 	/* loop through change-points, determining affect on the new memmap */
 	for (chgidx = 0; chgidx < chg_nr; chgidx++) {
 		/* keep track of all overlapping memmap entries */
@@ -286,14 +329,14 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 			if (overlap_list[i]->type > current_type)
 				current_type = overlap_list[i]->type;
 		/* continue building up new memmap based on this information */
-		if (current_type != last_type)	{
+		if (current_type != last_type) {
 			if (last_type != 0) {
 				new_map[new_entry].size =
 					change_point[chgidx]->addr - last_addr;
 				/* move forward only if the new size was non-zero */
 				if (new_map[new_entry].size != 0)
 					if (++new_entry >= BFIN_MEMMAP_MAX)
-						break; 	/* no more space left for new entries */
+						break;	/* no more space left for new entries */
 			}
 			if (current_type != 0) {
 				new_map[new_entry].addr = change_point[chgidx]->addr;
@@ -303,9 +346,9 @@ static int __init sanitize_memmap(struct bfin_memmap_entry *map, int *pnr_map)
 			last_type = current_type;
 		}
 	}
-	new_nr = new_entry;   /* retain count for new entries */
+	new_nr = new_entry;	/* retain count for new entries */
 
-	/* copy new  mapping into original location */
+	/* copy new mapping into original location */
 	memcpy(map, new_map, new_nr*sizeof(struct bfin_memmap_entry));
 	*pnr_map = new_nr;
 
@@ -361,7 +404,6 @@ static __init int parse_memmap(char *arg)
  *  - "memmap=XXX[KkmM][@][$]XXX[KkmM]" defines a memory region
  *       @ from <start> to <start>+<mem>, type RAM
  *       $ from <start> to <start>+<mem>, type RESERVED
- *
  */
 static __init void parse_cmdline_early(char *cmdline_p)
 {
@@ -383,14 +425,15 @@ static __init void parse_cmdline_early(char *cmdline_p)
 					if (*to != ' ') {
 						if (*to == '$'
 						    || *(to + 1) == '$')
-							reserved_mem_dcache_on =
-							    1;
+							reserved_mem_dcache_on = 1;
 						if (*to == '#'
 						    || *(to + 1) == '#')
-							reserved_mem_icache_on =
-							    1;
+							reserved_mem_icache_on = 1;
 					}
 				}
+			} else if (!memcmp(to, "clkin_hz=", 9)) {
+				to += 9;
+				early_init_clkin_hz(to);
 			} else if (!memcmp(to, "earlyprintk=", 12)) {
 				to += 12;
 				setup_early_printk(to);
@@ -417,9 +460,8 @@ static __init void parse_cmdline_early(char *cmdline_p)
  *	[_ramend - DMA_UNCACHED_REGION,
  *		_ramend]:			uncached DMA region
  *  [_ramend, physical_mem_end]:	memory not managed by kernel
- *
  */
-static __init void  memory_setup(void)
+static __init void memory_setup(void)
 {
 #ifdef CONFIG_MTD_UCLINUX
 	unsigned long mtd_phys = 0;
@@ -436,7 +478,7 @@ static __init void  memory_setup(void)
 	memory_end = _ramend - DMA_UNCACHED_REGION;
 
 #ifdef CONFIG_MPU
-	/* Round up to multiple of 4MB.  */
+	/* Round up to multiple of 4MB */
 	memory_start = (_ramstart + 0x3fffff) & ~0x3fffff;
 #else
 	memory_start = PAGE_ALIGN(_ramstart);
@@ -616,7 +658,7 @@ static __init void setup_bootmem_allocator(void)
 	end_pfn = memory_end >> PAGE_SHIFT;
 
 	/*
-	 * give all the memory to the bootmap allocator,  tell it to put the
+	 * give all the memory to the bootmap allocator, tell it to put the
 	 * boot mem_map at the start of memory.
 	 */
 	bootmap_size = init_bootmem_node(NODE_DATA(0),
@@ -791,7 +833,11 @@ void __init setup_arch(char **cmdline_p)
 	bfin_write_SWRST(_bfin_swrst | DOUBLE_FAULT);
 #endif
 
+#ifdef CONFIG_SMP
+	if (_bfin_swrst & SWRST_DBL_FAULT_A) {
+#else
 	if (_bfin_swrst & RESET_DOUBLE) {
+#endif
 		printk(KERN_EMERG "Recovering from DOUBLE FAULT event\n");
 #ifdef CONFIG_DEBUG_DOUBLEFAULT
 		/* We assume the crashing kernel, and the current symbol table match */
@@ -823,9 +869,12 @@ void __init setup_arch(char **cmdline_p)
 			if (bfin_compiled_revid() == -1)
 				printk(KERN_ERR "Warning: Compiled for Rev none, but running on Rev %d\n",
 				       bfin_revid());
-			else if (bfin_compiled_revid() != 0xffff)
+			else if (bfin_compiled_revid() != 0xffff) {
 				printk(KERN_ERR "Warning: Compiled for Rev %d, but running on Rev %d\n",
 				       bfin_compiled_revid(), bfin_revid());
+				if (bfin_compiled_revid() > bfin_revid())
+					panic("Error: you are missing anomaly workarounds for this rev\n");
+			}
 		}
 		if (bfin_revid() < CONFIG_BF_REV_MIN || bfin_revid() > CONFIG_BF_REV_MAX)
 			printk(KERN_ERR "Warning: Unsupported Chip Revision ADSP-%s Rev 0.%d detected\n",
@@ -835,7 +884,7 @@ void __init setup_arch(char **cmdline_p)
 	printk(KERN_INFO "Blackfin Linux support by http://blackfin.uclinux.org/\n");
 
 	printk(KERN_INFO "Processor Speed: %lu MHz core clock and %lu MHz System Clock\n",
-	       cclk / 1000000,  sclk / 1000000);
+	       cclk / 1000000, sclk / 1000000);
 
 	if (ANOMALY_05000273 && (cclk >> 1) <= sclk)
 		printk("\n\n\nANOMALY_05000273: CCLK must be >= 2*SCLK !!!\n\n\n");
@@ -867,18 +916,21 @@ void __init setup_arch(char **cmdline_p)
 	BUG_ON((char *)&safe_user_instruction - (char *)&fixed_code_start
 		!= SAFE_USER_INSTRUCTION - FIXED_CODE_START);
 
+#ifdef CONFIG_SMP
+	platform_init_cpus();
+#endif
 	init_exception_vectors();
-	bfin_cache_init();
+	bfin_cache_init();	/* Initialize caches for the boot CPU */
 }
 
 static int __init topology_init(void)
 {
-	int cpu;
+	unsigned int cpu;
+	/* Record CPU-private information for the boot processor. */
+	bfin_setup_cpudata(0);
 
 	for_each_possible_cpu(cpu) {
-		struct cpu *c = &per_cpu(cpu_devices, cpu);
-
-		register_cpu(c, cpu);
+		register_cpu(&per_cpu(cpu_data, cpu).cpu, cpu);
 	}
 
 	return 0;
@@ -886,36 +938,54 @@ static int __init topology_init(void)
 
 subsys_initcall(topology_init);
 
+/* Get the input clock frequency */
+static u_long cached_clkin_hz = CONFIG_CLKIN_HZ;
+static u_long get_clkin_hz(void)
+{
+	return cached_clkin_hz;
+}
+static int __init early_init_clkin_hz(char *buf)
+{
+	cached_clkin_hz = simple_strtoul(buf, NULL, 0);
+#ifdef BFIN_KERNEL_CLOCK
+	if (cached_clkin_hz != CONFIG_CLKIN_HZ)
+		panic("cannot change clkin_hz when reprogramming clocks");
+#endif
+	return 1;
+}
+early_param("clkin_hz=", early_init_clkin_hz);
+
 /* Get the voltage input multiplier */
-static u_long cached_vco_pll_ctl, cached_vco;
 static u_long get_vco(void)
 {
-	u_long msel;
+	static u_long cached_vco;
+	u_long msel, pll_ctl;
 
-	u_long pll_ctl = bfin_read_PLL_CTL();
-	if (pll_ctl == cached_vco_pll_ctl)
+	/* The assumption here is that VCO never changes at runtime.
+	 * If, someday, we support that, then we'll have to change this.
+	 */
+	if (cached_vco)
 		return cached_vco;
-	else
-		cached_vco_pll_ctl = pll_ctl;
 
+	pll_ctl = bfin_read_PLL_CTL();
 	msel = (pll_ctl >> 9) & 0x3F;
 	if (0 == msel)
 		msel = 64;
 
-	cached_vco = CONFIG_CLKIN_HZ;
+	cached_vco = get_clkin_hz();
 	cached_vco >>= (1 & pll_ctl);	/* DF bit */
 	cached_vco *= msel;
 	return cached_vco;
 }
 
 /* Get the Core clock */
-static u_long cached_cclk_pll_div, cached_cclk;
 u_long get_cclk(void)
 {
+	static u_long cached_cclk_pll_div, cached_cclk;
 	u_long csel, ssel;
 
 	if (bfin_read_PLL_STAT() & 0x1)
-		return CONFIG_CLKIN_HZ;
+		return get_clkin_hz();
 
 	ssel = bfin_read_PLL_DIV();
 	if (ssel == cached_cclk_pll_div)
@@ -934,21 +1004,21 @@ u_long get_cclk(void)
 EXPORT_SYMBOL(get_cclk);
 
 /* Get the System clock */
-static u_long cached_sclk_pll_div, cached_sclk;
 u_long get_sclk(void)
 {
+	static u_long cached_sclk;
 	u_long ssel;
 
-	if (bfin_read_PLL_STAT() & 0x1)
-		return CONFIG_CLKIN_HZ;
-
-	ssel = bfin_read_PLL_DIV();
-	if (ssel == cached_sclk_pll_div)
+	/* The assumption here is that SCLK never changes at runtime.
+	 * If, someday, we support that, then we'll have to change this.
+	 */
+	if (cached_sclk)
 		return cached_sclk;
-	else
-		cached_sclk_pll_div = ssel;
 
-	ssel &= 0xf;
+	if (bfin_read_PLL_STAT() & 0x1)
+		return get_clkin_hz();
+
+	ssel = bfin_read_PLL_DIV() & 0xf;
 	if (0 == ssel) {
 		printk(KERN_WARNING "Invalid System Clock\n");
 		ssel = 1;
@@ -982,17 +1052,18 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 {
 	char *cpu, *mmu, *fpu, *vendor, *cache;
 	uint32_t revid;
-
-	u_long cclk = 0, sclk = 0;
+	int cpu_num = *(unsigned int *)v;
+	u_long sclk, cclk;
 	u_int icache_size = BFIN_ICACHESIZE / 1024, dcache_size = 0, dsup_banks = 0;
+	struct blackfin_cpudata *cpudata = &per_cpu(cpu_data, cpu_num);
 
 	cpu = CPU;
 	mmu = "none";
 	fpu = "none";
 	revid = bfin_revid();
 
-	cclk = get_cclk();
 	sclk = get_sclk();
+	cclk = get_cclk();
 
 	switch (bfin_read_CHIPID() & CHIPID_MANUFACTURE) {
 	case 0xca:
@@ -1003,10 +1074,7 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		break;
 	}
 
-	seq_printf(m, "processor\t: %d\n"
-		"vendor_id\t: %s\n",
-		*(unsigned int *)v,
-		vendor);
+	seq_printf(m, "processor\t: %d\n" "vendor_id\t: %s\n", cpu_num, vendor);
 
 	if (CPUID == bfin_cpuid())
 		seq_printf(m, "cpu family\t: 0x%04x\n", CPUID);
@@ -1029,12 +1097,12 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		sclk/1000000, sclk%1000000);
 	seq_printf(m, "bogomips\t: %lu.%02lu\n"
 		"Calibration\t: %lu loops\n",
-		(loops_per_jiffy * HZ) / 500000,
-		((loops_per_jiffy * HZ) / 5000) % 100,
-		(loops_per_jiffy * HZ));
+		(cpudata->loops_per_jiffy * HZ) / 500000,
+		((cpudata->loops_per_jiffy * HZ) / 5000) % 100,
+		(cpudata->loops_per_jiffy * HZ));
 
 	/* Check Cache configutation */
-	switch (bfin_read_DMEM_CONTROL() & (1 << DMC0_P | 1 << DMC1_P)) {
+	switch (cpudata->dmemctl & (1 << DMC0_P | 1 << DMC1_P)) {
 	case ACACHE_BSRAM:
 		cache = "dbank-A/B\t: cache/sram";
 		dcache_size = 16;
@@ -1058,10 +1126,10 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 	}
 
 	/* Is it turned on? */
-	if ((bfin_read_DMEM_CONTROL() & (ENDCPLB | DMC_ENABLE)) != (ENDCPLB | DMC_ENABLE))
+	if ((cpudata->dmemctl & (ENDCPLB | DMC_ENABLE)) != (ENDCPLB | DMC_ENABLE))
 		dcache_size = 0;
 
-	if ((bfin_read_IMEM_CONTROL() & (IMC | ENICPLB)) != (IMC | ENICPLB))
+	if ((cpudata->imemctl & (IMC | ENICPLB)) != (IMC | ENICPLB))
 		icache_size = 0;
 
 	seq_printf(m, "cache size\t: %d KB(L1 icache) "
@@ -1086,8 +1154,11 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		   "dcache setup\t: %d Super-banks/%d Sub-banks/%d Ways, %d Lines/Way\n",
 		   dsup_banks, BFIN_DSUBBANKS, BFIN_DWAYS,
 		   BFIN_DLINES);
+#ifdef __ARCH_SYNC_CORE_DCACHE
+	seq_printf(m, "SMP Dcache Flushes\t: %lu\n\n", cpudata->dcache_invld_count);
+#endif
 #ifdef CONFIG_BFIN_ICACHE_LOCK
-	switch ((bfin_read_IMEM_CONTROL() >> 3) & WAYALL_L) {
+	switch ((cpudata->imemctl >> 3) & WAYALL_L) {
 	case WAY0_L:
 		seq_printf(m, "Way0 Locked-Down\n");
 		break;
@@ -1137,6 +1208,12 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		seq_printf(m, "No Ways are locked\n");
 	}
 #endif
+
+	if (cpu_num != num_possible_cpus() - 1)
+		return 0;
+
+	if (L2_LENGTH)
+		seq_printf(m, "L2 SRAM\t\t: %dKB\n", L2_LENGTH/0x400);
 	seq_printf(m, "board name\t: %s\n", bfin_board_name);
 	seq_printf(m, "board memory\t: %ld kB (0x%p -> 0x%p)\n",
 		 physical_mem_end >> 10, (void *)0, (void *)physical_mem_end);
@@ -1144,6 +1221,7 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 		((int)memory_end - (int)_stext) >> 10,
 		_stext,
 		(void *)memory_end);
+	seq_printf(m, "\n");
 
 	return 0;
 }
diff --git a/arch/blackfin/kernel/time.c b/arch/blackfin/kernel/time.c
index eb235232045..172b4c58846 100644
--- a/arch/blackfin/kernel/time.c
+++ b/arch/blackfin/kernel/time.c
@@ -1,32 +1,11 @@
 /*
- * File:         arch/blackfin/kernel/time.c
- * Based on:     none - original work
- * Author:
+ * arch/blackfin/kernel/time.c
  *
- * Created:
- * Description:  This file contains the bfin-specific time handling details.
- *               Most of the stuff is located in the machine specific files.
- *		 FIXME: (This file is subject for removal)
+ * This file contains the Blackfin-specific time handling details.
+ * Most of the stuff is located in the machine specific files.
  *
- * Modified:
- *               Copyright 2004-2008 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
+ * Copyright 2004-2008 Analog Devices Inc.
+ * Licensed under the GPL-2 or later.
  */
 
 #include <linux/module.h>
@@ -34,23 +13,43 @@
 #include <linux/interrupt.h>
 #include <linux/time.h>
 #include <linux/irq.h>
+#include <linux/delay.h>
 
 #include <asm/blackfin.h>
 #include <asm/time.h>
+#include <asm/gptimers.h>
 
 /* This is an NTP setting */
 #define	TICK_SIZE (tick_nsec / 1000)
 
-static void time_sched_init(irq_handler_t timer_routine);
-static unsigned long gettimeoffset(void);
-
 static struct irqaction bfin_timer_irq = {
-	.name = "BFIN Timer Tick",
+	.name = "Blackfin Timer Tick",
+#ifdef CONFIG_IRQ_PER_CPU
+	.flags = IRQF_DISABLED | IRQF_PERCPU,
+#else
 	.flags = IRQF_DISABLED
+#endif
 };
 
-static void
-time_sched_init(irq_handler_t timer_routine)
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE)
+void __init setup_system_timer0(void)
+{
+	/* Power down the core timer, just to play safe. */
+	bfin_write_TCNTL(0);
+
+	disable_gptimers(TIMER0bit);
+	set_gptimer_status(0, TIMER_STATUS_TRUN0);
+	while (get_gptimer_status(0) & TIMER_STATUS_TRUN0)
+		udelay(10);
+
+	set_gptimer_config(0, 0x59); /* IRQ enable, periodic, PWM_OUT, SCLKed, OUT PAD disabled */
+	set_gptimer_period(TIMER0_id, get_sclk() / HZ);
+	set_gptimer_pwidth(TIMER0_id, 1);
+	SSYNC();
+	enable_gptimers(TIMER0bit);
+}
+#else
+void __init setup_core_timer(void)
 {
 	u32 tcount;
 
@@ -58,10 +57,8 @@ time_sched_init(irq_handler_t timer_routine)
 	bfin_write_TCNTL(1);
 	CSYNC();
 
-	/*
-	 * the TSCALE prescaler counter.
-	 */
-	bfin_write_TSCALE((TIME_SCALE - 1));
+	/* the TSCALE prescaler counter */
+	bfin_write_TSCALE(TIME_SCALE - 1);
 
 	tcount = ((get_cclk() / (HZ * TIME_SCALE)) - 1);
 	bfin_write_TPERIOD(tcount);
@@ -71,35 +68,52 @@ time_sched_init(irq_handler_t timer_routine)
 	CSYNC();
 
 	bfin_write_TCNTL(7);
+}
+#endif
 
-	bfin_timer_irq.handler = (irq_handler_t)timer_routine;
-	/* call setup_irq instead of request_irq because request_irq calls
-	 * kmalloc which has not been initialized yet
-	 */
+static void __init
+time_sched_init(irqreturn_t(*timer_routine) (int, void *))
+{
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE)
+	setup_system_timer0();
+	bfin_timer_irq.handler = timer_routine;
+	setup_irq(IRQ_TIMER0, &bfin_timer_irq);
+#else
+	setup_core_timer();
+	bfin_timer_irq.handler = timer_routine;
 	setup_irq(IRQ_CORETMR, &bfin_timer_irq);
+#endif
 }
 
 /*
  * Should return useconds since last timer tick
  */
+#ifndef CONFIG_GENERIC_TIME
 static unsigned long gettimeoffset(void)
 {
 	unsigned long offset;
 	unsigned long clocks_per_jiffy;
 
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) || defined(CONFIG_IPIPE)
+	clocks_per_jiffy = bfin_read_TIMER0_PERIOD();
+	offset = bfin_read_TIMER0_COUNTER() / \
+		(((clocks_per_jiffy + 1) * HZ) / USEC_PER_SEC);
+
+	if ((get_gptimer_status(0) & TIMER_STATUS_TIMIL0) && offset < (100000 / HZ / 2))
+		offset += (USEC_PER_SEC / HZ);
+#else
 	clocks_per_jiffy = bfin_read_TPERIOD();
-	offset =
-	    (clocks_per_jiffy -
-	     bfin_read_TCOUNT()) / (((clocks_per_jiffy + 1) * HZ) /
-				    USEC_PER_SEC);
+	offset = (clocks_per_jiffy - bfin_read_TCOUNT()) / \
+		(((clocks_per_jiffy + 1) * HZ) / USEC_PER_SEC);
 
 	/* Check if we just wrapped the counters and maybe missed a tick */
 	if ((bfin_read_ILAT() & (1 << IRQ_CORETMR))
-	    && (offset < (100000 / HZ / 2)))
+		&& (offset < (100000 / HZ / 2)))
 		offset += (USEC_PER_SEC / HZ);
-
+#endif
 	return offset;
 }
+#endif
 
 static inline int set_rtc_mmss(unsigned long nowtime)
 {
@@ -111,43 +125,49 @@ static inline int set_rtc_mmss(unsigned long nowtime)
  * as well as call the "do_timer()" routine every clocktick
  */
 #ifdef CONFIG_CORE_TIMER_IRQ_L1
-irqreturn_t timer_interrupt(int irq, void *dummy)__attribute__((l1_text));
+__attribute__((l1_text))
 #endif
-
 irqreturn_t timer_interrupt(int irq, void *dummy)
 {
 	/* last time the cmos clock got updated */
 	static long last_rtc_update;
 
 	write_seqlock(&xtime_lock);
-
-	do_timer(1);
-
-	profile_tick(CPU_PROFILING);
-
-	/*
-	 * If we have an externally synchronized Linux clock, then update
-	 * CMOS clock accordingly every ~11 minutes. Set_rtc_mmss() has to be
-	 * called as close as possible to 500 ms before the new second starts.
-	 */
-
-	if (ntp_synced() &&
-	    xtime.tv_sec > last_rtc_update + 660 &&
-	    (xtime.tv_nsec / NSEC_PER_USEC) >=
-	    500000 - ((unsigned)TICK_SIZE) / 2
-	    && (xtime.tv_nsec / NSEC_PER_USEC) <=
-	    500000 + ((unsigned)TICK_SIZE) / 2) {
-		if (set_rtc_mmss(xtime.tv_sec) == 0)
-			last_rtc_update = xtime.tv_sec;
-		else
-			/* Do it again in 60s. */
-			last_rtc_update = xtime.tv_sec - 600;
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) && !defined(CONFIG_IPIPE)
+/* FIXME: Here TIMIL0 is not set when IPIPE enabled, why? */
+	if (get_gptimer_status(0) & TIMER_STATUS_TIMIL0) {
+#endif
+		do_timer(1);
+
+		/*
+		 * If we have an externally synchronized Linux clock, then update
+		 * CMOS clock accordingly every ~11 minutes. Set_rtc_mmss() has to be
+		 * called as close as possible to 500 ms before the new second starts.
+		 */
+		if (ntp_synced() &&
+		    xtime.tv_sec > last_rtc_update + 660 &&
+		    (xtime.tv_nsec / NSEC_PER_USEC) >=
+		    500000 - ((unsigned)TICK_SIZE) / 2
+		    && (xtime.tv_nsec / NSEC_PER_USEC) <=
+		    500000 + ((unsigned)TICK_SIZE) / 2) {
+			if (set_rtc_mmss(xtime.tv_sec) == 0)
+				last_rtc_update = xtime.tv_sec;
+			else
+				/* Do it again in 60s. */
+				last_rtc_update = xtime.tv_sec - 600;
+		}
+#if defined(CONFIG_TICK_SOURCE_SYSTMR0) && !defined(CONFIG_IPIPE)
+		set_gptimer_status(0, TIMER_STATUS_TIMIL0);
 	}
+#endif
 	write_sequnlock(&xtime_lock);
 
-#ifndef CONFIG_SMP
+#ifdef CONFIG_IPIPE
+	update_root_process_times(get_irq_regs());
+#else
 	update_process_times(user_mode(get_irq_regs()));
 #endif
+	profile_tick(CPU_PROFILING);
 
 	return IRQ_HANDLED;
 }
diff --git a/arch/blackfin/kernel/traps.c b/arch/blackfin/kernel/traps.c
index bef025b0744..17d8e417289 100644
--- a/arch/blackfin/kernel/traps.c
+++ b/arch/blackfin/kernel/traps.c
@@ -75,16 +75,6 @@ void __init trap_init(void)
 	CSYNC();
 }
 
-/*
- * Used to save the RETX, SEQSTAT, I/D CPLB FAULT ADDR
- * values across the transition from exception to IRQ5.
- * We put these in L1, so they are going to be in a valid
- * location during exception context
- */
-__attribute__((l1_data))
-unsigned long saved_retx, saved_seqstat,
-	saved_icplb_fault_addr, saved_dcplb_fault_addr;
-
 static void decode_address(char *buf, unsigned long address)
 {
 #ifdef CONFIG_DEBUG_VERBOSE
@@ -211,18 +201,18 @@ asmlinkage void double_fault_c(struct pt_regs *fp)
 	printk(KERN_EMERG "\n" KERN_EMERG "Double Fault\n");
 #ifdef CONFIG_DEBUG_DOUBLEFAULT_PRINT
 	if (((long)fp->seqstat &  SEQSTAT_EXCAUSE) == VEC_UNCOV) {
+		unsigned int cpu = smp_processor_id();
 		char buf[150];
-		decode_address(buf, saved_retx);
+		decode_address(buf, cpu_pda[cpu].retx);
 		printk(KERN_EMERG "While handling exception (EXCAUSE = 0x%x) at %s:\n",
-			(int)saved_seqstat & SEQSTAT_EXCAUSE, buf);
-		decode_address(buf, saved_dcplb_fault_addr);
+			(unsigned int)cpu_pda[cpu].seqstat & SEQSTAT_EXCAUSE, buf);
+		decode_address(buf, cpu_pda[cpu].dcplb_fault_addr);
 		printk(KERN_NOTICE "   DCPLB_FAULT_ADDR: %s\n", buf);
-		decode_address(buf, saved_icplb_fault_addr);
+		decode_address(buf, cpu_pda[cpu].icplb_fault_addr);
 		printk(KERN_NOTICE "   ICPLB_FAULT_ADDR: %s\n", buf);
 
 		decode_address(buf, fp->retx);
-		printk(KERN_NOTICE "The instruction at %s caused a double exception\n",
-			buf);
+		printk(KERN_NOTICE "The instruction at %s caused a double exception\n", buf);
 	} else
 #endif
 	{
@@ -240,6 +230,9 @@ asmlinkage void trap_c(struct pt_regs *fp)
 #ifdef CONFIG_DEBUG_BFIN_HWTRACE_ON
 	int j;
 #endif
+#ifdef CONFIG_DEBUG_HUNT_FOR_ZERO
+	unsigned int cpu = smp_processor_id();
+#endif
 	int sig = 0;
 	siginfo_t info;
 	unsigned long trapnr = fp->seqstat & SEQSTAT_EXCAUSE;
@@ -417,7 +410,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
 		info.si_code = ILL_CPLB_MULHIT;
 		sig = SIGSEGV;
 #ifdef CONFIG_DEBUG_HUNT_FOR_ZERO
-		if (saved_dcplb_fault_addr < FIXED_CODE_START)
+		if (cpu_pda[cpu].dcplb_fault_addr < FIXED_CODE_START)
 			verbose_printk(KERN_NOTICE "NULL pointer access\n");
 		else
 #endif
@@ -471,7 +464,7 @@ asmlinkage void trap_c(struct pt_regs *fp)
 		info.si_code = ILL_CPLB_MULHIT;
 		sig = SIGSEGV;
 #ifdef CONFIG_DEBUG_HUNT_FOR_ZERO
-		if (saved_icplb_fault_addr < FIXED_CODE_START)
+		if (cpu_pda[cpu].icplb_fault_addr < FIXED_CODE_START)
 			verbose_printk(KERN_NOTICE "Jump to NULL address\n");
 		else
 #endif
@@ -584,10 +577,15 @@ asmlinkage void trap_c(struct pt_regs *fp)
 		}
 	}
 
-	info.si_signo = sig;
-	info.si_errno = 0;
-	info.si_addr = (void __user *)fp->pc;
-	force_sig_info(sig, &info, current);
+#ifdef CONFIG_IPIPE
+	if (!ipipe_trap_notify(fp->seqstat & 0x3f, fp))
+#endif
+	{
+		info.si_signo = sig;
+		info.si_errno = 0;
+		info.si_addr = (void __user *)fp->pc;
+		force_sig_info(sig, &info, current);
+	}
 
 	trace_buffer_restore(j);
 	return;
@@ -656,13 +654,13 @@ static bool get_instruction(unsigned short *val, unsigned short *address)
 	return false;
 }
 
-/* 
+/*
  * decode the instruction if we are printing out the trace, as it
  * makes things easier to follow, without running it through objdump
  * These are the normal instructions which cause change of flow, which
  * would be at the source of the trace buffer
  */
-#ifdef CONFIG_DEBUG_VERBOSE
+#if defined(CONFIG_DEBUG_VERBOSE) && defined(CONFIG_DEBUG_BFIN_HWTRACE_ON)
 static void decode_instruction(unsigned short *address)
 {
 	unsigned short opcode;
@@ -846,7 +844,7 @@ void show_stack(struct task_struct *task, unsigned long *stack)
 	}
 	if (fp) {
 		frame = fp;
-		printk(" FP: (0x%p)\n", fp);
+		printk(KERN_NOTICE " FP: (0x%p)\n", fp);
 	} else
 		frame = 0;
 
@@ -960,6 +958,7 @@ void dump_bfin_process(struct pt_regs *fp)
 		else
 			verbose_printk(KERN_NOTICE "COMM= invalid\n");
 
+		printk(KERN_NOTICE "CPU = %d\n", current_thread_info()->cpu);
 		if (!((unsigned long)current->mm & 0x3) && (unsigned long)current->mm >= FIXED_CODE_START)
 			verbose_printk(KERN_NOTICE  "TEXT = 0x%p-0x%p        DATA = 0x%p-0x%p\n"
 				KERN_NOTICE " BSS = 0x%p-0x%p  USER-STACK = 0x%p\n"
@@ -1053,6 +1052,7 @@ void show_regs(struct pt_regs *fp)
 	struct irqaction *action;
 	unsigned int i;
 	unsigned long flags;
+	unsigned int cpu = smp_processor_id();
 
 	verbose_printk(KERN_NOTICE "\n" KERN_NOTICE "SEQUENCER STATUS:\t\t%s\n", print_tainted());
 	verbose_printk(KERN_NOTICE " SEQSTAT: %08lx  IPEND: %04lx  SYSCFG: %04lx\n",
@@ -1112,9 +1112,9 @@ unlock:
 
 	if (((long)fp->seqstat &  SEQSTAT_EXCAUSE) &&
 	    (((long)fp->seqstat & SEQSTAT_EXCAUSE) != VEC_HWERR)) {
-		decode_address(buf, saved_dcplb_fault_addr);
+		decode_address(buf, cpu_pda[cpu].dcplb_fault_addr);
 		verbose_printk(KERN_NOTICE "DCPLB_FAULT_ADDR: %s\n", buf);
-		decode_address(buf, saved_icplb_fault_addr);
+		decode_address(buf, cpu_pda[cpu].icplb_fault_addr);
 		verbose_printk(KERN_NOTICE "ICPLB_FAULT_ADDR: %s\n", buf);
 	}
 
@@ -1153,20 +1153,21 @@ unlock:
 asmlinkage int sys_bfin_spinlock(int *spinlock)__attribute__((l1_text));
 #endif
 
-asmlinkage int sys_bfin_spinlock(int *spinlock)
+static DEFINE_SPINLOCK(bfin_spinlock_lock);
+
+asmlinkage int sys_bfin_spinlock(int *p)
 {
-	int ret = 0;
-	int tmp = 0;
+	int ret, tmp = 0;
 
-	local_irq_disable();
-	ret = get_user(tmp, spinlock);
-	if (ret == 0) {
-		if (tmp)
+	spin_lock(&bfin_spinlock_lock);	/* This would also hold kernel preemption. */
+	ret = get_user(tmp, p);
+	if (likely(ret == 0)) {
+		if (unlikely(tmp))
 			ret = 1;
-		tmp = 1;
-		put_user(tmp, spinlock);
+		else
+			put_user(1, p);
 	}
-	local_irq_enable();
+	spin_unlock(&bfin_spinlock_lock);
 	return ret;
 }
 
diff --git a/arch/blackfin/kernel/vmlinux.lds.S b/arch/blackfin/kernel/vmlinux.lds.S
index 7d12c6692a6..4b4341da058 100644
--- a/arch/blackfin/kernel/vmlinux.lds.S
+++ b/arch/blackfin/kernel/vmlinux.lds.S
@@ -68,6 +68,8 @@ SECTIONS
 		__etext = .;
 	}
 
+	NOTES
+
 	/* Just in case the first read only is a 32-bit access */
 	RO_DATA(4)
 
@@ -109,7 +111,6 @@ SECTIONS
 #endif
 
 		DATA_DATA
-		*(.data.*)
 		CONSTRUCTORS
 
 		/* make sure the init_task is aligned to the
@@ -161,12 +162,14 @@ SECTIONS
 		*(.con_initcall.init)
 		___con_initcall_end = .;
 	}
+	PERCPU(4)
 	SECURITY_INIT
 	.init.ramfs :
 	{
 		. = ALIGN(4);
 		___initramfs_start = .;
 		*(.init.ramfs)
+		. = ALIGN(4);
 		___initramfs_end = .;
 	}
 
@@ -212,7 +215,7 @@ SECTIONS
 		__ebss_b_l1 = .;
 	}
 
-	__l2_lma_start = .;
+	__l2_lma_start = LOADADDR(.data_b_l1) + SIZEOF(.data_b_l1);
 
 	.text_data_l2 L2_START : AT(LOADADDR(.data_b_l1) + SIZEOF(.data_b_l1))
 	{
@@ -240,7 +243,7 @@ SECTIONS
 	/* Force trailing alignment of our init section so that when we
 	 * free our init memory, we don't leave behind a partial page.
 	 */
-	. = LOADADDR(.data_b_l1) + SIZEOF(.data_b_l1);
+	. = LOADADDR(.text_data_l2) + SIZEOF(.text_data_l2);
 	. = ALIGN(PAGE_SIZE);
 	___init_end = .;