Merge branch 'mpi-master' into wip-k-fmlpwip-k-fmlp

Conflicts:
	litmus/sched_cedf.c

13 files changed, 686 insertions, 853 deletions

diff --git a/arch/blackfin/mach-common/Makefile b/arch/blackfin/mach-common/Makefile
index 814cb483853b..ff299f24aba0 100644
--- a/arch/blackfin/mach-common/Makefile
+++ b/arch/blackfin/mach-common/Makefile
@@ -11,4 +11,3 @@ 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
-obj-$(CONFIG_DEBUG_ICACHE_CHECK) += irqpanic.o
diff --git a/arch/blackfin/mach-common/arch_checks.c b/arch/blackfin/mach-common/arch_checks.c
index bceb98126c21..d8643fdd0fcf 100644
--- a/arch/blackfin/mach-common/arch_checks.c
+++ b/arch/blackfin/mach-common/arch_checks.c
@@ -61,6 +61,6 @@
 # error "Anomaly 05000220 does not allow you to use Write Back cache with L2 or External Memory"
 #endif
 
-#if ANOMALY_05000491 && !defined(CONFIG_CACHE_FLUSH_L1)
+#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.S b/arch/blackfin/mach-common/cache.S
index 790c767ca95a..9f4dd35bfd74 100644
--- a/arch/blackfin/mach-common/cache.S
+++ b/arch/blackfin/mach-common/cache.S
@@ -11,12 +11,6 @@
 #include <asm/cache.h>
 #include <asm/page.h>
 
-#ifdef CONFIG_CACHE_FLUSH_L1
-.section .l1.text
-#else
-.text
-#endif
-
 /* 05000443 - IFLUSH cannot be last instruction in hardware loop */
 #if ANOMALY_05000443
 # define BROK_FLUSH_INST "IFLUSH"
@@ -58,6 +52,8 @@
 1:
 .ifeqs "\flushins", BROK_FLUSH_INST
         \flushins [P0++];
+        nop;
+        nop;
 2:      nop;
 .else
 2:      \flushins [P0++];
@@ -66,11 +62,43 @@
         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
diff --git a/arch/blackfin/mach-common/cpufreq.c b/arch/blackfin/mach-common/cpufreq.c
index 4391d03dc845..85dc6d69f9c0 100644
--- a/arch/blackfin/mach-common/cpufreq.c
+++ b/arch/blackfin/mach-common/cpufreq.c
@@ -1,7 +1,7 @@
 /*
  * Blackfin core clock scaling
  *
- * Copyright 2008-2009 Analog Devices Inc.
+ * Copyright 2008-2011 Analog Devices Inc.
  *
  * Licensed under the GPL-2 or later.
  */
@@ -16,10 +16,8 @@
 #include <asm/time.h>
 #include <asm/dpmc.h>
 
-#define CPUFREQ_CPU 0
-
 /* this is the table of CCLK frequencies, in Hz */
-/* .index is the entry in the auxillary dpm_state_table[] */
+/* .index is the entry in the auxiliary dpm_state_table[] */
 static struct cpufreq_frequency_table bfin_freq_table[] = {
         {
                 .frequency = CPUFREQ_TABLE_END,
@@ -46,7 +44,7 @@ static struct bfin_dpm_state {
 
 #if defined(CONFIG_CYCLES_CLOCKSOURCE)
 /*
- * normalized to maximum frequncy offset for CYCLES,
+ * normalized to maximum frequency offset for CYCLES,
  * used in time-ts cycles clock source, but could be used
  * somewhere also.
  */
@@ -134,7 +132,7 @@ static int bfin_target(struct cpufreq_policy *poli,
 
                 cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
                 if (cpu == CPUFREQ_CPU) {
-                        local_irq_save_hw(flags);
+                        flags = hard_local_irq_save();
                         plldiv = (bfin_read_PLL_DIV() & SSEL) |
                                                 dpm_state_table[index].csel;
                         bfin_write_PLL_DIV(plldiv);
@@ -155,7 +153,7 @@ static int bfin_target(struct cpufreq_policy *poli,
                                 loops_per_jiffy = cpufreq_scale(lpj_ref,
                                                 lpj_ref_freq, freqs.new);
                         }
-                        local_irq_restore_hw(flags);
+                        hard_local_irq_restore(flags);
                 }
                 /* TODO: just test case for cycles clock source, remove later */
                 cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
diff --git a/arch/blackfin/mach-common/dpmc.c b/arch/blackfin/mach-common/dpmc.c
index 02c7efd1bcf4..f5685a496c58 100644
--- a/arch/blackfin/mach-common/dpmc.c
+++ b/arch/blackfin/mach-common/dpmc.c
@@ -19,9 +19,6 @@
 
 #define DRIVER_NAME "bfin dpmc"
 
-#define dprintk(msg...) \
-        cpufreq_debug_printk(CPUFREQ_DEBUG_DRIVER, DRIVER_NAME, msg)
-
 struct bfin_dpmc_platform_data *pdata;
 
 /**
@@ -61,17 +58,64 @@ err_out:
 }
 
 #ifdef CONFIG_CPU_FREQ
+# ifdef CONFIG_SMP
+static void bfin_idle_this_cpu(void *info)
+{
+        unsigned long flags = 0;
+        unsigned long iwr0, iwr1, iwr2;
+        unsigned int cpu = smp_processor_id();
+
+        local_irq_save_hw(flags);
+        bfin_iwr_set_sup0(&iwr0, &iwr1, &iwr2);
+
+        platform_clear_ipi(cpu, IRQ_SUPPLE_0);
+        SSYNC();
+        asm("IDLE;");
+        bfin_iwr_restore(iwr0, iwr1, iwr2);
+
+        local_irq_restore_hw(flags);
+}
+
+static void bfin_idle_cpu(void)
+{
+        smp_call_function(bfin_idle_this_cpu, NULL, 0);
+}
+
+static void bfin_wakeup_cpu(void)
+{
+        unsigned int cpu;
+        unsigned int this_cpu = smp_processor_id();
+        cpumask_t mask;
+
+        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 */
-
-        } else if (val == CPUFREQ_POSTCHANGE && freq->old > freq->new)
+                bfin_wakeup_cpu();
+        } else if (val == CPUFREQ_POSTCHANGE && freq->old > freq->new) {
+                bfin_idle_cpu();
                 bfin_set_vlev(bfin_get_vlev(freq->new));
+                bfin_wakeup_cpu();
+        }
 
         return 0;
 }
diff --git a/arch/blackfin/mach-common/dpmc_modes.S b/arch/blackfin/mach-common/dpmc_modes.S
index 5969d86836a5..9cfdd49a3127 100644
--- a/arch/blackfin/mach-common/dpmc_modes.S
+++ b/arch/blackfin/mach-common/dpmc_modes.S
@@ -292,13 +292,7 @@ ENTRY(_do_hibernate)
 #ifdef SIC_IMASK
         PM_SYS_PUSH(SIC_IMASK)
 #endif
-#ifdef SICA_IMASK0
-        PM_SYS_PUSH(SICA_IMASK0)
-#endif
-#ifdef SICA_IMASK1
-        PM_SYS_PUSH(SICA_IMASK1)
-#endif
-#ifdef SIC_IAR2
+#ifdef SIC_IAR0
         PM_SYS_PUSH(SIC_IAR0)
         PM_SYS_PUSH(SIC_IAR1)
         PM_SYS_PUSH(SIC_IAR2)
@@ -321,17 +315,6 @@ ENTRY(_do_hibernate)
         PM_SYS_PUSH(SIC_IAR11)
 #endif
 
-#ifdef SICA_IAR0
-        PM_SYS_PUSH(SICA_IAR0)
-        PM_SYS_PUSH(SICA_IAR1)
-        PM_SYS_PUSH(SICA_IAR2)
-        PM_SYS_PUSH(SICA_IAR3)
-        PM_SYS_PUSH(SICA_IAR4)
-        PM_SYS_PUSH(SICA_IAR5)
-        PM_SYS_PUSH(SICA_IAR6)
-        PM_SYS_PUSH(SICA_IAR7)
-#endif
-
 #ifdef SIC_IWR
         PM_SYS_PUSH(SIC_IWR)
 #endif
@@ -344,12 +327,6 @@ ENTRY(_do_hibernate)
 #ifdef SIC_IWR2
         PM_SYS_PUSH(SIC_IWR2)
 #endif
-#ifdef SICA_IWR0
-        PM_SYS_PUSH(SICA_IWR0)
-#endif
-#ifdef SICA_IWR1
-        PM_SYS_PUSH(SICA_IWR1)
-#endif
 
 #ifdef PINT0_ASSIGN
         PM_SYS_PUSH(PINT0_MASK_SET)
@@ -750,12 +727,6 @@ ENTRY(_do_hibernate)
         PM_SYS_POP(PINT0_MASK_SET)
 #endif
 
-#ifdef SICA_IWR1
-        PM_SYS_POP(SICA_IWR1)
-#endif
-#ifdef SICA_IWR0
-        PM_SYS_POP(SICA_IWR0)
-#endif
 #ifdef SIC_IWR2
         PM_SYS_POP(SIC_IWR2)
 #endif
@@ -769,17 +740,6 @@ ENTRY(_do_hibernate)
         PM_SYS_POP(SIC_IWR)
 #endif
 
-#ifdef SICA_IAR0
-        PM_SYS_POP(SICA_IAR7)
-        PM_SYS_POP(SICA_IAR6)
-        PM_SYS_POP(SICA_IAR5)
-        PM_SYS_POP(SICA_IAR4)
-        PM_SYS_POP(SICA_IAR3)
-        PM_SYS_POP(SICA_IAR2)
-        PM_SYS_POP(SICA_IAR1)
-        PM_SYS_POP(SICA_IAR0)
-#endif
-
 #ifdef SIC_IAR8
         PM_SYS_POP(SIC_IAR11)
         PM_SYS_POP(SIC_IAR10)
@@ -797,17 +757,11 @@ ENTRY(_do_hibernate)
 #ifdef SIC_IAR3
         PM_SYS_POP(SIC_IAR3)
 #endif
-#ifdef SIC_IAR2
+#ifdef SIC_IAR0
         PM_SYS_POP(SIC_IAR2)
         PM_SYS_POP(SIC_IAR1)
         PM_SYS_POP(SIC_IAR0)
 #endif
-#ifdef SICA_IMASK1
-        PM_SYS_POP(SICA_IMASK1)
-#endif
-#ifdef SICA_IMASK0
-        PM_SYS_POP(SICA_IMASK0)
-#endif
 #ifdef SIC_IMASK
         PM_SYS_POP(SIC_IMASK)
 #endif
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
index af1bffa21dc1..225d311c9701 100644
--- a/arch/blackfin/mach-common/entry.S
+++ b/arch/blackfin/mach-common/entry.S
@@ -268,7 +268,7 @@ ENTRY(_handle_bad_cplb)
         /* 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 alot of stuff to
+         * "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
          */
@@ -615,7 +615,7 @@ ENTRY(_system_call)
 #ifdef CONFIG_IPIPE
         r0 = sp;
         SP += -12;
-        call ___ipipe_syscall_root;
+        pseudo_long_call ___ipipe_syscall_root, p0;
         SP += 12;
         cc = r0 == 1;
         if cc jump .Lsyscall_really_exit;
@@ -692,7 +692,7 @@ ENTRY(_system_call)
         [--sp] = reti;
         SP += 4; /* don't merge with next insn to keep the pattern obvious */
         SP += -12;
-        call ___ipipe_sync_root;
+        pseudo_long_call ___ipipe_sync_root, p4;
         SP += 12;
         jump .Lresume_userspace_1;
 .Lsyscall_no_irqsync:
@@ -817,7 +817,7 @@ _new_old_task:
         rets = [sp++];
 
         /*
-         * When we come out of resume, r0 carries "old" task, becuase we are
+         * When we come out of resume, r0 carries "old" task, because we are
          * in "new" task.
          */
         rts;
@@ -889,11 +889,80 @@ ENTRY(_ret_from_exception)
         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;
@@ -901,9 +970,57 @@ _resume_kernel_from_int:
         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  2f
+
+#define _resume_kernel_from_int .Lreturn_to_kernel
 #endif
 
 ENTRY(_return_from_int)
@@ -913,7 +1030,7 @@ ENTRY(_return_from_int)
         p2.h = hi(ILAT);
         r0 = [p2];
         cc = bittst (r0, EVT_IVG15_P);
-        if cc jump 2f;
+        if cc jump .Lreturn_to_kernel;
 
         /* if not return to user mode, get out */
         p2.l = lo(IPEND);
@@ -945,7 +1062,7 @@ ENTRY(_return_from_int)
         STI r0;
         raise 15;       /* raise evt15 to do signal or reschedule */
         rti;
-2:
+.Lreturn_to_kernel:
         rts;
 ENDPROC(_return_from_int)
 
@@ -1631,6 +1748,13 @@ ENTRY(_sys_call_table)
         .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 */
 
         .rept NR_syscalls-(.-_sys_call_table)/4
         .long _sys_ni_syscall
diff --git a/arch/blackfin/mach-common/head.S b/arch/blackfin/mach-common/head.S
index 4391621d9048..76de5724c1e3 100644
--- a/arch/blackfin/mach-common/head.S
+++ b/arch/blackfin/mach-common/head.S
@@ -31,6 +31,7 @@ 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;
@@ -38,76 +39,49 @@ ENTRY(__start)
         R0 = SYSCFG_SNEN | SYSCFG_CCEN;
 #endif
         SYSCFG = R0;
-        R0 = 0;
-
-        /* Clear Out All the data and pointer Registers */
-        R1 = R0;
-        R2 = R0;
-        R3 = R0;
-        R4 = R0;
-        R5 = R0;
-        R6 = R0;
-
-        P0 = R0;
-        P1 = R0;
-        P2 = R0;
-        P3 = R0;
-        P4 = R0;
-        P5 = R0;
-
-        LC0 = r0;
-        LC1 = r0;
-        L0 = r0;
-        L1 = r0;
-        L2 = r0;
-        L3 = r0;
-
-        /* Clear Out All the DAG Registers */
-        B0 = r0;
-        B1 = r0;
-        B2 = r0;
-        B3 = r0;
-
-        I0 = r0;
-        I1 = r0;
-        I2 = r0;
-        I3 = r0;
-
-        M0 = r0;
-        M1 = r0;
-        M2 = r0;
-        M3 = r0;
+
+        /* 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
          */
-        p0.L = LO(DTEST_COMMAND);
-        p0.H = HI(DTEST_COMMAND);
-        [p0] = R0;
-        [p0 + (ITEST_COMMAND - DTEST_COMMAND)] = R0;
+        [p5 + (DTEST_COMMAND - COREMMR_BASE)] = r6;
+        [p5 + (ITEST_COMMAND - COREMMR_BASE)] = r6;
         CSYNC;
 
         trace_buffer_init(p0,r0);
-        P0 = R1;
-        R0 = R1;
 
         /* Turn off the icache */
-        p0.l = LO(IMEM_CONTROL);
-        p0.h = HI(IMEM_CONTROL);
-        R1 = [p0];
-        R0 = ~ENICPLB;
-        R0 = R0 & R1;
-        [p0] = R0;
+        r1 = [p5 + (IMEM_CONTROL - COREMMR_BASE)];
+        BITCLR (r1, ENICPLB_P);
+        [p5 + (IMEM_CONTROL - COREMMR_BASE)] = r1;
         SSYNC;
 
         /* Turn off the dcache */
-        p0.l = LO(DMEM_CONTROL);
-        p0.h = HI(DMEM_CONTROL);
-        R1 = [p0];
-        R0 = ~ENDCPLB;
-        R0 = R0 & R1;
-        [p0] = R0;
+        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 */
@@ -122,25 +96,25 @@ ENTRY(__start)
          * below
          */
         GET_PDA(p0, r0);
-        r6 = [p0 + PDA_DF_RETX];
+        r5 = [p0 + PDA_DF_RETX];
         p1.l = _init_saved_retx;
         p1.h = _init_saved_retx;
-        [p1] = r6;
+        [p1] = r5;
 
-        r6 = [p0 + PDA_DF_DCPLB];
+        r5 = [p0 + PDA_DF_DCPLB];
         p1.l = _init_saved_dcplb_fault_addr;
         p1.h = _init_saved_dcplb_fault_addr;
-        [p1] = r6;
+        [p1] = r5;
 
-        r6 = [p0 + PDA_DF_ICPLB];
+        r5 = [p0 + PDA_DF_ICPLB];
         p1.l = _init_saved_icplb_fault_addr;
         p1.h = _init_saved_icplb_fault_addr;
-        [p1] = r6;
+        [p1] = r5;
 
-        r6 = [p0 + PDA_DF_SEQSTAT];
+        r5 = [p0 + PDA_DF_SEQSTAT];
         p1.l = _init_saved_seqstat;
         p1.h = _init_saved_seqstat;
-        [p1] = r6;
+        [p1] = r5;
 #endif
 
         /* Initialize stack pointer */
@@ -155,7 +129,7 @@ ENTRY(__start)
         sti r0;
 #endif
 
-        r0 = 0 (x);
+        r0 = r6;
         /* Zero out all of the fun bss regions */
 #if L1_DATA_A_LENGTH > 0
         r1.l = __sbss_l1;
@@ -200,7 +174,7 @@ ENTRY(__start)
         sp.l = lo(KERNEL_CLOCK_STACK);
         sp.h = hi(KERNEL_CLOCK_STACK);
         call _init_clocks;
-        sp = usp;       /* usp hasnt been touched, so restore from there */
+        sp = usp;       /* usp hasn't been touched, so restore from there */
 #endif
 
         /* This section keeps the processor in supervisor mode
@@ -210,11 +184,9 @@ ENTRY(__start)
 
         /* EVT15 = _real_start */
 
-        p0.l = lo(EVT15);
-        p0.h = hi(EVT15);
         p1.l = _real_start;
         p1.h = _real_start;
-        [p0] = p1;
+        [p5 + (EVT15 - COREMMR_BASE)] = p1;
         csync;
 
 #ifdef CONFIG_EARLY_PRINTK
diff --git a/arch/blackfin/mach-common/interrupt.S b/arch/blackfin/mach-common/interrupt.S
index cee62cf4acd4..469ce7282dc8 100644
--- a/arch/blackfin/mach-common/interrupt.S
+++ b/arch/blackfin/mach-common/interrupt.S
@@ -116,7 +116,24 @@ __common_int_entry:
         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;
@@ -257,16 +274,16 @@ ENDPROC(_evt_system_call)
  * level to EVT14 to prepare the caller for a normal interrupt
  * return through RTI.
  *
- * We currently use this facility in two occasions:
+ * We currently use this feature in two occasions:
  *
- * - to branch to __ipipe_irq_tail_hook as requested by a high
+ * - 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.
  *
- * - to branch to sync_root_irqs, in order to play any interrupt
+ * - 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
diff --git a/arch/blackfin/mach-common/ints-priority.c b/arch/blackfin/mach-common/ints-priority.c
index 1c8c4c7245c3..1177369f9922 100644
--- a/arch/blackfin/mach-common/ints-priority.c
+++ b/arch/blackfin/mach-common/ints-priority.c
@@ -15,35 +15,18 @@
 #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
-#ifdef CONFIG_KGDB
-#include <linux/kgdb.h>
-#endif
 #include <asm/traps.h>
 #include <asm/blackfin.h>
 #include <asm/gpio.h>
 #include <asm/irq_handler.h>
 #include <asm/dpmc.h>
-#include <asm/bfin5xx_spi.h>
-#include <asm/bfin_sport.h>
-#include <asm/bfin_can.h>
 
 #define SIC_SYSIRQ(irq) (irq - (IRQ_CORETMR + 1))
 
-#ifdef BF537_FAMILY
-# define BF537_GENERIC_ERROR_INT_DEMUX
-# define SPI_ERR_MASK   (BIT_STAT_TXCOL | BIT_STAT_RBSY | BIT_STAT_MODF | BIT_STAT_TXE) /* SPI_STAT */
-# define SPORT_ERR_MASK (ROVF | RUVF | TOVF | TUVF)     /* SPORT_STAT */
-# define PPI_ERR_MASK   (0xFFFF & ~FLD) /* PPI_STATUS */
-# define EMAC_ERR_MASK  (PHYINT | MMCINT | RXFSINT | TXFSINT | WAKEDET | RXDMAERR | TXDMAERR | STMDONE) /* EMAC_SYSTAT */
-# define UART_ERR_MASK  (0x6)   /* UART_IIR */
-# define CAN_ERR_MASK   (EWTIF | EWRIF | EPIF | BOIF | WUIF | UIAIF | AAIF | RMLIF | UCEIF | EXTIF | ADIF)      /* CAN_GIF */
-#else
-# undef BF537_GENERIC_ERROR_INT_DEMUX
-#endif
-
 /*
  * NOTES:
  * - we have separated the physical Hardware interrupt from the
@@ -62,22 +45,19 @@ unsigned long bfin_irq_flags = 0x1f;
 EXPORT_SYMBOL(bfin_irq_flags);
 #endif
 
-/* The number of spurious interrupts */
-atomic_t num_spurious;
-
 #ifdef CONFIG_PM
 unsigned long bfin_sic_iwr[3];  /* Up to 3 SIC_IWRx registers */
 unsigned vr_wakeup;
 #endif
 
-struct ivgx {
+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];
 
-struct ivg_slice {
+static struct ivg_slice {
         /* position of first irq in ivg_table for given ivg */
         struct ivgx *ifirst;
         struct ivgx *istop;
@@ -124,21 +104,21 @@ static void __init search_IAR(void)
  * This is for core internal IRQs
  */
 
-static void bfin_ack_noop(unsigned int irq)
+void bfin_ack_noop(struct irq_data *d)
 {
         /* Dummy function.  */
 }
 
-static void bfin_core_mask_irq(unsigned int irq)
+static void bfin_core_mask_irq(struct irq_data *d)
 {
-        bfin_irq_flags &= ~(1 << irq);
-        if (!irqs_disabled_hw())
-                local_irq_enable_hw();
+        bfin_irq_flags &= ~(1 << d->irq);
+        if (!hard_irqs_disabled())
+                hard_local_irq_enable();
 }
 
-static void bfin_core_unmask_irq(unsigned int irq)
+static void bfin_core_unmask_irq(struct irq_data *d)
 {
-        bfin_irq_flags |= 1 << irq;
+        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
@@ -148,82 +128,88 @@ static void bfin_core_unmask_irq(unsigned int irq)
          * local_irq_enable just does "STI bfin_irq_flags", so it's exactly
          * what we need.
          */
-        if (!irqs_disabled_hw())
-                local_irq_enable_hw();
+        if (!hard_irqs_disabled())
+                hard_local_irq_enable();
         return;
 }
 
-static void bfin_internal_mask_irq(unsigned int irq)
+void bfin_internal_mask_irq(unsigned int irq)
 {
-        unsigned long flags;
+        unsigned long flags = hard_local_irq_save();
 
-#ifdef CONFIG_BF53x
-        local_irq_save_hw(flags);
-        bfin_write_SIC_IMASK(bfin_read_SIC_IMASK() &
-                             ~(1 << SIC_SYSIRQ(irq)));
-#else
-        unsigned mask_bank, mask_bit;
-        local_irq_save_hw(flags);
-        mask_bank = SIC_SYSIRQ(irq) / 32;
-        mask_bit = SIC_SYSIRQ(irq) % 32;
+#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
+# 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
-#endif
-        local_irq_restore_hw(flags);
+
+        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
-static void bfin_internal_unmask_irq(unsigned int irq)
+void bfin_internal_unmask_irq(unsigned int irq)
 #endif
 {
-        unsigned long flags;
+        unsigned long flags = hard_local_irq_save();
 
-#ifdef CONFIG_BF53x
-        local_irq_save_hw(flags);
-        bfin_write_SIC_IMASK(bfin_read_SIC_IMASK() |
-                             (1 << SIC_SYSIRQ(irq)));
-#else
-        unsigned mask_bank, mask_bit;
-        local_irq_save_hw(flags);
-        mask_bank = SIC_SYSIRQ(irq) / 32;
-        mask_bit = SIC_SYSIRQ(irq) % 32;
-#ifdef CONFIG_SMP
+#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
+# endif
                 bfin_write_SIC_IMASK(mask_bank,
                         bfin_read_SIC_IMASK(mask_bank) |
                         (1 << mask_bit));
-#ifdef CONFIG_SMP
+# 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
-#endif
-        local_irq_restore_hw(flags);
+
+        hard_local_irq_restore(flags);
 }
 
 #ifdef CONFIG_SMP
-static void bfin_internal_unmask_irq(unsigned int irq)
+static void bfin_internal_unmask_irq_chip(struct irq_data *d)
 {
-        struct irq_desc *desc = irq_to_desc(irq);
-        bfin_internal_unmask_irq_affinity(irq, desc->affinity);
+        bfin_internal_unmask_irq_affinity(d->irq, d->affinity);
 }
 
-static int bfin_internal_set_affinity(unsigned int irq, const struct cpumask *mask)
+static int bfin_internal_set_affinity(struct irq_data *d,
+                                      const struct cpumask *mask, bool force)
 {
-        bfin_internal_mask_irq(irq);
-        bfin_internal_unmask_irq_affinity(irq, mask);
+        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
@@ -264,7 +250,7 @@ int bfin_internal_set_wake(unsigned int irq, unsigned int state)
         break;
         }
 
-        local_irq_save_hw(flags);
+        flags = hard_local_irq_save();
 
         if (state) {
                 bfin_sic_iwr[bank] |= (1 << bit);
@@ -275,36 +261,41 @@ int bfin_internal_set_wake(unsigned int irq, unsigned int state)
                 vr_wakeup  &= ~wakeup;
         }
 
-        local_irq_restore_hw(flags);
+        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",
-        .ack = bfin_ack_noop,
-        .mask = bfin_core_mask_irq,
-        .unmask = bfin_core_unmask_irq,
+        .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",
-        .ack = bfin_ack_noop,
-        .mask = bfin_internal_mask_irq,
-        .unmask = bfin_internal_unmask_irq,
-        .mask_ack = bfin_internal_mask_irq,
-        .disable = bfin_internal_mask_irq,
-        .enable = bfin_internal_unmask_irq,
+        .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
-        .set_affinity = bfin_internal_set_affinity,
-#endif
-#ifdef CONFIG_PM
-        .set_wake = bfin_internal_set_wake,
+        .irq_set_affinity = bfin_internal_set_affinity,
 #endif
+        .irq_set_wake = bfin_internal_set_wake_chip,
 };
 
-static void bfin_handle_irq(unsigned irq)
+void bfin_handle_irq(unsigned irq)
 {
 #ifdef CONFIG_IPIPE
         struct pt_regs regs;    /* Contents not used. */
@@ -312,107 +303,10 @@ static void bfin_handle_irq(unsigned irq)
         __ipipe_handle_irq(irq, &regs);
         ipipe_trace_irq_exit(irq);
 #else /* !CONFIG_IPIPE */
-        struct irq_desc *desc = irq_desc + irq;
-        desc->handle_irq(irq, desc);
+        generic_handle_irq(irq);
 #endif  /* !CONFIG_IPIPE */
 }
 
-#ifdef BF537_GENERIC_ERROR_INT_DEMUX
-static int error_int_mask;
-
-static void bfin_generic_error_mask_irq(unsigned int irq)
-{
-        error_int_mask &= ~(1L << (irq - IRQ_PPI_ERROR));
-        if (!error_int_mask)
-                bfin_internal_mask_irq(IRQ_GENERIC_ERROR);
-}
-
-static void bfin_generic_error_unmask_irq(unsigned int irq)
-{
-        bfin_internal_unmask_irq(IRQ_GENERIC_ERROR);
-        error_int_mask |= 1L << (irq - IRQ_PPI_ERROR);
-}
-
-static struct irq_chip bfin_generic_error_irqchip = {
-        .name = "ERROR",
-        .ack = bfin_ack_noop,
-        .mask_ack = bfin_generic_error_mask_irq,
-        .mask = bfin_generic_error_mask_irq,
-        .unmask = bfin_generic_error_unmask_irq,
-};
-
-static void bfin_demux_error_irq(unsigned int int_err_irq,
-                                 struct irq_desc *inta_desc)
-{
-        int irq = 0;
-
-#if (defined(CONFIG_BF537) || defined(CONFIG_BF536))
-        if (bfin_read_EMAC_SYSTAT() & EMAC_ERR_MASK)
-                irq = IRQ_MAC_ERROR;
-        else
-#endif
-        if (bfin_read_SPORT0_STAT() & SPORT_ERR_MASK)
-                irq = IRQ_SPORT0_ERROR;
-        else if (bfin_read_SPORT1_STAT() & SPORT_ERR_MASK)
-                irq = IRQ_SPORT1_ERROR;
-        else if (bfin_read_PPI_STATUS() & PPI_ERR_MASK)
-                irq = IRQ_PPI_ERROR;
-        else if (bfin_read_CAN_GIF() & CAN_ERR_MASK)
-                irq = IRQ_CAN_ERROR;
-        else if (bfin_read_SPI_STAT() & SPI_ERR_MASK)
-                irq = IRQ_SPI_ERROR;
-        else if ((bfin_read_UART0_IIR() & UART_ERR_MASK) == UART_ERR_MASK)
-                irq = IRQ_UART0_ERROR;
-        else if ((bfin_read_UART1_IIR() & UART_ERR_MASK) == UART_ERR_MASK)
-                irq = IRQ_UART1_ERROR;
-
-        if (irq) {
-                if (error_int_mask & (1L << (irq - IRQ_PPI_ERROR)))
-                        bfin_handle_irq(irq);
-                else {
-
-                        switch (irq) {
-                        case IRQ_PPI_ERROR:
-                                bfin_write_PPI_STATUS(PPI_ERR_MASK);
-                                break;
-#if (defined(CONFIG_BF537) || defined(CONFIG_BF536))
-                        case IRQ_MAC_ERROR:
-                                bfin_write_EMAC_SYSTAT(EMAC_ERR_MASK);
-                                break;
-#endif
-                        case IRQ_SPORT0_ERROR:
-                                bfin_write_SPORT0_STAT(SPORT_ERR_MASK);
-                                break;
-
-                        case IRQ_SPORT1_ERROR:
-                                bfin_write_SPORT1_STAT(SPORT_ERR_MASK);
-                                break;
-
-                        case IRQ_CAN_ERROR:
-                                bfin_write_CAN_GIS(CAN_ERR_MASK);
-                                break;
-
-                        case IRQ_SPI_ERROR:
-                                bfin_write_SPI_STAT(SPI_ERR_MASK);
-                                break;
-
-                        default:
-                                break;
-                        }
-
-                        pr_debug("IRQ %d:"
-                                 " MASKED PERIPHERAL ERROR INTERRUPT ASSERTED\n",
-                                 irq);
-                }
-        } else
-                printk(KERN_ERR
-                       "%s : %s : LINE %d :\nIRQ ?: PERIPHERAL ERROR"
-                       " INTERRUPT ASSERTED BUT NO SOURCE FOUND\n",
-                       __func__, __FILE__, __LINE__);
-
-}
-#endif                          /* BF537_GENERIC_ERROR_INT_DEMUX */
-
 #if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
 static int mac_stat_int_mask;
 
@@ -448,10 +342,12 @@ static void bfin_mac_status_ack_irq(unsigned int irq)
         }
 }
 
-static void bfin_mac_status_mask_irq(unsigned int irq)
+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_GENERIC_ERROR_INT_DEMUX
+#ifdef BF537_FAMILY
         switch (irq) {
         case IRQ_MAC_PHYINT:
                 bfin_write_EMAC_SYSCTL(bfin_read_EMAC_SYSCTL() & ~PHYIE);
@@ -466,9 +362,11 @@ static void bfin_mac_status_mask_irq(unsigned int irq)
         bfin_mac_status_ack_irq(irq);
 }
 
-static void bfin_mac_status_unmask_irq(unsigned int irq)
+static void bfin_mac_status_unmask_irq(struct irq_data *d)
 {
-#ifdef BF537_GENERIC_ERROR_INT_DEMUX
+        unsigned int irq = d->irq;
+
+#ifdef BF537_FAMILY
         switch (irq) {
         case IRQ_MAC_PHYINT:
                 bfin_write_EMAC_SYSCTL(bfin_read_EMAC_SYSCTL() | PHYIE);
@@ -484,34 +382,34 @@ static void bfin_mac_status_unmask_irq(unsigned int irq)
 }
 
 #ifdef CONFIG_PM
-int bfin_mac_status_set_wake(unsigned int irq, unsigned int state)
+int bfin_mac_status_set_wake(struct irq_data *d, unsigned int state)
 {
-#ifdef BF537_GENERIC_ERROR_INT_DEMUX
+#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",
-        .ack = bfin_ack_noop,
-        .mask_ack = bfin_mac_status_mask_irq,
-        .mask = bfin_mac_status_mask_irq,
-        .unmask = bfin_mac_status_unmask_irq,
-#ifdef CONFIG_PM
-        .set_wake = bfin_mac_status_set_wake,
-#endif
+        .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,
 };
 
-static void bfin_demux_mac_status_irq(unsigned int int_err_irq,
-                                 struct irq_desc *inta_desc)
+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++)
+        for (i = 0; i <= (IRQ_MAC_STMDONE - IRQ_MAC_PHYINT); i++)
                 if (status & (1L << i)) {
                         irq = IRQ_MAC_PHYINT + i;
                         break;
@@ -529,21 +427,18 @@ static void bfin_demux_mac_status_irq(unsigned int int_err_irq,
         } else
                 printk(KERN_ERR
                        "%s : %s : LINE %d :\nIRQ ?: MAC ERROR"
-                       " INTERRUPT ASSERTED BUT NO SOURCE FOUND\n",
-                       __func__, __FILE__, __LINE__);
+                       " 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
-        _set_irq_handler(irq, handle_level_irq);
-#else
-        struct irq_desc *desc = irq_desc + irq;
-        /* May not call generic set_irq_handler() due to spinlock
-           recursion. */
-        desc->handle_irq = handle;
+        handle = handle_level_irq;
 #endif
+        __irq_set_handler_locked(irq, handle);
 }
 
 static DECLARE_BITMAP(gpio_enabled, MAX_BLACKFIN_GPIOS);
@@ -551,58 +446,59 @@ extern void bfin_gpio_irq_prepare(unsigned gpio);
 
 #if !defined(CONFIG_BF54x)
 
-static void bfin_gpio_ack_irq(unsigned int irq)
+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(irq), 0);
+        set_gpio_data(irq_to_gpio(d->irq), 0);
 }
 
-static void bfin_gpio_mask_ack_irq(unsigned int irq)
+static void bfin_gpio_mask_ack_irq(struct irq_data *d)
 {
-        struct irq_desc *desc = irq_desc + irq;
+        unsigned int irq = d->irq;
         u32 gpionr = irq_to_gpio(irq);
 
-        if (desc->handle_irq == handle_edge_irq)
+        if (!irqd_is_level_type(d))
                 set_gpio_data(gpionr, 0);
 
         set_gpio_maska(gpionr, 0);
 }
 
-static void bfin_gpio_mask_irq(unsigned int irq)
+static void bfin_gpio_mask_irq(struct irq_data *d)
 {
-        set_gpio_maska(irq_to_gpio(irq), 0);
+        set_gpio_maska(irq_to_gpio(d->irq), 0);
 }
 
-static void bfin_gpio_unmask_irq(unsigned int irq)
+static void bfin_gpio_unmask_irq(struct irq_data *d)
 {
-        set_gpio_maska(irq_to_gpio(irq), 1);
+        set_gpio_maska(irq_to_gpio(d->irq), 1);
 }
 
-static unsigned int bfin_gpio_irq_startup(unsigned int irq)
+static unsigned int bfin_gpio_irq_startup(struct irq_data *d)
 {
-        u32 gpionr = irq_to_gpio(irq);
+        u32 gpionr = irq_to_gpio(d->irq);
 
         if (__test_and_set_bit(gpionr, gpio_enabled))
                 bfin_gpio_irq_prepare(gpionr);
 
-        bfin_gpio_unmask_irq(irq);
+        bfin_gpio_unmask_irq(d);
 
         return 0;
 }
 
-static void bfin_gpio_irq_shutdown(unsigned int irq)
+static void bfin_gpio_irq_shutdown(struct irq_data *d)
 {
-        u32 gpionr = irq_to_gpio(irq);
+        u32 gpionr = irq_to_gpio(d->irq);
 
-        bfin_gpio_mask_irq(irq);
+        bfin_gpio_mask_irq(d);
         __clear_bit(gpionr, gpio_enabled);
         bfin_gpio_irq_free(gpionr);
 }
 
-static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
+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);
@@ -663,29 +559,48 @@ static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
 }
 
 #ifdef CONFIG_PM
-int bfin_gpio_set_wake(unsigned int irq, unsigned int state)
+static int bfin_gpio_set_wake(struct irq_data *d, unsigned int state)
 {
-        return gpio_pm_wakeup_ctrl(irq_to_gpio(irq), 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_irq(unsigned int inta_irq,
-                                struct irq_desc *desc)
+static void bfin_demux_gpio_block(unsigned int irq)
 {
-        unsigned int i, gpio, mask, irq, search = 0;
+        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(CONFIG_BF53x)
-        case IRQ_PROG_INTA:
-                irq = IRQ_PF0;
-                search = 1;
+#if defined(BF537_FAMILY)
+        case IRQ_PF_INTA_PG_INTA:
+                bfin_demux_gpio_block(IRQ_PF0);
+                irq = IRQ_PG0;
                 break;
-# if defined(BF537_FAMILY) && !(defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE))
-        case IRQ_MAC_RX:
+        case IRQ_PH_INTA_MAC_RX:
                 irq = IRQ_PH0;
                 break;
-# endif
-#elif defined(CONFIG_BF538) || defined(CONFIG_BF539)
+#elif defined(BF533_FAMILY)
+        case IRQ_PROG_INTA:
+                irq = IRQ_PF0;
+                break;
+#elif defined(BF538_FAMILY)
         case IRQ_PORTF_INTA:
                 irq = IRQ_PF0;
                 break;
@@ -715,31 +630,7 @@ static void bfin_demux_gpio_irq(unsigned int inta_irq,
                 return;
         }
 
-        if (search) {
-                for (i = 0; i < MAX_BLACKFIN_GPIOS; i += GPIO_BANKSIZE) {
-                        irq += i;
-
-                        mask = get_gpiop_data(i) & get_gpiop_maska(i);
-
-                        while (mask) {
-                                if (mask & 1)
-                                        bfin_handle_irq(irq);
-                                irq++;
-                                mask >>= 1;
-                        }
-                }
-        } else {
-                        gpio = irq_to_gpio(irq);
-                        mask = get_gpiop_data(gpio) & get_gpiop_maska(gpio);
-
-                        do {
-                                if (mask & 1)
-                                        bfin_handle_irq(irq);
-                                irq++;
-                                mask >>= 1;
-                        } while (mask);
-        }
-
+        bfin_demux_gpio_block(irq);
 }
 
 #else                           /* CONFIG_BF54x */
@@ -817,14 +708,13 @@ void init_pint_lut(void)
         }
 }
 
-static void bfin_gpio_ack_irq(unsigned int irq)
+static void bfin_gpio_ack_irq(struct irq_data *d)
 {
-        struct irq_desc *desc = irq_desc + irq;
-        u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
+        u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
         u32 pintbit = PINT_BIT(pint_val);
         u32 bank = PINT_2_BANK(pint_val);
 
-        if ((desc->status & IRQ_TYPE_SENSE_MASK) == IRQ_TYPE_EDGE_BOTH) {
+        if (irqd_get_trigger_type(d) == IRQ_TYPE_EDGE_BOTH) {
                 if (pint[bank]->invert_set & pintbit)
                         pint[bank]->invert_clear = pintbit;
                 else
@@ -834,14 +724,13 @@ static void bfin_gpio_ack_irq(unsigned int irq)
 
 }
 
-static void bfin_gpio_mask_ack_irq(unsigned int irq)
+static void bfin_gpio_mask_ack_irq(struct irq_data *d)
 {
-        struct irq_desc *desc = irq_desc + irq;
-        u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
+        u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
         u32 pintbit = PINT_BIT(pint_val);
         u32 bank = PINT_2_BANK(pint_val);
 
-        if ((desc->status & IRQ_TYPE_SENSE_MASK) == IRQ_TYPE_EDGE_BOTH) {
+        if (irqd_get_trigger_type(d) == IRQ_TYPE_EDGE_BOTH) {
                 if (pint[bank]->invert_set & pintbit)
                         pint[bank]->invert_clear = pintbit;
                 else
@@ -852,25 +741,25 @@ static void bfin_gpio_mask_ack_irq(unsigned int irq)
         pint[bank]->mask_clear = pintbit;
 }
 
-static void bfin_gpio_mask_irq(unsigned int irq)
+static void bfin_gpio_mask_irq(struct irq_data *d)
 {
-        u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
+        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(unsigned int irq)
+static void bfin_gpio_unmask_irq(struct irq_data *d)
 {
-        u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
+        u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
         u32 pintbit = PINT_BIT(pint_val);
         u32 bank = PINT_2_BANK(pint_val);
 
-        pint[bank]->request = pintbit;
         pint[bank]->mask_set = pintbit;
 }
 
-static unsigned int bfin_gpio_irq_startup(unsigned int irq)
+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];
 
@@ -884,22 +773,23 @@ static unsigned int bfin_gpio_irq_startup(unsigned int irq)
         if (__test_and_set_bit(gpionr, gpio_enabled))
                 bfin_gpio_irq_prepare(gpionr);
 
-        bfin_gpio_unmask_irq(irq);
+        bfin_gpio_unmask_irq(d);
 
         return 0;
 }
 
-static void bfin_gpio_irq_shutdown(unsigned int irq)
+static void bfin_gpio_irq_shutdown(struct irq_data *d)
 {
-        u32 gpionr = irq_to_gpio(irq);
+        u32 gpionr = irq_to_gpio(d->irq);
 
-        bfin_gpio_mask_irq(irq);
+        bfin_gpio_mask_irq(d);
         __clear_bit(gpionr, gpio_enabled);
         bfin_gpio_irq_free(gpionr);
 }
 
-static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
+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);
@@ -958,15 +848,11 @@ static int bfin_gpio_irq_type(unsigned int irq, unsigned int type)
 }
 
 #ifdef CONFIG_PM
-u32 pint_saved_masks[NR_PINT_SYS_IRQS];
-u32 pint_wakeup_masks[NR_PINT_SYS_IRQS];
-
-int bfin_gpio_set_wake(unsigned int irq, unsigned int state)
+static int bfin_gpio_set_wake(struct irq_data *d, unsigned int state)
 {
         u32 pint_irq;
-        u32 pint_val = irq2pint_lut[irq - SYS_IRQS];
+        u32 pint_val = irq2pint_lut[d->irq - SYS_IRQS];
         u32 bank = PINT_2_BANK(pint_val);
-        u32 pintbit = PINT_BIT(pint_val);
 
         switch (bank) {
         case 0:
@@ -987,46 +873,14 @@ int bfin_gpio_set_wake(unsigned int irq, unsigned int state)
 
         bfin_internal_set_wake(pint_irq, state);
 
-        if (state)
-                pint_wakeup_masks[bank] |= pintbit;
-        else
-                pint_wakeup_masks[bank] &= ~pintbit;
-
-        return 0;
-}
-
-u32 bfin_pm_setup(void)
-{
-        u32 val, i;
-
-        for (i = 0; i < NR_PINT_SYS_IRQS; i++) {
-                val = pint[i]->mask_clear;
-                pint_saved_masks[i] = val;
-                if (val ^ pint_wakeup_masks[i]) {
-                        pint[i]->mask_clear = val;
-                        pint[i]->mask_set = pint_wakeup_masks[i];
-                }
-        }
-
         return 0;
 }
-
-void bfin_pm_restore(void)
-{
-        u32 i, val;
-
-        for (i = 0; i < NR_PINT_SYS_IRQS; i++) {
-                val = pint_saved_masks[i];
-                if (val ^ pint_wakeup_masks[i]) {
-                        pint[i]->mask_clear = pint[i]->mask_clear;
-                        pint[i]->mask_set = val;
-                }
-        }
-}
+#else
+# define bfin_gpio_set_wake NULL
 #endif
 
-static void bfin_demux_gpio_irq(unsigned int inta_irq,
-                                struct irq_desc *desc)
+void bfin_demux_gpio_irq(unsigned int inta_irq,
+                         struct irq_desc *desc)
 {
         u32 bank, pint_val;
         u32 request, irq;
@@ -1066,18 +920,16 @@ static void bfin_demux_gpio_irq(unsigned int inta_irq,
 
 static struct irq_chip bfin_gpio_irqchip = {
         .name = "GPIO",
-        .ack = bfin_gpio_ack_irq,
-        .mask = bfin_gpio_mask_irq,
-        .mask_ack = bfin_gpio_mask_ack_irq,
-        .unmask = bfin_gpio_unmask_irq,
-        .disable = bfin_gpio_mask_irq,
-        .enable = bfin_gpio_unmask_irq,
-        .set_type = bfin_gpio_irq_type,
-        .startup = bfin_gpio_irq_startup,
-        .shutdown = bfin_gpio_irq_shutdown,
-#ifdef CONFIG_PM
-        .set_wake = bfin_gpio_set_wake,
-#endif
+        .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)
@@ -1111,12 +963,12 @@ int __init init_arch_irq(void)
 {
         int irq;
         unsigned long ilat = 0;
+
         /*  Disable all the peripheral intrs  - page 4-29 HW Ref manual */
-#if defined(CONFIG_BF54x) || defined(CONFIG_BF52x) || defined(CONFIG_BF561) \
-        || defined(BF538_FAMILY) || defined(CONFIG_BF51x)
+#ifdef SIC_IMASK0
         bfin_write_SIC_IMASK0(SIC_UNMASK_ALL);
         bfin_write_SIC_IMASK1(SIC_UNMASK_ALL);
-# ifdef CONFIG_BF54x
+# ifdef SIC_IMASK2
         bfin_write_SIC_IMASK2(SIC_UNMASK_ALL);
 # endif
 # ifdef CONFIG_SMP
@@ -1129,11 +981,6 @@ int __init init_arch_irq(void)
 
         local_irq_disable();
 
-#if (defined(CONFIG_BF537) || defined(CONFIG_BF536))
-        /* Clear EMAC Interrupt Status bits so we can demux it later */
-        bfin_write_EMAC_SYSTAT(-1);
-#endif
-
 #ifdef CONFIG_BF54x
 # ifdef CONFIG_PINTx_REASSIGN
         pint[0]->assign = CONFIG_PINT0_ASSIGN;
@@ -1147,16 +994,16 @@ int __init init_arch_irq(void)
 
         for (irq = 0; irq <= SYS_IRQS; irq++) {
                 if (irq <= IRQ_CORETMR)
-                        set_irq_chip(irq, &bfin_core_irqchip);
+                        irq_set_chip(irq, &bfin_core_irqchip);
                 else
-                        set_irq_chip(irq, &bfin_internal_irqchip);
+                        irq_set_chip(irq, &bfin_internal_irqchip);
 
                 switch (irq) {
-#if defined(CONFIG_BF53x)
+#if defined(BF537_FAMILY)
+                case IRQ_PH_INTA_MAC_RX:
+                case IRQ_PF_INTA_PG_INTA:
+#elif defined(BF533_FAMILY)
                 case IRQ_PROG_INTA:
-# if defined(BF537_FAMILY) && !(defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE))
-                case IRQ_MAC_RX:
-# endif
 #elif defined(CONFIG_BF54x)
                 case IRQ_PINT0:
                 case IRQ_PINT1:
@@ -1170,76 +1017,61 @@ int __init init_arch_irq(void)
                 case IRQ_PROG0_INTA:
                 case IRQ_PROG1_INTA:
                 case IRQ_PROG2_INTA:
-#elif defined(CONFIG_BF538) || defined(CONFIG_BF539)
+#elif defined(BF538_FAMILY)
                 case IRQ_PORTF_INTA:
 #endif
-                        set_irq_chained_handler(irq,
-                                                bfin_demux_gpio_irq);
+                        irq_set_chained_handler(irq, bfin_demux_gpio_irq);
                         break;
-#ifdef BF537_GENERIC_ERROR_INT_DEMUX
-                case IRQ_GENERIC_ERROR:
-                        set_irq_chained_handler(irq, bfin_demux_error_irq);
-                        break;
-#endif
 #if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
                 case IRQ_MAC_ERROR:
-                        set_irq_chained_handler(irq, bfin_demux_mac_status_irq);
+                        irq_set_chained_handler(irq,
+                                                bfin_demux_mac_status_irq);
                         break;
 #endif
 #ifdef CONFIG_SMP
                 case IRQ_SUPPLE_0:
                 case IRQ_SUPPLE_1:
-                        set_irq_handler(irq, handle_percpu_irq);
+                        irq_set_handler(irq, handle_percpu_irq);
                         break;
 #endif
 
 #ifdef CONFIG_TICKSOURCE_CORETMR
                 case IRQ_CORETMR:
 # ifdef CONFIG_SMP
-                        set_irq_handler(irq, handle_percpu_irq);
-                        break;
+                        irq_set_handler(irq, handle_percpu_irq);
 # else
-                        set_irq_handler(irq, handle_simple_irq);
-                        break;
+                        irq_set_handler(irq, handle_simple_irq);
 # endif
+                        break;
 #endif
 
 #ifdef CONFIG_TICKSOURCE_GPTMR0
                 case IRQ_TIMER0:
-                        set_irq_handler(irq, handle_simple_irq);
+                        irq_set_handler(irq, handle_simple_irq);
                         break;
 #endif
 
-#ifdef CONFIG_IPIPE
                 default:
-                        set_irq_handler(irq, handle_level_irq);
-                        break;
-#else /* !CONFIG_IPIPE */
-                default:
-                        set_irq_handler(irq, handle_simple_irq);
+#ifdef CONFIG_IPIPE
+                        irq_set_handler(irq, handle_level_irq);
+#else
+                        irq_set_handler(irq, handle_simple_irq);
+#endif
                         break;
-#endif /* !CONFIG_IPIPE */
                 }
         }
 
-#ifdef BF537_GENERIC_ERROR_INT_DEMUX
-        for (irq = IRQ_PPI_ERROR; irq <= IRQ_UART1_ERROR; irq++)
-                set_irq_chip_and_handler(irq, &bfin_generic_error_irqchip,
-                                         handle_level_irq);
-#if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
-        set_irq_chained_handler(IRQ_MAC_ERROR, bfin_demux_mac_status_irq);
-#endif
-#endif
+        init_mach_irq();
 
 #if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
         for (irq = IRQ_MAC_PHYINT; irq <= IRQ_MAC_STMDONE; irq++)
-                set_irq_chip_and_handler(irq, &bfin_mac_status_irqchip,
+                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++)
-                set_irq_chip_and_handler(irq, &bfin_gpio_irqchip,
+                irq_set_chip_and_handler(irq, &bfin_gpio_irqchip,
                                          handle_level_irq);
 
         bfin_write_IMASK(0);
@@ -1291,53 +1123,54 @@ int __init init_arch_irq(void)
 #ifdef CONFIG_DO_IRQ_L1
 __attribute__((l1_text))
 #endif
-void do_irq(int vec, struct pt_regs *fp)
+static int vec_to_irq(int vec)
 {
-        if (vec == EVT_IVTMR_P) {
-                vec = IRQ_CORETMR;
-        } else {
-                struct ivgx *ivg = ivg7_13[vec - IVG7].ifirst;
-                struct ivgx *ivg_stop = ivg7_13[vec - IVG7].istop;
-#if defined(SIC_ISR0) || defined(SICA_ISR0)
-                unsigned long sic_status[3];
+        struct ivgx *ivg = ivg7_13[vec - IVG7].ifirst;
+        struct ivgx *ivg_stop = ivg7_13[vec - IVG7].istop;
+        unsigned long sic_status[3];
 
-                if (smp_processor_id()) {
+        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();
-                }
-# ifdef SIC_ISR2
-                sic_status[2] = bfin_read_SIC_ISR2() & bfin_read_SIC_IMASK2();
+                /* 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
-                for (;; ivg++) {
-                        if (ivg >= ivg_stop) {
-                                atomic_inc(&num_spurious);
-                                return;
-                        }
-                        if (sic_status[(ivg->irqno - IVG7) / 32] & ivg->isrflag)
-                                break;
-                }
-#else
-                unsigned long sic_status;
-
-                sic_status = bfin_read_SIC_IMASK() & bfin_read_SIC_ISR();
+        } 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) {
-                                atomic_inc(&num_spurious);
-                                return;
-                        } else if (sic_status & ivg->isrflag)
-                                break;
-                }
+        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
-                vec = ivg->irqno;
+                        return ivg->irqno;
         }
-        asm_do_IRQ(vec, fp);
+}
+
+#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
@@ -1373,42 +1206,11 @@ asmlinkage int __ipipe_grab_irq(int vec, struct pt_regs *regs)
         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;
+        int irq, s = 0;
 
-        if (likely(vec == EVT_IVTMR_P))
-                irq = IRQ_CORETMR;
-        else {
-#if defined(SIC_ISR0) || defined(SICA_ISR0)
-                unsigned long sic_status[3];
-
-                sic_status[0] = bfin_read_SIC_ISR0() & bfin_read_SIC_IMASK0();
-                sic_status[1] = bfin_read_SIC_ISR1() & bfin_read_SIC_IMASK1();
-# ifdef SIC_ISR2
-                sic_status[2] = bfin_read_SIC_ISR2() & bfin_read_SIC_IMASK2();
-# endif
-                for (;; ivg++) {
-                        if (ivg >= ivg_stop) {
-                                atomic_inc(&num_spurious);
-                                return 0;
-                        }
-                        if (sic_status[(ivg->irqno - IVG7) / 32] & ivg->isrflag)
-                                break;
-                }
-#else
-                unsigned long sic_status;
-
-                sic_status = bfin_read_SIC_IMASK() & bfin_read_SIC_ISR();
-
-                for (;; ivg++) {
-                        if (ivg >= ivg_stop) {
-                                atomic_inc(&num_spurious);
-                                return 0;
-                        } else if (sic_status & ivg->isrflag)
-                                break;
-                }
-#endif
-                irq = ivg->irqno;
-        }
+        irq = vec_to_irq(vec);
+        if (irq == -1)
+                return 0;
 
         if (irq == IRQ_SYSTMR) {
 #if !defined(CONFIG_GENERIC_CLOCKEVENTS) || defined(CONFIG_TICKSOURCE_GPTMR0)
@@ -1423,6 +1225,21 @@ asmlinkage int __ipipe_grab_irq(int vec, struct pt_regs *regs)
                         __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();
@@ -1432,6 +1249,24 @@ asmlinkage int __ipipe_grab_irq(int vec, struct pt_regs *regs)
         __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) {
diff --git a/arch/blackfin/mach-common/irqpanic.c b/arch/blackfin/mach-common/irqpanic.c
deleted file mode 100644
index c6496249e2bc..000000000000
--- a/arch/blackfin/mach-common/irqpanic.c
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
- * panic kernel with dump information
- *
- * Copyright 2005-2009 Analog Devices Inc.
- *
- * Licensed under the GPL-2 or later.
- */
-
-#include <linux/module.h>
-#include <linux/kernel_stat.h>
-#include <linux/sched.h>
-#include <asm/blackfin.h>
-
-#define L1_ICACHE_START 0xffa10000
-#define L1_ICACHE_END   0xffa13fff
-
-/*
- * irq_panic - calls panic with string setup
- */
-__attribute__ ((l1_text))
-asmlinkage void irq_panic(int reason, struct pt_regs *regs)
-{
-        unsigned int cmd, tag, ca, cache_hi, cache_lo, *pa;
-        unsigned short i, j, die;
-        unsigned int bad[10][6];
-
-        /* check entire cache for coherency
-         * Since printk is in cacheable memory,
-         * don't call it until you have checked everything
-        */
-
-        die = 0;
-        i = 0;
-
-        /* check icache */
-
-        for (ca = L1_ICACHE_START; ca <= L1_ICACHE_END && i < 10; ca += 32) {
-
-                /* Grab various address bits for the itest_cmd fields                      */
-                cmd = (((ca & 0x3000) << 4) |   /* ca[13:12] for SBNK[1:0]             */
-                       ((ca & 0x0c00) << 16) |  /* ca[11:10] for WAYSEL[1:0]           */
-                       ((ca & 0x3f8)) | /* ca[09:03] for SET[4:0] and DW[1:0]  */
-                       0);      /* Access Tag, Read access             */
-
-                SSYNC();
-                bfin_write_ITEST_COMMAND(cmd);
-                SSYNC();
-                tag = bfin_read_ITEST_DATA0();
-                SSYNC();
-
-                /* if tag is marked as valid, check it */
-                if (tag & 1) {
-                        /* The icache is arranged in 4 groups of 64-bits */
-                        for (j = 0; j < 32; j += 8) {
-                                cmd = ((((ca + j) & 0x3000) << 4) |     /* ca[13:12] for SBNK[1:0]             */
-                                       (((ca + j) & 0x0c00) << 16) |    /* ca[11:10] for WAYSEL[1:0]           */
-                                       (((ca + j) & 0x3f8)) |   /* ca[09:03] for SET[4:0] and DW[1:0]  */
-                                       4);      /* Access Data, Read access             */
-
-                                SSYNC();
-                                bfin_write_ITEST_COMMAND(cmd);
-                                SSYNC();
-
-                                cache_hi = bfin_read_ITEST_DATA1();
-                                cache_lo = bfin_read_ITEST_DATA0();
-
-                                pa = ((unsigned int *)((tag & 0xffffcc00) |
-                                                       ((ca + j) & ~(0xffffcc00))));
-
-                                /*
-                                 * Debugging this, enable
-                                 *
-                                 * printk("addr: %08x %08x%08x | %08x%08x\n",
-                                 *  ((unsigned int *)((tag & 0xffffcc00)  | ((ca+j) & ~(0xffffcc00)))),
-                                 *   cache_hi, cache_lo, *(pa+1), *pa);
-                                 */
-
-                                if (cache_hi != *(pa + 1) || cache_lo != *pa) {
-                                        /* Since icache is not working, stay out of it, by not printing */
-                                        die = 1;
-                                        bad[i][0] = (ca + j);
-                                        bad[i][1] = cache_hi;
-                                        bad[i][2] = cache_lo;
-                                        bad[i][3] = ((tag & 0xffffcc00) |
-                                                ((ca + j) & ~(0xffffcc00)));
-                                        bad[i][4] = *(pa + 1);
-                                        bad[i][5] = *(pa);
-                                        i++;
-                                }
-                        }
-                }
-        }
-        if (die) {
-                printk(KERN_EMERG "icache coherency error\n");
-                for (j = 0; j <= i; j++) {
-                        printk(KERN_EMERG
-                            "cache address   : %08x  cache value : %08x%08x\n",
-                             bad[j][0], bad[j][1], bad[j][2]);
-                        printk(KERN_EMERG
-                            "physical address: %08x  SDRAM value : %08x%08x\n",
-                             bad[j][3], bad[j][4], bad[j][5]);
-                }
-                panic("icache coherency error");
-        } else
-                printk(KERN_EMERG "icache checked, and OK\n");
-}
diff --git a/arch/blackfin/mach-common/pm.c b/arch/blackfin/mach-common/pm.c
index 09c1fb410748..3c648a077e75 100644
--- a/arch/blackfin/mach-common/pm.c
+++ b/arch/blackfin/mach-common/pm.c
@@ -23,9 +23,6 @@
 
 void bfin_pm_suspend_standby_enter(void)
 {
-        unsigned long flags;
-
-        local_irq_save_hw(flags);
         bfin_pm_standby_setup();
 
 #ifdef CONFIG_PM_BFIN_SLEEP_DEEPER
@@ -55,8 +52,6 @@ void bfin_pm_suspend_standby_enter(void)
 #else
         bfin_write_SIC_IWR(IWR_DISABLE_ALL);
 #endif
-
-        local_irq_restore_hw(flags);
 }
 
 int bf53x_suspend_l1_mem(unsigned char *memptr)
@@ -127,7 +122,6 @@ static void flushinv_all_dcache(void)
 
 int bfin_pm_suspend_mem_enter(void)
 {
-        unsigned long flags;
         int wakeup, ret;
 
         unsigned char *memptr = kmalloc(L1_CODE_LENGTH + L1_DATA_A_LENGTH
@@ -149,12 +143,9 @@ int bfin_pm_suspend_mem_enter(void)
         wakeup |= GPWE;
 #endif
 
-        local_irq_save_hw(flags);
-
         ret = blackfin_dma_suspend();
 
         if (ret) {
-                local_irq_restore_hw(flags);
                 kfree(memptr);
                 return ret;
         }
@@ -178,7 +169,6 @@ int bfin_pm_suspend_mem_enter(void)
         bfin_gpio_pm_hibernate_restore();
         blackfin_dma_resume();
 
-        local_irq_restore_hw(flags);
         kfree(memptr);
 
         return 0;
@@ -233,7 +223,7 @@ static int bfin_pm_enter(suspend_state_t state)
         return 0;
 }
 
-struct platform_suspend_ops bfin_pm_ops = {
+static const struct platform_suspend_ops bfin_pm_ops = {
         .enter = bfin_pm_enter,
         .valid  = bfin_pm_valid,
 };
diff --git a/arch/blackfin/mach-common/smp.c b/arch/blackfin/mach-common/smp.c
index a17107a700d5..35e7e1eb0188 100644
--- a/arch/blackfin/mach-common/smp.c
+++ b/arch/blackfin/mach-common/smp.c
@@ -19,11 +19,13 @@
 #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 <asm/atomic.h>
 #include <asm/cacheflush.h>
+#include <asm/irq_handler.h>
 #include <asm/mmu_context.h>
 #include <asm/pgtable.h>
 #include <asm/pgalloc.h>
@@ -39,16 +41,14 @@
  */
 struct corelock_slot corelock __attribute__ ((__section__(".l2.bss")));
 
+#ifdef CONFIG_ICACHE_FLUSH_L1
+unsigned long blackfin_iflush_l1_entry[NR_CPUS];
+#endif
+
 void __cpuinitdata *init_retx_coreb, *init_saved_retx_coreb,
         *init_saved_seqstat_coreb, *init_saved_icplb_fault_addr_coreb,
         *init_saved_dcplb_fault_addr_coreb;
 
-cpumask_t cpu_possible_map;
-EXPORT_SYMBOL(cpu_possible_map);
-
-cpumask_t cpu_online_map;
-EXPORT_SYMBOL(cpu_online_map);
-
 #define BFIN_IPI_RESCHEDULE   0
 #define BFIN_IPI_CALL_FUNC    1
 #define BFIN_IPI_CPU_STOP     2
@@ -65,8 +65,7 @@ struct smp_call_struct {
         void (*func)(void *info);
         void *info;
         int wait;
-        cpumask_t pending;
-        cpumask_t waitmask;
+        cpumask_t *waitmask;
 };
 
 static struct blackfin_flush_data smp_flush_data;
@@ -74,15 +73,19 @@ static struct blackfin_flush_data smp_flush_data;
 static DEFINE_SPINLOCK(stop_lock);
 
 struct ipi_message {
-        struct list_head list;
         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 {
-        struct list_head head;
         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);
@@ -94,7 +97,7 @@ static void ipi_cpu_stop(unsigned int cpu)
         dump_stack();
         spin_unlock(&stop_lock);
 
-        cpu_clear(cpu, cpu_online_map);
+        set_cpu_online(cpu, false);
 
         local_irq_disable();
 
@@ -110,6 +113,19 @@ static void ipi_flush_icache(void *info)
         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);
 }
 
@@ -121,7 +137,6 @@ static void ipi_call_function(unsigned int cpu, struct ipi_message *msg)
         func = msg->call_struct.func;
         info = msg->call_struct.info;
         wait = msg->call_struct.wait;
-        cpu_clear(cpu, msg->call_struct.pending);
         func(info);
         if (wait) {
 #ifdef __ARCH_SYNC_CORE_DCACHE
@@ -132,51 +147,60 @@ static void ipi_call_function(unsigned int cpu, struct ipi_message *msg)
                  */
                 resync_core_dcache();
 #endif
-                cpu_clear(cpu, msg->call_struct.waitmask);
-        } else
-                kfree(msg);
+                cpumask_clear_cpu(cpu, msg->call_struct.waitmask);
+        }
 }
 
-static irqreturn_t ipi_handler(int irq, void *dev_instance)
+/* 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;
+}
+
+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);
+        platform_clear_ipi(cpu, IRQ_SUPPLE_1);
 
         msg_queue = &__get_cpu_var(ipi_msg_queue);
-        msg_queue->count++;
 
-        spin_lock(&msg_queue->lock);
-        while (!list_empty(&msg_queue->head)) {
-                msg = list_entry(msg_queue->head.next, typeof(*msg), list);
-                list_del(&msg->list);
+        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_RESCHEDULE:
-                        /* That's the easiest one; leave it to
-                         * return_from_int. */
-                        kfree(msg);
+                        scheduler_ipi();
                         break;
                 case BFIN_IPI_CALL_FUNC:
-                        spin_unlock(&msg_queue->lock);
+                        spin_unlock_irqrestore(&msg_queue->lock, flags);
                         ipi_call_function(cpu, msg);
-                        spin_lock(&msg_queue->lock);
+                        spin_lock_irqsave(&msg_queue->lock, flags);
                         break;
                 case BFIN_IPI_CPU_STOP:
-                        spin_unlock(&msg_queue->lock);
+                        spin_unlock_irqrestore(&msg_queue->lock, flags);
                         ipi_cpu_stop(cpu);
-                        spin_lock(&msg_queue->lock);
-                        kfree(msg);
+                        spin_lock_irqsave(&msg_queue->lock, flags);
                         break;
                 default:
                         printk(KERN_CRIT "CPU%u: Unknown IPI message 0x%lx\n",
                                cpu, msg->type);
-                        kfree(msg);
                         break;
                 }
+                msg_queue->head++;
+                msg_queue->head %= BFIN_IPI_MSGQ_LEN;
+                msg_queue->count--;
         }
-        spin_unlock(&msg_queue->lock);
+        spin_unlock_irqrestore(&msg_queue->lock, flags);
         return IRQ_HANDLED;
 }
 
@@ -186,48 +210,48 @@ static void ipi_queue_init(void)
         struct ipi_message_queue *msg_queue;
         for_each_possible_cpu(cpu) {
                 msg_queue = &per_cpu(ipi_msg_queue, cpu);
-                INIT_LIST_HEAD(&msg_queue->head);
                 spin_lock_init(&msg_queue->lock);
                 msg_queue->count = 0;
+                msg_queue->head = 0;
         }
 }
 
-int smp_call_function(void (*func)(void *info), void *info, int wait)
+static inline void smp_send_message(cpumask_t callmap, unsigned long type,
+                                        void (*func) (void *info), void *info, int wait)
 {
         unsigned int cpu;
-        cpumask_t callmap;
-        unsigned long flags;
         struct ipi_message_queue *msg_queue;
         struct ipi_message *msg;
+        unsigned long flags, next_msg;
+        cpumask_t waitmask; /* waitmask is shared by all cpus */
 
-        callmap = cpu_online_map;
-        cpu_clear(smp_processor_id(), callmap);
-        if (cpus_empty(callmap))
-                return 0;
-
-        msg = kmalloc(sizeof(*msg), GFP_ATOMIC);
-        if (!msg)
-                return -ENOMEM;
-        INIT_LIST_HEAD(&msg->list);
-        msg->call_struct.func = func;
-        msg->call_struct.info = info;
-        msg->call_struct.wait = wait;
-        msg->call_struct.pending = callmap;
-        msg->call_struct.waitmask = callmap;
-        msg->type = BFIN_IPI_CALL_FUNC;
-
-        for_each_cpu_mask(cpu, callmap) {
+        cpumask_copy(&waitmask, &callmap);
+        for_each_cpu(cpu, &callmap) {
                 msg_queue = &per_cpu(ipi_msg_queue, cpu);
                 spin_lock_irqsave(&msg_queue->lock, flags);
-                list_add_tail(&msg->list, &msg_queue->head);
+                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);
+                platform_send_ipi_cpu(cpu, IRQ_SUPPLE_1);
         }
+
         if (wait) {
-                while (!cpus_empty(msg->call_struct.waitmask))
+                while (!cpumask_empty(&waitmask))
                         blackfin_dcache_invalidate_range(
-                                (unsigned long)(&msg->call_struct.waitmask),
-                                (unsigned long)(&msg->call_struct.waitmask));
+                                (unsigned long)(&waitmask),
+                                (unsigned long)(&waitmask));
 #ifdef __ARCH_SYNC_CORE_DCACHE
                 /*
                  * Invalidate D cache in case shared data was changed by
@@ -235,8 +259,21 @@ int smp_call_function(void (*func)(void *info), void *info, int wait)
                  */
                 resync_core_dcache();
 #endif
-                kfree(msg);
         }
+}
+
+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);
@@ -246,100 +283,40 @@ int smp_call_function_single(int cpuid, void (*func) (void *info), void *info,
 {
         unsigned int cpu = cpuid;
         cpumask_t callmap;
-        unsigned long flags;
-        struct ipi_message_queue *msg_queue;
-        struct ipi_message *msg;
 
         if (cpu_is_offline(cpu))
                 return 0;
-        cpus_clear(callmap);
-        cpu_set(cpu, callmap);
-
-        msg = kmalloc(sizeof(*msg), GFP_ATOMIC);
-        if (!msg)
-                return -ENOMEM;
-        INIT_LIST_HEAD(&msg->list);
-        msg->call_struct.func = func;
-        msg->call_struct.info = info;
-        msg->call_struct.wait = wait;
-        msg->call_struct.pending = callmap;
-        msg->call_struct.waitmask = callmap;
-        msg->type = BFIN_IPI_CALL_FUNC;
-
-        msg_queue = &per_cpu(ipi_msg_queue, cpu);
-        spin_lock_irqsave(&msg_queue->lock, flags);
-        list_add_tail(&msg->list, &msg_queue->head);
-        spin_unlock_irqrestore(&msg_queue->lock, flags);
-        platform_send_ipi_cpu(cpu);
+        cpumask_clear(&callmap);
+        cpumask_set_cpu(cpu, &callmap);
+
+        smp_send_message(callmap, BFIN_IPI_CALL_FUNC, func, info, wait);
 
-        if (wait) {
-                while (!cpus_empty(msg->call_struct.waitmask))
-                        blackfin_dcache_invalidate_range(
-                                (unsigned long)(&msg->call_struct.waitmask),
-                                (unsigned long)(&msg->call_struct.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
-                kfree(msg);
-        }
         return 0;
 }
 EXPORT_SYMBOL_GPL(smp_call_function_single);
 
 void smp_send_reschedule(int cpu)
 {
-        unsigned long flags;
-        struct ipi_message_queue *msg_queue;
-        struct ipi_message *msg;
-
+        /* simply trigger an ipi */
         if (cpu_is_offline(cpu))
                 return;
-
-        msg = kzalloc(sizeof(*msg), GFP_ATOMIC);
-        if (!msg)
-                return;
-        INIT_LIST_HEAD(&msg->list);
-        msg->type = BFIN_IPI_RESCHEDULE;
-
-        msg_queue = &per_cpu(ipi_msg_queue, cpu);
-        spin_lock_irqsave(&msg_queue->lock, flags);
-        list_add_tail(&msg->list, &msg_queue->head);
-        spin_unlock_irqrestore(&msg_queue->lock, flags);
-        platform_send_ipi_cpu(cpu);
+        platform_send_ipi_cpu(cpu, IRQ_SUPPLE_0);
 
         return;
 }
 
 void smp_send_stop(void)
 {
-        unsigned int cpu;
         cpumask_t callmap;
-        unsigned long flags;
-        struct ipi_message_queue *msg_queue;
-        struct ipi_message *msg;
 
-        callmap = cpu_online_map;
-        cpu_clear(smp_processor_id(), callmap);
-        if (cpus_empty(callmap))
-                return;
+        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);
 
-        msg = kzalloc(sizeof(*msg), GFP_ATOMIC);
-        if (!msg)
-                return;
-        INIT_LIST_HEAD(&msg->list);
-        msg->type = BFIN_IPI_CPU_STOP;
+        preempt_enable();
 
-        for_each_cpu_mask(cpu, callmap) {
-                msg_queue = &per_cpu(ipi_msg_queue, cpu);
-                spin_lock_irqsave(&msg_queue->lock, flags);
-                list_add_tail(&msg->list, &msg_queue->head);
-                spin_unlock_irqrestore(&msg_queue->lock, flags);
-                platform_send_ipi_cpu(cpu);
-        }
         return;
 }
 
@@ -408,8 +385,6 @@ void __cpuinit secondary_start_kernel(void)
          */
         init_exception_vectors();
 
-        bfin_setup_caches(cpu);
-
         local_irq_disable();
 
         /* Attach the new idle task to the global mm. */
@@ -428,6 +403,8 @@ void __cpuinit secondary_start_kernel(void)
 
         local_irq_enable();
 
+        bfin_setup_caches(cpu);
+
         /*
          * Calibrate loops per jiffy value.
          * IRQs need to be enabled here - D-cache can be invalidated
@@ -446,7 +423,8 @@ void __init smp_prepare_cpus(unsigned int max_cpus)
 {
         platform_prepare_cpus(max_cpus);
         ipi_queue_init();
-        platform_request_ipi(&ipi_handler);
+        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)