Moved, renamed, and deleted files

The original directory structure was scattered and unorganized.
Changes are basically to make it look like kernel structure.

14 files changed, 6039 insertions, 0 deletions

diff --git a/arch/blackfin/mach-common/Makefile b/arch/blackfin/mach-common/Makefile
new file mode 100644
index 00000000..ff299f24
--- /dev/null
+++ b/arch/blackfin/mach-common/Makefile
@@ -0,0 +1,13 @@
+#
+# arch/blackfin/mach-common/Makefile
+#
+
+obj-y := \
+	cache.o cache-c.o entry.o head.o \
+	interrupt.o arch_checks.o ints-priority.o
+
+obj-$(CONFIG_PM)          += pm.o dpmc_modes.o
+obj-$(CONFIG_CPU_FREQ)    += cpufreq.o
+obj-$(CONFIG_CPU_VOLTAGE) += dpmc.o
+obj-$(CONFIG_SMP)         += smp.o
+obj-$(CONFIG_BFIN_KERNEL_CLOCK) += clocks-init.o
diff --git a/arch/blackfin/mach-common/arch_checks.c b/arch/blackfin/mach-common/arch_checks.c
new file mode 100644
index 00000000..d8643fdd
--- /dev/null
+++ b/arch/blackfin/mach-common/arch_checks.c
@@ -0,0 +1,66 @@
+/*
+ * Do some checking to make sure things are OK
+ *
+ * Copyright 2007-2010 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <asm/fixed_code.h>
+#include <mach/anomaly.h>
+#include <asm/clocks.h>
+
+#ifdef CONFIG_BFIN_KERNEL_CLOCK
+
+# if (CONFIG_VCO_HZ > CONFIG_MAX_VCO_HZ)
+#  error "VCO selected is more than maximum value. Please change the VCO multipler"
+# endif
+
+# if (CONFIG_SCLK_HZ > CONFIG_MAX_SCLK_HZ)
+# error "Sclk value selected is more than maximum. Please select a proper value for SCLK multiplier"
+# endif
+
+# if (CONFIG_SCLK_HZ < CONFIG_MIN_SCLK_HZ)
+# error "Sclk value selected is less than minimum. Please select a proper value for SCLK multiplier"
+# endif
+
+# if (ANOMALY_05000273) && (CONFIG_SCLK_HZ * 2 > CONFIG_CCLK_HZ)
+# error "ANOMALY 05000273, please make sure CCLK is at least 2x SCLK"
+# endif
+
+# if (CONFIG_SCLK_HZ > CONFIG_CCLK_HZ) && (CONFIG_SCLK_HZ != CONFIG_CLKIN_HZ) && (CONFIG_CCLK_HZ != CONFIG_CLKIN_HZ)
+# error "Please select sclk less than cclk"
+# endif
+
+#endif /* CONFIG_BFIN_KERNEL_CLOCK */
+
+#if CONFIG_BOOT_LOAD < FIXED_CODE_END
+# error "The kernel load address must be after the fixed code section"
+#endif
+
+#if (CONFIG_BOOT_LOAD & 0x3)
+# error "The kernel load address must be 4 byte aligned"
+#endif
+
+/* The entire kernel must be able to make a 24bit pcrel call to start of L1 */
+#if ((0xffffffff - L1_CODE_START + 1) + CONFIG_BOOT_LOAD) > 0x1000000
+# error "The kernel load address is too high; keep it below 10meg for safety"
+#endif
+
+#if ANOMALY_05000263 && defined(CONFIG_MPU)
+# error the MPU will not function safely while Anomaly 05000263 applies
+#endif
+
+#if ANOMALY_05000448
+# error You are using a part with anomaly 05000448, this issue causes random memory read/write failures - that means random crashes.
+#endif
+
+/* if 220 exists, can not set External Memory WB and L2 not_cached, either External Memory not_cached and L2 WB */
+#if ANOMALY_05000220 && \
+	(defined(CONFIG_BFIN_EXTMEM_WRITEBACK) || defined(CONFIG_BFIN_L2_WRITEBACK))
+# error "Anomaly 05000220 does not allow you to use Write Back cache with L2 or External Memory"
+#endif
+
+#if ANOMALY_05000491 && !defined(CONFIG_ICACHE_FLUSH_L1)
+# error You need IFLUSH in L1 inst while Anomaly 05000491 applies
+#endif
diff --git a/arch/blackfin/mach-common/cache-c.c b/arch/blackfin/mach-common/cache-c.c
new file mode 100644
index 00000000..a60a24f5
--- /dev/null
+++ b/arch/blackfin/mach-common/cache-c.c
@@ -0,0 +1,76 @@
+/*
+ * Blackfin cache control code (simpler control-style functions)
+ *
+ * Copyright 2004-2009 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/init.h>
+#include <asm/blackfin.h>
+#include <asm/cplbinit.h>
+
+/* Invalidate the Entire Data cache by
+ * clearing DMC[1:0] bits
+ */
+void blackfin_invalidate_entire_dcache(void)
+{
+	u32 dmem = bfin_read_DMEM_CONTROL();
+	bfin_write_DMEM_CONTROL(dmem & ~0xc);
+	SSYNC();
+	bfin_write_DMEM_CONTROL(dmem);
+	SSYNC();
+}
+
+/* Invalidate the Entire Instruction cache by
+ * clearing IMC bit
+ */
+void blackfin_invalidate_entire_icache(void)
+{
+	u32 imem = bfin_read_IMEM_CONTROL();
+	bfin_write_IMEM_CONTROL(imem & ~0x4);
+	SSYNC();
+	bfin_write_IMEM_CONTROL(imem);
+	SSYNC();
+}
+
+#if defined(CONFIG_BFIN_ICACHE) || defined(CONFIG_BFIN_DCACHE)
+
+static void
+bfin_cache_init(struct cplb_entry *cplb_tbl, unsigned long cplb_addr,
+                unsigned long cplb_data, unsigned long mem_control,
+                unsigned long mem_mask)
+{
+	int i;
+
+	for (i = 0; i < MAX_CPLBS; i++) {
+		bfin_write32(cplb_addr + i * 4, cplb_tbl[i].addr);
+		bfin_write32(cplb_data + i * 4, cplb_tbl[i].data);
+	}
+
+	_enable_cplb(mem_control, mem_mask);
+}
+
+#ifdef CONFIG_BFIN_ICACHE
+void __cpuinit bfin_icache_init(struct cplb_entry *icplb_tbl)
+{
+	bfin_cache_init(icplb_tbl, ICPLB_ADDR0, ICPLB_DATA0, IMEM_CONTROL,
+		(IMC | ENICPLB));
+}
+#endif
+
+#ifdef CONFIG_BFIN_DCACHE
+void __cpuinit bfin_dcache_init(struct cplb_entry *dcplb_tbl)
+{
+	/*
+	 *  Anomaly notes:
+	 *  05000287 - We implement workaround #2 - Change the DMEM_CONTROL
+	 *  register, so that the port preferences for DAG0 and DAG1 are set
+	 *  to port B
+	 */
+	bfin_cache_init(dcplb_tbl, DCPLB_ADDR0, DCPLB_DATA0, DMEM_CONTROL,
+		(DMEM_CNTR | PORT_PREF0 | (ANOMALY_05000287 ? PORT_PREF1 : 0)));
+}
+#endif
+
+#endif
diff --git a/arch/blackfin/mach-common/cache.S b/arch/blackfin/mach-common/cache.S
new file mode 100644
index 00000000..9f4dd35b
--- /dev/null
+++ b/arch/blackfin/mach-common/cache.S
@@ -0,0 +1,124 @@
+/*
+ * Blackfin cache control code
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/linkage.h>
+#include <asm/blackfin.h>
+#include <asm/cache.h>
+#include <asm/page.h>
+
+/* 05000443 - IFLUSH cannot be last instruction in hardware loop */
+#if ANOMALY_05000443
+# define BROK_FLUSH_INST "IFLUSH"
+#else
+# define BROK_FLUSH_INST "no anomaly! yeah!"
+#endif
+
+/* Since all L1 caches work the same way, we use the same method for flushing
+ * them.  Only the actual flush instruction differs.  We write this in asm as
+ * GCC can be hard to coax into writing nice hardware loops.
+ *
+ * Also, we assume the following register setup:
+ * R0 = start address
+ * R1 = end address
+ */
+.macro do_flush flushins:req label
+
+	R2 = -L1_CACHE_BYTES;
+
+	/* start = (start & -L1_CACHE_BYTES) */
+	R0 = R0 & R2;
+
+	/* end = ((end - 1) & -L1_CACHE_BYTES) + L1_CACHE_BYTES; */
+	R1 += -1;
+	R1 = R1 & R2;
+	R1 += L1_CACHE_BYTES;
+
+	/* count = (end - start) >> L1_CACHE_SHIFT */
+	R2 = R1 - R0;
+	R2 >>= L1_CACHE_SHIFT;
+	P1 = R2;
+
+.ifnb \label
+\label :
+.endif
+	P0 = R0;
+
+	LSETUP (1f, 2f) LC1 = P1;
+1:
+.ifeqs "\flushins", BROK_FLUSH_INST
+	\flushins [P0++];
+	nop;
+	nop;
+2:	nop;
+.else
+2:	\flushins [P0++];
+.endif
+
+	RTS;
+.endm
+
+#ifdef CONFIG_ICACHE_FLUSH_L1
+.section .l1.text
+#else
+.text
+#endif
+
+/* Invalidate all instruction cache lines assocoiated with this memory area */
+#ifdef CONFIG_SMP
+# define _blackfin_icache_flush_range _blackfin_icache_flush_range_l1
+#endif
+ENTRY(_blackfin_icache_flush_range)
+	do_flush IFLUSH
+ENDPROC(_blackfin_icache_flush_range)
+
+#ifdef CONFIG_SMP
+.text
+# undef _blackfin_icache_flush_range
+ENTRY(_blackfin_icache_flush_range)
+	p0.L = LO(DSPID);
+	p0.H = HI(DSPID);
+	r3 = [p0];
+	r3 = r3.b (z);
+	p2 = r3;
+	p0.L = _blackfin_iflush_l1_entry;
+	p0.H = _blackfin_iflush_l1_entry;
+	p0 = p0 + (p2 << 2);
+	p1 = [p0];
+	jump (p1);
+ENDPROC(_blackfin_icache_flush_range)
+#endif
+
+#ifdef CONFIG_DCACHE_FLUSH_L1
+.section .l1.text
+#else
+.text
+#endif
+
+/* Throw away all D-cached data in specified region without any obligation to
+ * write them back.  Since the Blackfin ISA does not have an "invalidate"
+ * instruction, we use flush/invalidate.  Perhaps as a speed optimization we
+ * could bang on the DTEST MMRs ...
+ */
+ENTRY(_blackfin_dcache_invalidate_range)
+	do_flush FLUSHINV
+ENDPROC(_blackfin_dcache_invalidate_range)
+
+/* Flush all data cache lines assocoiated with this memory area */
+ENTRY(_blackfin_dcache_flush_range)
+	do_flush FLUSH, .Ldfr
+ENDPROC(_blackfin_dcache_flush_range)
+
+/* Our headers convert the page structure to an address, so just need to flush
+ * its contents like normal.  We know the start address is page aligned (which
+ * greater than our cache alignment), as is the end address.  So just jump into
+ * the middle of the dcache flush function.
+ */
+ENTRY(_blackfin_dflush_page)
+	P1 = 1 << (PAGE_SHIFT - L1_CACHE_SHIFT);
+	jump .Ldfr;
+ENDPROC(_blackfin_dflush_page)
diff --git a/arch/blackfin/mach-common/clocks-init.c b/arch/blackfin/mach-common/clocks-init.c
new file mode 100644
index 00000000..d5cfe611
--- /dev/null
+++ b/arch/blackfin/mach-common/clocks-init.c
@@ -0,0 +1,96 @@
+/*
+ * arch/blackfin/mach-common/clocks-init.c - reprogram clocks / memory
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/blackfin.h>
+
+#include <asm/dma.h>
+#include <asm/clocks.h>
+#include <asm/mem_init.h>
+#include <asm/dpmc.h>
+
+#define SDGCTL_WIDTH (1 << 31)	/* SDRAM external data path width */
+#define PLL_CTL_VAL \
+	(((CONFIG_VCO_MULT & 63) << 9) | CLKIN_HALF | \
+	 (PLL_BYPASS << 8) | (ANOMALY_05000305 ? 0 : 0x8000))
+
+__attribute__((l1_text))
+static void do_sync(void)
+{
+	__builtin_bfin_ssync();
+}
+
+__attribute__((l1_text))
+void init_clocks(void)
+{
+	/* Kill any active DMAs as they may trigger external memory accesses
+	 * in the middle of reprogramming things, and that'll screw us up.
+	 * For example, any automatic DMAs left by U-Boot for splash screens.
+	 */
+	size_t i;
+	for (i = 0; i < MAX_DMA_CHANNELS; ++i) {
+		struct dma_register *dma = dma_io_base_addr[i];
+		dma->cfg = 0;
+	}
+
+	do_sync();
+
+#ifdef SIC_IWR0
+	bfin_write_SIC_IWR0(IWR_ENABLE(0));
+# ifdef SIC_IWR1
+	/* BF52x system reset does not properly reset SIC_IWR1 which
+	 * will screw up the bootrom as it relies on MDMA0/1 waking it
+	 * up from IDLE instructions.  See this report for more info:
+	 * http://blackfin.uclinux.org/gf/tracker/4323
+	 */
+	if (ANOMALY_05000435)
+		bfin_write_SIC_IWR1(IWR_ENABLE(10) | IWR_ENABLE(11));
+	else
+		bfin_write_SIC_IWR1(IWR_DISABLE_ALL);
+# endif
+# ifdef SIC_IWR2
+	bfin_write_SIC_IWR2(IWR_DISABLE_ALL);
+# endif
+#else
+	bfin_write_SIC_IWR(IWR_ENABLE(0));
+#endif
+	do_sync();
+#ifdef EBIU_SDGCTL
+	bfin_write_EBIU_SDGCTL(bfin_read_EBIU_SDGCTL() | SRFS);
+	do_sync();
+#endif
+
+#ifdef CLKBUFOE
+	bfin_write16(VR_CTL, bfin_read_VR_CTL() | CLKBUFOE);
+	do_sync();
+	__asm__ __volatile__("IDLE;");
+#endif
+	bfin_write_PLL_LOCKCNT(0x300);
+	do_sync();
+	/* We always write PLL_CTL thus avoiding Anomaly 05000242 */
+	bfin_write16(PLL_CTL, PLL_CTL_VAL);
+	__asm__ __volatile__("IDLE;");
+	bfin_write_PLL_DIV(CONFIG_CCLK_ACT_DIV | CONFIG_SCLK_DIV);
+#ifdef EBIU_SDGCTL
+	bfin_write_EBIU_SDRRC(mem_SDRRC);
+	bfin_write_EBIU_SDGCTL((bfin_read_EBIU_SDGCTL() & SDGCTL_WIDTH) | mem_SDGCTL);
+#else
+	bfin_write_EBIU_RSTCTL(bfin_read_EBIU_RSTCTL() & ~(SRREQ));
+	do_sync();
+	bfin_write_EBIU_RSTCTL(bfin_read_EBIU_RSTCTL() | 0x1);
+	bfin_write_EBIU_DDRCTL0(mem_DDRCTL0);
+	bfin_write_EBIU_DDRCTL1(mem_DDRCTL1);
+	bfin_write_EBIU_DDRCTL2(mem_DDRCTL2);
+#ifdef CONFIG_MEM_EBIU_DDRQUE
+	bfin_write_EBIU_DDRQUE(CONFIG_MEM_EBIU_DDRQUE);
+#endif
+#endif
+	do_sync();
+	bfin_read16(0);
+}
diff --git a/arch/blackfin/mach-common/cpufreq.c b/arch/blackfin/mach-common/cpufreq.c
new file mode 100644
index 00000000..2e6eefd8
--- /dev/null
+++ b/arch/blackfin/mach-common/cpufreq.c
@@ -0,0 +1,220 @@
+/*
+ * Blackfin core clock scaling
+ *
+ * Copyright 2008-2011 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/cpufreq.h>
+#include <linux/fs.h>
+#include <linux/delay.h>
+#include <asm/blackfin.h>
+#include <asm/time.h>
+#include <asm/dpmc.h>
+
+/* this is the table of CCLK frequencies, in Hz */
+/* .index is the entry in the auxiliary dpm_state_table[] */
+static struct cpufreq_frequency_table bfin_freq_table[] = {
+	{
+		.frequency = CPUFREQ_TABLE_END,
+		.index = 0,
+	},
+	{
+		.frequency = CPUFREQ_TABLE_END,
+		.index = 1,
+	},
+	{
+		.frequency = CPUFREQ_TABLE_END,
+		.index = 2,
+	},
+	{
+		.frequency = CPUFREQ_TABLE_END,
+		.index = 0,
+	},
+};
+
+static struct bfin_dpm_state {
+	unsigned int csel; /* system clock divider */
+	unsigned int tscale; /* change the divider on the core timer interrupt */
+} dpm_state_table[3];
+
+#if defined(CONFIG_CYCLES_CLOCKSOURCE)
+/*
+ * normalized to maximum frequency offset for CYCLES,
+ * used in time-ts cycles clock source, but could be used
+ * somewhere also.
+ */
+unsigned long long __bfin_cycles_off;
+unsigned int __bfin_cycles_mod;
+#endif
+
+/**************************************************************************/
+static void __init bfin_init_tables(unsigned long cclk, unsigned long sclk)
+{
+
+	unsigned long csel, min_cclk;
+	int index;
+
+	/* Anomaly 273 seems to still exist on non-BF54x w/dcache turned on */
+#if ANOMALY_05000273 || ANOMALY_05000274 || \
+	(!defined(CONFIG_BF54x) && defined(CONFIG_BFIN_EXTMEM_DCACHEABLE))
+	min_cclk = sclk * 2;
+#else
+	min_cclk = sclk;
+#endif
+	csel = ((bfin_read_PLL_DIV() & CSEL) >> 4);
+
+	for (index = 0;  (cclk >> index) >= min_cclk && csel <= 3; index++, csel++) {
+		bfin_freq_table[index].frequency = cclk >> index;
+		dpm_state_table[index].csel = csel << 4; /* Shift now into PLL_DIV bitpos */
+		dpm_state_table[index].tscale =  (TIME_SCALE / (1 << csel)) - 1;
+
+		pr_debug("cpufreq: freq:%d csel:0x%x tscale:%d\n",
+						 bfin_freq_table[index].frequency,
+						 dpm_state_table[index].csel,
+						 dpm_state_table[index].tscale);
+	}
+	return;
+}
+
+static void bfin_adjust_core_timer(void *info)
+{
+	unsigned int tscale;
+	unsigned int index = *(unsigned int *)info;
+
+	/* we have to adjust the core timer, because it is using cclk */
+	tscale = dpm_state_table[index].tscale;
+	bfin_write_TSCALE(tscale);
+	return;
+}
+
+static unsigned int bfin_getfreq_khz(unsigned int cpu)
+{
+	/* Both CoreA/B have the same core clock */
+	return get_cclk() / 1000;
+}
+
+static int bfin_target(struct cpufreq_policy *poli,
+			unsigned int target_freq, unsigned int relation)
+{
+	unsigned int index, plldiv, cpu;
+	unsigned long flags, cclk_hz;
+	struct cpufreq_freqs freqs;
+	static unsigned long lpj_ref;
+	static unsigned int  lpj_ref_freq;
+
+#if defined(CONFIG_CYCLES_CLOCKSOURCE)
+	cycles_t cycles;
+#endif
+
+	for_each_online_cpu(cpu) {
+		struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
+
+		if (!policy)
+			continue;
+
+		if (cpufreq_frequency_table_target(policy, bfin_freq_table,
+				 target_freq, relation, &index))
+			return -EINVAL;
+
+		cclk_hz = bfin_freq_table[index].frequency;
+
+		freqs.old = bfin_getfreq_khz(0);
+		freqs.new = cclk_hz;
+		freqs.cpu = cpu;
+
+		pr_debug("cpufreq: changing cclk to %lu; target = %u, oldfreq = %u\n",
+			 cclk_hz, target_freq, freqs.old);
+
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+		if (cpu == CPUFREQ_CPU) {
+			flags = hard_local_irq_save();
+			plldiv = (bfin_read_PLL_DIV() & SSEL) |
+						dpm_state_table[index].csel;
+			bfin_write_PLL_DIV(plldiv);
+			on_each_cpu(bfin_adjust_core_timer, &index, 1);
+#if defined(CONFIG_CYCLES_CLOCKSOURCE)
+			cycles = get_cycles();
+			SSYNC();
+			cycles += 10; /* ~10 cycles we lose after get_cycles() */
+			__bfin_cycles_off +=
+			    (cycles << __bfin_cycles_mod) - (cycles << index);
+			__bfin_cycles_mod = index;
+#endif
+			if (!lpj_ref_freq) {
+				lpj_ref = loops_per_jiffy;
+				lpj_ref_freq = freqs.old;
+			}
+			if (freqs.new != freqs.old) {
+				loops_per_jiffy = cpufreq_scale(lpj_ref,
+						lpj_ref_freq, freqs.new);
+			}
+			hard_local_irq_restore(flags);
+		}
+		/* TODO: just test case for cycles clock source, remove later */
+		cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+	}
+
+	pr_debug("cpufreq: done\n");
+	return 0;
+}
+
+static int bfin_verify_speed(struct cpufreq_policy *policy)
+{
+	return cpufreq_frequency_table_verify(policy, bfin_freq_table);
+}
+
+static int __init __bfin_cpu_init(struct cpufreq_policy *policy)
+{
+
+	unsigned long cclk, sclk;
+
+	cclk = get_cclk() / 1000;
+	sclk = get_sclk() / 1000;
+
+	if (policy->cpu == CPUFREQ_CPU)
+		bfin_init_tables(cclk, sclk);
+
+	policy->cpuinfo.transition_latency = 50000; /* 50us assumed */
+
+	policy->cur = cclk;
+	cpufreq_frequency_table_get_attr(bfin_freq_table, policy->cpu);
+	return cpufreq_frequency_table_cpuinfo(policy, bfin_freq_table);
+}
+
+static struct freq_attr *bfin_freq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver bfin_driver = {
+	.verify = bfin_verify_speed,
+	.target = bfin_target,
+	.get = bfin_getfreq_khz,
+	.init = __bfin_cpu_init,
+	.name = "bfin cpufreq",
+	.owner = THIS_MODULE,
+	.attr = bfin_freq_attr,
+};
+
+static int __init bfin_cpu_init(void)
+{
+	return cpufreq_register_driver(&bfin_driver);
+}
+
+static void __exit bfin_cpu_exit(void)
+{
+	cpufreq_unregister_driver(&bfin_driver);
+}
+
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("cpufreq driver for Blackfin");
+MODULE_LICENSE("GPL");
+
+module_init(bfin_cpu_init);
+module_exit(bfin_cpu_exit);
diff --git a/arch/blackfin/mach-common/dpmc.c b/arch/blackfin/mach-common/dpmc.c
new file mode 100644
index 00000000..f5685a49
--- /dev/null
+++ b/arch/blackfin/mach-common/dpmc.c
@@ -0,0 +1,181 @@
+/*
+ * Copyright 2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/types.h>
+#include <linux/cpufreq.h>
+
+#include <asm/delay.h>
+#include <asm/dpmc.h>
+
+#define DRIVER_NAME "bfin dpmc"
+
+struct bfin_dpmc_platform_data *pdata;
+
+/**
+ *	bfin_set_vlev - Update VLEV field in VR_CTL Reg.
+ *			Avoid BYPASS sequence
+ */
+static void bfin_set_vlev(unsigned int vlev)
+{
+	unsigned pll_lcnt;
+
+	pll_lcnt = bfin_read_PLL_LOCKCNT();
+
+	bfin_write_PLL_LOCKCNT(1);
+	bfin_write_VR_CTL((bfin_read_VR_CTL() & ~VLEV) | vlev);
+	bfin_write_PLL_LOCKCNT(pll_lcnt);
+}
+
+/**
+ *	bfin_get_vlev - Get CPU specific VLEV from platform device data
+ */
+static unsigned int bfin_get_vlev(unsigned int freq)
+{
+	int i;
+
+	if (!pdata)
+		goto err_out;
+
+	freq >>= 16;
+
+	for (i = 0; i < pdata->tabsize; i++)
+		if (freq <= (pdata->tuple_tab[i] & 0xFFFF))
+			return pdata->tuple_tab[i] >> 16;
+
+err_out:
+	printk(KERN_WARNING "DPMC: No suitable CCLK VDDINT voltage pair found\n");
+	return VLEV_120;
+}
+
+#ifdef CONFIG_CPU_FREQ
+# ifdef CONFIG_SMP
+static void bfin_idle_this_cpu(void *info)
+{
+	unsigned long flags = 0;
+	unsigned long iwr0, iwr1, iwr2;
+	unsigned int cpu = smp_processor_id();
+
+	local_irq_save_hw(flags);
+	bfin_iwr_set_sup0(&iwr0, &iwr1, &iwr2);
+
+	platform_clear_ipi(cpu, IRQ_SUPPLE_0);
+	SSYNC();
+	asm("IDLE;");
+	bfin_iwr_restore(iwr0, iwr1, iwr2);
+
+	local_irq_restore_hw(flags);
+}
+
+static void bfin_idle_cpu(void)
+{
+	smp_call_function(bfin_idle_this_cpu, NULL, 0);
+}
+
+static void bfin_wakeup_cpu(void)
+{
+	unsigned int cpu;
+	unsigned int this_cpu = smp_processor_id();
+	cpumask_t mask;
+
+	cpumask_copy(&mask, cpu_online_mask);
+	cpumask_clear_cpu(this_cpu, &mask);
+	for_each_cpu(cpu, &mask)
+		platform_send_ipi_cpu(cpu, IRQ_SUPPLE_0);
+}
+
+# else
+static void bfin_idle_cpu(void) {}
+static void bfin_wakeup_cpu(void) {}
+# endif
+
+static int
+vreg_cpufreq_notifier(struct notifier_block *nb, unsigned long val, void *data)
+{
+	struct cpufreq_freqs *freq = data;
+
+	if (freq->cpu != CPUFREQ_CPU)
+		return 0;
+
+	if (val == CPUFREQ_PRECHANGE && freq->old < freq->new) {
+		bfin_idle_cpu();
+		bfin_set_vlev(bfin_get_vlev(freq->new));
+		udelay(pdata->vr_settling_time); /* Wait until Volatge settled */
+		bfin_wakeup_cpu();
+	} else if (val == CPUFREQ_POSTCHANGE && freq->old > freq->new) {
+		bfin_idle_cpu();
+		bfin_set_vlev(bfin_get_vlev(freq->new));
+		bfin_wakeup_cpu();
+	}
+
+	return 0;
+}
+
+static struct notifier_block vreg_cpufreq_notifier_block = {
+	.notifier_call	= vreg_cpufreq_notifier
+};
+#endif /* CONFIG_CPU_FREQ */
+
+/**
+ *	bfin_dpmc_probe -
+ *
+ */
+static int __devinit bfin_dpmc_probe(struct platform_device *pdev)
+{
+	if (pdev->dev.platform_data)
+		pdata = pdev->dev.platform_data;
+	else
+		return -EINVAL;
+
+	return cpufreq_register_notifier(&vreg_cpufreq_notifier_block,
+					 CPUFREQ_TRANSITION_NOTIFIER);
+}
+
+/**
+ *	bfin_dpmc_remove -
+ */
+static int __devexit bfin_dpmc_remove(struct platform_device *pdev)
+{
+	pdata = NULL;
+	return cpufreq_unregister_notifier(&vreg_cpufreq_notifier_block,
+					 CPUFREQ_TRANSITION_NOTIFIER);
+}
+
+struct platform_driver bfin_dpmc_device_driver = {
+	.probe   = bfin_dpmc_probe,
+	.remove  = __devexit_p(bfin_dpmc_remove),
+	.driver  = {
+		.name = DRIVER_NAME,
+	}
+};
+
+/**
+ *	bfin_dpmc_init - Init driver
+ */
+static int __init bfin_dpmc_init(void)
+{
+	return platform_driver_register(&bfin_dpmc_device_driver);
+}
+module_init(bfin_dpmc_init);
+
+/**
+ *	bfin_dpmc_exit - break down driver
+ */
+static void __exit bfin_dpmc_exit(void)
+{
+	platform_driver_unregister(&bfin_dpmc_device_driver);
+}
+module_exit(bfin_dpmc_exit);
+
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("cpu power management driver for Blackfin");
+MODULE_LICENSE("GPL");
diff --git a/arch/blackfin/mach-common/dpmc_modes.S b/arch/blackfin/mach-common/dpmc_modes.S
new file mode 100644
index 00000000..1c534d29
--- /dev/null
+++ b/arch/blackfin/mach-common/dpmc_modes.S
@@ -0,0 +1,894 @@
+/*
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/linkage.h>
+#include <asm/blackfin.h>
+#include <mach/irq.h>
+#include <asm/dpmc.h>
+
+.section .l1.text
+
+ENTRY(_sleep_mode)
+	[--SP] = (R7:4, P5:3);
+	[--SP] = RETS;
+
+	call _set_sic_iwr;
+
+	P0.H = hi(PLL_CTL);
+	P0.L = lo(PLL_CTL);
+	R1 = W[P0](z);
+	BITSET (R1, 3);
+	W[P0] = R1.L;
+
+	CLI R2;
+	SSYNC;
+	IDLE;
+	STI R2;
+
+	call _test_pll_locked;
+
+	R0 = IWR_ENABLE(0);
+	R1 = IWR_DISABLE_ALL;
+	R2 = IWR_DISABLE_ALL;
+
+	call _set_sic_iwr;
+
+	P0.H = hi(PLL_CTL);
+	P0.L = lo(PLL_CTL);
+	R7 = w[p0](z);
+	BITCLR (R7, 3);
+	BITCLR (R7, 5);
+	w[p0] = R7.L;
+	IDLE;
+	call _test_pll_locked;
+
+	RETS = [SP++];
+	(R7:4, P5:3) = [SP++];
+	RTS;
+ENDPROC(_sleep_mode)
+
+/*
+ * This func never returns as it puts the part into hibernate, and
+ * is only called from do_hibernate, so we don't bother saving or
+ * restoring any of the normal C runtime state.  When we wake up,
+ * the entry point will be in do_hibernate and not here.
+ *
+ * We accept just one argument -- the value to write to VR_CTL.
+ */
+ENTRY(_hibernate_mode)
+	/* Save/setup the regs we need early for minor pipeline optimization */
+	R4 = R0;
+	P3.H = hi(VR_CTL);
+	P3.L = lo(VR_CTL);
+
+	/* Disable all wakeup sources */
+	R0 = IWR_DISABLE_ALL;
+	R1 = IWR_DISABLE_ALL;
+	R2 = IWR_DISABLE_ALL;
+	call _set_sic_iwr;
+	call _set_dram_srfs;
+	SSYNC;
+
+	/* Finally, we climb into our cave to hibernate */
+	W[P3] = R4.L;
+	CLI R2;
+	IDLE;
+.Lforever:
+	jump .Lforever;
+ENDPROC(_hibernate_mode)
+
+ENTRY(_sleep_deeper)
+	[--SP] = (R7:4, P5:3);
+	[--SP] = RETS;
+
+	CLI R4;
+
+	P3 = R0;
+	P4 = R1;
+	P5 = R2;
+
+	R0 = IWR_ENABLE(0);
+	R1 = IWR_DISABLE_ALL;
+	R2 = IWR_DISABLE_ALL;
+
+	call _set_sic_iwr;
+	call _set_dram_srfs;	/* Set SDRAM Self Refresh */
+
+	P0.H = hi(PLL_DIV);
+	P0.L = lo(PLL_DIV);
+	R6 = W[P0](z);
+	R0.L = 0xF;
+	W[P0] = R0.l;		/* Set Max VCO to SCLK divider */
+
+	P0.H = hi(PLL_CTL);
+	P0.L = lo(PLL_CTL);
+	R5 = W[P0](z);
+	R0.L = (CONFIG_MIN_VCO_HZ/CONFIG_CLKIN_HZ) << 9;
+	W[P0] = R0.l;		/* Set Min CLKIN to VCO multiplier */
+
+	SSYNC;
+	IDLE;
+
+	call _test_pll_locked;
+
+	P0.H = hi(VR_CTL);
+	P0.L = lo(VR_CTL);
+	R7 = W[P0](z);
+	R1 = 0x6;
+	R1 <<= 16;
+	R2 = 0x0404(Z);
+	R1 = R1|R2;
+
+	R2 = DEPOSIT(R7, R1);
+	W[P0] = R2;		/* Set Min Core Voltage */
+
+	SSYNC;
+	IDLE;
+
+	call _test_pll_locked;
+
+	R0 = P3;
+	R1 = P4;
+	R3 = P5;
+	call _set_sic_iwr;	/* Set Awake from IDLE */
+
+	P0.H = hi(PLL_CTL);
+	P0.L = lo(PLL_CTL);
+	R0 = W[P0](z);
+	BITSET (R0, 3);
+	W[P0] = R0.L;		/* Turn CCLK OFF */
+	SSYNC;
+	IDLE;
+
+	call _test_pll_locked;
+
+	R0 = IWR_ENABLE(0);
+	R1 = IWR_DISABLE_ALL;
+	R2 = IWR_DISABLE_ALL;
+
+	call _set_sic_iwr;	/* Set Awake from IDLE PLL */
+
+	P0.H = hi(VR_CTL);
+	P0.L = lo(VR_CTL);
+	W[P0]= R7;
+
+	SSYNC;
+	IDLE;
+
+	call _test_pll_locked;
+
+	P0.H = hi(PLL_DIV);
+	P0.L = lo(PLL_DIV);
+	W[P0]= R6;		/* Restore CCLK and SCLK divider */
+
+	P0.H = hi(PLL_CTL);
+	P0.L = lo(PLL_CTL);
+	w[p0] = R5;		/* Restore VCO multiplier */
+	IDLE;
+	call _test_pll_locked;
+
+	call _unset_dram_srfs;	/* SDRAM Self Refresh Off */
+
+	STI R4;
+
+	RETS = [SP++];
+	(R7:4, P5:3) = [SP++];
+	RTS;
+ENDPROC(_sleep_deeper)
+
+ENTRY(_set_dram_srfs)
+	/*  set the dram to self refresh mode */
+	SSYNC;
+#if defined(EBIU_RSTCTL)	/* DDR */
+	P0.H = hi(EBIU_RSTCTL);
+	P0.L = lo(EBIU_RSTCTL);
+	R2 = [P0];
+	BITSET(R2, 3); /* SRREQ enter self-refresh mode */
+	[P0] = R2;
+	SSYNC;
+1:
+	R2 = [P0];
+	CC = BITTST(R2, 4);
+	if !CC JUMP 1b;
+#else 				/* SDRAM */
+	P0.L = lo(EBIU_SDGCTL);
+	P0.H = hi(EBIU_SDGCTL);
+	P1.L = lo(EBIU_SDSTAT);
+	P1.H = hi(EBIU_SDSTAT);
+
+	R2 = [P0];
+	BITSET(R2, 24); /* SRFS enter self-refresh mode */
+	[P0] = R2;
+	SSYNC;
+
+1:
+	R2 = w[P1];
+	SSYNC;
+	cc = BITTST(R2, 1); /* SDSRA poll self-refresh status */
+	if !cc jump 1b;
+
+	R2 = [P0];
+	BITCLR(R2, 0); /* SCTLE disable CLKOUT */
+	[P0] = R2;
+#endif
+	RTS;
+ENDPROC(_set_dram_srfs)
+
+ENTRY(_unset_dram_srfs)
+	/*  set the dram out of self refresh mode */
+
+#if defined(EBIU_RSTCTL)	/* DDR */
+	P0.H = hi(EBIU_RSTCTL);
+	P0.L = lo(EBIU_RSTCTL);
+	R2 = [P0];
+	BITCLR(R2, 3); /* clear SRREQ bit */
+	[P0] = R2;
+#elif defined(EBIU_SDGCTL)	/* SDRAM */
+	/* release CLKOUT from self-refresh */
+	P0.L = lo(EBIU_SDGCTL);
+	P0.H = hi(EBIU_SDGCTL);
+
+	R2 = [P0];
+	BITSET(R2, 0); /* SCTLE enable CLKOUT */
+	[P0] = R2
+	SSYNC;
+
+	/* release SDRAM from self-refresh */
+	R2 = [P0];
+	BITCLR(R2, 24); /* clear SRFS bit */
+	[P0] = R2
+#endif
+
+	SSYNC;
+	RTS;
+ENDPROC(_unset_dram_srfs)
+
+ENTRY(_set_sic_iwr)
+#ifdef SIC_IWR0
+	P0.H = hi(SYSMMR_BASE);
+	P0.L = lo(SYSMMR_BASE);
+	[P0 + (SIC_IWR0 - SYSMMR_BASE)] = R0;
+	[P0 + (SIC_IWR1 - SYSMMR_BASE)] = R1;
+# ifdef SIC_IWR2
+	[P0 + (SIC_IWR2 - SYSMMR_BASE)] = R2;
+# endif
+#else
+	P0.H = hi(SIC_IWR);
+	P0.L = lo(SIC_IWR);
+	[P0] = R0;
+#endif
+
+	SSYNC;
+	RTS;
+ENDPROC(_set_sic_iwr)
+
+ENTRY(_test_pll_locked)
+	P0.H = hi(PLL_STAT);
+	P0.L = lo(PLL_STAT);
+1:
+	R0 = W[P0] (Z);
+	CC = BITTST(R0,5);
+	IF !CC JUMP 1b;
+	RTS;
+ENDPROC(_test_pll_locked)
+
+.section .text
+
+#define PM_REG0  R7
+#define PM_REG1  R6
+#define PM_REG2  R5
+#define PM_REG3  R4
+#define PM_REG4  R3
+#define PM_REG5  R2
+#define PM_REG6  R1
+#define PM_REG7  R0
+#define PM_REG8  P5
+#define PM_REG9  P4
+#define PM_REG10 P3
+#define PM_REG11 P2
+#define PM_REG12 P1
+#define PM_REG13 P0
+
+#define PM_REGSET0  R7:7
+#define PM_REGSET1  R7:6
+#define PM_REGSET2  R7:5
+#define PM_REGSET3  R7:4
+#define PM_REGSET4  R7:3
+#define PM_REGSET5  R7:2
+#define PM_REGSET6  R7:1
+#define PM_REGSET7  R7:0
+#define PM_REGSET8  R7:0, P5:5
+#define PM_REGSET9  R7:0, P5:4
+#define PM_REGSET10 R7:0, P5:3
+#define PM_REGSET11 R7:0, P5:2
+#define PM_REGSET12 R7:0, P5:1
+#define PM_REGSET13 R7:0, P5:0
+
+#define _PM_PUSH(n, x, w, base) PM_REG##n = w[FP + ((x) - (base))];
+#define _PM_POP(n, x, w, base)  w[FP + ((x) - (base))] = PM_REG##n;
+#define PM_PUSH_SYNC(n)         [--sp] = (PM_REGSET##n);
+#define PM_POP_SYNC(n)          (PM_REGSET##n) = [sp++];
+#define PM_PUSH(n, x)           PM_REG##n = [FP++];
+#define PM_POP(n, x)            [FP--] = PM_REG##n;
+#define PM_CORE_PUSH(n, x)      _PM_PUSH(n, x, , COREMMR_BASE)
+#define PM_CORE_POP(n, x)       _PM_POP(n, x, , COREMMR_BASE)
+#define PM_SYS_PUSH(n, x)       _PM_PUSH(n, x, , SYSMMR_BASE)
+#define PM_SYS_POP(n, x)        _PM_POP(n, x, , SYSMMR_BASE)
+#define PM_SYS_PUSH16(n, x)     _PM_PUSH(n, x, w, SYSMMR_BASE)
+#define PM_SYS_POP16(n, x)      _PM_POP(n, x, w, SYSMMR_BASE)
+
+ENTRY(_do_hibernate)
+	/*
+	 * Save the core regs early so we can blow them away when
+	 * saving/restoring MMR states
+	 */
+	[--sp] = (R7:0, P5:0);
+	[--sp] = fp;
+	[--sp] = usp;
+
+	[--sp] = i0;
+	[--sp] = i1;
+	[--sp] = i2;
+	[--sp] = i3;
+
+	[--sp] = m0;
+	[--sp] = m1;
+	[--sp] = m2;
+	[--sp] = m3;
+
+	[--sp] = l0;
+	[--sp] = l1;
+	[--sp] = l2;
+	[--sp] = l3;
+
+	[--sp] = b0;
+	[--sp] = b1;
+	[--sp] = b2;
+	[--sp] = b3;
+	[--sp] = a0.x;
+	[--sp] = a0.w;
+	[--sp] = a1.x;
+	[--sp] = a1.w;
+
+	[--sp] = LC0;
+	[--sp] = LC1;
+	[--sp] = LT0;
+	[--sp] = LT1;
+	[--sp] = LB0;
+	[--sp] = LB1;
+
+	/* We can't push RETI directly as that'll change IPEND[4] */
+	r7 = RETI;
+	[--sp] = RETS;
+	[--sp] = ASTAT;
+	[--sp] = CYCLES;
+	[--sp] = CYCLES2;
+	[--sp] = SYSCFG;
+	[--sp] = RETX;
+	[--sp] = SEQSTAT;
+	[--sp] = r7;
+
+	/* Save first func arg in M3 */
+	M3 = R0;
+
+	/* Save system MMRs */
+	FP.H = hi(SYSMMR_BASE);
+	FP.L = lo(SYSMMR_BASE);
+
+#ifdef SIC_IMASK0
+	PM_SYS_PUSH(0, SIC_IMASK0)
+	PM_SYS_PUSH(1, SIC_IMASK1)
+# ifdef SIC_IMASK2
+	PM_SYS_PUSH(2, SIC_IMASK2)
+# endif
+#else
+	PM_SYS_PUSH(0, SIC_IMASK)
+#endif
+#ifdef SIC_IAR0
+	PM_SYS_PUSH(3, SIC_IAR0)
+	PM_SYS_PUSH(4, SIC_IAR1)
+	PM_SYS_PUSH(5, SIC_IAR2)
+#endif
+#ifdef SIC_IAR3
+	PM_SYS_PUSH(6, SIC_IAR3)
+#endif
+#ifdef SIC_IAR4
+	PM_SYS_PUSH(7, SIC_IAR4)
+	PM_SYS_PUSH(8, SIC_IAR5)
+	PM_SYS_PUSH(9, SIC_IAR6)
+#endif
+#ifdef SIC_IAR7
+	PM_SYS_PUSH(10, SIC_IAR7)
+#endif
+#ifdef SIC_IAR8
+	PM_SYS_PUSH(11, SIC_IAR8)
+	PM_SYS_PUSH(12, SIC_IAR9)
+	PM_SYS_PUSH(13, SIC_IAR10)
+#endif
+	PM_PUSH_SYNC(13)
+#ifdef SIC_IAR11
+	PM_SYS_PUSH(0, SIC_IAR11)
+#endif
+
+#ifdef SIC_IWR
+	PM_SYS_PUSH(1, SIC_IWR)
+#endif
+#ifdef SIC_IWR0
+	PM_SYS_PUSH(1, SIC_IWR0)
+#endif
+#ifdef SIC_IWR1
+	PM_SYS_PUSH(2, SIC_IWR1)
+#endif
+#ifdef SIC_IWR2
+	PM_SYS_PUSH(3, SIC_IWR2)
+#endif
+
+#ifdef PINT0_ASSIGN
+	PM_SYS_PUSH(4, PINT0_MASK_SET)
+	PM_SYS_PUSH(5, PINT1_MASK_SET)
+	PM_SYS_PUSH(6, PINT2_MASK_SET)
+	PM_SYS_PUSH(7, PINT3_MASK_SET)
+	PM_SYS_PUSH(8, PINT0_ASSIGN)
+	PM_SYS_PUSH(9, PINT1_ASSIGN)
+	PM_SYS_PUSH(10, PINT2_ASSIGN)
+	PM_SYS_PUSH(11, PINT3_ASSIGN)
+	PM_SYS_PUSH(12, PINT0_INVERT_SET)
+	PM_SYS_PUSH(13, PINT1_INVERT_SET)
+	PM_PUSH_SYNC(13)
+	PM_SYS_PUSH(0, PINT2_INVERT_SET)
+	PM_SYS_PUSH(1, PINT3_INVERT_SET)
+	PM_SYS_PUSH(2, PINT0_EDGE_SET)
+	PM_SYS_PUSH(3, PINT1_EDGE_SET)
+	PM_SYS_PUSH(4, PINT2_EDGE_SET)
+	PM_SYS_PUSH(5, PINT3_EDGE_SET)
+#endif
+
+	PM_SYS_PUSH16(6, SYSCR)
+
+	PM_SYS_PUSH16(7, EBIU_AMGCTL)
+	PM_SYS_PUSH(8, EBIU_AMBCTL0)
+	PM_SYS_PUSH(9, EBIU_AMBCTL1)
+#ifdef EBIU_FCTL
+	PM_SYS_PUSH(10, EBIU_MBSCTL)
+	PM_SYS_PUSH(11, EBIU_MODE)
+	PM_SYS_PUSH(12, EBIU_FCTL)
+	PM_PUSH_SYNC(12)
+#else
+	PM_PUSH_SYNC(9)
+#endif
+
+	/* Save Core MMRs */
+	I0.H = hi(COREMMR_BASE);
+	I0.L = lo(COREMMR_BASE);
+	I1 = I0;
+	I2 = I0;
+	I3 = I0;
+	B0 = I0;
+	B1 = I0;
+	B2 = I0;
+	B3 = I0;
+	I1.L = lo(DCPLB_ADDR0);
+	I2.L = lo(DCPLB_DATA0);
+	I3.L = lo(ICPLB_ADDR0);
+	B0.L = lo(ICPLB_DATA0);
+	B1.L = lo(EVT2);
+	B2.L = lo(IMASK);
+	B3.L = lo(TCNTL);
+
+	/* DCPLB Addr */
+	FP = I1;
+	PM_PUSH(0, DCPLB_ADDR0)
+	PM_PUSH(1, DCPLB_ADDR1)
+	PM_PUSH(2, DCPLB_ADDR2)
+	PM_PUSH(3, DCPLB_ADDR3)
+	PM_PUSH(4, DCPLB_ADDR4)
+	PM_PUSH(5, DCPLB_ADDR5)
+	PM_PUSH(6, DCPLB_ADDR6)
+	PM_PUSH(7, DCPLB_ADDR7)
+	PM_PUSH(8, DCPLB_ADDR8)
+	PM_PUSH(9, DCPLB_ADDR9)
+	PM_PUSH(10, DCPLB_ADDR10)
+	PM_PUSH(11, DCPLB_ADDR11)
+	PM_PUSH(12, DCPLB_ADDR12)
+	PM_PUSH(13, DCPLB_ADDR13)
+	PM_PUSH_SYNC(13)
+	PM_PUSH(0, DCPLB_ADDR14)
+	PM_PUSH(1, DCPLB_ADDR15)
+
+	/* DCPLB Data */
+	FP = I2;
+	PM_PUSH(2, DCPLB_DATA0)
+	PM_PUSH(3, DCPLB_DATA1)
+	PM_PUSH(4, DCPLB_DATA2)
+	PM_PUSH(5, DCPLB_DATA3)
+	PM_PUSH(6, DCPLB_DATA4)
+	PM_PUSH(7, DCPLB_DATA5)
+	PM_PUSH(8, DCPLB_DATA6)
+	PM_PUSH(9, DCPLB_DATA7)
+	PM_PUSH(10, DCPLB_DATA8)
+	PM_PUSH(11, DCPLB_DATA9)
+	PM_PUSH(12, DCPLB_DATA10)
+	PM_PUSH(13, DCPLB_DATA11)
+	PM_PUSH_SYNC(13)
+	PM_PUSH(0, DCPLB_DATA12)
+	PM_PUSH(1, DCPLB_DATA13)
+	PM_PUSH(2, DCPLB_DATA14)
+	PM_PUSH(3, DCPLB_DATA15)
+
+	/* ICPLB Addr */
+	FP = I3;
+	PM_PUSH(4, ICPLB_ADDR0)
+	PM_PUSH(5, ICPLB_ADDR1)
+	PM_PUSH(6, ICPLB_ADDR2)
+	PM_PUSH(7, ICPLB_ADDR3)
+	PM_PUSH(8, ICPLB_ADDR4)
+	PM_PUSH(9, ICPLB_ADDR5)
+	PM_PUSH(10, ICPLB_ADDR6)
+	PM_PUSH(11, ICPLB_ADDR7)
+	PM_PUSH(12, ICPLB_ADDR8)
+	PM_PUSH(13, ICPLB_ADDR9)
+	PM_PUSH_SYNC(13)
+	PM_PUSH(0, ICPLB_ADDR10)
+	PM_PUSH(1, ICPLB_ADDR11)
+	PM_PUSH(2, ICPLB_ADDR12)
+	PM_PUSH(3, ICPLB_ADDR13)
+	PM_PUSH(4, ICPLB_ADDR14)
+	PM_PUSH(5, ICPLB_ADDR15)
+
+	/* ICPLB Data */
+	FP = B0;
+	PM_PUSH(6, ICPLB_DATA0)
+	PM_PUSH(7, ICPLB_DATA1)
+	PM_PUSH(8, ICPLB_DATA2)
+	PM_PUSH(9, ICPLB_DATA3)
+	PM_PUSH(10, ICPLB_DATA4)
+	PM_PUSH(11, ICPLB_DATA5)
+	PM_PUSH(12, ICPLB_DATA6)
+	PM_PUSH(13, ICPLB_DATA7)
+	PM_PUSH_SYNC(13)
+	PM_PUSH(0, ICPLB_DATA8)
+	PM_PUSH(1, ICPLB_DATA9)
+	PM_PUSH(2, ICPLB_DATA10)
+	PM_PUSH(3, ICPLB_DATA11)
+	PM_PUSH(4, ICPLB_DATA12)
+	PM_PUSH(5, ICPLB_DATA13)
+	PM_PUSH(6, ICPLB_DATA14)
+	PM_PUSH(7, ICPLB_DATA15)
+
+	/* Event Vectors */
+	FP = B1;
+	PM_PUSH(8, EVT2)
+	PM_PUSH(9, EVT3)
+	FP += 4;	/* EVT4 */
+	PM_PUSH(10, EVT5)
+	PM_PUSH(11, EVT6)
+	PM_PUSH(12, EVT7)
+	PM_PUSH(13, EVT8)
+	PM_PUSH_SYNC(13)
+	PM_PUSH(0, EVT9)
+	PM_PUSH(1, EVT10)
+	PM_PUSH(2, EVT11)
+	PM_PUSH(3, EVT12)
+	PM_PUSH(4, EVT13)
+	PM_PUSH(5, EVT14)
+	PM_PUSH(6, EVT15)
+
+	/* CEC */
+	FP = B2;
+	PM_PUSH(7, IMASK)
+	FP += 4;	/* IPEND */
+	PM_PUSH(8, ILAT)
+	PM_PUSH(9, IPRIO)
+
+	/* Core Timer */
+	FP = B3;
+	PM_PUSH(10, TCNTL)
+	PM_PUSH(11, TPERIOD)
+	PM_PUSH(12, TSCALE)
+	PM_PUSH(13, TCOUNT)
+	PM_PUSH_SYNC(13)
+
+	/* Misc non-contiguous registers */
+	FP = I0;
+	PM_CORE_PUSH(0, DMEM_CONTROL);
+	PM_CORE_PUSH(1, IMEM_CONTROL);
+	PM_CORE_PUSH(2, TBUFCTL);
+	PM_PUSH_SYNC(2)
+
+	/* Setup args to hibernate mode early for pipeline optimization */
+	R0 = M3;
+	P1.H = _hibernate_mode;
+	P1.L = _hibernate_mode;
+
+	/* Save Magic, return address and Stack Pointer */
+	P0 = 0;
+	R1.H = 0xDEAD;	/* Hibernate Magic */
+	R1.L = 0xBEEF;
+	R2.H = .Lpm_resume_here;
+	R2.L = .Lpm_resume_here;
+	[P0++] = R1;	/* Store Hibernate Magic */
+	[P0++] = R2;	/* Save Return Address */
+	[P0++] = SP;	/* Save Stack Pointer */
+
+	/* Must use an indirect call as we need to jump to L1 */
+	call (P1); /* Goodbye */
+
+.Lpm_resume_here:
+
+	/* Restore Core MMRs */
+	I0.H = hi(COREMMR_BASE);
+	I0.L = lo(COREMMR_BASE);
+	I1 = I0;
+	I2 = I0;
+	I3 = I0;
+	B0 = I0;
+	B1 = I0;
+	B2 = I0;
+	B3 = I0;
+	I1.L = lo(DCPLB_ADDR15);
+	I2.L = lo(DCPLB_DATA15);
+	I3.L = lo(ICPLB_ADDR15);
+	B0.L = lo(ICPLB_DATA15);
+	B1.L = lo(EVT15);
+	B2.L = lo(IPRIO);
+	B3.L = lo(TCOUNT);
+
+	/* Misc non-contiguous registers */
+	FP = I0;
+	PM_POP_SYNC(2)
+	PM_CORE_POP(2, TBUFCTL)
+	PM_CORE_POP(1, IMEM_CONTROL)
+	PM_CORE_POP(0, DMEM_CONTROL)
+
+	/* Core Timer */
+	PM_POP_SYNC(13)
+	FP = B3;
+	PM_POP(13, TCOUNT)
+	PM_POP(12, TSCALE)
+	PM_POP(11, TPERIOD)
+	PM_POP(10, TCNTL)
+
+	/* CEC */
+	FP = B2;
+	PM_POP(9, IPRIO)
+	PM_POP(8, ILAT)
+	FP += -4;	/* IPEND */
+	PM_POP(7, IMASK)
+
+	/* Event Vectors */
+	FP = B1;
+	PM_POP(6, EVT15)
+	PM_POP(5, EVT14)
+	PM_POP(4, EVT13)
+	PM_POP(3, EVT12)
+	PM_POP(2, EVT11)
+	PM_POP(1, EVT10)
+	PM_POP(0, EVT9)
+	PM_POP_SYNC(13)
+	PM_POP(13, EVT8)
+	PM_POP(12, EVT7)
+	PM_POP(11, EVT6)
+	PM_POP(10, EVT5)
+	FP += -4;	/* EVT4 */
+	PM_POP(9, EVT3)
+	PM_POP(8, EVT2)
+
+	/* ICPLB Data */
+	FP = B0;
+	PM_POP(7, ICPLB_DATA15)
+	PM_POP(6, ICPLB_DATA14)
+	PM_POP(5, ICPLB_DATA13)
+	PM_POP(4, ICPLB_DATA12)
+	PM_POP(3, ICPLB_DATA11)
+	PM_POP(2, ICPLB_DATA10)
+	PM_POP(1, ICPLB_DATA9)
+	PM_POP(0, ICPLB_DATA8)
+	PM_POP_SYNC(13)
+	PM_POP(13, ICPLB_DATA7)
+	PM_POP(12, ICPLB_DATA6)
+	PM_POP(11, ICPLB_DATA5)
+	PM_POP(10, ICPLB_DATA4)
+	PM_POP(9, ICPLB_DATA3)
+	PM_POP(8, ICPLB_DATA2)
+	PM_POP(7, ICPLB_DATA1)
+	PM_POP(6, ICPLB_DATA0)
+
+	/* ICPLB Addr */
+	FP = I3;
+	PM_POP(5, ICPLB_ADDR15)
+	PM_POP(4, ICPLB_ADDR14)
+	PM_POP(3, ICPLB_ADDR13)
+	PM_POP(2, ICPLB_ADDR12)
+	PM_POP(1, ICPLB_ADDR11)
+	PM_POP(0, ICPLB_ADDR10)
+	PM_POP_SYNC(13)
+	PM_POP(13, ICPLB_ADDR9)
+	PM_POP(12, ICPLB_ADDR8)
+	PM_POP(11, ICPLB_ADDR7)
+	PM_POP(10, ICPLB_ADDR6)
+	PM_POP(9, ICPLB_ADDR5)
+	PM_POP(8, ICPLB_ADDR4)
+	PM_POP(7, ICPLB_ADDR3)
+	PM_POP(6, ICPLB_ADDR2)
+	PM_POP(5, ICPLB_ADDR1)
+	PM_POP(4, ICPLB_ADDR0)
+
+	/* DCPLB Data */
+	FP = I2;
+	PM_POP(3, DCPLB_DATA15)
+	PM_POP(2, DCPLB_DATA14)
+	PM_POP(1, DCPLB_DATA13)
+	PM_POP(0, DCPLB_DATA12)
+	PM_POP_SYNC(13)
+	PM_POP(13, DCPLB_DATA11)
+	PM_POP(12, DCPLB_DATA10)
+	PM_POP(11, DCPLB_DATA9)
+	PM_POP(10, DCPLB_DATA8)
+	PM_POP(9, DCPLB_DATA7)
+	PM_POP(8, DCPLB_DATA6)
+	PM_POP(7, DCPLB_DATA5)
+	PM_POP(6, DCPLB_DATA4)
+	PM_POP(5, DCPLB_DATA3)
+	PM_POP(4, DCPLB_DATA2)
+	PM_POP(3, DCPLB_DATA1)
+	PM_POP(2, DCPLB_DATA0)
+
+	/* DCPLB Addr */
+	FP = I1;
+	PM_POP(1, DCPLB_ADDR15)
+	PM_POP(0, DCPLB_ADDR14)
+	PM_POP_SYNC(13)
+	PM_POP(13, DCPLB_ADDR13)
+	PM_POP(12, DCPLB_ADDR12)
+	PM_POP(11, DCPLB_ADDR11)
+	PM_POP(10, DCPLB_ADDR10)
+	PM_POP(9, DCPLB_ADDR9)
+	PM_POP(8, DCPLB_ADDR8)
+	PM_POP(7, DCPLB_ADDR7)
+	PM_POP(6, DCPLB_ADDR6)
+	PM_POP(5, DCPLB_ADDR5)
+	PM_POP(4, DCPLB_ADDR4)
+	PM_POP(3, DCPLB_ADDR3)
+	PM_POP(2, DCPLB_ADDR2)
+	PM_POP(1, DCPLB_ADDR1)
+	PM_POP(0, DCPLB_ADDR0)
+
+	/* Restore System MMRs */
+	FP.H = hi(SYSMMR_BASE);
+	FP.L = lo(SYSMMR_BASE);
+
+#ifdef EBIU_FCTL
+	PM_POP_SYNC(12)
+	PM_SYS_POP(12, EBIU_FCTL)
+	PM_SYS_POP(11, EBIU_MODE)
+	PM_SYS_POP(10, EBIU_MBSCTL)
+#else
+	PM_POP_SYNC(9)
+#endif
+	PM_SYS_POP(9, EBIU_AMBCTL1)
+	PM_SYS_POP(8, EBIU_AMBCTL0)
+	PM_SYS_POP16(7, EBIU_AMGCTL)
+
+	PM_SYS_POP16(6, SYSCR)
+
+#ifdef PINT0_ASSIGN
+	PM_SYS_POP(5, PINT3_EDGE_SET)
+	PM_SYS_POP(4, PINT2_EDGE_SET)
+	PM_SYS_POP(3, PINT1_EDGE_SET)
+	PM_SYS_POP(2, PINT0_EDGE_SET)
+	PM_SYS_POP(1, PINT3_INVERT_SET)
+	PM_SYS_POP(0, PINT2_INVERT_SET)
+	PM_POP_SYNC(13)
+	PM_SYS_POP(13, PINT1_INVERT_SET)
+	PM_SYS_POP(12, PINT0_INVERT_SET)
+	PM_SYS_POP(11, PINT3_ASSIGN)
+	PM_SYS_POP(10, PINT2_ASSIGN)
+	PM_SYS_POP(9, PINT1_ASSIGN)
+	PM_SYS_POP(8, PINT0_ASSIGN)
+	PM_SYS_POP(7, PINT3_MASK_SET)
+	PM_SYS_POP(6, PINT2_MASK_SET)
+	PM_SYS_POP(5, PINT1_MASK_SET)
+	PM_SYS_POP(4, PINT0_MASK_SET)
+#endif
+
+#ifdef SIC_IWR2
+	PM_SYS_POP(3, SIC_IWR2)
+#endif
+#ifdef SIC_IWR1
+	PM_SYS_POP(2, SIC_IWR1)
+#endif
+#ifdef SIC_IWR0
+	PM_SYS_POP(1, SIC_IWR0)
+#endif
+#ifdef SIC_IWR
+	PM_SYS_POP(1, SIC_IWR)
+#endif
+
+#ifdef SIC_IAR11
+	PM_SYS_POP(0, SIC_IAR11)
+#endif
+	PM_POP_SYNC(13)
+#ifdef SIC_IAR8
+	PM_SYS_POP(13, SIC_IAR10)
+	PM_SYS_POP(12, SIC_IAR9)
+	PM_SYS_POP(11, SIC_IAR8)
+#endif
+#ifdef SIC_IAR7
+	PM_SYS_POP(10, SIC_IAR7)
+#endif
+#ifdef SIC_IAR6
+	PM_SYS_POP(9, SIC_IAR6)
+	PM_SYS_POP(8, SIC_IAR5)
+	PM_SYS_POP(7, SIC_IAR4)
+#endif
+#ifdef SIC_IAR3
+	PM_SYS_POP(6, SIC_IAR3)
+#endif
+#ifdef SIC_IAR0
+	PM_SYS_POP(5, SIC_IAR2)
+	PM_SYS_POP(4, SIC_IAR1)
+	PM_SYS_POP(3, SIC_IAR0)
+#endif
+#ifdef SIC_IMASK0
+# ifdef SIC_IMASK2
+	PM_SYS_POP(2, SIC_IMASK2)
+# endif
+	PM_SYS_POP(1, SIC_IMASK1)
+	PM_SYS_POP(0, SIC_IMASK0)
+#else
+	PM_SYS_POP(0, SIC_IMASK)
+#endif
+
+	/* Restore Core Registers */
+	RETI = [sp++];
+	SEQSTAT = [sp++];
+	RETX = [sp++];
+	SYSCFG = [sp++];
+	CYCLES2 = [sp++];
+	CYCLES = [sp++];
+	ASTAT = [sp++];
+	RETS = [sp++];
+
+	LB1 = [sp++];
+	LB0 = [sp++];
+	LT1 = [sp++];
+	LT0 = [sp++];
+	LC1 = [sp++];
+	LC0 = [sp++];
+
+	a1.w = [sp++];
+	a1.x = [sp++];
+	a0.w = [sp++];
+	a0.x = [sp++];
+	b3 = [sp++];
+	b2 = [sp++];
+	b1 = [sp++];
+	b0 = [sp++];
+
+	l3 = [sp++];
+	l2 = [sp++];
+	l1 = [sp++];
+	l0 = [sp++];
+
+	m3 = [sp++];
+	m2 = [sp++];
+	m1 = [sp++];
+	m0 = [sp++];
+
+	i3 = [sp++];
+	i2 = [sp++];
+	i1 = [sp++];
+	i0 = [sp++];
+
+	usp = [sp++];
+	fp = [sp++];
+	(R7:0, P5:0) = [sp++];
+
+	[--sp] = RETI;	/* Clear Global Interrupt Disable */
+	SP += 4;
+
+	RTS;
+ENDPROC(_do_hibernate)
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
new file mode 100644
index 00000000..4698a980
--- /dev/null
+++ b/arch/blackfin/mach-common/entry.S
@@ -0,0 +1,1764 @@
+/*
+ * Contains the system-call and fault low-level handling routines.
+ * This also contains the timer-interrupt handler, as well as all
+ * interrupts and faults that can result in a task-switch.
+ *
+ * Copyright 2005-2009 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+/* NOTE: This code handles signal-recognition, which happens every time
+ * after a timer-interrupt and after each system call.
+ */
+
+#include <linux/init.h>
+#include <linux/linkage.h>
+#include <linux/unistd.h>
+#include <asm/blackfin.h>
+#include <asm/errno.h>
+#include <asm/fixed_code.h>
+#include <asm/thread_info.h>  /* TIF_NEED_RESCHED */
+#include <asm/asm-offsets.h>
+#include <asm/trace.h>
+#include <asm/traps.h>
+
+#include <asm/context.S>
+
+#if defined(CONFIG_BFIN_SCRATCH_REG_RETN)
+# define EX_SCRATCH_REG RETN
+#elif defined(CONFIG_BFIN_SCRATCH_REG_RETE)
+# define EX_SCRATCH_REG RETE
+#else
+# define EX_SCRATCH_REG CYCLES
+#endif
+
+#ifdef CONFIG_EXCPT_IRQ_SYSC_L1
+.section .l1.text
+#else
+.text
+#endif
+
+/* Slightly simplified and streamlined entry point for CPLB misses.
+ * This one does not lower the level to IRQ5, and thus can be used to
+ * patch up CPLB misses on the kernel stack.
+ */
+#if ANOMALY_05000261
+#define _ex_dviol _ex_workaround_261
+#define _ex_dmiss _ex_workaround_261
+#define _ex_dmult _ex_workaround_261
+
+ENTRY(_ex_workaround_261)
+	/*
+	 * Work around an anomaly: if we see a new DCPLB fault, return
+	 * without doing anything.  Then, if we get the same fault again,
+	 * handle it.
+	 */
+	P4 = R7;	/* Store EXCAUSE */
+
+	GET_PDA(p5, r7);
+	r7 = [p5 + PDA_LFRETX];
+	r6 = retx;
+	[p5 + PDA_LFRETX] = r6;
+	cc = r6 == r7;
+	if !cc jump _bfin_return_from_exception;
+	/* fall through */
+	R7 = P4;
+	R6 = VEC_CPLB_M;	/* Data CPLB Miss */
+	cc = R6 == R7;
+	if cc jump _ex_dcplb_miss (BP);
+#ifdef CONFIG_MPU
+	R6 = VEC_CPLB_VL;	/* Data CPLB Violation */
+	cc = R6 == R7;
+	if cc jump _ex_dcplb_viol (BP);
+#endif
+	/* Handle Data CPLB Protection Violation
+	 * and Data CPLB Multiple Hits - Linux Trap Zero
+	 */
+	jump _ex_trap_c;
+ENDPROC(_ex_workaround_261)
+
+#else
+#ifdef CONFIG_MPU
+#define _ex_dviol _ex_dcplb_viol
+#else
+#define _ex_dviol _ex_trap_c
+#endif
+#define _ex_dmiss _ex_dcplb_miss
+#define _ex_dmult _ex_trap_c
+#endif
+
+
+ENTRY(_ex_dcplb_viol)
+ENTRY(_ex_dcplb_miss)
+ENTRY(_ex_icplb_miss)
+	(R7:6,P5:4) = [sp++];
+	/* We leave the previously pushed ASTAT on the stack.  */
+	SAVE_CONTEXT_CPLB
+
+	/* We must load R1 here, _before_ DEBUG_HWTRACE_SAVE, since that
+	 * will change the stack pointer.  */
+	R0 = SEQSTAT;
+	R1 = SP;
+
+	DEBUG_HWTRACE_SAVE(p5, r7)
+
+	sp += -12;
+	call _cplb_hdr;
+	sp += 12;
+	CC = R0 == 0;
+	IF !CC JUMP _handle_bad_cplb;
+
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+	/* While we were processing this, did we double fault? */
+	r7 = SEQSTAT;           /* reason code is in bit 5:0 */
+	r6.l = lo(SEQSTAT_EXCAUSE);
+	r6.h = hi(SEQSTAT_EXCAUSE);
+	r7 = r7 & r6;
+	r6 = 0x25;
+	CC = R7 == R6;
+	if CC JUMP _double_fault;
+#endif
+
+	DEBUG_HWTRACE_RESTORE(p5, r7)
+	RESTORE_CONTEXT_CPLB
+	ASTAT = [SP++];
+	SP = EX_SCRATCH_REG;
+	rtx;
+ENDPROC(_ex_icplb_miss)
+
+ENTRY(_ex_syscall)
+	raise 15;		/* invoked by TRAP #0, for sys call */
+	jump.s _bfin_return_from_exception;
+ENDPROC(_ex_syscall)
+
+ENTRY(_ex_single_step)
+	/* If we just returned from an interrupt, the single step event is
+	   for the RTI instruction.  */
+	r7 = retx;
+	r6 = reti;
+	cc = r7 == r6;
+	if cc jump _bfin_return_from_exception;
+
+#ifdef CONFIG_KGDB
+	/* Don't do single step in hardware exception handler */
+        p5.l = lo(IPEND);
+        p5.h = hi(IPEND);
+	r6 = [p5];
+	cc = bittst(r6, 4);
+	if cc jump _bfin_return_from_exception;
+	cc = bittst(r6, 5);
+	if cc jump _bfin_return_from_exception;
+
+	/* skip single step if current interrupt priority is higher than
+	 * that of the first instruction, from which gdb starts single step */
+	r6 >>= 6;
+	r7 = 10;
+.Lfind_priority_start:
+	cc = bittst(r6, 0);
+	if cc jump .Lfind_priority_done;
+	r6 >>= 1;
+	r7 += -1;
+	cc = r7 == 0;
+	if cc jump .Lfind_priority_done;
+	jump.s .Lfind_priority_start;
+.Lfind_priority_done:
+	p4.l = _kgdb_single_step;
+	p4.h = _kgdb_single_step;
+	r6 = [p4];
+	cc = r6 == 0;
+	if cc jump .Ldo_single_step;
+	r6 += -1;
+	cc = r6 < r7;
+	if cc jump 1f;
+.Ldo_single_step:
+#else
+	/* If we were in user mode, do the single step normally.  */
+	p5.l = lo(IPEND);
+	p5.h = hi(IPEND);
+	r6 = [p5];
+	r7 = 0xffe0 (z);
+	r7 = r7 & r6;
+	cc = r7 == 0;
+	if !cc jump 1f;
+#endif
+#ifdef CONFIG_EXACT_HWERR
+	/* Read the ILAT, and to check to see if the process we are
+	 * single stepping caused a previous hardware error
+	 * If so, do not single step, (which lowers to IRQ5, and makes
+	 * us miss the error).
+	 */
+	p5.l = lo(ILAT);
+	p5.h = hi(ILAT);
+	r7 = [p5];
+	cc = bittst(r7, EVT_IVHW_P);
+	if cc jump 1f;
+#endif
+	/* Single stepping only a single instruction, so clear the trace
+	 * bit here.  */
+	r7 = syscfg;
+	bitclr (r7, SYSCFG_SSSTEP_P);
+	syscfg = R7;
+	jump _ex_trap_c;
+
+1:
+	/*
+	 * We were in an interrupt handler.  By convention, all of them save
+	 * SYSCFG with their first instruction, so by checking whether our
+	 * RETX points at the entry point, we can determine whether to allow
+	 * a single step, or whether to clear SYSCFG.
+	 *
+	 * First, find out the interrupt level and the event vector for it.
+	 */
+	p5.l = lo(EVT0);
+	p5.h = hi(EVT0);
+	p5 += -4;
+2:
+	r7 = rot r7 by -1;
+	p5 += 4;
+	if !cc jump 2b;
+
+	/* What we actually do is test for the _second_ instruction in the
+	 * IRQ handler.  That way, if there are insns following the restore
+	 * of SYSCFG after leaving the handler, we will not turn off SYSCFG
+	 * for them.  */
+
+	r7 = [p5];
+	r7 += 2;
+	r6 = RETX;
+	cc = R7 == R6;
+	if !cc jump _bfin_return_from_exception;
+
+	r7 = syscfg;
+	bitclr (r7, SYSCFG_SSSTEP_P);	/* Turn off single step */
+	syscfg = R7;
+
+	/* Fall through to _bfin_return_from_exception.  */
+ENDPROC(_ex_single_step)
+
+ENTRY(_bfin_return_from_exception)
+#if ANOMALY_05000257
+	R7=LC0;
+	LC0=R7;
+	R7=LC1;
+	LC1=R7;
+#endif
+
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+	/* While we were processing the current exception,
+	 * did we cause another, and double fault?
+	 */
+	r7 = SEQSTAT;           /* reason code is in bit 5:0 */
+	r6.l = lo(SEQSTAT_EXCAUSE);
+	r6.h = hi(SEQSTAT_EXCAUSE);
+	r7 = r7 & r6;
+	r6 = VEC_UNCOV;
+	CC = R7 == R6;
+	if CC JUMP _double_fault;
+#endif
+
+	(R7:6,P5:4) = [sp++];
+	ASTAT = [sp++];
+	sp = EX_SCRATCH_REG;
+	rtx;
+ENDPROC(_bfin_return_from_exception)
+
+ENTRY(_handle_bad_cplb)
+	DEBUG_HWTRACE_RESTORE(p5, r7)
+	/* To get here, we just tried and failed to change a CPLB
+	 * so, handle things in trap_c (C code), by lowering to
+	 * IRQ5, just like we normally do. Since this is not a
+	 * "normal" return path, we have a do a lot of stuff to
+	 * the stack to get ready so, we can fall through - we
+	 * need to make a CPLB exception look like a normal exception
+	 */
+	RESTORE_CONTEXT_CPLB
+	/* ASTAT is still on the stack, where it is needed.  */
+	[--sp] = (R7:6,P5:4);
+
+ENTRY(_ex_replaceable)
+	nop;
+
+ENTRY(_ex_trap_c)
+	/* The only thing that has been saved in this context is
+	 * (R7:6,P5:4), ASTAT & SP - don't use anything else
+	 */
+
+	GET_PDA(p5, r6);
+
+	/* Make sure we are not in a double fault */
+	p4.l = lo(IPEND);
+	p4.h = hi(IPEND);
+	r7 = [p4];
+	CC = BITTST (r7, 5);
+	if CC jump _double_fault;
+	[p5 + PDA_EXIPEND] = r7;
+
+	/* Call C code (trap_c) to handle the exception, which most
+	 * likely involves sending a signal to the current process.
+	 * To avoid double faults, lower our priority to IRQ5 first.
+	 */
+	r7.h = _exception_to_level5;
+	r7.l = _exception_to_level5;
+	p4.l = lo(EVT5);
+	p4.h = hi(EVT5);
+	[p4] = r7;
+	csync;
+
+	/*
+	 * Save these registers, as they are only valid in exception context
+	 *  (where we are now - as soon as we defer to IRQ5, they can change)
+	 * DCPLB_STATUS and ICPLB_STATUS are also only valid in EVT3,
+	 * but they are not very interesting, so don't save them
+	 */
+
+	p4.l = lo(DCPLB_FAULT_ADDR);
+	p4.h = hi(DCPLB_FAULT_ADDR);
+	r7 = [p4];
+	[p5 + PDA_DCPLB] = r7;
+
+	p4.l = lo(ICPLB_FAULT_ADDR);
+	p4.h = hi(ICPLB_FAULT_ADDR);
+	r6 = [p4];
+	[p5 + PDA_ICPLB] = r6;
+
+	r6 = retx;
+	[p5 + PDA_RETX] = r6;
+
+	r6 = SEQSTAT;
+	[p5 + PDA_SEQSTAT] = r6;
+
+	/* Save the state of single stepping */
+	r6 = SYSCFG;
+	[p5 + PDA_SYSCFG] = r6;
+	/* Clear it while we handle the exception in IRQ5 mode */
+	BITCLR(r6, SYSCFG_SSSTEP_P);
+	SYSCFG = r6;
+
+	/* Save the current IMASK, since we change in order to jump to level 5 */
+	cli r6;
+	[p5 + PDA_EXIMASK] = r6;
+
+	p4.l = lo(SAFE_USER_INSTRUCTION);
+	p4.h = hi(SAFE_USER_INSTRUCTION);
+	retx = p4;
+
+	/* Disable all interrupts, but make sure level 5 is enabled so
+	 * we can switch to that level.
+	 */
+	r6 = 0x3f;
+	sti r6;
+
+	/* In case interrupts are disabled IPEND[4] (global interrupt disable bit)
+	 * clear it (re-enabling interrupts again) by the special sequence of pushing
+	 * RETI onto the stack.  This way we can lower ourselves to IVG5 even if the
+	 * exception was taken after the interrupt handler was called but before it
+	 * got a chance to enable global interrupts itself.
+	 */
+	[--sp] = reti;
+	sp += 4;
+
+	raise 5;
+	jump.s _bfin_return_from_exception;
+ENDPROC(_ex_trap_c)
+
+/* We just realized we got an exception, while we were processing a different
+ * exception. This is a unrecoverable event, so crash.
+ * Note: this cannot be ENTRY() as we jump here with "if cc jump" ...
+ */
+ENTRY(_double_fault)
+	/* Turn caches & protection off, to ensure we don't get any more
+	 * double exceptions
+	 */
+
+	P4.L = LO(IMEM_CONTROL);
+	P4.H = HI(IMEM_CONTROL);
+
+	R5 = [P4];              /* Control Register*/
+	BITCLR(R5,ENICPLB_P);
+	CSYNC;          /* Disabling of CPLBs should be proceeded by a CSYNC */
+	[P4] = R5;
+	SSYNC;
+
+	P4.L = LO(DMEM_CONTROL);
+	P4.H = HI(DMEM_CONTROL);
+	R5 = [P4];
+	BITCLR(R5,ENDCPLB_P);
+	CSYNC;          /* Disabling of CPLBs should be proceeded by a CSYNC */
+	[P4] = R5;
+	SSYNC;
+
+	/* Fix up the stack */
+	(R7:6,P5:4) = [sp++];
+	ASTAT = [sp++];
+	SP = EX_SCRATCH_REG;
+
+	/* We should be out of the exception stack, and back down into
+	 * kernel or user space stack
+	 */
+	SAVE_ALL_SYS
+
+	/* The dumping functions expect the return address in the RETI
+	 * slot.  */
+	r6 = retx;
+	[sp + PT_PC] = r6;
+
+	r0 = sp;        /* stack frame pt_regs pointer argument ==> r0 */
+	SP += -12;
+	pseudo_long_call _double_fault_c, p5;
+	SP += 12;
+.L_double_fault_panic:
+        JUMP .L_double_fault_panic
+
+ENDPROC(_double_fault)
+
+ENTRY(_exception_to_level5)
+	SAVE_ALL_SYS
+
+	GET_PDA(p5, r7);        /* Fetch current PDA */
+	r6 = [p5 + PDA_RETX];
+	[sp + PT_PC] = r6;
+
+	r6 = [p5 + PDA_SYSCFG];
+	[sp + PT_SYSCFG] = r6;
+
+	r6 = [p5 + PDA_SEQSTAT]; /* Read back seqstat */
+	[sp + PT_SEQSTAT] = r6;
+
+	/* Restore the hardware error vector.  */
+	r7.h = _evt_ivhw;
+	r7.l = _evt_ivhw;
+	p4.l = lo(EVT5);
+	p4.h = hi(EVT5);
+	[p4] = r7;
+	csync;
+
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+	/* Now that we have the hardware error vector programmed properly
+	 * we can re-enable interrupts (IPEND[4]), so if the _trap_c causes
+	 * another hardware error, we can catch it (self-nesting).
+	 */
+	[--sp] = reti;
+	sp += 4;
+#endif
+
+	r7 = [p5 + PDA_EXIPEND]	/* Read the IPEND from the Exception state */
+	[sp + PT_IPEND] = r7;   /* Store IPEND onto the stack */
+
+	r0 = sp; 	/* stack frame pt_regs pointer argument ==> r0 */
+	SP += -12;
+	pseudo_long_call _trap_c, p4;
+	SP += 12;
+
+	/* If interrupts were off during the exception (IPEND[4] = 1), turn them off
+	 * before we return.
+	 */
+	CC = BITTST(r7, EVT_IRPTEN_P)
+	if !CC jump 1f;
+	/* this will load a random value into the reti register - but that is OK,
+	 * since we do restore it to the correct value in the 'RESTORE_ALL_SYS' macro
+	 */
+	sp += -4;
+	reti = [sp++];
+1:
+	/* restore the interrupt mask (IMASK) */
+	r6 = [p5 + PDA_EXIMASK];
+	sti r6;
+
+	call _ret_from_exception;
+	RESTORE_ALL_SYS
+	rti;
+ENDPROC(_exception_to_level5)
+
+ENTRY(_trap) /* Exception: 4th entry into system event table(supervisor mode)*/
+	/* Since the kernel stack can be anywhere, it's not guaranteed to be
+	 * covered by a CPLB.  Switch to an exception stack; use RETN as a
+	 * scratch register (for want of a better option).
+	 */
+	EX_SCRATCH_REG = sp;
+	GET_PDA_SAFE(sp);
+	sp = [sp + PDA_EXSTACK];
+	/* Try to deal with syscalls quickly.  */
+	[--sp] = ASTAT;
+	[--sp] = (R7:6,P5:4);
+
+	ANOMALY_283_315_WORKAROUND(p5, r7)
+
+#ifdef CONFIG_EXACT_HWERR
+	/* Make sure all pending read/writes complete. This will ensure any
+	 * accesses which could cause hardware errors completes, and signal
+	 * the the hardware before we do something silly, like crash the
+	 * kernel. We don't need to work around anomaly 05000312, since
+	 * we are already atomic
+	 */
+	ssync;
+#endif
+
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+	/*
+	 * Save these registers, as they are only valid in exception context
+	 * (where we are now - as soon as we defer to IRQ5, they can change)
+	 * DCPLB_STATUS and ICPLB_STATUS are also only valid in EVT3,
+	 * but they are not very interesting, so don't save them
+	 */
+
+	GET_PDA(p5, r7);
+	p4.l = lo(DCPLB_FAULT_ADDR);
+	p4.h = hi(DCPLB_FAULT_ADDR);
+	r7 = [p4];
+	[p5 + PDA_DF_DCPLB] = r7;
+
+	p4.l = lo(ICPLB_FAULT_ADDR);
+	p4.h = hi(ICPLB_FAULT_ADDR);
+	r7 = [p4];
+	[p5 + PDA_DF_ICPLB] = r7;
+
+	r7 = retx;
+	[p5 + PDA_DF_RETX] = r7;
+
+	r7 = SEQSTAT;		/* reason code is in bit 5:0 */
+	[p5 + PDA_DF_SEQSTAT] = r7;
+#else
+	r7 = SEQSTAT;           /* reason code is in bit 5:0 */
+#endif
+	r6.l = lo(SEQSTAT_EXCAUSE);
+	r6.h = hi(SEQSTAT_EXCAUSE);
+	r7 = r7 & r6;
+	p5.h = _ex_table;
+	p5.l = _ex_table;
+	p4 = r7;
+	p5 = p5 + (p4 << 2);
+	p4 = [p5];
+	jump (p4);
+
+.Lbadsys:
+	r7 = -ENOSYS; 		/* signextending enough */
+	[sp + PT_R0] = r7;	/* return value from system call */
+	jump .Lsyscall_really_exit;
+ENDPROC(_trap)
+
+ENTRY(_kernel_execve)
+	link SIZEOF_PTREGS;
+	p0 = sp;
+	r3 = SIZEOF_PTREGS / 4;
+	r4 = 0(x);
+.Lclear_regs:
+	[p0++] = r4;
+	r3 += -1;
+	cc = r3 == 0;
+	if !cc jump .Lclear_regs (bp);
+
+	p0 = sp;
+	sp += -16;
+	[sp + 12] = p0;
+	pseudo_long_call _do_execve, p5;
+	SP += 16;
+	cc = r0 == 0;
+	if ! cc jump .Lexecve_failed;
+	/* Success.  Copy our temporary pt_regs to the top of the kernel
+	 * stack and do a normal exception return.
+	 */
+	r1 = sp;
+	r0 = (-KERNEL_STACK_SIZE) (x);
+	r1 = r1 & r0;
+	p2 = r1;
+	p3 = [p2];
+	r0 = KERNEL_STACK_SIZE - 4 (z);
+	p1 = r0;
+	p1 = p1 + p2;
+
+	p0 = fp;
+	r4 = [p0--];
+	r3 = SIZEOF_PTREGS / 4;
+.Lcopy_regs:
+	r4 = [p0--];
+	[p1--] = r4;
+	r3 += -1;
+	cc = r3 == 0;
+	if ! cc jump .Lcopy_regs (bp);
+
+	r0 = (KERNEL_STACK_SIZE - SIZEOF_PTREGS) (z);
+	p1 = r0;
+	p1 = p1 + p2;
+	sp = p1;
+	r0 = syscfg;
+	[SP + PT_SYSCFG] = r0;
+	[p3 + (TASK_THREAD + THREAD_KSP)] = sp;
+
+	RESTORE_CONTEXT;
+	rti;
+.Lexecve_failed:
+	unlink;
+	rts;
+ENDPROC(_kernel_execve)
+
+ENTRY(_system_call)
+	/* Store IPEND */
+	p2.l = lo(IPEND);
+	p2.h = hi(IPEND);
+	csync;
+	r0 = [p2];
+	[sp + PT_IPEND] = r0;
+
+	/* Store RETS for now */
+	r0 = rets;
+	[sp + PT_RESERVED] = r0;
+	/* Set the stack for the current process */
+	r7 = sp;
+	r6.l = lo(ALIGN_PAGE_MASK);
+	r6.h = hi(ALIGN_PAGE_MASK);
+	r7 = r7 & r6;  		/* thread_info */
+	p2 = r7;
+	p2 = [p2];
+
+	[p2+(TASK_THREAD+THREAD_KSP)] = sp;
+#ifdef CONFIG_IPIPE
+	r0 = sp;
+	SP += -12;
+	pseudo_long_call ___ipipe_syscall_root, p0;
+	SP += 12;
+	cc = r0 == 1;
+	if cc jump .Lsyscall_really_exit;
+	cc = r0 == -1;
+	if cc jump .Lresume_userspace;
+	r3 = [sp + PT_R3];
+	r4 = [sp + PT_R4];
+	p0 = [sp + PT_ORIG_P0];
+#endif /* CONFIG_IPIPE */
+
+	/* are we tracing syscalls?*/
+	r7 = sp;
+	r6.l = lo(ALIGN_PAGE_MASK);
+	r6.h = hi(ALIGN_PAGE_MASK);
+	r7 = r7 & r6;
+	p2 = r7;
+	r7 = [p2+TI_FLAGS];
+	CC = BITTST(r7,TIF_SYSCALL_TRACE);
+	if CC JUMP _sys_trace;
+	CC = BITTST(r7,TIF_SINGLESTEP);
+	if CC JUMP _sys_trace;
+
+	/* Make sure the system call # is valid */
+	p4 = __NR_syscall;
+	/* System call number is passed in P0 */
+	cc = p4 <= p0;
+	if cc jump .Lbadsys;
+
+	/* Execute the appropriate system call */
+
+	p4 = p0;
+	p5.l = _sys_call_table;
+	p5.h = _sys_call_table;
+	p5 = p5 + (p4 << 2);
+	r0 = [sp + PT_R0];
+	r1 = [sp + PT_R1];
+	r2 = [sp + PT_R2];
+	p5 = [p5];
+
+	[--sp] = r5;
+	[--sp] = r4;
+	[--sp] = r3;
+	SP += -12;
+	call (p5);
+	SP += 24;
+	[sp + PT_R0] = r0;
+
+.Lresume_userspace:
+	r7 = sp;
+	r4.l = lo(ALIGN_PAGE_MASK);
+	r4.h = hi(ALIGN_PAGE_MASK);
+	r7 = r7 & r4;		/* thread_info->flags */
+	p5 = r7;
+.Lresume_userspace_1:
+	/* Disable interrupts.  */
+	[--sp] = reti;
+	reti = [sp++];
+
+	r7 = [p5 + TI_FLAGS];
+	r4.l = lo(_TIF_WORK_MASK);
+	r4.h = hi(_TIF_WORK_MASK);
+	r7 =  r7 & r4;
+
+.Lsyscall_resched:
+#ifdef CONFIG_IPIPE
+	cc = BITTST(r7, TIF_IRQ_SYNC);
+	if !cc jump .Lsyscall_no_irqsync;
+	/*
+	 * Clear IPEND[4] manually to undo what resume_userspace_1 just did;
+	 * we need this so that high priority domain interrupts may still
+	 * preempt the current domain while the pipeline log is being played
+	 * back.
+	 */
+	[--sp] = reti;
+	SP += 4; /* don't merge with next insn to keep the pattern obvious */
+	SP += -12;
+	pseudo_long_call ___ipipe_sync_root, p4;
+	SP += 12;
+	jump .Lresume_userspace_1;
+.Lsyscall_no_irqsync:
+#endif
+	cc = BITTST(r7, TIF_NEED_RESCHED);
+	if !cc jump .Lsyscall_sigpending;
+
+	/* Reenable interrupts.  */
+	[--sp] = reti;
+	sp += 4;
+
+	SP += -12;
+	pseudo_long_call _schedule, p4;
+	SP += 12;
+
+	jump .Lresume_userspace_1;
+
+.Lsyscall_sigpending:
+	cc = BITTST(r7, TIF_RESTORE_SIGMASK);
+	if cc jump .Lsyscall_do_signals;
+	cc = BITTST(r7, TIF_SIGPENDING);
+	if cc jump .Lsyscall_do_signals;
+	cc = BITTST(r7, TIF_NOTIFY_RESUME);
+	if !cc jump .Lsyscall_really_exit;
+.Lsyscall_do_signals:
+	/* Reenable interrupts.  */
+	[--sp] = reti;
+	sp += 4;
+
+	r0 = sp;
+	SP += -12;
+	pseudo_long_call _do_notify_resume, p5;
+	SP += 12;
+
+.Lsyscall_really_exit:
+	r5 = [sp + PT_RESERVED];
+	rets = r5;
+	rts;
+ENDPROC(_system_call)
+
+/* Do not mark as ENTRY() to avoid error in assembler ...
+ * this symbol need not be global anyways, so ...
+ */
+_sys_trace:
+	r0 = sp;
+	pseudo_long_call _syscall_trace_enter, p5;
+
+	/* Make sure the system call # is valid */
+	p4 = [SP + PT_P0];
+	p3 = __NR_syscall;
+	cc = p3 <= p4;
+	r0 = -ENOSYS;
+	if cc jump .Lsys_trace_badsys;
+
+	/* Execute the appropriate system call */
+	p5.l = _sys_call_table;
+	p5.h = _sys_call_table;
+	p5 = p5 + (p4 << 2);
+	r0 = [sp + PT_R0];
+	r1 = [sp + PT_R1];
+	r2 = [sp + PT_R2];
+	r3 = [sp + PT_R3];
+	r4 = [sp + PT_R4];
+	r5 = [sp + PT_R5];
+	p5 = [p5];
+
+	[--sp] = r5;
+	[--sp] = r4;
+	[--sp] = r3;
+	SP += -12;
+	call (p5);
+	SP += 24;
+.Lsys_trace_badsys:
+	[sp + PT_R0] = r0;
+
+	r0 = sp;
+	pseudo_long_call _syscall_trace_leave, p5;
+	jump .Lresume_userspace;
+ENDPROC(_sys_trace)
+
+ENTRY(_resume)
+	/*
+	 * Beware - when entering resume, prev (the current task) is
+	 * in r0, next (the new task) is in r1.
+	 */
+	p0 = r0;
+	p1 = r1;
+	[--sp] = rets;
+	[--sp] = fp;
+	[--sp] = (r7:4, p5:3);
+
+	/* save usp */
+	p2 = usp;
+	[p0+(TASK_THREAD+THREAD_USP)] = p2;
+
+	/* save current kernel stack pointer */
+	[p0+(TASK_THREAD+THREAD_KSP)] = sp;
+
+	/* save program counter */
+	r1.l = _new_old_task;
+	r1.h = _new_old_task;
+	[p0+(TASK_THREAD+THREAD_PC)] = r1;
+
+	/* restore the kernel stack pointer */
+	sp = [p1+(TASK_THREAD+THREAD_KSP)];
+
+	/* restore user stack pointer */
+	p0 = [p1+(TASK_THREAD+THREAD_USP)];
+	usp = p0;
+
+	/* restore pc */
+	p0 = [p1+(TASK_THREAD+THREAD_PC)];
+	jump (p0);
+
+	/*
+	 * Following code actually lands up in a new (old) task.
+	 */
+
+_new_old_task:
+	(r7:4, p5:3) = [sp++];
+	fp = [sp++];
+	rets = [sp++];
+
+	/*
+	 * When we come out of resume, r0 carries "old" task, because we are
+	 * in "new" task.
+	 */
+	rts;
+ENDPROC(_resume)
+
+ENTRY(_ret_from_exception)
+#ifdef CONFIG_IPIPE
+	p2.l = _ipipe_percpu_domain;
+	p2.h = _ipipe_percpu_domain;
+	r0.l = _ipipe_root;
+	r0.h = _ipipe_root;
+	r2 = [p2];
+	cc = r0 == r2;
+	if !cc jump 4f;  /* not on behalf of the root domain, get out */
+#endif /* CONFIG_IPIPE */
+	p2.l = lo(IPEND);
+	p2.h = hi(IPEND);
+
+	csync;
+	r0 = [p2];
+	[sp + PT_IPEND] = r0;
+
+1:
+	r2 = LO(~0x37) (Z);
+	r0 = r2 & r0;
+	cc = r0 == 0;
+	if !cc jump 4f;	/* if not return to user mode, get out */
+
+	/* Make sure any pending system call or deferred exception
+	 * return in ILAT for this process to get executed, otherwise
+	 * in case context switch happens, system call of
+	 * first process (i.e in ILAT) will be carried
+	 * forward to the switched process
+	 */
+
+	p2.l = lo(ILAT);
+	p2.h = hi(ILAT);
+	r0 = [p2];
+	r1 = (EVT_IVG14 | EVT_IVG15) (z);
+	r0 = r0 & r1;
+	cc = r0 == 0;
+	if !cc jump 5f;
+
+	/* Set the stack for the current process */
+	r7 = sp;
+	r4.l = lo(ALIGN_PAGE_MASK);
+	r4.h = hi(ALIGN_PAGE_MASK);
+	r7 = r7 & r4;		/* thread_info->flags */
+	p5 = r7;
+	r7 = [p5 + TI_FLAGS];
+	r4.l = lo(_TIF_WORK_MASK);
+	r4.h = hi(_TIF_WORK_MASK);
+	r7 =  r7 & r4;
+	cc = r7 == 0;
+	if cc jump 4f;
+
+	p0.l = lo(EVT15);
+	p0.h = hi(EVT15);
+	p1.l = _schedule_and_signal;
+	p1.h = _schedule_and_signal;
+	[p0] = p1;
+	csync;
+	raise 15;		/* raise evt15 to do signal or reschedule */
+4:
+	r0 = syscfg;
+	bitclr(r0, SYSCFG_SSSTEP_P);		/* Turn off single step */
+	syscfg = r0;
+5:
+	rts;
+ENDPROC(_ret_from_exception)
+
+#if defined(CONFIG_PREEMPT)
+
+ENTRY(_up_to_irq14)
+#if ANOMALY_05000281 || ANOMALY_05000461
+	r0.l = lo(SAFE_USER_INSTRUCTION);
+	r0.h = hi(SAFE_USER_INSTRUCTION);
+	reti = r0;
+#endif
+
+#ifdef CONFIG_DEBUG_HWERR
+	/* enable irq14 & hwerr interrupt, until we transition to _evt_evt14 */
+	r0 = (EVT_IVG14 | EVT_IVHW | EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+#else
+	/* Only enable irq14 interrupt, until we transition to _evt_evt14 */
+	r0 = (EVT_IVG14 | EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+#endif
+	sti r0;
+
+	p0.l = lo(EVT14);
+	p0.h = hi(EVT14);
+	p1.l = _evt_up_evt14;
+	p1.h = _evt_up_evt14;
+	[p0] = p1;
+	csync;
+
+	raise 14;
+1:
+	jump 1b;
+ENDPROC(_up_to_irq14)
+
+ENTRY(_evt_up_evt14)
+#ifdef CONFIG_DEBUG_HWERR
+	r0 = (EVT_IVHW | EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+	sti r0;
+#else
+	cli r0;
+#endif
+#ifdef CONFIG_TRACE_IRQFLAGS
+	[--sp] = rets;
+	sp += -12;
+	call _trace_hardirqs_off;
+	sp += 12;
+	rets = [sp++];
+#endif
+	[--sp] = RETI;
+	SP += 4;
+
+	/* restore normal evt14 */
+	p0.l = lo(EVT14);
+	p0.h = hi(EVT14);
+	p1.l = _evt_evt14;
+	p1.h = _evt_evt14;
+	[p0] = p1;
+	csync;
+
+	rts;
+ENDPROC(_evt_up_evt14)
+
+#endif
+
+#ifdef CONFIG_IPIPE
+
+_resume_kernel_from_int:
+	r1 = LO(~0x8000) (Z);
+	r1 = r0 & r1;
+	r0 = 1;
+	r0 = r1 - r0;
+	r2 = r1 & r0;
+	cc = r2 == 0;
+	/* Sync the root stage only from the outer interrupt level. */
+	if !cc jump .Lnosync;
+	r0.l = ___ipipe_sync_root;
+	r0.h = ___ipipe_sync_root;
+	[--sp] = reti;
+	[--sp] = rets;
+	[--sp] = ( r7:4, p5:3 );
+	SP += -12;
+	call ___ipipe_call_irqtail
+	SP += 12;
+	( r7:4, p5:3 ) = [sp++];
+	rets = [sp++];
+	reti = [sp++];
+.Lnosync:
+	rts
+#elif defined(CONFIG_PREEMPT)
+
+_resume_kernel_from_int:
+	/* check preempt_count */
+	r7 = sp;
+	r4.l = lo(ALIGN_PAGE_MASK);
+	r4.h = hi(ALIGN_PAGE_MASK);
+	r7 = r7 & r4;
+	p5 = r7;
+	r7 = [p5 + TI_PREEMPT];
+	cc = r7 == 0x0;
+	if !cc jump .Lreturn_to_kernel;
+.Lneed_schedule:
+	r7 = [p5 + TI_FLAGS];
+	r4.l = lo(_TIF_WORK_MASK);
+	r4.h = hi(_TIF_WORK_MASK);
+	r7 =  r7 & r4;
+	cc = BITTST(r7, TIF_NEED_RESCHED);
+	if !cc jump .Lreturn_to_kernel;
+	/*
+	 * let schedule done at level 15, otherwise sheduled process will run
+	 * at high level and block low level interrupt
+	 */
+	r6 = reti;  /* save reti */
+	r5.l = .Lkernel_schedule;
+	r5.h = .Lkernel_schedule;
+	reti = r5;
+	rti;
+.Lkernel_schedule:
+	[--sp] = rets;
+	sp += -12;
+	pseudo_long_call _preempt_schedule_irq, p4;
+	sp += 12;
+	rets = [sp++];
+
+	[--sp] = rets;
+	sp += -12;
+	/* up to irq14 so that reti after restore_all can return to irq15(kernel) */
+	pseudo_long_call _up_to_irq14, p4;
+	sp += 12;
+	rets = [sp++];
+
+	reti = r6; /* restore reti so that origin process can return to interrupted point */
+
+	jump .Lneed_schedule;
+#else
+
+#define _resume_kernel_from_int	.Lreturn_to_kernel
+#endif
+
+ENTRY(_return_from_int)
+	/* If someone else already raised IRQ 15, do nothing.  */
+	csync;
+	p2.l = lo(ILAT);
+	p2.h = hi(ILAT);
+	r0 = [p2];
+	cc = bittst (r0, EVT_IVG15_P);
+	if cc jump .Lreturn_to_kernel;
+
+	/* if not return to user mode, get out */
+	p2.l = lo(IPEND);
+	p2.h = hi(IPEND);
+	r0 = [p2];
+	r1 = 0x17(Z);
+	r2 = ~r1;
+	r2.h = 0;
+	r0 = r2 & r0;
+	r1 = 1;
+	r1 = r0 - r1;
+	r2 = r0 & r1;
+	cc = r2 == 0;
+	if !cc jump _resume_kernel_from_int;
+
+	/* Lower the interrupt level to 15.  */
+	p0.l = lo(EVT15);
+	p0.h = hi(EVT15);
+	p1.l = _schedule_and_signal_from_int;
+	p1.h = _schedule_and_signal_from_int;
+	[p0] = p1;
+	csync;
+#if ANOMALY_05000281 || ANOMALY_05000461
+	r0.l = lo(SAFE_USER_INSTRUCTION);
+	r0.h = hi(SAFE_USER_INSTRUCTION);
+	reti = r0;
+#endif
+	r0 = 0x801f (z);
+	STI r0;
+	raise 15;	/* raise evt15 to do signal or reschedule */
+	rti;
+.Lreturn_to_kernel:
+	rts;
+ENDPROC(_return_from_int)
+
+ENTRY(_lower_to_irq14)
+#if ANOMALY_05000281 || ANOMALY_05000461
+	r0.l = lo(SAFE_USER_INSTRUCTION);
+	r0.h = hi(SAFE_USER_INSTRUCTION);
+	reti = r0;
+#endif
+
+#ifdef CONFIG_DEBUG_HWERR
+	/* enable irq14 & hwerr interrupt, until we transition to _evt_evt14 */
+	r0 = (EVT_IVG14 | EVT_IVHW | EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+#else
+	/* Only enable irq14 interrupt, until we transition to _evt_evt14 */
+	r0 = (EVT_IVG14 | EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+#endif
+	sti r0;
+	raise 14;
+	rti;
+ENDPROC(_lower_to_irq14)
+
+ENTRY(_evt_evt14)
+#ifdef CONFIG_DEBUG_HWERR
+	r0 = (EVT_IVHW | EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+	sti r0;
+#else
+	cli r0;
+#endif
+#ifdef CONFIG_TRACE_IRQFLAGS
+	[--sp] = rets;
+	sp += -12;
+	call _trace_hardirqs_off;
+	sp += 12;
+	rets = [sp++];
+#endif
+	[--sp] = RETI;
+	SP += 4;
+	rts;
+ENDPROC(_evt_evt14)
+
+ENTRY(_schedule_and_signal_from_int)
+	/* To end up here, vector 15 was changed - so we have to change it
+	 * back.
+	 */
+	p0.l = lo(EVT15);
+	p0.h = hi(EVT15);
+	p1.l = _evt_system_call;
+	p1.h = _evt_system_call;
+	[p0] = p1;
+	csync;
+
+	/* Set orig_p0 to -1 to indicate this isn't the end of a syscall.  */
+	r0 = -1 (x);
+	[sp + PT_ORIG_P0] = r0;
+
+	p1 = rets;
+	[sp + PT_RESERVED] = p1;
+
+#ifdef CONFIG_TRACE_IRQFLAGS
+	/* trace_hardirqs_on() checks if all irqs are disabled. But here IRQ 15
+	 * is turned on, so disable all irqs. */
+	cli r0;
+	sp += -12;
+	call _trace_hardirqs_on;
+	sp += 12;
+#endif
+#ifdef CONFIG_SMP
+	GET_PDA(p0, r0); 	/* Fetch current PDA (can't migrate to other CPU here) */
+	r0 = [p0 + PDA_IRQFLAGS];
+#else
+	p0.l = _bfin_irq_flags;
+	p0.h = _bfin_irq_flags;
+	r0 = [p0];
+#endif
+	sti r0;
+
+	/* finish the userspace "atomic" functions for it */
+	r1 = FIXED_CODE_END;
+	r2 = [sp + PT_PC];
+	cc = r1 <= r2;
+	if cc jump .Lresume_userspace (bp);
+
+	r0 = sp;
+	sp += -12;
+
+	pseudo_long_call _finish_atomic_sections, p5;
+	sp += 12;
+	jump.s .Lresume_userspace;
+ENDPROC(_schedule_and_signal_from_int)
+
+ENTRY(_schedule_and_signal)
+	SAVE_CONTEXT_SYSCALL
+	/* To end up here, vector 15 was changed - so we have to change it
+	 * back.
+	 */
+	p0.l = lo(EVT15);
+	p0.h = hi(EVT15);
+	p1.l = _evt_system_call;
+	p1.h = _evt_system_call;
+	[p0] = p1;
+	csync;
+	p0.l = 1f;
+	p0.h = 1f;
+	[sp + PT_RESERVED] = P0;
+	call .Lresume_userspace;
+1:
+	RESTORE_CONTEXT
+	rti;
+ENDPROC(_schedule_and_signal)
+
+/* We handle this 100% in exception space - to reduce overhead
+ * Only potiential problem is if the software buffer gets swapped out of the
+ * CPLB table - then double fault. - so we don't let this happen in other places
+ */
+#ifdef CONFIG_DEBUG_BFIN_HWTRACE_EXPAND
+ENTRY(_ex_trace_buff_full)
+	[--sp] = P3;
+	[--sp] = P2;
+	[--sp] = LC0;
+	[--sp] = LT0;
+	[--sp] = LB0;
+	P5.L = _trace_buff_offset;
+	P5.H = _trace_buff_offset;
+	P3 = [P5];              /* trace_buff_offset */
+	P5.L = lo(TBUFSTAT);
+	P5.H = hi(TBUFSTAT);
+	R7 = [P5];
+	R7 <<= 1;               /* double, since we need to read twice */
+	LC0 = R7;
+	R7 <<= 2;               /* need to shift over again,
+				 * to get the number of bytes */
+	P5.L = lo(TBUF);
+	P5.H = hi(TBUF);
+	R6 = ((1 << CONFIG_DEBUG_BFIN_HWTRACE_EXPAND_LEN)*1024) - 1;
+
+	P2 = R7;
+	P3 = P3 + P2;
+	R7 = P3;
+	R7 = R7 & R6;
+	P3 = R7;
+	P2.L = _trace_buff_offset;
+	P2.H = _trace_buff_offset;
+	[P2] = P3;
+
+	P2.L = _software_trace_buff;
+	P2.H = _software_trace_buff;
+
+	LSETUP (.Lstart, .Lend) LC0;
+.Lstart:
+	R7 = [P5];      /* read TBUF */
+	P4 = P3 + P2;
+	[P4] = R7;
+	P3 += -4;
+	R7 = P3;
+	R7 = R7 & R6;
+.Lend:
+	P3 = R7;
+
+	LB0 = [sp++];
+	LT0 = [sp++];
+	LC0 = [sp++];
+	P2 = [sp++];
+	P3 = [sp++];
+	jump _bfin_return_from_exception;
+ENDPROC(_ex_trace_buff_full)
+
+#if CONFIG_DEBUG_BFIN_HWTRACE_EXPAND_LEN == 4
+.data
+#else
+.section .l1.data.B
+#endif /* CONFIG_DEBUG_BFIN_HWTRACE_EXPAND_LEN */
+ENTRY(_trace_buff_offset)
+        .long 0;
+ALIGN
+ENTRY(_software_trace_buff)
+	.rept ((1 << CONFIG_DEBUG_BFIN_HWTRACE_EXPAND_LEN)*256);
+	.long 0
+	.endr
+#endif /* CONFIG_DEBUG_BFIN_HWTRACE_EXPAND */
+
+#ifdef CONFIG_EARLY_PRINTK
+__INIT
+ENTRY(_early_trap)
+	SAVE_ALL_SYS
+	trace_buffer_stop(p0,r0);
+
+	ANOMALY_283_315_WORKAROUND(p4, r5)
+
+	/* Turn caches off, to ensure we don't get double exceptions */
+
+	P4.L = LO(IMEM_CONTROL);
+	P4.H = HI(IMEM_CONTROL);
+
+	R5 = [P4];              /* Control Register*/
+	BITCLR(R5,ENICPLB_P);
+	CSYNC;          /* Disabling of CPLBs should be proceeded by a CSYNC */
+	[P4] = R5;
+	SSYNC;
+
+	P4.L = LO(DMEM_CONTROL);
+	P4.H = HI(DMEM_CONTROL);
+	R5 = [P4];
+	BITCLR(R5,ENDCPLB_P);
+	CSYNC;          /* Disabling of CPLBs should be proceeded by a CSYNC */
+	[P4] = R5;
+	SSYNC;
+
+	r0 = sp;        /* stack frame pt_regs pointer argument ==> r0 */
+	r1 = RETX;
+
+	SP += -12;
+	call _early_trap_c;
+	SP += 12;
+ENDPROC(_early_trap)
+__FINIT
+#endif /* CONFIG_EARLY_PRINTK */
+
+/*
+ * Put these in the kernel data section - that should always be covered by
+ * a CPLB. This is needed to ensure we don't get double fault conditions
+ */
+
+#ifdef CONFIG_SYSCALL_TAB_L1
+.section .l1.data
+#else
+.data
+#endif
+
+ENTRY(_ex_table)
+	/* entry for each EXCAUSE[5:0]
+	 * This table must be in sync with the table in ./kernel/traps.c
+	 * EXCPT instruction can provide 4 bits of EXCAUSE, allowing 16 to be user defined
+	 */
+	.long _ex_syscall       /* 0x00 - User Defined - Linux Syscall */
+	.long _ex_trap_c        /* 0x01 - User Defined - Software breakpoint */
+#ifdef	CONFIG_KGDB
+	.long _ex_trap_c	/* 0x02 - User Defined - KGDB initial connection
+							 and break signal trap */
+#else
+	.long _ex_replaceable   /* 0x02 - User Defined */
+#endif
+	.long _ex_trap_c        /* 0x03 - User Defined - userspace stack overflow */
+	.long _ex_trap_c        /* 0x04 - User Defined - dump trace buffer */
+	.long _ex_replaceable   /* 0x05 - User Defined */
+	.long _ex_replaceable   /* 0x06 - User Defined */
+	.long _ex_replaceable   /* 0x07 - User Defined */
+	.long _ex_replaceable   /* 0x08 - User Defined */
+	.long _ex_replaceable   /* 0x09 - User Defined */
+	.long _ex_replaceable   /* 0x0A - User Defined */
+	.long _ex_replaceable   /* 0x0B - User Defined */
+	.long _ex_replaceable   /* 0x0C - User Defined */
+	.long _ex_replaceable   /* 0x0D - User Defined */
+	.long _ex_replaceable   /* 0x0E - User Defined */
+	.long _ex_replaceable   /* 0x0F - User Defined */
+	.long _ex_single_step   /* 0x10 - HW Single step */
+#ifdef CONFIG_DEBUG_BFIN_HWTRACE_EXPAND
+	.long _ex_trace_buff_full /* 0x11 - Trace Buffer Full */
+#else
+	.long _ex_trap_c        /* 0x11 - Trace Buffer Full */
+#endif
+	.long _ex_trap_c        /* 0x12 - Reserved */
+	.long _ex_trap_c        /* 0x13 - Reserved */
+	.long _ex_trap_c        /* 0x14 - Reserved */
+	.long _ex_trap_c        /* 0x15 - Reserved */
+	.long _ex_trap_c        /* 0x16 - Reserved */
+	.long _ex_trap_c        /* 0x17 - Reserved */
+	.long _ex_trap_c        /* 0x18 - Reserved */
+	.long _ex_trap_c        /* 0x19 - Reserved */
+	.long _ex_trap_c        /* 0x1A - Reserved */
+	.long _ex_trap_c        /* 0x1B - Reserved */
+	.long _ex_trap_c        /* 0x1C - Reserved */
+	.long _ex_trap_c        /* 0x1D - Reserved */
+	.long _ex_trap_c        /* 0x1E - Reserved */
+	.long _ex_trap_c        /* 0x1F - Reserved */
+	.long _ex_trap_c        /* 0x20 - Reserved */
+	.long _ex_trap_c        /* 0x21 - Undefined Instruction */
+	.long _ex_trap_c        /* 0x22 - Illegal Instruction Combination */
+	.long _ex_dviol         /* 0x23 - Data CPLB Protection Violation */
+	.long _ex_trap_c        /* 0x24 - Data access misaligned */
+	.long _ex_trap_c        /* 0x25 - Unrecoverable Event */
+	.long _ex_dmiss         /* 0x26 - Data CPLB Miss */
+	.long _ex_dmult         /* 0x27 - Data CPLB Multiple Hits - Linux Trap Zero */
+	.long _ex_trap_c        /* 0x28 - Emulation Watchpoint */
+	.long _ex_trap_c        /* 0x29 - Instruction fetch access error (535 only) */
+	.long _ex_trap_c        /* 0x2A - Instruction fetch misaligned */
+	.long _ex_trap_c        /* 0x2B - Instruction CPLB protection Violation */
+	.long _ex_icplb_miss    /* 0x2C - Instruction CPLB miss */
+	.long _ex_trap_c        /* 0x2D - Instruction CPLB Multiple Hits */
+	.long _ex_trap_c        /* 0x2E - Illegal use of Supervisor Resource */
+	.long _ex_trap_c        /* 0x2E - Illegal use of Supervisor Resource */
+	.long _ex_trap_c        /* 0x2F - Reserved */
+	.long _ex_trap_c        /* 0x30 - Reserved */
+	.long _ex_trap_c        /* 0x31 - Reserved */
+	.long _ex_trap_c        /* 0x32 - Reserved */
+	.long _ex_trap_c        /* 0x33 - Reserved */
+	.long _ex_trap_c        /* 0x34 - Reserved */
+	.long _ex_trap_c        /* 0x35 - Reserved */
+	.long _ex_trap_c        /* 0x36 - Reserved */
+	.long _ex_trap_c        /* 0x37 - Reserved */
+	.long _ex_trap_c        /* 0x38 - Reserved */
+	.long _ex_trap_c        /* 0x39 - Reserved */
+	.long _ex_trap_c        /* 0x3A - Reserved */
+	.long _ex_trap_c        /* 0x3B - Reserved */
+	.long _ex_trap_c        /* 0x3C - Reserved */
+	.long _ex_trap_c        /* 0x3D - Reserved */
+	.long _ex_trap_c        /* 0x3E - Reserved */
+	.long _ex_trap_c        /* 0x3F - Reserved */
+END(_ex_table)
+
+ENTRY(_sys_call_table)
+	.long _sys_restart_syscall	/* 0 */
+	.long _sys_exit
+	.long _sys_fork
+	.long _sys_read
+	.long _sys_write
+	.long _sys_open		/* 5 */
+	.long _sys_close
+	.long _sys_ni_syscall	/* old waitpid */
+	.long _sys_creat
+	.long _sys_link
+	.long _sys_unlink	/* 10 */
+	.long _sys_execve
+	.long _sys_chdir
+	.long _sys_time
+	.long _sys_mknod
+	.long _sys_chmod		/* 15 */
+	.long _sys_chown	/* chown16 */
+	.long _sys_ni_syscall	/* old break syscall holder */
+	.long _sys_ni_syscall	/* old stat */
+	.long _sys_lseek
+	.long _sys_getpid	/* 20 */
+	.long _sys_mount
+	.long _sys_ni_syscall	/* old umount */
+	.long _sys_setuid
+	.long _sys_getuid
+	.long _sys_stime		/* 25 */
+	.long _sys_ptrace
+	.long _sys_alarm
+	.long _sys_ni_syscall	/* old fstat */
+	.long _sys_pause
+	.long _sys_ni_syscall	/* old utime */ /* 30 */
+	.long _sys_ni_syscall	/* old stty syscall holder */
+	.long _sys_ni_syscall	/* old gtty syscall holder */
+	.long _sys_access
+	.long _sys_nice
+	.long _sys_ni_syscall	/* 35 */ /* old ftime syscall holder */
+	.long _sys_sync
+	.long _sys_kill
+	.long _sys_rename
+	.long _sys_mkdir
+	.long _sys_rmdir		/* 40 */
+	.long _sys_dup
+	.long _sys_pipe
+	.long _sys_times
+	.long _sys_ni_syscall	/* old prof syscall holder */
+	.long _sys_brk		/* 45 */
+	.long _sys_setgid
+	.long _sys_getgid
+	.long _sys_ni_syscall	/* old sys_signal */
+	.long _sys_geteuid	/* geteuid16 */
+	.long _sys_getegid	/* getegid16 */	/* 50 */
+	.long _sys_acct
+	.long _sys_umount	/* recycled never used phys() */
+	.long _sys_ni_syscall	/* old lock syscall holder */
+	.long _sys_ioctl
+	.long _sys_fcntl		/* 55 */
+	.long _sys_ni_syscall	/* old mpx syscall holder */
+	.long _sys_setpgid
+	.long _sys_ni_syscall	/* old ulimit syscall holder */
+	.long _sys_ni_syscall	/* old old uname */
+	.long _sys_umask		/* 60 */
+	.long _sys_chroot
+	.long _sys_ustat
+	.long _sys_dup2
+	.long _sys_getppid
+	.long _sys_getpgrp	/* 65 */
+	.long _sys_setsid
+	.long _sys_ni_syscall	/* old sys_sigaction */
+	.long _sys_sgetmask
+	.long _sys_ssetmask
+	.long _sys_setreuid	/* setreuid16 */	/* 70 */
+	.long _sys_setregid	/* setregid16 */
+	.long _sys_ni_syscall	/* old sys_sigsuspend */
+	.long _sys_ni_syscall	/* old sys_sigpending */
+	.long _sys_sethostname
+	.long _sys_setrlimit	/* 75 */
+	.long _sys_ni_syscall	/* old getrlimit */
+	.long _sys_getrusage
+	.long _sys_gettimeofday
+	.long _sys_settimeofday
+	.long _sys_getgroups	/* getgroups16 */	/* 80 */
+	.long _sys_setgroups	/* setgroups16 */
+	.long _sys_ni_syscall	/* old_select */
+	.long _sys_symlink
+	.long _sys_ni_syscall	/* old lstat */
+	.long _sys_readlink	/* 85 */
+	.long _sys_uselib
+	.long _sys_ni_syscall	/* sys_swapon */
+	.long _sys_reboot
+	.long _sys_ni_syscall	/* old_readdir */
+	.long _sys_ni_syscall	/* sys_mmap */	/* 90 */
+	.long _sys_munmap
+	.long _sys_truncate
+	.long _sys_ftruncate
+	.long _sys_fchmod
+	.long _sys_fchown	/* fchown16 */	/* 95 */
+	.long _sys_getpriority
+	.long _sys_setpriority
+	.long _sys_ni_syscall	/* old profil syscall holder */
+	.long _sys_statfs
+	.long _sys_fstatfs	/* 100 */
+	.long _sys_ni_syscall
+	.long _sys_ni_syscall	/* old sys_socketcall */
+	.long _sys_syslog
+	.long _sys_setitimer
+	.long _sys_getitimer	/* 105 */
+	.long _sys_newstat
+	.long _sys_newlstat
+	.long _sys_newfstat
+	.long _sys_ni_syscall	/* old uname */
+	.long _sys_ni_syscall	/* iopl for i386 */ /* 110 */
+	.long _sys_vhangup
+	.long _sys_ni_syscall	/* obsolete idle() syscall */
+	.long _sys_ni_syscall	/* vm86old for i386 */
+	.long _sys_wait4
+	.long _sys_ni_syscall	/* 115 */ /* sys_swapoff */
+	.long _sys_sysinfo
+	.long _sys_ni_syscall	/* old sys_ipc */
+	.long _sys_fsync
+	.long _sys_ni_syscall	/* old sys_sigreturn */
+	.long _sys_clone		/* 120 */
+	.long _sys_setdomainname
+	.long _sys_newuname
+	.long _sys_ni_syscall	/* old sys_modify_ldt */
+	.long _sys_adjtimex
+	.long _sys_mprotect	/* 125 */
+	.long _sys_ni_syscall	/* old sys_sigprocmask */
+	.long _sys_ni_syscall	/* old "creat_module" */
+	.long _sys_init_module
+	.long _sys_delete_module
+	.long _sys_ni_syscall	/* 130: old "get_kernel_syms" */
+	.long _sys_quotactl
+	.long _sys_getpgid
+	.long _sys_fchdir
+	.long _sys_bdflush
+	.long _sys_ni_syscall	/* 135 */ /* sys_sysfs */
+	.long _sys_personality
+	.long _sys_ni_syscall	/* for afs_syscall */
+	.long _sys_setfsuid	/* setfsuid16 */
+	.long _sys_setfsgid	/* setfsgid16 */
+	.long _sys_llseek	/* 140 */
+	.long _sys_getdents
+	.long _sys_ni_syscall	/* sys_select */
+	.long _sys_flock
+	.long _sys_msync
+	.long _sys_readv		/* 145 */
+	.long _sys_writev
+	.long _sys_getsid
+	.long _sys_fdatasync
+	.long _sys_sysctl
+	.long _sys_mlock	/* 150 */
+	.long _sys_munlock
+	.long _sys_mlockall
+	.long _sys_munlockall
+	.long _sys_sched_setparam
+	.long _sys_sched_getparam /* 155 */
+	.long _sys_sched_setscheduler
+	.long _sys_sched_getscheduler
+	.long _sys_sched_yield
+	.long _sys_sched_get_priority_max
+	.long _sys_sched_get_priority_min  /* 160 */
+	.long _sys_sched_rr_get_interval
+	.long _sys_nanosleep
+	.long _sys_mremap
+	.long _sys_setresuid	/* setresuid16 */
+	.long _sys_getresuid	/* getresuid16 */	/* 165 */
+	.long _sys_ni_syscall	/* for vm86 */
+	.long _sys_ni_syscall	/* old "query_module" */
+	.long _sys_ni_syscall	/* sys_poll */
+	.long _sys_ni_syscall   /* old nfsservctl */
+	.long _sys_setresgid	/* setresgid16 */	/* 170 */
+	.long _sys_getresgid	/* getresgid16 */
+	.long _sys_prctl
+	.long _sys_rt_sigreturn
+	.long _sys_rt_sigaction
+	.long _sys_rt_sigprocmask /* 175 */
+	.long _sys_rt_sigpending
+	.long _sys_rt_sigtimedwait
+	.long _sys_rt_sigqueueinfo
+	.long _sys_rt_sigsuspend
+	.long _sys_pread64	/* 180 */
+	.long _sys_pwrite64
+	.long _sys_lchown	/* lchown16 */
+	.long _sys_getcwd
+	.long _sys_capget
+	.long _sys_capset	/* 185 */
+	.long _sys_sigaltstack
+	.long _sys_sendfile
+	.long _sys_ni_syscall	/* streams1 */
+	.long _sys_ni_syscall	/* streams2 */
+	.long _sys_vfork		/* 190 */
+	.long _sys_getrlimit
+	.long _sys_mmap_pgoff
+	.long _sys_truncate64
+	.long _sys_ftruncate64
+	.long _sys_stat64	/* 195 */
+	.long _sys_lstat64
+	.long _sys_fstat64
+	.long _sys_chown
+	.long _sys_getuid
+	.long _sys_getgid	/* 200 */
+	.long _sys_geteuid
+	.long _sys_getegid
+	.long _sys_setreuid
+	.long _sys_setregid
+	.long _sys_getgroups	/* 205 */
+	.long _sys_setgroups
+	.long _sys_fchown
+	.long _sys_setresuid
+	.long _sys_getresuid
+	.long _sys_setresgid	/* 210 */
+	.long _sys_getresgid
+	.long _sys_lchown
+	.long _sys_setuid
+	.long _sys_setgid
+	.long _sys_setfsuid	/* 215 */
+	.long _sys_setfsgid
+	.long _sys_pivot_root
+	.long _sys_mincore
+	.long _sys_madvise
+	.long _sys_getdents64	/* 220 */
+	.long _sys_fcntl64
+	.long _sys_ni_syscall	/* reserved for TUX */
+	.long _sys_ni_syscall
+	.long _sys_gettid
+	.long _sys_readahead	/* 225 */
+	.long _sys_setxattr
+	.long _sys_lsetxattr
+	.long _sys_fsetxattr
+	.long _sys_getxattr
+	.long _sys_lgetxattr	/* 230 */
+	.long _sys_fgetxattr
+	.long _sys_listxattr
+	.long _sys_llistxattr
+	.long _sys_flistxattr
+	.long _sys_removexattr	/* 235 */
+	.long _sys_lremovexattr
+	.long _sys_fremovexattr
+	.long _sys_tkill
+	.long _sys_sendfile64
+	.long _sys_futex		/* 240 */
+	.long _sys_sched_setaffinity
+	.long _sys_sched_getaffinity
+	.long _sys_ni_syscall	/* sys_set_thread_area */
+	.long _sys_ni_syscall	/* sys_get_thread_area */
+	.long _sys_io_setup	/* 245 */
+	.long _sys_io_destroy
+	.long _sys_io_getevents
+	.long _sys_io_submit
+	.long _sys_io_cancel
+	.long _sys_ni_syscall	/* 250 */ /* sys_alloc_hugepages */
+	.long _sys_ni_syscall	/* sys_freec_hugepages */
+	.long _sys_exit_group
+	.long _sys_lookup_dcookie
+	.long _sys_bfin_spinlock
+	.long _sys_epoll_create	/* 255 */
+	.long _sys_epoll_ctl
+	.long _sys_epoll_wait
+	.long _sys_ni_syscall /* remap_file_pages */
+	.long _sys_set_tid_address
+	.long _sys_timer_create	/* 260 */
+	.long _sys_timer_settime
+	.long _sys_timer_gettime
+	.long _sys_timer_getoverrun
+	.long _sys_timer_delete
+	.long _sys_clock_settime /* 265 */
+	.long _sys_clock_gettime
+	.long _sys_clock_getres
+	.long _sys_clock_nanosleep
+	.long _sys_statfs64
+	.long _sys_fstatfs64	/* 270 */
+	.long _sys_tgkill
+	.long _sys_utimes
+	.long _sys_fadvise64_64
+	.long _sys_ni_syscall /* vserver */
+	.long _sys_mbind	/* 275 */
+	.long _sys_ni_syscall /* get_mempolicy */
+	.long _sys_ni_syscall /* set_mempolicy */
+	.long _sys_mq_open
+	.long _sys_mq_unlink
+	.long _sys_mq_timedsend	/* 280 */
+	.long _sys_mq_timedreceive
+	.long _sys_mq_notify
+	.long _sys_mq_getsetattr
+	.long _sys_ni_syscall /* kexec_load */
+	.long _sys_waitid	/* 285 */
+	.long _sys_add_key
+	.long _sys_request_key
+	.long _sys_keyctl
+	.long _sys_ioprio_set
+	.long _sys_ioprio_get	/* 290 */
+	.long _sys_inotify_init
+	.long _sys_inotify_add_watch
+	.long _sys_inotify_rm_watch
+	.long _sys_ni_syscall /* migrate_pages */
+	.long _sys_openat	/* 295 */
+	.long _sys_mkdirat
+	.long _sys_mknodat
+	.long _sys_fchownat
+	.long _sys_futimesat
+	.long _sys_fstatat64	/* 300 */
+	.long _sys_unlinkat
+	.long _sys_renameat
+	.long _sys_linkat
+	.long _sys_symlinkat
+	.long _sys_readlinkat	/* 305 */
+	.long _sys_fchmodat
+	.long _sys_faccessat
+	.long _sys_pselect6
+	.long _sys_ppoll
+	.long _sys_unshare	/* 310 */
+	.long _sys_sram_alloc
+	.long _sys_sram_free
+	.long _sys_dma_memcpy
+	.long _sys_accept
+	.long _sys_bind		/* 315 */
+	.long _sys_connect
+	.long _sys_getpeername
+	.long _sys_getsockname
+	.long _sys_getsockopt
+	.long _sys_listen	/* 320 */
+	.long _sys_recv
+	.long _sys_recvfrom
+	.long _sys_recvmsg
+	.long _sys_send
+	.long _sys_sendmsg	/* 325 */
+	.long _sys_sendto
+	.long _sys_setsockopt
+	.long _sys_shutdown
+	.long _sys_socket
+	.long _sys_socketpair	/* 330 */
+	.long _sys_semctl
+	.long _sys_semget
+	.long _sys_semop
+	.long _sys_msgctl
+	.long _sys_msgget	/* 335 */
+	.long _sys_msgrcv
+	.long _sys_msgsnd
+	.long _sys_shmat
+	.long _sys_shmctl
+	.long _sys_shmdt	/* 340 */
+	.long _sys_shmget
+	.long _sys_splice
+	.long _sys_sync_file_range
+	.long _sys_tee
+	.long _sys_vmsplice	/* 345 */
+	.long _sys_epoll_pwait
+	.long _sys_utimensat
+	.long _sys_signalfd
+	.long _sys_timerfd_create
+	.long _sys_eventfd	/* 350 */
+	.long _sys_pread64
+	.long _sys_pwrite64
+	.long _sys_fadvise64
+	.long _sys_set_robust_list
+	.long _sys_get_robust_list	/* 355 */
+	.long _sys_fallocate
+	.long _sys_semtimedop
+	.long _sys_timerfd_settime
+	.long _sys_timerfd_gettime
+	.long _sys_signalfd4		/* 360 */
+	.long _sys_eventfd2
+	.long _sys_epoll_create1
+	.long _sys_dup3
+	.long _sys_pipe2
+	.long _sys_inotify_init1	/* 365 */
+	.long _sys_preadv
+	.long _sys_pwritev
+	.long _sys_rt_tgsigqueueinfo
+	.long _sys_perf_event_open
+	.long _sys_recvmmsg		/* 370 */
+	.long _sys_fanotify_init
+	.long _sys_fanotify_mark
+	.long _sys_prlimit64
+	.long _sys_cacheflush
+	.long _sys_name_to_handle_at	/* 375 */
+	.long _sys_open_by_handle_at
+	.long _sys_clock_adjtime
+	.long _sys_syncfs
+	.long _sys_setns
+	.long _sys_sendmmsg		/* 380 */
+	.long _sys_process_vm_readv
+	.long _sys_process_vm_writev
+
+	.rept NR_syscalls-(.-_sys_call_table)/4
+	.long _sys_ni_syscall
+	.endr
+END(_sys_call_table)
diff --git a/arch/blackfin/mach-common/head.S b/arch/blackfin/mach-common/head.S
new file mode 100644
index 00000000..8b4d9885
--- /dev/null
+++ b/arch/blackfin/mach-common/head.S
@@ -0,0 +1,231 @@
+/*
+ * Common Blackfin startup code
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/blackfin.h>
+#include <asm/thread_info.h>
+#include <asm/trace.h>
+#include <asm/asm-offsets.h>
+
+__INIT
+
+ENTRY(__init_clear_bss)
+	r2 = r2 - r1;
+	cc = r2 == 0;
+	if cc jump .L_bss_done;
+	r2 >>= 2;
+	p1 = r1;
+	p2 = r2;
+	lsetup (1f, 1f) lc0 = p2;
+1:	[p1++] = r0;
+.L_bss_done:
+	rts;
+ENDPROC(__init_clear_bss)
+
+ENTRY(__start)
+	/* R0: argument of command line string, passed from uboot, save it */
+	R7 = R0;
+
+	/* Enable Cycle Counter and Nesting Of Interrupts */
+#ifdef CONFIG_BFIN_SCRATCH_REG_CYCLES
+	R0 = SYSCFG_SNEN;
+#else
+	R0 = SYSCFG_SNEN | SYSCFG_CCEN;
+#endif
+	SYSCFG = R0;
+
+	/* Optimization register tricks: keep a base value in the
+	 * reserved P registers so we use the load/store with an
+	 * offset syntax.  R0 = [P5 + <constant>];
+	 *   P5 - core MMR base
+	 *   R6 - 0
+	 */
+	r6 = 0;
+	p5.l = 0;
+	p5.h = hi(COREMMR_BASE);
+
+	/* Zero out registers required by Blackfin ABI */
+
+	/* Disable circular buffers */
+	L0 = r6;
+	L1 = r6;
+	L2 = r6;
+	L3 = r6;
+
+	/* Disable hardware loops in case we were started by 'go' */
+	LC0 = r6;
+	LC1 = r6;
+
+	/*
+	 * Clear ITEST_COMMAND and DTEST_COMMAND registers,
+	 * Leaving these as non-zero can confuse the emulator
+	 */
+	[p5 + (DTEST_COMMAND - COREMMR_BASE)] = r6;
+	[p5 + (ITEST_COMMAND - COREMMR_BASE)] = r6;
+	CSYNC;
+
+	trace_buffer_init(p0,r0);
+
+	/* Turn off the icache */
+	r1 = [p5 + (IMEM_CONTROL - COREMMR_BASE)];
+	BITCLR (r1, ENICPLB_P);
+	[p5 + (IMEM_CONTROL - COREMMR_BASE)] = r1;
+	SSYNC;
+
+	/* Turn off the dcache */
+	r1 = [p5 + (DMEM_CONTROL - COREMMR_BASE)];
+	BITCLR (r1, ENDCPLB_P);
+	[p5 + (DMEM_CONTROL - COREMMR_BASE)] = r1;
+	SSYNC;
+
+	/* in case of double faults, save a few things */
+	p1.l = _initial_pda;
+	p1.h = _initial_pda;
+	r4 = RETX;
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+	/* Only save these if we are storing them,
+	 * This happens here, since L1 gets clobbered
+	 * below
+	 */
+	GET_PDA(p0, r0);
+	r0 = [p0 + PDA_DF_RETX];
+	r1 = [p0 + PDA_DF_DCPLB];
+	r2 = [p0 + PDA_DF_ICPLB];
+	r3 = [p0 + PDA_DF_SEQSTAT];
+	[p1 + PDA_INIT_DF_RETX] = r0;
+	[p1 + PDA_INIT_DF_DCPLB] = r1;
+	[p1 + PDA_INIT_DF_ICPLB] = r2;
+	[p1 + PDA_INIT_DF_SEQSTAT] = r3;
+#endif
+	[p1 + PDA_INIT_RETX] = r4;
+
+	/* Initialize stack pointer */
+	sp.l = _init_thread_union + THREAD_SIZE;
+	sp.h = _init_thread_union + THREAD_SIZE;
+	fp = sp;
+	usp = sp;
+
+#ifdef CONFIG_EARLY_PRINTK
+	call _init_early_exception_vectors;
+	r0 = (EVT_IVHW | EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+	sti r0;
+#endif
+
+	r0 = r6;
+	/* Zero out all of the fun bss regions */
+#if L1_DATA_A_LENGTH > 0
+	r1.l = __sbss_l1;
+	r1.h = __sbss_l1;
+	r2.l = __ebss_l1;
+	r2.h = __ebss_l1;
+	call __init_clear_bss
+#endif
+#if L1_DATA_B_LENGTH > 0
+	r1.l = __sbss_b_l1;
+	r1.h = __sbss_b_l1;
+	r2.l = __ebss_b_l1;
+	r2.h = __ebss_b_l1;
+	call __init_clear_bss
+#endif
+#if L2_LENGTH > 0
+	r1.l = __sbss_l2;
+	r1.h = __sbss_l2;
+	r2.l = __ebss_l2;
+	r2.h = __ebss_l2;
+	call __init_clear_bss
+#endif
+	r1.l = ___bss_start;
+	r1.h = ___bss_start;
+	r2.l = ___bss_stop;
+	r2.h = ___bss_stop;
+	call __init_clear_bss
+
+	/* Put The Code for PLL Programming and SDRAM Programming in L1 ISRAM */
+	call _bfin_relocate_l1_mem;
+
+#ifdef CONFIG_ROMKERNEL
+	call _bfin_relocate_xip_data;
+#endif
+
+#ifdef CONFIG_BFIN_KERNEL_CLOCK
+	/* Only use on-chip scratch space for stack when absolutely required
+	 * to avoid Anomaly 05000227 ... we know the init_clocks() func only
+	 * uses L1 text and stack space and no other memory region.
+	 */
+# define KERNEL_CLOCK_STACK (L1_SCRATCH_START + L1_SCRATCH_LENGTH - 12)
+	sp.l = lo(KERNEL_CLOCK_STACK);
+	sp.h = hi(KERNEL_CLOCK_STACK);
+	call _init_clocks;
+	sp = usp;	/* usp hasn't been touched, so restore from there */
+#endif
+
+	/* This section keeps the processor in supervisor mode
+	 * during kernel boot.  Switches to user mode at end of boot.
+	 * See page 3-9 of Hardware Reference manual for documentation.
+	 */
+
+	/* EVT15 = _real_start */
+
+	p1.l = _real_start;
+	p1.h = _real_start;
+	[p5 + (EVT15 - COREMMR_BASE)] = p1;
+	csync;
+
+#ifdef CONFIG_EARLY_PRINTK
+	r0 = (EVT_IVG15 | EVT_IVHW | EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU) (z);
+#else
+	r0 = EVT_IVG15 (z);
+#endif
+	sti r0;
+
+	raise 15;
+#ifdef CONFIG_EARLY_PRINTK
+	p0.l = _early_trap;
+	p0.h = _early_trap;
+#else
+	p0.l = .LWAIT_HERE;
+	p0.h = .LWAIT_HERE;
+#endif
+	reti = p0;
+#if ANOMALY_05000281
+	nop; nop; nop;
+#endif
+	rti;
+
+.LWAIT_HERE:
+	jump .LWAIT_HERE;
+ENDPROC(__start)
+
+/* A little BF561 glue ... */
+#ifndef WDOG_CTL
+# define WDOG_CTL WDOGA_CTL
+#endif
+
+ENTRY(_real_start)
+	/* Enable nested interrupts */
+	[--sp] = reti;
+
+	/* watchdog off for now */
+	p0.l = lo(WDOG_CTL);
+	p0.h = hi(WDOG_CTL);
+	r0 = 0xAD6(z);
+	w[p0] = r0;
+	ssync;
+
+	/* Pass the u-boot arguments to the global value command line */
+	R0 = R7;
+	call _cmdline_init;
+
+	sp += -12 + 4; /* +4 is for reti loading above */
+	call _init_pda
+	sp += 12;
+	jump.l _start_kernel;
+ENDPROC(_real_start)
+
+__FINIT
diff --git a/arch/blackfin/mach-common/interrupt.S b/arch/blackfin/mach-common/interrupt.S
new file mode 100644
index 00000000..469ce728
--- /dev/null
+++ b/arch/blackfin/mach-common/interrupt.S
@@ -0,0 +1,326 @@
+/*
+ * Interrupt Entries
+ *
+ * Copyright 2005-2009 Analog Devices Inc.
+ *               D. Jeff Dionne <jeff@ryeham.ee.ryerson.ca>
+ *               Kenneth Albanowski <kjahds@kjahds.com>
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <asm/blackfin.h>
+#include <mach/irq.h>
+#include <linux/linkage.h>
+#include <asm/entry.h>
+#include <asm/asm-offsets.h>
+#include <asm/trace.h>
+#include <asm/traps.h>
+#include <asm/thread_info.h>
+
+#include <asm/context.S>
+
+.extern _ret_from_exception
+
+#ifdef CONFIG_I_ENTRY_L1
+.section .l1.text
+#else
+.text
+#endif
+
+.align 4 	/* just in case */
+
+/* Common interrupt entry code.	 First we do CLI, then push
+ * RETI, to keep interrupts disabled, but to allow this state to be changed
+ * by local_bh_enable.
+ * R0 contains the interrupt number, while R1 may contain the value of IPEND,
+ * or garbage if IPEND won't be needed by the ISR.  */
+__common_int_entry:
+	[--sp] = fp;
+	[--sp] = usp;
+
+	[--sp] = i0;
+	[--sp] = i1;
+	[--sp] = i2;
+	[--sp] = i3;
+
+	[--sp] = m0;
+	[--sp] = m1;
+	[--sp] = m2;
+	[--sp] = m3;
+
+	[--sp] = l0;
+	[--sp] = l1;
+	[--sp] = l2;
+	[--sp] = l3;
+
+	[--sp] = b0;
+	[--sp] = b1;
+	[--sp] = b2;
+	[--sp] = b3;
+	[--sp] = a0.x;
+	[--sp] = a0.w;
+	[--sp] = a1.x;
+	[--sp] = a1.w;
+
+	[--sp] = LC0;
+	[--sp] = LC1;
+	[--sp] = LT0;
+	[--sp] = LT1;
+	[--sp] = LB0;
+	[--sp] = LB1;
+
+	[--sp] = ASTAT;
+
+	[--sp] = r0;	/* Skip reserved */
+	[--sp] = RETS;
+	r2 = RETI;
+	[--sp] = r2;
+	[--sp] = RETX;
+	[--sp] = RETN;
+	[--sp] = RETE;
+	[--sp] = SEQSTAT;
+	[--sp] = r1;	/* IPEND - R1 may or may not be set up before jumping here. */
+
+	/* Switch to other method of keeping interrupts disabled.  */
+#ifdef CONFIG_DEBUG_HWERR
+	r1 = 0x3f;
+	sti r1;
+#else
+	cli r1;
+#endif
+#ifdef CONFIG_TRACE_IRQFLAGS
+	[--sp] = r0;
+	sp += -12;
+	call _trace_hardirqs_off;
+	sp += 12;
+	r0 = [sp++];
+#endif
+	[--sp] = RETI;  /* orig_pc */
+	/* Clear all L registers.  */
+	r1 = 0 (x);
+	l0 = r1;
+	l1 = r1;
+	l2 = r1;
+	l3 = r1;
+#ifdef CONFIG_FRAME_POINTER
+	fp = 0;
+#endif
+
+	ANOMALY_283_315_WORKAROUND(p5, r7)
+
+	r1 =  sp;
+	SP += -12;
+#ifdef CONFIG_IPIPE
+	call ___ipipe_grab_irq
+	SP += 12;
+	cc = r0 == 0;
+	if cc jump .Lcommon_restore_context;
+#else /* CONFIG_IPIPE */
+
+#ifdef CONFIG_PREEMPT
+	r7 = sp;
+	r4.l = lo(ALIGN_PAGE_MASK);
+	r4.h = hi(ALIGN_PAGE_MASK);
+	r7 = r7 & r4;
+	p5 = r7;
+	r7 = [p5 + TI_PREEMPT]; /* get preempt count */
+	r7 += 1;                /* increment it */
+	[p5 + TI_PREEMPT] = r7;
+#endif
+	pseudo_long_call _do_irq, p2;
+
+#ifdef CONFIG_PREEMPT
+	r7 += -1;
+	[p5 + TI_PREEMPT] = r7; /* restore preempt count */
+#endif
+
+	SP += 12;
+#endif /* CONFIG_IPIPE */
+	pseudo_long_call _return_from_int, p2;
+.Lcommon_restore_context:
+	RESTORE_CONTEXT
+	rti;
+
+/* interrupt routine for ivhw - 5 */
+ENTRY(_evt_ivhw)
+	/* In case a single action kicks off multiple memory transactions, (like
+	 * a cache line fetch, - this can cause multiple hardware errors, let's
+	 * catch them all. First - make sure all the actions are complete, and
+	 * the core sees the hardware errors.
+	 */
+	SSYNC;
+	SSYNC;
+
+	SAVE_ALL_SYS
+#ifdef CONFIG_FRAME_POINTER
+	fp = 0;
+#endif
+
+	ANOMALY_283_315_WORKAROUND(p5, r7)
+
+	/* Handle all stacked hardware errors
+	 * To make sure we don't hang forever, only do it 10 times
+	 */
+	R0 = 0;
+	R2 = 10;
+1:
+	P0.L = LO(ILAT);
+	P0.H = HI(ILAT);
+	R1 = [P0];
+	CC = BITTST(R1, EVT_IVHW_P);
+	IF ! CC JUMP 2f;
+	/* OK a hardware error is pending - clear it */
+	R1 = EVT_IVHW_P;
+	[P0] = R1;
+	R0 += 1;
+	CC = R1 == R2;
+	if CC JUMP 2f;
+	JUMP 1b;
+2:
+	# We are going to dump something out, so make sure we print IPEND properly
+	p2.l = lo(IPEND);
+	p2.h = hi(IPEND);
+	r0 = [p2];
+	[sp + PT_IPEND] = r0;
+
+	/* set the EXCAUSE to HWERR for trap_c */
+	r0 = [sp + PT_SEQSTAT];
+	R1.L = LO(VEC_HWERR);
+	R1.H = HI(VEC_HWERR);
+	R0 = R0 | R1;
+	[sp + PT_SEQSTAT] = R0;
+
+	r0 = sp;        /* stack frame pt_regs pointer argument ==> r0 */
+	SP += -12;
+	pseudo_long_call _trap_c, p5;
+	SP += 12;
+
+#ifdef EBIU_ERRMST
+	/* make sure EBIU_ERRMST is clear */
+	p0.l = LO(EBIU_ERRMST);
+	p0.h = HI(EBIU_ERRMST);
+	r0.l = (CORE_ERROR | CORE_MERROR);
+	w[p0] = r0.l;
+#endif
+
+	pseudo_long_call _ret_from_exception, p2;
+
+.Lcommon_restore_all_sys:
+	RESTORE_ALL_SYS
+	rti;
+ENDPROC(_evt_ivhw)
+
+/* Interrupt routine for evt2 (NMI).
+ * For inner circle type details, please see:
+ * http://docs.blackfin.uclinux.org/doku.php?id=linux-kernel:nmi
+ */
+ENTRY(_evt_nmi)
+#ifndef CONFIG_NMI_WATCHDOG
+.weak _evt_nmi
+#else
+	/* Not take account of CPLBs, this handler will not return */
+	SAVE_ALL_SYS
+	r0 = sp;
+	r1 = retn;
+	[sp + PT_PC] = r1;
+	trace_buffer_save(p4,r5);
+
+	ANOMALY_283_315_WORKAROUND(p4, r5)
+
+	SP += -12;
+	call _do_nmi;
+	SP += 12;
+1:
+	jump 1b;
+#endif
+	rtn;
+ENDPROC(_evt_nmi)
+
+/* interrupt routine for core timer - 6 */
+ENTRY(_evt_timer)
+	TIMER_INTERRUPT_ENTRY(EVT_IVTMR_P)
+
+/* interrupt routine for evt7 - 7 */
+ENTRY(_evt_evt7)
+	INTERRUPT_ENTRY(EVT_IVG7_P)
+ENTRY(_evt_evt8)
+	INTERRUPT_ENTRY(EVT_IVG8_P)
+ENTRY(_evt_evt9)
+	INTERRUPT_ENTRY(EVT_IVG9_P)
+ENTRY(_evt_evt10)
+	INTERRUPT_ENTRY(EVT_IVG10_P)
+ENTRY(_evt_evt11)
+	INTERRUPT_ENTRY(EVT_IVG11_P)
+ENTRY(_evt_evt12)
+	INTERRUPT_ENTRY(EVT_IVG12_P)
+ENTRY(_evt_evt13)
+	INTERRUPT_ENTRY(EVT_IVG13_P)
+
+
+ /* interrupt routine for system_call - 15 */
+ENTRY(_evt_system_call)
+	SAVE_CONTEXT_SYSCALL
+#ifdef CONFIG_FRAME_POINTER
+	fp = 0;
+#endif
+	pseudo_long_call _system_call, p2;
+	jump .Lcommon_restore_context;
+ENDPROC(_evt_system_call)
+
+#ifdef CONFIG_IPIPE
+/*
+ * __ipipe_call_irqtail: lowers the current priority level to EVT15
+ * before running a user-defined routine, then raises the priority
+ * level to EVT14 to prepare the caller for a normal interrupt
+ * return through RTI.
+ *
+ * We currently use this feature in two occasions:
+ *
+ * - before branching to __ipipe_irq_tail_hook as requested by a high
+ *   priority domain after the pipeline delivered an interrupt,
+ *   e.g. such as Xenomai, in order to start its rescheduling
+ *   procedure, since we may not switch tasks when IRQ levels are
+ *   nested on the Blackfin, so we have to fake an interrupt return
+ *   so that we may reschedule immediately.
+ *
+ * - before branching to __ipipe_sync_root(), in order to play any interrupt
+ *   pending for the root domain (i.e. the Linux kernel). This lowers
+ *   the core priority level enough so that Linux IRQ handlers may
+ *   never delay interrupts handled by high priority domains; we defer
+ *   those handlers until this point instead. This is a substitute
+ *   to using a threaded interrupt model for the Linux kernel.
+ *
+ * r0: address of user-defined routine
+ * context: caller must have preempted EVT15, hw interrupts must be off.
+ */
+ENTRY(___ipipe_call_irqtail)
+	p0 = r0;
+	r0.l = 1f;
+	r0.h = 1f;
+	reti = r0;
+	rti;
+1:
+	[--sp] = rets;
+	[--sp] = ( r7:4, p5:3 );
+	sp += -12;
+	call (p0);
+	sp += 12;
+	( r7:4, p5:3 ) = [sp++];
+	rets = [sp++];
+
+#ifdef CONFIG_DEBUG_HWERR
+	/* enable irq14 & hwerr interrupt, until we transition to _evt_evt14 */
+	r0 = (EVT_IVG14 | EVT_IVHW | \
+		EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+#else
+	/* Only enable irq14 interrupt, until we transition to _evt_evt14 */
+	r0 = (EVT_IVG14 | \
+		EVT_IRPTEN | EVT_EVX | EVT_NMI | EVT_RST | EVT_EMU);
+#endif
+	sti r0;
+	raise 14;		/* Branches to _evt_evt14 */
+2:
+	jump 2b;                /* Likely paranoid. */
+ENDPROC(___ipipe_call_irqtail)
+
+#endif /* CONFIG_IPIPE */
diff --git a/arch/blackfin/mach-common/ints-priority.c b/arch/blackfin/mach-common/ints-priority.c
new file mode 100644
index 00000000..332dace6
--- /dev/null
+++ b/arch/blackfin/mach-common/ints-priority.c
@@ -0,0 +1,1268 @@
+/*
+ * Set up the interrupt priorities
+ *
+ * Copyright  2004-2009 Analog Devices Inc.
+ *                 2003 Bas Vermeulen <bas@buyways.nl>
+ *                 2002 Arcturus Networks Inc. MaTed <mated@sympatico.ca>
+ *            2000-2001 Lineo, Inc. D. Jefff Dionne <jeff@lineo.ca>
+ *                 1999 D. Jeff Dionne <jeff@uclinux.org>
+ *                 1996 Roman Zippel
+ *
+ * Licensed under the GPL-2
+ */
+
+#include <linux/module.h>
+#include <linux/kernel_stat.h>
+#include <linux/seq_file.h>
+#include <linux/irq.h>
+#include <linux/sched.h>
+#ifdef CONFIG_IPIPE
+#include <linux/ipipe.h>
+#endif
+#include <asm/traps.h>
+#include <asm/blackfin.h>
+#include <asm/gpio.h>
+#include <asm/irq_handler.h>
+#include <asm/dpmc.h>
+
+#define SIC_SYSIRQ(irq)	(irq - (IRQ_CORETMR + 1))
+
+/*
+ * NOTES:
+ * - we have separated the physical Hardware interrupt from the
+ * levels that the LINUX kernel sees (see the description in irq.h)
+ * -
+ */
+
+#ifndef CONFIG_SMP
+/* Initialize this to an actual value to force it into the .data
+ * section so that we know it is properly initialized at entry into
+ * the kernel but before bss is initialized to zero (which is where
+ * it would live otherwise).  The 0x1f magic represents the IRQs we
+ * cannot actually mask out in hardware.
+ */
+unsigned long bfin_irq_flags = 0x1f;
+EXPORT_SYMBOL(bfin_irq_flags);
+#endif
+
+#ifdef CONFIG_PM
+unsigned long bfin_sic_iwr[3];	/* Up to 3 SIC_IWRx registers */
+unsigned vr_wakeup;
+#endif
+
+static struct ivgx {
+	/* irq number for request_irq, available in mach-bf5xx/irq.h */
+	unsigned int irqno;
+	/* corresponding bit in the SIC_ISR register */
+	unsigned int isrflag;
+} ivg_table[NR_PERI_INTS];
+
+static struct ivg_slice {
+	/* position of first irq in ivg_table for given ivg */
+	struct ivgx *ifirst;
+	struct ivgx *istop;
+} ivg7_13[IVG13 - IVG7 + 1];
+
+
+/*
+ * Search SIC_IAR and fill tables with the irqvalues
+ * and their positions in the SIC_ISR register.
+ */
+static void __init search_IAR(void)
+{
+	unsigned ivg, irq_pos = 0;
+	for (ivg = 0; ivg <= IVG13 - IVG7; ivg++) {
+		int irqN;
+
+		ivg7_13[ivg].istop = ivg7_13[ivg].ifirst = &ivg_table[irq_pos];
+
+		for (irqN = 0; irqN < NR_PERI_INTS; irqN += 4) {
+			int irqn;
+			u32 iar = bfin_read32((unsigned long *)SIC_IAR0 +
+#if defined(CONFIG_BF51x) || defined(CONFIG_BF52x) || \
+	defined(CONFIG_BF538) || defined(CONFIG_BF539)
+				((irqN % 32) >> 3) + ((irqN / 32) * ((SIC_IAR4 - SIC_IAR0) / 4))
+#else
+				(irqN >> 3)
+#endif
+				);
+
+			for (irqn = irqN; irqn < irqN + 4; ++irqn) {
+				int iar_shift = (irqn & 7) * 4;
+				if (ivg == (0xf & (iar >> iar_shift))) {
+					ivg_table[irq_pos].irqno = IVG7 + irqn;
+					ivg_table[irq_pos].isrflag = 1 << (irqn % 32);
+					ivg7_13[ivg].istop++;
+					irq_pos++;
+				}
+			}
+		}
+	}
+}
+
+/*
+ * This is for core internal IRQs
+ */
+
+void bfin_ack_noop(struct irq_data *d)
+{
+	/* Dummy function.  */
+}
+
+static void bfin_core_mask_irq(struct irq_data *d)
+{
+	bfin_irq_flags &= ~(1 << d->irq);
+	if (!hard_irqs_disabled())
+		hard_local_irq_enable();
+}
+
+static void bfin_core_unmask_irq(struct irq_data *d)
+{
+	bfin_irq_flags |= 1 << d->irq;
+	/*
+	 * If interrupts are enabled, IMASK must contain the same value
+	 * as bfin_irq_flags.  Make sure that invariant holds.  If interrupts
+	 * are currently disabled we need not do anything; one of the
+	 * callers will take care of setting IMASK to the proper value
+	 * when reenabling interrupts.
+	 * local_irq_enable just does "STI bfin_irq_flags", so it's exactly
+	 * what we need.
+	 */
+	if (!hard_irqs_disabled())
+		hard_local_irq_enable();
+	return;
+}
+
+void bfin_internal_mask_irq(unsigned int irq)
+{
+	unsigned long flags = hard_local_irq_save();
+
+#ifdef SIC_IMASK0
+	unsigned mask_bank = SIC_SYSIRQ(irq) / 32;
+	unsigned mask_bit = SIC_SYSIRQ(irq) % 32;
+	bfin_write_SIC_IMASK(mask_bank, bfin_read_SIC_IMASK(mask_bank) &
+			     ~(1 << mask_bit));
+# ifdef CONFIG_SMP
+	bfin_write_SICB_IMASK(mask_bank, bfin_read_SICB_IMASK(mask_bank) &
+			     ~(1 << mask_bit));
+# endif
+#else
+	bfin_write_SIC_IMASK(bfin_read_SIC_IMASK() &
+			     ~(1 << SIC_SYSIRQ(irq)));
+#endif
+
+	hard_local_irq_restore(flags);
+}
+
+static void bfin_internal_mask_irq_chip(struct irq_data *d)
+{
+	bfin_internal_mask_irq(d->irq);
+}
+
+#ifdef CONFIG_SMP
+static void bfin_internal_unmask_irq_affinity(unsigned int irq,
+		const struct cpumask *affinity)
+#else
+void bfin_internal_unmask_irq(unsigned int irq)
+#endif
+{
+	unsigned long flags = hard_local_irq_save();
+
+#ifdef SIC_IMASK0
+	unsigned mask_bank = SIC_SYSIRQ(irq) / 32;
+	unsigned mask_bit = SIC_SYSIRQ(irq) % 32;
+# ifdef CONFIG_SMP
+	if (cpumask_test_cpu(0, affinity))
+# endif
+		bfin_write_SIC_IMASK(mask_bank,
+			bfin_read_SIC_IMASK(mask_bank) |
+			(1 << mask_bit));
+# ifdef CONFIG_SMP
+	if (cpumask_test_cpu(1, affinity))
+		bfin_write_SICB_IMASK(mask_bank,
+			bfin_read_SICB_IMASK(mask_bank) |
+			(1 << mask_bit));
+# endif
+#else
+	bfin_write_SIC_IMASK(bfin_read_SIC_IMASK() |
+			     (1 << SIC_SYSIRQ(irq)));
+#endif
+
+	hard_local_irq_restore(flags);
+}
+
+#ifdef CONFIG_SMP
+static void bfin_internal_unmask_irq_chip(struct irq_data *d)
+{
+	bfin_internal_unmask_irq_affinity(d->irq, d->affinity);
+}
+
+static int bfin_internal_set_affinity(struct irq_data *d,
+				      const struct cpumask *mask, bool force)
+{
+	bfin_internal_mask_irq(d->irq);
+	bfin_internal_unmask_irq_affinity(d->irq, mask);
+
+	return 0;
+}
+#else
+static void bfin_internal_unmask_irq_chip(struct irq_data *d)
+{
+	bfin_internal_unmask_irq(d->irq);
+}
+#endif
+
+#ifdef CONFIG_PM
+int bfin_internal_set_wake(unsigned int irq, unsigned int state)
+{
+	u32 bank, bit, wakeup = 0;
+	unsigned long flags;
+	bank = SIC_SYSIRQ(irq) / 32;
+	bit = SIC_SYSIRQ(irq) % 32;
+
+	switch (irq) {
+#ifdef IRQ_RTC
+	case IRQ_RTC:
+	wakeup |= WAKE;
+	break;
+#endif
+#ifdef IRQ_CAN0_RX
+	case IRQ_CAN0_RX:
+	wakeup |= CANWE;
+	break;
+#endif
+#ifdef IRQ_CAN1_RX
+	case IRQ_CAN1_RX:
+	wakeup |= CANWE;
+	break;
+#endif
+#ifdef IRQ_USB_INT0
+	case IRQ_USB_INT0:
+	wakeup |= USBWE;
+	break;
+#endif
+#ifdef CONFIG_BF54x
+	case IRQ_CNT:
+	wakeup |= ROTWE;
+	break;
+#endif
+	default:
+	break;
+	}
+
+	flags = hard_local_irq_save();
+
+	if (state) {
+		bfin_sic_iwr[bank] |= (1 << bit);
+		vr_wakeup  |= wakeup;
+
+	} else {
+		bfin_sic_iwr[bank] &= ~(1 << bit);
+		vr_wakeup  &= ~wakeup;
+	}
+
+	hard_local_irq_restore(flags);
+
+	return 0;
+}
+
+static int bfin_internal_set_wake_chip(struct irq_data *d, unsigned int state)
+{
+	return bfin_internal_set_wake(d->irq, state);
+}
+#else
+# define bfin_internal_set_wake_chip NULL
+#endif
+
+static struct irq_chip bfin_core_irqchip = {
+	.name = "CORE",
+	.irq_ack = bfin_ack_noop,
+	.irq_mask = bfin_core_mask_irq,
+	.irq_unmask = bfin_core_unmask_irq,
+};
+
+static struct irq_chip bfin_internal_irqchip = {
+	.name = "INTN",
+	.irq_ack = bfin_ack_noop,
+	.irq_mask = bfin_internal_mask_irq_chip,
+	.irq_unmask = bfin_internal_unmask_irq_chip,
+	.irq_mask_ack = bfin_internal_mask_irq_chip,
+	.irq_disable = bfin_internal_mask_irq_chip,
+	.irq_enable = bfin_internal_unmask_irq_chip,
+#ifdef CONFIG_SMP
+	.irq_set_affinity = bfin_internal_set_affinity,
+#endif
+	.irq_set_wake = bfin_internal_set_wake_chip,
+};
+
+void bfin_handle_irq(unsigned irq)
+{
+#ifdef CONFIG_IPIPE
+	struct pt_regs regs;    /* Contents not used. */
+	ipipe_trace_irq_entry(irq);
+	__ipipe_handle_irq(irq, &regs);
+	ipipe_trace_irq_exit(irq);
+#else /* !CONFIG_IPIPE */
+	generic_handle_irq(irq);
+#endif  /* !CONFIG_IPIPE */
+}
+
+#if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
+static int mac_stat_int_mask;
+
+static void bfin_mac_status_ack_irq(unsigned int irq)
+{
+	switch (irq) {
+	case IRQ_MAC_MMCINT:
+		bfin_write_EMAC_MMC_TIRQS(
+			bfin_read_EMAC_MMC_TIRQE() &
+			bfin_read_EMAC_MMC_TIRQS());
+		bfin_write_EMAC_MMC_RIRQS(
+			bfin_read_EMAC_MMC_RIRQE() &
+			bfin_read_EMAC_MMC_RIRQS());
+		break;
+	case IRQ_MAC_RXFSINT:
+		bfin_write_EMAC_RX_STKY(
+			bfin_read_EMAC_RX_IRQE() &
+			bfin_read_EMAC_RX_STKY());
+		break;
+	case IRQ_MAC_TXFSINT:
+		bfin_write_EMAC_TX_STKY(
+			bfin_read_EMAC_TX_IRQE() &
+			bfin_read_EMAC_TX_STKY());
+		break;
+	case IRQ_MAC_WAKEDET:
+		 bfin_write_EMAC_WKUP_CTL(
+			bfin_read_EMAC_WKUP_CTL() | MPKS | RWKS);
+		break;
+	default:
+		/* These bits are W1C */
+		bfin_write_EMAC_SYSTAT(1L << (irq - IRQ_MAC_PHYINT));
+		break;
+	}
+}
+
+static void bfin_mac_status_mask_irq(struct irq_data *d)
+{
+	unsigned int irq = d->irq;
+
+	mac_stat_int_mask &= ~(1L << (irq - IRQ_MAC_PHYINT));
+#ifdef BF537_FAMILY
+	switch (irq) {
+	case IRQ_MAC_PHYINT:
+		bfin_write_EMAC_SYSCTL(bfin_read_EMAC_SYSCTL() & ~PHYIE);
+		break;
+	default:
+		break;
+	}
+#else
+	if (!mac_stat_int_mask)
+		bfin_internal_mask_irq(IRQ_MAC_ERROR);
+#endif
+	bfin_mac_status_ack_irq(irq);
+}
+
+static void bfin_mac_status_unmask_irq(struct irq_data *d)
+{
+	unsigned int irq = d->irq;
+
+#ifdef BF537_FAMILY
+	switch (irq) {
+	case IRQ_MAC_PHYINT:
+		bfin_write_EMAC_SYSCTL(bfin_read_EMAC_SYSCTL() | PHYIE);
+		break;
+	default:
+		break;
+	}
+#else
+	if (!mac_stat_int_mask)
+		bfin_internal_unmask_irq(IRQ_MAC_ERROR);
+#endif
+	mac_stat_int_mask |= 1L << (irq - IRQ_MAC_PHYINT);
+}
+
+#ifdef CONFIG_PM
+int bfin_mac_status_set_wake(struct irq_data *d, unsigned int state)
+{
+#ifdef BF537_FAMILY
+	return bfin_internal_set_wake(IRQ_GENERIC_ERROR, state);
+#else
+	return bfin_internal_set_wake(IRQ_MAC_ERROR, state);
+#endif
+}
+#else
+# define bfin_mac_status_set_wake NULL
+#endif
+
+static struct irq_chip bfin_mac_status_irqchip = {
+	.name = "MACST",
+	.irq_ack = bfin_ack_noop,
+	.irq_mask_ack = bfin_mac_status_mask_irq,
+	.irq_mask = bfin_mac_status_mask_irq,
+	.irq_unmask = bfin_mac_status_unmask_irq,
+	.irq_set_wake = bfin_mac_status_set_wake,
+};
+
+void bfin_demux_mac_status_irq(unsigned int int_err_irq,
+			       struct irq_desc *inta_desc)
+{
+	int i, irq = 0;
+	u32 status = bfin_read_EMAC_SYSTAT();
+
+	for (i = 0; i <= (IRQ_MAC_STMDONE - IRQ_MAC_PHYINT); i++)
+		if (status & (1L << i)) {
+			irq = IRQ_MAC_PHYINT + i;
+			break;
+		}
+
+	if (irq) {
+		if (mac_stat_int_mask & (1L << (irq - IRQ_MAC_PHYINT))) {
+			bfin_handle_irq(irq);
+		} else {
+			bfin_mac_status_ack_irq(irq);
+			pr_debug("IRQ %d:"
+				 " MASKED MAC ERROR INTERRUPT ASSERTED\n",
+				 irq);
+		}
+	} else
+		printk(KERN_ERR
+		       "%s : %s : LINE %d :\nIRQ ?: MAC ERROR"
+		       " INTERRUPT ASSERTED BUT NO SOURCE FOUND"
+		       "(EMAC_SYSTAT=0x%X)\n",
+		       __func__, __FILE__, __LINE__, status);
+}
+#endif
+
+static inline void bfin_set_irq_handler(unsigned irq, irq_flow_handler_t handle)
+{
+#ifdef CONFIG_IPIPE
+	handle = handle_level_irq;
+#endif
+	__irq_set_handler_locked(irq, handle);
+}
+
+static DECLARE_BITMAP(gpio_enabled, MAX_BLACKFIN_GPIOS);
+extern void bfin_gpio_irq_prepare(unsigned gpio);
+
+#if !BFIN_GPIO_PINT
+
+static void bfin_gpio_ack_irq(struct irq_data *d)
+{
+	/* AFAIK ack_irq in case mask_ack is provided
+	 * get's only called for edge sense irqs
+	 */
+	set_gpio_data(irq_to_gpio(d->irq), 0);
+}
+
+static void bfin_gpio_mask_ack_irq(struct irq_data *d)
+{
+	unsigned int irq = d->irq;
+	u32 gpionr = irq_to_gpio(irq);
+
+	if (!irqd_is_level_type(d))
+		set_gpio_data(gpionr, 0);
+
+	set_gpio_maska(gpionr, 0);
+}
+
+static void bfin_gpio_mask_irq(struct irq_data *d)
+{
+	set_gpio_maska(irq_to_gpio(d->irq), 0);
+}
+
+static void bfin_gpio_unmask_irq(struct irq_data *d)
+{
+	set_gpio_maska(irq_to_gpio(d->irq), 1);
+}
+
+static unsigned int bfin_gpio_irq_startup(struct irq_data *d)
+{
+	u32 gpionr = irq_to_gpio(d->irq);
+
+	if (__test_and_set_bit(gpionr, gpio_enabled))
+		bfin_gpio_irq_prepare(gpionr);
+
+	bfin_gpio_unmask_irq(d);
+
+	return 0;
+}
+
+static void bfin_gpio_irq_shutdown(struct irq_data *d)
+{
+	u32 gpionr = irq_to_gpio(d->irq);
+
+	bfin_gpio_mask_irq(d);
+	__clear_bit(gpionr, gpio_enabled);
+	bfin_gpio_irq_free(gpionr);
+}
+
+static int bfin_gpio_irq_type(struct irq_data *d, unsigned int type)
+{
+	unsigned int irq = d->irq;
+	int ret;
+	char buf[16];
+	u32 gpionr = irq_to_gpio(irq);
+
+	if (type == IRQ_TYPE_PROBE) {
+		/* only probe unenabled GPIO interrupt lines */
+		if (test_bit(gpionr, gpio_enabled))
+			return 0;
+		type = IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING;
+	}
+
+	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING |
+		    IRQ_TYPE_LEVEL_HIGH | IRQ_TYPE_LEVEL_LOW)) {
+
+		snprintf(buf, 16, "gpio-irq%d", irq);
+		ret = bfin_gpio_irq_request(gpionr, buf);
+		if (ret)
+			return ret;
+
+		if (__test_and_set_bit(gpionr, gpio_enabled))
+			bfin_gpio_irq_prepare(gpionr);
+
+	} else {
+		__clear_bit(gpionr, gpio_enabled);
+		return 0;
+	}
+
+	set_gpio_inen(gpionr, 0);
+	set_gpio_dir(gpionr, 0);
+
+	if ((type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING))
+	    == (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING))
+		set_gpio_both(gpionr, 1);
+	else
+		set_gpio_both(gpionr, 0);
+
+	if ((type & (IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_LEVEL_LOW)))
+		set_gpio_polar(gpionr, 1);	/* low or falling edge denoted by one */
+	else
+		set_gpio_polar(gpionr, 0);	/* high or rising edge denoted by zero */
+
+	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING)) {
+		set_gpio_edge(gpionr, 1);
+		set_gpio_inen(gpionr, 1);
+		set_gpio_data(gpionr, 0);
+
+	} else {
+		set_gpio_edge(gpionr, 0);
+		set_gpio_inen(gpionr, 1);
+	}
+
+	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING))
+		bfin_set_irq_handler(irq, handle_edge_irq);
+	else
+		bfin_set_irq_handler(irq, handle_level_irq);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int bfin_gpio_set_wake(struct irq_data *d, unsigned int state)
+{
+	return gpio_pm_wakeup_ctrl(irq_to_gpio(d->irq), state);
+}
+#else
+# define bfin_gpio_set_wake NULL
+#endif
+
+static void bfin_demux_gpio_block(unsigned int irq)
+{
+	unsigned int gpio, mask;
+
+	gpio = irq_to_gpio(irq);
+	mask = get_gpiop_data(gpio) & get_gpiop_maska(gpio);
+
+	while (mask) {
+		if (mask & 1)
+			bfin_handle_irq(irq);
+		irq++;
+		mask >>= 1;
+	}
+}
+
+void bfin_demux_gpio_irq(unsigned int inta_irq,
+			 struct irq_desc *desc)
+{
+	unsigned int irq;
+
+	switch (inta_irq) {
+#if defined(BF537_FAMILY)
+	case IRQ_PF_INTA_PG_INTA:
+		bfin_demux_gpio_block(IRQ_PF0);
+		irq = IRQ_PG0;
+		break;
+	case IRQ_PH_INTA_MAC_RX:
+		irq = IRQ_PH0;
+		break;
+#elif defined(BF533_FAMILY)
+	case IRQ_PROG_INTA:
+		irq = IRQ_PF0;
+		break;
+#elif defined(BF538_FAMILY)
+	case IRQ_PORTF_INTA:
+		irq = IRQ_PF0;
+		break;
+#elif defined(CONFIG_BF52x) || defined(CONFIG_BF51x)
+	case IRQ_PORTF_INTA:
+		irq = IRQ_PF0;
+		break;
+	case IRQ_PORTG_INTA:
+		irq = IRQ_PG0;
+		break;
+	case IRQ_PORTH_INTA:
+		irq = IRQ_PH0;
+		break;
+#elif defined(CONFIG_BF561)
+	case IRQ_PROG0_INTA:
+		irq = IRQ_PF0;
+		break;
+	case IRQ_PROG1_INTA:
+		irq = IRQ_PF16;
+		break;
+	case IRQ_PROG2_INTA:
+		irq = IRQ_PF32;
+		break;
+#endif
+	default:
+		BUG();
+		return;
+	}
+
+	bfin_demux_gpio_block(irq);
+}
+
+#else
+
+#define NR_PINT_SYS_IRQS	4
+#define NR_PINT_BITS		32
+#define NR_PINTS		160
+#define IRQ_NOT_AVAIL		0xFF
+
+#define PINT_2_BANK(x)		((x) >> 5)
+#define PINT_2_BIT(x)		((x) & 0x1F)
+#define PINT_BIT(x)		(1 << (PINT_2_BIT(x)))
+
+static unsigned char irq2pint_lut[NR_PINTS];
+static unsigned char pint2irq_lut[NR_PINT_SYS_IRQS * NR_PINT_BITS];
+
+static struct bfin_pint_regs * const pint[NR_PINT_SYS_IRQS] = {
+	(struct bfin_pint_regs *)PINT0_MASK_SET,
+	(struct bfin_pint_regs *)PINT1_MASK_SET,
+	(struct bfin_pint_regs *)PINT2_MASK_SET,
+	(struct bfin_pint_regs *)PINT3_MASK_SET,
+};
+
+inline unsigned int get_irq_base(u32 bank, u8 bmap)
+{
+	unsigned int irq_base;
+
+	if (bank < 2) {		/*PA-PB */
+		irq_base = IRQ_PA0 + bmap * 16;
+	} else {		/*PC-PJ */
+		irq_base = IRQ_PC0 + bmap * 16;
+	}
+
+	return irq_base;
+}
+
+	/* Whenever PINTx_ASSIGN is altered init_pint_lut() must be executed! */
+void init_pint_lut(void)
+{
+	u16 bank, bit, irq_base, bit_pos;
+	u32 pint_assign;
+	u8 bmap;
+
+	memset(irq2pint_lut, IRQ_NOT_AVAIL, sizeof(irq2pint_lut));
+
+	for (bank = 0; bank < NR_PINT_SYS_IRQS; bank++) {
+
+		pint_assign = pint[bank]->assign;
+
+		for (bit = 0; bit < NR_PINT_BITS; bit++) {
+
+			bmap = (pint_assign >> ((bit / 8) * 8)) & 0xFF;
+
+			irq_base = get_irq_base(bank, bmap);
+
+			irq_base += (bit % 8) + ((bit / 8) & 1 ? 8 : 0);
+			bit_pos = bit + bank * NR_PINT_BITS;
+
+			pint2irq_lut[bit_pos] = irq_base - SYS_IRQS;
+			irq2pint_lut[irq_base - SYS_IRQS] = bit_pos;
+		}
+	}
+}
+
+static void bfin_gpio_ack_irq(struct irq_data *d)
+{
+	u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
+	u32 pintbit = PINT_BIT(pint_val);
+	u32 bank = PINT_2_BANK(pint_val);
+
+	if (irqd_get_trigger_type(d) == IRQ_TYPE_EDGE_BOTH) {
+		if (pint[bank]->invert_set & pintbit)
+			pint[bank]->invert_clear = pintbit;
+		else
+			pint[bank]->invert_set = pintbit;
+	}
+	pint[bank]->request = pintbit;
+
+}
+
+static void bfin_gpio_mask_ack_irq(struct irq_data *d)
+{
+	u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
+	u32 pintbit = PINT_BIT(pint_val);
+	u32 bank = PINT_2_BANK(pint_val);
+
+	if (irqd_get_trigger_type(d) == IRQ_TYPE_EDGE_BOTH) {
+		if (pint[bank]->invert_set & pintbit)
+			pint[bank]->invert_clear = pintbit;
+		else
+			pint[bank]->invert_set = pintbit;
+	}
+
+	pint[bank]->request = pintbit;
+	pint[bank]->mask_clear = pintbit;
+}
+
+static void bfin_gpio_mask_irq(struct irq_data *d)
+{
+	u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
+
+	pint[PINT_2_BANK(pint_val)]->mask_clear = PINT_BIT(pint_val);
+}
+
+static void bfin_gpio_unmask_irq(struct irq_data *d)
+{
+	u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
+	u32 pintbit = PINT_BIT(pint_val);
+	u32 bank = PINT_2_BANK(pint_val);
+
+	pint[bank]->mask_set = pintbit;
+}
+
+static unsigned int bfin_gpio_irq_startup(struct irq_data *d)
+{
+	unsigned int irq = d->irq;
+	u32 gpionr = irq_to_gpio(irq);
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
+
+	if (pint_val == IRQ_NOT_AVAIL) {
+		printk(KERN_ERR
+		"GPIO IRQ %d :Not in PINT Assign table "
+		"Reconfigure Interrupt to Port Assignemt\n", irq);
+		return -ENODEV;
+	}
+
+	if (__test_and_set_bit(gpionr, gpio_enabled))
+		bfin_gpio_irq_prepare(gpionr);
+
+	bfin_gpio_unmask_irq(d);
+
+	return 0;
+}
+
+static void bfin_gpio_irq_shutdown(struct irq_data *d)
+{
+	u32 gpionr = irq_to_gpio(d->irq);
+
+	bfin_gpio_mask_irq(d);
+	__clear_bit(gpionr, gpio_enabled);
+	bfin_gpio_irq_free(gpionr);
+}
+
+static int bfin_gpio_irq_type(struct irq_data *d, unsigned int type)
+{
+	unsigned int irq = d->irq;
+	int ret;
+	char buf[16];
+	u32 gpionr = irq_to_gpio(irq);
+	u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
+	u32 pintbit = PINT_BIT(pint_val);
+	u32 bank = PINT_2_BANK(pint_val);
+
+	if (pint_val == IRQ_NOT_AVAIL)
+		return -ENODEV;
+
+	if (type == IRQ_TYPE_PROBE) {
+		/* only probe unenabled GPIO interrupt lines */
+		if (test_bit(gpionr, gpio_enabled))
+			return 0;
+		type = IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING;
+	}
+
+	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING |
+		    IRQ_TYPE_LEVEL_HIGH | IRQ_TYPE_LEVEL_LOW)) {
+
+		snprintf(buf, 16, "gpio-irq%d", irq);
+		ret = bfin_gpio_irq_request(gpionr, buf);
+		if (ret)
+			return ret;
+
+		if (__test_and_set_bit(gpionr, gpio_enabled))
+			bfin_gpio_irq_prepare(gpionr);
+
+	} else {
+		__clear_bit(gpionr, gpio_enabled);
+		return 0;
+	}
+
+	if ((type & (IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_LEVEL_LOW)))
+		pint[bank]->invert_set = pintbit;	/* low or falling edge denoted by one */
+	else
+		pint[bank]->invert_clear = pintbit;	/* high or rising edge denoted by zero */
+
+	if ((type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING))
+	    == (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING)) {
+		if (gpio_get_value(gpionr))
+			pint[bank]->invert_set = pintbit;
+		else
+			pint[bank]->invert_clear = pintbit;
+	}
+
+	if (type & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING)) {
+		pint[bank]->edge_set = pintbit;
+		bfin_set_irq_handler(irq, handle_edge_irq);
+	} else {
+		pint[bank]->edge_clear = pintbit;
+		bfin_set_irq_handler(irq, handle_level_irq);
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int bfin_gpio_set_wake(struct irq_data *d, unsigned int state)
+{
+	u32 pint_irq;
+	u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
+	u32 bank = PINT_2_BANK(pint_val);
+
+	switch (bank) {
+	case 0:
+		pint_irq = IRQ_PINT0;
+		break;
+	case 2:
+		pint_irq = IRQ_PINT2;
+		break;
+	case 3:
+		pint_irq = IRQ_PINT3;
+		break;
+	case 1:
+		pint_irq = IRQ_PINT1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	bfin_internal_set_wake(pint_irq, state);
+
+	return 0;
+}
+#else
+# define bfin_gpio_set_wake NULL
+#endif
+
+void bfin_demux_gpio_irq(unsigned int inta_irq,
+			 struct irq_desc *desc)
+{
+	u32 bank, pint_val;
+	u32 request, irq;
+
+	switch (inta_irq) {
+	case IRQ_PINT0:
+		bank = 0;
+		break;
+	case IRQ_PINT2:
+		bank = 2;
+		break;
+	case IRQ_PINT3:
+		bank = 3;
+		break;
+	case IRQ_PINT1:
+		bank = 1;
+		break;
+	default:
+		return;
+	}
+
+	pint_val = bank * NR_PINT_BITS;
+
+	request = pint[bank]->request;
+
+	while (request) {
+		if (request & 1) {
+			irq = pint2irq_lut[pint_val] + SYS_IRQS;
+			bfin_handle_irq(irq);
+		}
+		pint_val++;
+		request >>= 1;
+	}
+
+}
+#endif
+
+static struct irq_chip bfin_gpio_irqchip = {
+	.name = "GPIO",
+	.irq_ack = bfin_gpio_ack_irq,
+	.irq_mask = bfin_gpio_mask_irq,
+	.irq_mask_ack = bfin_gpio_mask_ack_irq,
+	.irq_unmask = bfin_gpio_unmask_irq,
+	.irq_disable = bfin_gpio_mask_irq,
+	.irq_enable = bfin_gpio_unmask_irq,
+	.irq_set_type = bfin_gpio_irq_type,
+	.irq_startup = bfin_gpio_irq_startup,
+	.irq_shutdown = bfin_gpio_irq_shutdown,
+	.irq_set_wake = bfin_gpio_set_wake,
+};
+
+void __cpuinit init_exception_vectors(void)
+{
+	/* cannot program in software:
+	 * evt0 - emulation (jtag)
+	 * evt1 - reset
+	 */
+	bfin_write_EVT2(evt_nmi);
+	bfin_write_EVT3(trap);
+	bfin_write_EVT5(evt_ivhw);
+	bfin_write_EVT6(evt_timer);
+	bfin_write_EVT7(evt_evt7);
+	bfin_write_EVT8(evt_evt8);
+	bfin_write_EVT9(evt_evt9);
+	bfin_write_EVT10(evt_evt10);
+	bfin_write_EVT11(evt_evt11);
+	bfin_write_EVT12(evt_evt12);
+	bfin_write_EVT13(evt_evt13);
+	bfin_write_EVT14(evt_evt14);
+	bfin_write_EVT15(evt_system_call);
+	CSYNC();
+}
+
+/*
+ * This function should be called during kernel startup to initialize
+ * the BFin IRQ handling routines.
+ */
+
+int __init init_arch_irq(void)
+{
+	int irq;
+	unsigned long ilat = 0;
+
+	/*  Disable all the peripheral intrs  - page 4-29 HW Ref manual */
+#ifdef SIC_IMASK0
+	bfin_write_SIC_IMASK0(SIC_UNMASK_ALL);
+	bfin_write_SIC_IMASK1(SIC_UNMASK_ALL);
+# ifdef SIC_IMASK2
+	bfin_write_SIC_IMASK2(SIC_UNMASK_ALL);
+# endif
+# ifdef CONFIG_SMP
+	bfin_write_SICB_IMASK0(SIC_UNMASK_ALL);
+	bfin_write_SICB_IMASK1(SIC_UNMASK_ALL);
+# endif
+#else
+	bfin_write_SIC_IMASK(SIC_UNMASK_ALL);
+#endif
+
+	local_irq_disable();
+
+#if BFIN_GPIO_PINT
+# ifdef CONFIG_PINTx_REASSIGN
+	pint[0]->assign = CONFIG_PINT0_ASSIGN;
+	pint[1]->assign = CONFIG_PINT1_ASSIGN;
+	pint[2]->assign = CONFIG_PINT2_ASSIGN;
+	pint[3]->assign = CONFIG_PINT3_ASSIGN;
+# endif
+	/* Whenever PINTx_ASSIGN is altered init_pint_lut() must be executed! */
+	init_pint_lut();
+#endif
+
+	for (irq = 0; irq <= SYS_IRQS; irq++) {
+		if (irq <= IRQ_CORETMR)
+			irq_set_chip(irq, &bfin_core_irqchip);
+		else
+			irq_set_chip(irq, &bfin_internal_irqchip);
+
+		switch (irq) {
+#if BFIN_GPIO_PINT
+		case IRQ_PINT0:
+		case IRQ_PINT1:
+		case IRQ_PINT2:
+		case IRQ_PINT3:
+#elif defined(BF537_FAMILY)
+		case IRQ_PH_INTA_MAC_RX:
+		case IRQ_PF_INTA_PG_INTA:
+#elif defined(BF533_FAMILY)
+		case IRQ_PROG_INTA:
+#elif defined(CONFIG_BF52x) || defined(CONFIG_BF51x)
+		case IRQ_PORTF_INTA:
+		case IRQ_PORTG_INTA:
+		case IRQ_PORTH_INTA:
+#elif defined(CONFIG_BF561)
+		case IRQ_PROG0_INTA:
+		case IRQ_PROG1_INTA:
+		case IRQ_PROG2_INTA:
+#elif defined(BF538_FAMILY)
+		case IRQ_PORTF_INTA:
+#endif
+			irq_set_chained_handler(irq, bfin_demux_gpio_irq);
+			break;
+#if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
+		case IRQ_MAC_ERROR:
+			irq_set_chained_handler(irq,
+						bfin_demux_mac_status_irq);
+			break;
+#endif
+#ifdef CONFIG_SMP
+		case IRQ_SUPPLE_0:
+		case IRQ_SUPPLE_1:
+			irq_set_handler(irq, handle_percpu_irq);
+			break;
+#endif
+
+#ifdef CONFIG_TICKSOURCE_CORETMR
+		case IRQ_CORETMR:
+# ifdef CONFIG_SMP
+			irq_set_handler(irq, handle_percpu_irq);
+# else
+			irq_set_handler(irq, handle_simple_irq);
+# endif
+			break;
+#endif
+
+#ifdef CONFIG_TICKSOURCE_GPTMR0
+		case IRQ_TIMER0:
+			irq_set_handler(irq, handle_simple_irq);
+			break;
+#endif
+
+		default:
+#ifdef CONFIG_IPIPE
+			irq_set_handler(irq, handle_level_irq);
+#else
+			irq_set_handler(irq, handle_simple_irq);
+#endif
+			break;
+		}
+	}
+
+	init_mach_irq();
+
+#if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
+	for (irq = IRQ_MAC_PHYINT; irq <= IRQ_MAC_STMDONE; irq++)
+		irq_set_chip_and_handler(irq, &bfin_mac_status_irqchip,
+					 handle_level_irq);
+#endif
+	/* if configured as edge, then will be changed to do_edge_IRQ */
+	for (irq = GPIO_IRQ_BASE;
+		irq < (GPIO_IRQ_BASE + MAX_BLACKFIN_GPIOS); irq++)
+		irq_set_chip_and_handler(irq, &bfin_gpio_irqchip,
+					 handle_level_irq);
+
+	bfin_write_IMASK(0);
+	CSYNC();
+	ilat = bfin_read_ILAT();
+	CSYNC();
+	bfin_write_ILAT(ilat);
+	CSYNC();
+
+	printk(KERN_INFO "Configuring Blackfin Priority Driven Interrupts\n");
+	/* IMASK=xxx is equivalent to STI xx or bfin_irq_flags=xx,
+	 * local_irq_enable()
+	 */
+	program_IAR();
+	/* Therefore it's better to setup IARs before interrupts enabled */
+	search_IAR();
+
+	/* Enable interrupts IVG7-15 */
+	bfin_irq_flags |= IMASK_IVG15 |
+	    IMASK_IVG14 | IMASK_IVG13 | IMASK_IVG12 | IMASK_IVG11 |
+	    IMASK_IVG10 | IMASK_IVG9 | IMASK_IVG8 | IMASK_IVG7 | IMASK_IVGHW;
+
+	/* This implicitly covers ANOMALY_05000171
+	 * Boot-ROM code modifies SICA_IWRx wakeup registers
+	 */
+#ifdef SIC_IWR0
+	bfin_write_SIC_IWR0(IWR_DISABLE_ALL);
+# ifdef SIC_IWR1
+	/* BF52x/BF51x system reset does not properly reset SIC_IWR1 which
+	 * will screw up the bootrom as it relies on MDMA0/1 waking it
+	 * up from IDLE instructions.  See this report for more info:
+	 * http://blackfin.uclinux.org/gf/tracker/4323
+	 */
+	if (ANOMALY_05000435)
+		bfin_write_SIC_IWR1(IWR_ENABLE(10) | IWR_ENABLE(11));
+	else
+		bfin_write_SIC_IWR1(IWR_DISABLE_ALL);
+# endif
+# ifdef SIC_IWR2
+	bfin_write_SIC_IWR2(IWR_DISABLE_ALL);
+# endif
+#else
+	bfin_write_SIC_IWR(IWR_DISABLE_ALL);
+#endif
+
+	return 0;
+}
+
+#ifdef CONFIG_DO_IRQ_L1
+__attribute__((l1_text))
+#endif
+static int vec_to_irq(int vec)
+{
+	struct ivgx *ivg = ivg7_13[vec - IVG7].ifirst;
+	struct ivgx *ivg_stop = ivg7_13[vec - IVG7].istop;
+	unsigned long sic_status[3];
+
+	if (likely(vec == EVT_IVTMR_P))
+		return IRQ_CORETMR;
+
+#ifdef SIC_ISR
+	sic_status[0] = bfin_read_SIC_IMASK() & bfin_read_SIC_ISR();
+#else
+	if (smp_processor_id()) {
+# ifdef SICB_ISR0
+		/* This will be optimized out in UP mode. */
+		sic_status[0] = bfin_read_SICB_ISR0() & bfin_read_SICB_IMASK0();
+		sic_status[1] = bfin_read_SICB_ISR1() & bfin_read_SICB_IMASK1();
+# endif
+	} else {
+		sic_status[0] = bfin_read_SIC_ISR0() & bfin_read_SIC_IMASK0();
+		sic_status[1] = bfin_read_SIC_ISR1() & bfin_read_SIC_IMASK1();
+	}
+#endif
+#ifdef SIC_ISR2
+	sic_status[2] = bfin_read_SIC_ISR2() & bfin_read_SIC_IMASK2();
+#endif
+
+	for (;; ivg++) {
+		if (ivg >= ivg_stop)
+			return -1;
+#ifdef SIC_ISR
+		if (sic_status[0] & ivg->isrflag)
+#else
+		if (sic_status[(ivg->irqno - IVG7) / 32] & ivg->isrflag)
+#endif
+			return ivg->irqno;
+	}
+}
+
+#ifdef CONFIG_DO_IRQ_L1
+__attribute__((l1_text))
+#endif
+void do_irq(int vec, struct pt_regs *fp)
+{
+	int irq = vec_to_irq(vec);
+	if (irq == -1)
+		return;
+	asm_do_IRQ(irq, fp);
+}
+
+#ifdef CONFIG_IPIPE
+
+int __ipipe_get_irq_priority(unsigned irq)
+{
+	int ient, prio;
+
+	if (irq <= IRQ_CORETMR)
+		return irq;
+
+	for (ient = 0; ient < NR_PERI_INTS; ient++) {
+		struct ivgx *ivg = ivg_table + ient;
+		if (ivg->irqno == irq) {
+			for (prio = 0; prio <= IVG13-IVG7; prio++) {
+				if (ivg7_13[prio].ifirst <= ivg &&
+				    ivg7_13[prio].istop > ivg)
+					return IVG7 + prio;
+			}
+		}
+	}
+
+	return IVG15;
+}
+
+/* Hw interrupts are disabled on entry (check SAVE_CONTEXT). */
+#ifdef CONFIG_DO_IRQ_L1
+__attribute__((l1_text))
+#endif
+asmlinkage int __ipipe_grab_irq(int vec, struct pt_regs *regs)
+{
+	struct ipipe_percpu_domain_data *p = ipipe_root_cpudom_ptr();
+	struct ipipe_domain *this_domain = __ipipe_current_domain;
+	struct ivgx *ivg_stop = ivg7_13[vec-IVG7].istop;
+	struct ivgx *ivg = ivg7_13[vec-IVG7].ifirst;
+	int irq, s = 0;
+
+	irq = vec_to_irq(vec);
+	if (irq == -1)
+		return 0;
+
+	if (irq == IRQ_SYSTMR) {
+#if !defined(CONFIG_GENERIC_CLOCKEVENTS) || defined(CONFIG_TICKSOURCE_GPTMR0)
+		bfin_write_TIMER_STATUS(1); /* Latch TIMIL0 */
+#endif
+		/* This is basically what we need from the register frame. */
+		__raw_get_cpu_var(__ipipe_tick_regs).ipend = regs->ipend;
+		__raw_get_cpu_var(__ipipe_tick_regs).pc = regs->pc;
+		if (this_domain != ipipe_root_domain)
+			__raw_get_cpu_var(__ipipe_tick_regs).ipend &= ~0x10;
+		else
+			__raw_get_cpu_var(__ipipe_tick_regs).ipend |= 0x10;
+	}
+
+	/*
+	 * We don't want Linux interrupt handlers to run at the
+	 * current core priority level (i.e. < EVT15), since this
+	 * might delay other interrupts handled by a high priority
+	 * domain. Here is what we do instead:
+	 *
+	 * - we raise the SYNCDEFER bit to prevent
+	 * __ipipe_handle_irq() to sync the pipeline for the root
+	 * stage for the incoming interrupt. Upon return, that IRQ is
+	 * pending in the interrupt log.
+	 *
+	 * - we raise the TIF_IRQ_SYNC bit for the current thread, so
+	 * that _schedule_and_signal_from_int will eventually sync the
+	 * pipeline from EVT15.
+	 */
+	if (this_domain == ipipe_root_domain) {
+		s = __test_and_set_bit(IPIPE_SYNCDEFER_FLAG, &p->status);
+		barrier();
+	}
+
+	ipipe_trace_irq_entry(irq);
+	__ipipe_handle_irq(irq, regs);
+	ipipe_trace_irq_exit(irq);
+
+	if (user_mode(regs) &&
+	    !ipipe_test_foreign_stack() &&
+	    (current->ipipe_flags & PF_EVTRET) != 0) {
+		/*
+		 * Testing for user_regs() does NOT fully eliminate
+		 * foreign stack contexts, because of the forged
+		 * interrupt returns we do through
+		 * __ipipe_call_irqtail. In that case, we might have
+		 * preempted a foreign stack context in a high
+		 * priority domain, with a single interrupt level now
+		 * pending after the irqtail unwinding is done. In
+		 * which case user_mode() is now true, and the event
+		 * gets dispatched spuriously.
+		 */
+		current->ipipe_flags &= ~PF_EVTRET;
+		__ipipe_dispatch_event(IPIPE_EVENT_RETURN, regs);
+	}
+
+	if (this_domain == ipipe_root_domain) {
+		set_thread_flag(TIF_IRQ_SYNC);
+		if (!s) {
+			__clear_bit(IPIPE_SYNCDEFER_FLAG, &p->status);
+			return !test_bit(IPIPE_STALL_FLAG, &p->status);
+		}
+	}
+
+	return 0;
+}
+
+#endif /* CONFIG_IPIPE */
diff --git a/arch/blackfin/mach-common/pm.c b/arch/blackfin/mach-common/pm.c
new file mode 100644
index 00000000..3c648a07
--- /dev/null
+++ b/arch/blackfin/mach-common/pm.c
@@ -0,0 +1,237 @@
+/*
+ * Blackfin power management
+ *
+ * Copyright 2006-2009 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2
+ * based on arm/mach-omap/pm.c
+ *    Copyright 2001, Cliff Brake <cbrake@accelent.com> and others
+ */
+
+#include <linux/suspend.h>
+#include <linux/sched.h>
+#include <linux/proc_fs.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+
+#include <asm/cplb.h>
+#include <asm/gpio.h>
+#include <asm/dma.h>
+#include <asm/dpmc.h>
+
+
+void bfin_pm_suspend_standby_enter(void)
+{
+	bfin_pm_standby_setup();
+
+#ifdef CONFIG_PM_BFIN_SLEEP_DEEPER
+	sleep_deeper(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
+#else
+	sleep_mode(bfin_sic_iwr[0], bfin_sic_iwr[1], bfin_sic_iwr[2]);
+#endif
+
+	bfin_pm_standby_restore();
+
+#ifdef SIC_IWR0
+	bfin_write_SIC_IWR0(IWR_DISABLE_ALL);
+# ifdef SIC_IWR1
+	/* BF52x system reset does not properly reset SIC_IWR1 which
+	 * will screw up the bootrom as it relies on MDMA0/1 waking it
+	 * up from IDLE instructions.  See this report for more info:
+	 * http://blackfin.uclinux.org/gf/tracker/4323
+	 */
+	if (ANOMALY_05000435)
+		bfin_write_SIC_IWR1(IWR_ENABLE(10) | IWR_ENABLE(11));
+	else
+		bfin_write_SIC_IWR1(IWR_DISABLE_ALL);
+# endif
+# ifdef SIC_IWR2
+	bfin_write_SIC_IWR2(IWR_DISABLE_ALL);
+# endif
+#else
+	bfin_write_SIC_IWR(IWR_DISABLE_ALL);
+#endif
+}
+
+int bf53x_suspend_l1_mem(unsigned char *memptr)
+{
+	dma_memcpy_nocache(memptr, (const void *) L1_CODE_START,
+			L1_CODE_LENGTH);
+	dma_memcpy_nocache(memptr + L1_CODE_LENGTH,
+			(const void *) L1_DATA_A_START, L1_DATA_A_LENGTH);
+	dma_memcpy_nocache(memptr + L1_CODE_LENGTH + L1_DATA_A_LENGTH,
+			(const void *) L1_DATA_B_START, L1_DATA_B_LENGTH);
+	memcpy(memptr + L1_CODE_LENGTH + L1_DATA_A_LENGTH +
+			L1_DATA_B_LENGTH, (const void *) L1_SCRATCH_START,
+			L1_SCRATCH_LENGTH);
+
+	return 0;
+}
+
+int bf53x_resume_l1_mem(unsigned char *memptr)
+{
+	dma_memcpy_nocache((void *) L1_CODE_START, memptr, L1_CODE_LENGTH);
+	dma_memcpy_nocache((void *) L1_DATA_A_START, memptr + L1_CODE_LENGTH,
+			L1_DATA_A_LENGTH);
+	dma_memcpy_nocache((void *) L1_DATA_B_START, memptr + L1_CODE_LENGTH +
+			L1_DATA_A_LENGTH, L1_DATA_B_LENGTH);
+	memcpy((void *) L1_SCRATCH_START, memptr + L1_CODE_LENGTH +
+			L1_DATA_A_LENGTH + L1_DATA_B_LENGTH, L1_SCRATCH_LENGTH);
+
+	return 0;
+}
+
+#if defined(CONFIG_BFIN_EXTMEM_WRITEBACK) || defined(CONFIG_BFIN_L2_WRITEBACK)
+static void flushinv_all_dcache(void)
+{
+	u32 way, bank, subbank, set;
+	u32 status, addr;
+	u32 dmem_ctl = bfin_read_DMEM_CONTROL();
+
+	for (bank = 0; bank < 2; ++bank) {
+		if (!(dmem_ctl & (1 << (DMC1_P - bank))))
+			continue;
+
+		for (way = 0; way < 2; ++way)
+			for (subbank = 0; subbank < 4; ++subbank)
+				for (set = 0; set < 64; ++set) {
+
+					bfin_write_DTEST_COMMAND(
+						way << 26 |
+						bank << 23 |
+						subbank << 16 |
+						set << 5
+					);
+					CSYNC();
+					status = bfin_read_DTEST_DATA0();
+
+					/* only worry about valid/dirty entries */
+					if ((status & 0x3) != 0x3)
+						continue;
+
+					/* construct the address using the tag */
+					addr = (status & 0xFFFFC800) | (subbank << 12) | (set << 5);
+
+					/* flush it */
+					__asm__ __volatile__("FLUSHINV[%0];" : : "a"(addr));
+				}
+	}
+}
+#endif
+
+int bfin_pm_suspend_mem_enter(void)
+{
+	int wakeup, ret;
+
+	unsigned char *memptr = kmalloc(L1_CODE_LENGTH + L1_DATA_A_LENGTH
+					 + L1_DATA_B_LENGTH + L1_SCRATCH_LENGTH,
+					  GFP_KERNEL);
+
+	if (memptr == NULL) {
+		panic("bf53x_suspend_l1_mem malloc failed");
+		return -ENOMEM;
+	}
+
+	wakeup = bfin_read_VR_CTL() & ~FREQ;
+	wakeup |= SCKELOW;
+
+#ifdef CONFIG_PM_BFIN_WAKE_PH6
+	wakeup |= PHYWE;
+#endif
+#ifdef CONFIG_PM_BFIN_WAKE_GP
+	wakeup |= GPWE;
+#endif
+
+	ret = blackfin_dma_suspend();
+
+	if (ret) {
+		kfree(memptr);
+		return ret;
+	}
+
+	bfin_gpio_pm_hibernate_suspend();
+
+#if defined(CONFIG_BFIN_EXTMEM_WRITEBACK) || defined(CONFIG_BFIN_L2_WRITEBACK)
+	flushinv_all_dcache();
+#endif
+	_disable_dcplb();
+	_disable_icplb();
+	bf53x_suspend_l1_mem(memptr);
+
+	do_hibernate(wakeup | vr_wakeup);	/* See you later! */
+
+	bf53x_resume_l1_mem(memptr);
+
+	_enable_icplb();
+	_enable_dcplb();
+
+	bfin_gpio_pm_hibernate_restore();
+	blackfin_dma_resume();
+
+	kfree(memptr);
+
+	return 0;
+}
+
+/*
+ *	bfin_pm_valid - Tell the PM core that we only support the standby sleep
+ *			state
+ *	@state:		suspend state we're checking.
+ *
+ */
+static int bfin_pm_valid(suspend_state_t state)
+{
+	return (state == PM_SUSPEND_STANDBY
+#if !(defined(BF533_FAMILY) || defined(CONFIG_BF561))
+	/*
+	 * On BF533/2/1:
+	 * If we enter Hibernate the SCKE Pin is driven Low,
+	 * so that the SDRAM enters Self Refresh Mode.
+	 * However when the reset sequence that follows hibernate
+	 * state is executed, SCKE is driven High, taking the
+	 * SDRAM out of Self Refresh.
+	 *
+	 * If you reconfigure and access the SDRAM "very quickly",
+	 * you are likely to avoid errors, otherwise the SDRAM
+	 * start losing its contents.
+	 * An external HW workaround is possible using logic gates.
+	 */
+	|| state == PM_SUSPEND_MEM
+#endif
+	);
+}
+
+/*
+ *	bfin_pm_enter - Actually enter a sleep state.
+ *	@state:		State we're entering.
+ *
+ */
+static int bfin_pm_enter(suspend_state_t state)
+{
+	switch (state) {
+	case PM_SUSPEND_STANDBY:
+		bfin_pm_suspend_standby_enter();
+		break;
+	case PM_SUSPEND_MEM:
+		bfin_pm_suspend_mem_enter();
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct platform_suspend_ops bfin_pm_ops = {
+	.enter = bfin_pm_enter,
+	.valid	= bfin_pm_valid,
+};
+
+static int __init bfin_pm_init(void)
+{
+	suspend_set_ops(&bfin_pm_ops);
+	return 0;
+}
+
+__initcall(bfin_pm_init);
diff --git a/arch/blackfin/mach-common/smp.c b/arch/blackfin/mach-common/smp.c
new file mode 100644
index 00000000..ac8f8a43
--- /dev/null
+++ b/arch/blackfin/mach-common/smp.c
@@ -0,0 +1,543 @@
+/*
+ * IPI management based on arch/arm/kernel/smp.c (Copyright 2002 ARM Limited)
+ *
+ * Copyright 2007-2009 Analog Devices Inc.
+ *                         Philippe Gerum <rpm@xenomai.org>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/cache.h>
+#include <linux/clockchips.h>
+#include <linux/profile.h>
+#include <linux/errno.h>
+#include <linux/mm.h>
+#include <linux/cpu.h>
+#include <linux/smp.h>
+#include <linux/cpumask.h>
+#include <linux/seq_file.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/atomic.h>
+#include <asm/cacheflush.h>
+#include <asm/irq_handler.h>
+#include <asm/mmu_context.h>
+#include <asm/pgtable.h>
+#include <asm/pgalloc.h>
+#include <asm/processor.h>
+#include <asm/ptrace.h>
+#include <asm/cpu.h>
+#include <asm/time.h>
+#include <linux/err.h>
+
+/*
+ * Anomaly notes:
+ * 05000120 - we always define corelock as 32-bit integer in L2
+ */
+struct corelock_slot corelock __attribute__ ((__section__(".l2.bss")));
+
+#ifdef CONFIG_ICACHE_FLUSH_L1
+unsigned long blackfin_iflush_l1_entry[NR_CPUS];
+#endif
+
+struct blackfin_initial_pda __cpuinitdata initial_pda_coreb;
+
+#define BFIN_IPI_TIMER	      0
+#define BFIN_IPI_RESCHEDULE   1
+#define BFIN_IPI_CALL_FUNC    2
+#define BFIN_IPI_CPU_STOP     3
+
+struct blackfin_flush_data {
+	unsigned long start;
+	unsigned long end;
+};
+
+void *secondary_stack;
+
+
+struct smp_call_struct {
+	void (*func)(void *info);
+	void *info;
+	int wait;
+	cpumask_t *waitmask;
+};
+
+static struct blackfin_flush_data smp_flush_data;
+
+static DEFINE_SPINLOCK(stop_lock);
+
+struct ipi_message {
+	unsigned long type;
+	struct smp_call_struct call_struct;
+};
+
+/* A magic number - stress test shows this is safe for common cases */
+#define BFIN_IPI_MSGQ_LEN 5
+
+/* Simple FIFO buffer, overflow leads to panic */
+struct ipi_message_queue {
+	spinlock_t lock;
+	unsigned long count;
+	unsigned long head; /* head of the queue */
+	struct ipi_message ipi_message[BFIN_IPI_MSGQ_LEN];
+};
+
+static DEFINE_PER_CPU(struct ipi_message_queue, ipi_msg_queue);
+
+static void ipi_cpu_stop(unsigned int cpu)
+{
+	spin_lock(&stop_lock);
+	printk(KERN_CRIT "CPU%u: stopping\n", cpu);
+	dump_stack();
+	spin_unlock(&stop_lock);
+
+	set_cpu_online(cpu, false);
+
+	local_irq_disable();
+
+	while (1)
+		SSYNC();
+}
+
+static void ipi_flush_icache(void *info)
+{
+	struct blackfin_flush_data *fdata = info;
+
+	/* Invalidate the memory holding the bounds of the flushed region. */
+	blackfin_dcache_invalidate_range((unsigned long)fdata,
+					 (unsigned long)fdata + sizeof(*fdata));
+
+	/* Make sure all write buffers in the data side of the core
+	 * are flushed before trying to invalidate the icache.  This
+	 * needs to be after the data flush and before the icache
+	 * flush so that the SSYNC does the right thing in preventing
+	 * the instruction prefetcher from hitting things in cached
+	 * memory at the wrong time -- it runs much further ahead than
+	 * the pipeline.
+	 */
+	SSYNC();
+
+	/* ipi_flaush_icache is invoked by generic flush_icache_range,
+	 * so call blackfin arch icache flush directly here.
+	 */
+	blackfin_icache_flush_range(fdata->start, fdata->end);
+}
+
+static void ipi_call_function(unsigned int cpu, struct ipi_message *msg)
+{
+	int wait;
+	void (*func)(void *info);
+	void *info;
+	func = msg->call_struct.func;
+	info = msg->call_struct.info;
+	wait = msg->call_struct.wait;
+	func(info);
+	if (wait) {
+#ifdef __ARCH_SYNC_CORE_DCACHE
+		/*
+		 * 'wait' usually means synchronization between CPUs.
+		 * Invalidate D cache in case shared data was changed
+		 * by func() to ensure cache coherence.
+		 */
+		resync_core_dcache();
+#endif
+		cpumask_clear_cpu(cpu, msg->call_struct.waitmask);
+	}
+}
+
+/* Use IRQ_SUPPLE_0 to request reschedule.
+ * When returning from interrupt to user space,
+ * there is chance to reschedule */
+static irqreturn_t ipi_handler_int0(int irq, void *dev_instance)
+{
+	unsigned int cpu = smp_processor_id();
+
+	platform_clear_ipi(cpu, IRQ_SUPPLE_0);
+	return IRQ_HANDLED;
+}
+
+DECLARE_PER_CPU(struct clock_event_device, coretmr_events);
+void ipi_timer(void)
+{
+	int cpu = smp_processor_id();
+	struct clock_event_device *evt = &per_cpu(coretmr_events, cpu);
+	evt->event_handler(evt);
+}
+
+static irqreturn_t ipi_handler_int1(int irq, void *dev_instance)
+{
+	struct ipi_message *msg;
+	struct ipi_message_queue *msg_queue;
+	unsigned int cpu = smp_processor_id();
+	unsigned long flags;
+
+	platform_clear_ipi(cpu, IRQ_SUPPLE_1);
+
+	msg_queue = &__get_cpu_var(ipi_msg_queue);
+
+	spin_lock_irqsave(&msg_queue->lock, flags);
+
+	while (msg_queue->count) {
+		msg = &msg_queue->ipi_message[msg_queue->head];
+		switch (msg->type) {
+		case BFIN_IPI_TIMER:
+			ipi_timer();
+			break;
+		case BFIN_IPI_RESCHEDULE:
+			scheduler_ipi();
+			break;
+		case BFIN_IPI_CALL_FUNC:
+			ipi_call_function(cpu, msg);
+			break;
+		case BFIN_IPI_CPU_STOP:
+			ipi_cpu_stop(cpu);
+			break;
+		default:
+			printk(KERN_CRIT "CPU%u: Unknown IPI message 0x%lx\n",
+			       cpu, msg->type);
+			break;
+		}
+		msg_queue->head++;
+		msg_queue->head %= BFIN_IPI_MSGQ_LEN;
+		msg_queue->count--;
+	}
+	spin_unlock_irqrestore(&msg_queue->lock, flags);
+	return IRQ_HANDLED;
+}
+
+static void ipi_queue_init(void)
+{
+	unsigned int cpu;
+	struct ipi_message_queue *msg_queue;
+	for_each_possible_cpu(cpu) {
+		msg_queue = &per_cpu(ipi_msg_queue, cpu);
+		spin_lock_init(&msg_queue->lock);
+		msg_queue->count = 0;
+		msg_queue->head = 0;
+	}
+}
+
+static inline void smp_send_message(cpumask_t callmap, unsigned long type,
+					void (*func) (void *info), void *info, int wait)
+{
+	unsigned int cpu;
+	struct ipi_message_queue *msg_queue;
+	struct ipi_message *msg;
+	unsigned long flags, next_msg;
+	cpumask_t waitmask; /* waitmask is shared by all cpus */
+
+	cpumask_copy(&waitmask, &callmap);
+	for_each_cpu(cpu, &callmap) {
+		msg_queue = &per_cpu(ipi_msg_queue, cpu);
+		spin_lock_irqsave(&msg_queue->lock, flags);
+		if (msg_queue->count < BFIN_IPI_MSGQ_LEN) {
+			next_msg = (msg_queue->head + msg_queue->count)
+					% BFIN_IPI_MSGQ_LEN;
+			msg = &msg_queue->ipi_message[next_msg];
+			msg->type = type;
+			if (type == BFIN_IPI_CALL_FUNC) {
+				msg->call_struct.func = func;
+				msg->call_struct.info = info;
+				msg->call_struct.wait = wait;
+				msg->call_struct.waitmask = &waitmask;
+			}
+			msg_queue->count++;
+		} else
+			panic("IPI message queue overflow\n");
+		spin_unlock_irqrestore(&msg_queue->lock, flags);
+		platform_send_ipi_cpu(cpu, IRQ_SUPPLE_1);
+	}
+
+	if (wait) {
+		while (!cpumask_empty(&waitmask))
+			blackfin_dcache_invalidate_range(
+				(unsigned long)(&waitmask),
+				(unsigned long)(&waitmask));
+#ifdef __ARCH_SYNC_CORE_DCACHE
+		/*
+		 * Invalidate D cache in case shared data was changed by
+		 * other processors to ensure cache coherence.
+		 */
+		resync_core_dcache();
+#endif
+	}
+}
+
+int smp_call_function(void (*func)(void *info), void *info, int wait)
+{
+	cpumask_t callmap;
+
+	preempt_disable();
+	cpumask_copy(&callmap, cpu_online_mask);
+	cpumask_clear_cpu(smp_processor_id(), &callmap);
+	if (!cpumask_empty(&callmap))
+		smp_send_message(callmap, BFIN_IPI_CALL_FUNC, func, info, wait);
+
+	preempt_enable();
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(smp_call_function);
+
+int smp_call_function_single(int cpuid, void (*func) (void *info), void *info,
+				int wait)
+{
+	unsigned int cpu = cpuid;
+	cpumask_t callmap;
+
+	if (cpu_is_offline(cpu))
+		return 0;
+	cpumask_clear(&callmap);
+	cpumask_set_cpu(cpu, &callmap);
+
+	smp_send_message(callmap, BFIN_IPI_CALL_FUNC, func, info, wait);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(smp_call_function_single);
+
+void smp_send_reschedule(int cpu)
+{
+	cpumask_t callmap;
+	/* simply trigger an ipi */
+
+	cpumask_clear(&callmap);
+	cpumask_set_cpu(cpu, &callmap);
+
+	smp_send_message(callmap, BFIN_IPI_RESCHEDULE, NULL, NULL, 0);
+
+	return;
+}
+
+void smp_send_msg(const struct cpumask *mask, unsigned long type)
+{
+	smp_send_message(*mask, type, NULL, NULL, 0);
+}
+
+void smp_timer_broadcast(const struct cpumask *mask)
+{
+	smp_send_msg(mask, BFIN_IPI_TIMER);
+}
+
+void smp_send_stop(void)
+{
+	cpumask_t callmap;
+
+	preempt_disable();
+	cpumask_copy(&callmap, cpu_online_mask);
+	cpumask_clear_cpu(smp_processor_id(), &callmap);
+	if (!cpumask_empty(&callmap))
+		smp_send_message(callmap, BFIN_IPI_CPU_STOP, NULL, NULL, 0);
+
+	preempt_enable();
+
+	return;
+}
+
+int __cpuinit __cpu_up(unsigned int cpu)
+{
+	int ret;
+	struct blackfin_cpudata *ci = &per_cpu(cpu_data, cpu);
+	struct task_struct *idle = ci->idle;
+
+	if (idle) {
+		free_task(idle);
+		idle = NULL;
+	}
+
+	if (!idle) {
+		idle = fork_idle(cpu);
+		if (IS_ERR(idle)) {
+			printk(KERN_ERR "CPU%u: fork() failed\n", cpu);
+			return PTR_ERR(idle);
+		}
+		ci->idle = idle;
+	} else {
+		init_idle(idle, cpu);
+	}
+	secondary_stack = task_stack_page(idle) + THREAD_SIZE;
+
+	ret = platform_boot_secondary(cpu, idle);
+
+	secondary_stack = NULL;
+
+	return ret;
+}
+
+static void __cpuinit setup_secondary(unsigned int cpu)
+{
+	unsigned long ilat;
+
+	bfin_write_IMASK(0);
+	CSYNC();
+	ilat = bfin_read_ILAT();
+	CSYNC();
+	bfin_write_ILAT(ilat);
+	CSYNC();
+
+	/* Enable interrupt levels IVG7-15. IARs have been already
+	 * programmed by the boot CPU.  */
+	bfin_irq_flags |= IMASK_IVG15 |
+	    IMASK_IVG14 | IMASK_IVG13 | IMASK_IVG12 | IMASK_IVG11 |
+	    IMASK_IVG10 | IMASK_IVG9 | IMASK_IVG8 | IMASK_IVG7 | IMASK_IVGHW;
+}
+
+void __cpuinit secondary_start_kernel(void)
+{
+	unsigned int cpu = smp_processor_id();
+	struct mm_struct *mm = &init_mm;
+
+	if (_bfin_swrst & SWRST_DBL_FAULT_B) {
+		printk(KERN_EMERG "CoreB Recovering from DOUBLE FAULT event\n");
+#ifdef CONFIG_DEBUG_DOUBLEFAULT
+		printk(KERN_EMERG " While handling exception (EXCAUSE = %#x) at %pF\n",
+			initial_pda_coreb.seqstat_doublefault & SEQSTAT_EXCAUSE,
+			initial_pda_coreb.retx_doublefault);
+		printk(KERN_NOTICE "   DCPLB_FAULT_ADDR: %pF\n",
+			initial_pda_coreb.dcplb_doublefault_addr);
+		printk(KERN_NOTICE "   ICPLB_FAULT_ADDR: %pF\n",
+			initial_pda_coreb.icplb_doublefault_addr);
+#endif
+		printk(KERN_NOTICE " The instruction at %pF caused a double exception\n",
+			initial_pda_coreb.retx);
+	}
+
+	/*
+	 * We want the D-cache to be enabled early, in case the atomic
+	 * support code emulates cache coherence (see
+	 * __ARCH_SYNC_CORE_DCACHE).
+	 */
+	init_exception_vectors();
+
+	local_irq_disable();
+
+	/* Attach the new idle task to the global mm. */
+	atomic_inc(&mm->mm_users);
+	atomic_inc(&mm->mm_count);
+	current->active_mm = mm;
+
+	preempt_disable();
+
+	setup_secondary(cpu);
+
+	platform_secondary_init(cpu);
+
+	/* setup local core timer */
+	bfin_local_timer_setup();
+
+	local_irq_enable();
+
+	bfin_setup_caches(cpu);
+
+	notify_cpu_starting(cpu);
+	/*
+	 * Calibrate loops per jiffy value.
+	 * IRQs need to be enabled here - D-cache can be invalidated
+	 * in timer irq handler, so core B can read correct jiffies.
+	 */
+	calibrate_delay();
+
+	cpu_idle();
+}
+
+void __init smp_prepare_boot_cpu(void)
+{
+}
+
+void __init smp_prepare_cpus(unsigned int max_cpus)
+{
+	platform_prepare_cpus(max_cpus);
+	ipi_queue_init();
+	platform_request_ipi(IRQ_SUPPLE_0, ipi_handler_int0);
+	platform_request_ipi(IRQ_SUPPLE_1, ipi_handler_int1);
+}
+
+void __init smp_cpus_done(unsigned int max_cpus)
+{
+	unsigned long bogosum = 0;
+	unsigned int cpu;
+
+	for_each_online_cpu(cpu)
+		bogosum += loops_per_jiffy;
+
+	printk(KERN_INFO "SMP: Total of %d processors activated "
+	       "(%lu.%02lu BogoMIPS).\n",
+	       num_online_cpus(),
+	       bogosum / (500000/HZ),
+	       (bogosum / (5000/HZ)) % 100);
+}
+
+void smp_icache_flush_range_others(unsigned long start, unsigned long end)
+{
+	smp_flush_data.start = start;
+	smp_flush_data.end = end;
+
+	preempt_disable();
+	if (smp_call_function(&ipi_flush_icache, &smp_flush_data, 1))
+		printk(KERN_WARNING "SMP: failed to run I-cache flush request on other CPUs\n");
+	preempt_enable();
+}
+EXPORT_SYMBOL_GPL(smp_icache_flush_range_others);
+
+#ifdef __ARCH_SYNC_CORE_ICACHE
+unsigned long icache_invld_count[NR_CPUS];
+void resync_core_icache(void)
+{
+	unsigned int cpu = get_cpu();
+	blackfin_invalidate_entire_icache();
+	icache_invld_count[cpu]++;
+	put_cpu();
+}
+EXPORT_SYMBOL(resync_core_icache);
+#endif
+
+#ifdef __ARCH_SYNC_CORE_DCACHE
+unsigned long dcache_invld_count[NR_CPUS];
+unsigned long barrier_mask __attribute__ ((__section__(".l2.bss")));
+
+void resync_core_dcache(void)
+{
+	unsigned int cpu = get_cpu();
+	blackfin_invalidate_entire_dcache();
+	dcache_invld_count[cpu]++;
+	put_cpu();
+}
+EXPORT_SYMBOL(resync_core_dcache);
+#endif
+
+#ifdef CONFIG_HOTPLUG_CPU
+int __cpuexit __cpu_disable(void)
+{
+	unsigned int cpu = smp_processor_id();
+
+	if (cpu == 0)
+		return -EPERM;
+
+	set_cpu_online(cpu, false);
+	return 0;
+}
+
+static DECLARE_COMPLETION(cpu_killed);
+
+int __cpuexit __cpu_die(unsigned int cpu)
+{
+	return wait_for_completion_timeout(&cpu_killed, 5000);
+}
+
+void cpu_die(void)
+{
+	complete(&cpu_killed);
+
+	atomic_dec(&init_mm.mm_users);
+	atomic_dec(&init_mm.mm_count);
+
+	local_irq_disable();
+	platform_cpu_die();
+}
+#endif