6 files changed, 445 insertions, 150 deletions

diff --git a/arch/blackfin/mach-common/cpufreq.c b/arch/blackfin/mach-common/cpufreq.c
index 77758289725..4391d03dc84 100644
--- a/arch/blackfin/mach-common/cpufreq.c
+++ b/arch/blackfin/mach-common/cpufreq.c
@@ -11,10 +11,13 @@
 #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>
 
+#define CPUFREQ_CPU 0
+
 /* this is the table of CCLK frequencies, in Hz */
 /* .index is the entry in the auxillary dpm_state_table[] */
 static struct cpufreq_frequency_table bfin_freq_table[] = {
@@ -41,64 +44,124 @@ static struct bfin_dpm_state {
         unsigned int tscale; /* change the divider on the core timer interrupt */
 } dpm_state_table[3];
 
+#if defined(CONFIG_CYCLES_CLOCKSOURCE)
 /*
-   normalized to maximum frequncy offset for CYCLES,
-   used in time-ts cycles clock source, but could be used
-   somewhere also.
+ * normalized to maximum frequncy 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)
+{
 
-static unsigned int bfin_getfreq_khz(unsigned int cpu)
+        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)
 {
-        /* The driver only support single cpu */
-        if (cpu != 0)
-                return -1;
+        unsigned int tscale;
+        unsigned int index = *(unsigned int *)info;
 
-        return get_cclk() / 1000;
+        /* 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 *policy,
+static int bfin_target(struct cpufreq_policy *poli,
                         unsigned int target_freq, unsigned int relation)
 {
-        unsigned int index, plldiv, tscale;
+        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
 
-        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 = 0;
-
-        pr_debug("cpufreq: changing cclk to %lu; target = %u, oldfreq = %u\n",
-                 cclk_hz, target_freq, freqs.old);
-
-        cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
-        local_irq_save_hw(flags);
-                plldiv = (bfin_read_PLL_DIV() & SSEL) | dpm_state_table[index].csel;
-                tscale = dpm_state_table[index].tscale;
-                bfin_write_PLL_DIV(plldiv);
-                /* we have to adjust the core timer, because it is using cclk */
-                bfin_write_TSCALE(tscale);
-                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;
-        local_irq_restore_hw(flags);
-        /* TODO: just test case for cycles clock source, remove later */
-        pr_debug("cpufreq: done\n");
-        cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+        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) {
+                        local_irq_save_hw(flags);
+                        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);
+                        }
+                        local_irq_restore_hw(flags);
+                }
+                /* TODO: just test case for cycles clock source, remove later */
+                cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+        }
 
+        pr_debug("cpufreq: done\n");
         return 0;
 }
 
@@ -110,37 +173,16 @@ static int bfin_verify_speed(struct cpufreq_policy *policy)
 static int __init __bfin_cpu_init(struct cpufreq_policy *policy)
 {
 
-        unsigned long cclk, sclk, csel, min_cclk;
-        int index;
-
-        if (policy->cpu != 0)
-                return -EINVAL;
+        unsigned long cclk, sclk;
 
         cclk = get_cclk() / 1000;
         sclk = get_sclk() / 1000;
 
-#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);
-        }
+        if (policy->cpu == CPUFREQ_CPU)
+                bfin_init_tables(cclk, sclk);
 
         policy->cpuinfo.transition_latency = 50000; /* 50us assumed */
 
-        /*Now ,only support one cpu */
         policy->cur = cclk;
         cpufreq_frequency_table_get_attr(bfin_freq_table, policy->cpu);
         return cpufreq_frequency_table_cpuinfo(policy, bfin_freq_table);
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
index 01b2f58dfb9..a5847f5d67c 100644
--- a/arch/blackfin/mach-common/entry.S
+++ b/arch/blackfin/mach-common/entry.S
@@ -405,7 +405,7 @@ ENTRY(_double_fault)
 
         r0 = sp;        /* stack frame pt_regs pointer argument ==> r0 */
         SP += -12;
-        call _double_fault_c;
+        pseudo_long_call _double_fault_c, p5;
         SP += 12;
 .L_double_fault_panic:
         JUMP .L_double_fault_panic
@@ -447,7 +447,7 @@ ENTRY(_exception_to_level5)
 
         r0 = sp;        /* stack frame pt_regs pointer argument ==> r0 */
         SP += -12;
-        call _trap_c;
+        pseudo_long_call _trap_c, p4;
         SP += 12;
 
         /* If interrupts were off during the exception (IPEND[4] = 1), turn them off
@@ -482,6 +482,8 @@ ENTRY(_trap) /* Exception: 4th entry into system event table(supervisor mode)*/
         [--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
@@ -492,8 +494,6 @@ ENTRY(_trap) /* Exception: 4th entry into system event table(supervisor mode)*/
         ssync;
 #endif
 
-        ANOMALY_283_315_WORKAROUND(p5, r7)
-
 #ifdef CONFIG_DEBUG_DOUBLEFAULT
         /*
          * Save these registers, as they are only valid in exception context
@@ -551,7 +551,7 @@ ENTRY(_kernel_execve)
         p0 = sp;
         sp += -16;
         [sp + 12] = p0;
-        call _do_execve;
+        pseudo_long_call _do_execve, p5;
         SP += 16;
         cc = r0 == 0;
         if ! cc jump .Lexecve_failed;
@@ -626,13 +626,6 @@ ENTRY(_system_call)
         p0 = [sp + PT_ORIG_P0];
 #endif /* CONFIG_IPIPE */
 
-        /* Check the System Call */
-        r7 = __NR_syscall;
-        /* System call number is passed in P0 */
-        r6 = p0;
-        cc = r6 < r7;
-        if ! cc jump .Lbadsys;
-
         /* are we tracing syscalls?*/
         r7 = sp;
         r6.l = lo(ALIGN_PAGE_MASK);
@@ -642,6 +635,14 @@ ENTRY(_system_call)
         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 */
 
@@ -704,7 +705,7 @@ ENTRY(_system_call)
         sp += 4;
 
         SP += -12;
-        call _schedule;
+        pseudo_long_call _schedule, p4;
         SP += 12;
 
         jump .Lresume_userspace_1;
@@ -723,7 +724,7 @@ ENTRY(_system_call)
 
         r0 = sp;
         SP += -12;
-        call _do_notify_resume;
+        pseudo_long_call _do_notify_resume, p5;
         SP += 12;
 
 .Lsyscall_really_exit:
@@ -736,11 +737,17 @@ ENDPROC(_system_call)
  * this symbol need not be global anyways, so ...
  */
 _sys_trace:
-        call _syscall_trace;
-
-        /* Execute the appropriate system call */
+        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);
@@ -758,9 +765,11 @@ _sys_trace:
         SP += -12;
         call (p5);
         SP += 24;
+.Lsys_trace_badsys:
         [sp + PT_R0] = r0;
 
-        call _syscall_trace;
+        r0 = sp;
+        pseudo_long_call _syscall_trace_leave, p5;
         jump .Lresume_userspace;
 ENDPROC(_sys_trace)
 
@@ -966,6 +975,13 @@ ENTRY(_evt_evt14)
 #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;
@@ -989,6 +1005,14 @@ ENTRY(_schedule_and_signal_from_int)
         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];
@@ -1007,7 +1031,8 @@ ENTRY(_schedule_and_signal_from_int)
 
         r0 = sp;
         sp += -12;
-        call _finish_atomic_sections;
+
+        pseudo_long_call _finish_atomic_sections, p5;
         sp += 12;
         jump.s .Lresume_userspace;
 ENDPROC(_schedule_and_signal_from_int)
@@ -1357,7 +1382,7 @@ ENTRY(_sys_call_table)
         .long _sys_newuname
         .long _sys_ni_syscall   /* old sys_modify_ldt */
         .long _sys_adjtimex
-        .long _sys_ni_syscall   /* 125 */ /* sys_mprotect */
+        .long _sys_mprotect     /* 125 */
         .long _sys_ni_syscall   /* old sys_sigprocmask */
         .long _sys_ni_syscall   /* old "creat_module" */
         .long _sys_init_module
@@ -1376,16 +1401,16 @@ ENTRY(_sys_call_table)
         .long _sys_getdents
         .long _sys_ni_syscall   /* sys_select */
         .long _sys_flock
-        .long _sys_ni_syscall   /* sys_msync */
+        .long _sys_msync
         .long _sys_readv                /* 145 */
         .long _sys_writev
         .long _sys_getsid
         .long _sys_fdatasync
         .long _sys_sysctl
-        .long _sys_ni_syscall   /* 150 */ /* sys_mlock */
-        .long _sys_ni_syscall   /* sys_munlock */
-        .long _sys_ni_syscall   /* sys_mlockall */
-        .long _sys_ni_syscall   /* sys_munlockall */
+        .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
@@ -1450,8 +1475,8 @@ ENTRY(_sys_call_table)
         .long _sys_setfsuid     /* 215 */
         .long _sys_setfsgid
         .long _sys_pivot_root
-        .long _sys_ni_syscall   /* sys_mincore */
-        .long _sys_ni_syscall   /* sys_madvise */
+        .long _sys_mincore
+        .long _sys_madvise
         .long _sys_getdents64   /* 220 */
         .long _sys_fcntl64
         .long _sys_ni_syscall   /* reserved for TUX */
@@ -1507,7 +1532,7 @@ ENTRY(_sys_call_table)
         .long _sys_utimes
         .long _sys_fadvise64_64
         .long _sys_ni_syscall /* vserver */
-        .long _sys_ni_syscall /* 275, mbind */
+        .long _sys_mbind        /* 275 */
         .long _sys_ni_syscall /* get_mempolicy */
         .long _sys_ni_syscall /* set_mempolicy */
         .long _sys_mq_open
diff --git a/arch/blackfin/mach-common/head.S b/arch/blackfin/mach-common/head.S
index cab0a0031ee..4391621d904 100644
--- a/arch/blackfin/mach-common/head.S
+++ b/arch/blackfin/mach-common/head.S
@@ -144,8 +144,8 @@ ENTRY(__start)
 #endif
 
         /* Initialize stack pointer */
-        sp.l = _init_thread_union;
-        sp.h = _init_thread_union;
+        sp.l = _init_thread_union + THREAD_SIZE;
+        sp.h = _init_thread_union + THREAD_SIZE;
         fp = sp;
         usp = sp;
 
@@ -186,6 +186,11 @@ ENTRY(__start)
 
         /* 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
@@ -257,12 +262,7 @@ ENTRY(_real_start)
         R0 = R7;
         call _cmdline_init;
 
-        /* Load the current thread pointer and stack */
-        p1 = THREAD_SIZE + 4 (z);       /* +4 is for reti loading */
-        sp = sp + p1;
-        usp = sp;
-        fp = sp;
-        sp += -12;
+        sp += -12 + 4; /* +4 is for reti loading above */
         call _init_pda
         sp += 12;
         jump.l _start_kernel;
diff --git a/arch/blackfin/mach-common/interrupt.S b/arch/blackfin/mach-common/interrupt.S
index 8085ff1cce0..cee62cf4acd 100644
--- a/arch/blackfin/mach-common/interrupt.S
+++ b/arch/blackfin/mach-common/interrupt.S
@@ -88,6 +88,13 @@ __common_int_entry:
 #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);
@@ -109,10 +116,10 @@ __common_int_entry:
         cc = r0 == 0;
         if cc jump .Lcommon_restore_context;
 #else /* CONFIG_IPIPE */
-        call _do_irq;
+        pseudo_long_call _do_irq, p2;
         SP += 12;
 #endif /* CONFIG_IPIPE */
-        call _return_from_int;
+        pseudo_long_call _return_from_int, p2;
 .Lcommon_restore_context:
         RESTORE_CONTEXT
         rti;
@@ -168,7 +175,7 @@ ENTRY(_evt_ivhw)
 
         r0 = sp;        /* stack frame pt_regs pointer argument ==> r0 */
         SP += -12;
-        call _trap_c;
+        pseudo_long_call _trap_c, p5;
         SP += 12;
 
 #ifdef EBIU_ERRMST
@@ -179,7 +186,7 @@ ENTRY(_evt_ivhw)
         w[p0] = r0.l;
 #endif
 
-        call _ret_from_exception;
+        pseudo_long_call _ret_from_exception, p2;
 
 .Lcommon_restore_all_sys:
         RESTORE_ALL_SYS
@@ -187,12 +194,28 @@ ENTRY(_evt_ivhw)
 ENDPROC(_evt_ivhw)
 
 /* Interrupt routine for evt2 (NMI).
- * We don't actually use this, so just return.
  * 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)
 
@@ -223,7 +246,7 @@ ENTRY(_evt_system_call)
 #ifdef CONFIG_FRAME_POINTER
         fp = 0;
 #endif
-        call _system_call;
+        pseudo_long_call _system_call, p2;
         jump .Lcommon_restore_context;
 ENDPROC(_evt_system_call)
 
diff --git a/arch/blackfin/mach-common/ints-priority.c b/arch/blackfin/mach-common/ints-priority.c
index 1873b2c1fed..7ad8878bfa1 100644
--- a/arch/blackfin/mach-common/ints-priority.c
+++ b/arch/blackfin/mach-common/ints-priority.c
@@ -28,6 +28,7 @@
 #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))
 
@@ -172,7 +173,12 @@ static void bfin_internal_mask_irq(unsigned int irq)
         local_irq_restore_hw(flags);
 }
 
+#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)
+#endif
 {
         unsigned long flags;
 
@@ -185,16 +191,38 @@ static void bfin_internal_unmask_irq(unsigned int irq)
         local_irq_save_hw(flags);
         mask_bank = SIC_SYSIRQ(irq) / 32;
         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));
+        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
 #endif
         local_irq_restore_hw(flags);
 }
 
+#ifdef CONFIG_SMP
+static void bfin_internal_unmask_irq(unsigned int irq)
+{
+        struct irq_desc *desc = irq_to_desc(irq);
+        bfin_internal_unmask_irq_affinity(irq, desc->affinity);
+}
+
+static int bfin_internal_set_affinity(unsigned int irq, const struct cpumask *mask)
+{
+        bfin_internal_mask_irq(irq);
+        bfin_internal_unmask_irq_affinity(irq, mask);
+
+        return 0;
+}
+#endif
+
 #ifdef CONFIG_PM
 int bfin_internal_set_wake(unsigned int irq, unsigned int state)
 {
@@ -224,11 +252,6 @@ int bfin_internal_set_wake(unsigned int irq, unsigned int state)
         wakeup |= USBWE;
         break;
 #endif
-#ifdef IRQ_KEY
-        case IRQ_KEY:
-        wakeup |= KPADWE;
-        break;
-#endif
 #ifdef CONFIG_BF54x
         case IRQ_CNT:
         wakeup |= ROTWE;
@@ -270,6 +293,9 @@ static struct irq_chip bfin_internal_irqchip = {
         .mask_ack = bfin_internal_mask_irq,
         .disable = bfin_internal_mask_irq,
         .enable = bfin_internal_unmask_irq,
+#ifdef CONFIG_SMP
+        .set_affinity = bfin_internal_set_affinity,
+#endif
 #ifdef CONFIG_PM
         .set_wake = bfin_internal_set_wake,
 #endif
@@ -294,7 +320,6 @@ 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);
 }
@@ -385,6 +410,127 @@ static void bfin_demux_error_irq(unsigned int int_err_irq,
 }
 #endif                          /* BF537_GENERIC_ERROR_INT_DEMUX */
 
+#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(unsigned int irq)
+{
+        mac_stat_int_mask &= ~(1L << (irq - IRQ_MAC_PHYINT));
+#ifdef BF537_GENERIC_ERROR_INT_DEMUX
+        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(unsigned int irq)
+{
+#ifdef BF537_GENERIC_ERROR_INT_DEMUX
+        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(unsigned int irq, unsigned int state)
+{
+#ifdef BF537_GENERIC_ERROR_INT_DEMUX
+        return bfin_internal_set_wake(IRQ_GENERIC_ERROR, state);
+#else
+        return bfin_internal_set_wake(IRQ_MAC_ERROR, state);
+#endif
+}
+#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
+};
+
+static 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\n",
+                       __func__, __FILE__, __LINE__);
+}
+#endif
+
 static inline void bfin_set_irq_handler(unsigned irq, irq_flow_handler_t handle)
 {
 #ifdef CONFIG_IPIPE
@@ -1031,7 +1177,6 @@ int __init init_arch_irq(void)
 #elif defined(CONFIG_BF538) || defined(CONFIG_BF539)
                 case IRQ_PORTF_INTA:
 #endif
-
                         set_irq_chained_handler(irq,
                                                 bfin_demux_gpio_irq);
                         break;
@@ -1040,29 +1185,36 @@ int __init init_arch_irq(void)
                         set_irq_chained_handler(irq, bfin_demux_error_irq);
                         break;
 #endif
-
-#ifdef CONFIG_SMP
-#ifdef CONFIG_TICKSOURCE_GPTMR0
-                case IRQ_TIMER0:
-#endif
-#ifdef CONFIG_TICKSOURCE_CORETMR
-                case IRQ_CORETMR:
+#if defined(CONFIG_BFIN_MAC) || defined(CONFIG_BFIN_MAC_MODULE)
+                case IRQ_MAC_ERROR:
+                        set_irq_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);
                         break;
 #endif
 
-#ifdef CONFIG_IPIPE
-#ifndef CONFIG_TICKSOURCE_CORETMR
-                case IRQ_TIMER0:
+#ifdef CONFIG_TICKSOURCE_CORETMR
+                case IRQ_CORETMR:
+# ifdef CONFIG_SMP
+                        set_irq_handler(irq, handle_percpu_irq);
+                        break;
+# else
                         set_irq_handler(irq, handle_simple_irq);
                         break;
+# endif
 #endif
-                case IRQ_CORETMR:
+
+#ifdef CONFIG_TICKSOURCE_GPTMR0
+                case IRQ_TIMER0:
                         set_irq_handler(irq, handle_simple_irq);
                         break;
+#endif
+
+#ifdef CONFIG_IPIPE
                 default:
                         set_irq_handler(irq, handle_level_irq);
                         break;
@@ -1078,14 +1230,22 @@ int __init init_arch_irq(void)
         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
 
+#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,
+                                         handle_level_irq);
+#endif
         /* if configured as edge, then will be changed to do_edge_IRQ */
-        for (irq = GPIO_IRQ_BASE; irq < NR_IRQS; irq++)
+        for (irq = GPIO_IRQ_BASE;
+                irq < (GPIO_IRQ_BASE + MAX_BLACKFIN_GPIOS); irq++)
                 set_irq_chip_and_handler(irq, &bfin_gpio_irqchip,
                                          handle_level_irq);
 
-
         bfin_write_IMASK(0);
         CSYNC();
         ilat = bfin_read_ILAT();
diff --git a/arch/blackfin/mach-common/smp.c b/arch/blackfin/mach-common/smp.c
index 369e687582b..7803f22d2ca 100644
--- a/arch/blackfin/mach-common/smp.c
+++ b/arch/blackfin/mach-common/smp.c
@@ -122,9 +122,17 @@ static void ipi_call_function(unsigned int cpu, struct ipi_message *msg)
         wait = msg->call_struct.wait;
         cpu_clear(cpu, msg->call_struct.pending);
         func(info);
-        if (wait)
+        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
                 cpu_clear(cpu, msg->call_struct.waitmask);
-        else
+        } else
                 kfree(msg);
 }
 
@@ -219,6 +227,13 @@ int smp_call_function(void (*func)(void *info), void *info, int wait)
                         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;
@@ -261,6 +276,13 @@ int smp_call_function_single(int cpuid, void (*func) (void *info), void *info,
                         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;
@@ -322,8 +344,11 @@ void smp_send_stop(void)
 
 int __cpuinit __cpu_up(unsigned int cpu)
 {
-        struct task_struct *idle;
         int ret;
+        static struct task_struct *idle;
+
+        if (idle)
+                free_task(idle);
 
         idle = fork_idle(cpu);
         if (IS_ERR(idle)) {
@@ -332,7 +357,6 @@ int __cpuinit __cpu_up(unsigned int cpu)
         }
 
         secondary_stack = task_stack_page(idle) + THREAD_SIZE;
-        smp_wmb();
 
         ret = platform_boot_secondary(cpu, idle);
 
@@ -343,9 +367,6 @@ int __cpuinit __cpu_up(unsigned int cpu)
 
 static void __cpuinit setup_secondary(unsigned int cpu)
 {
-#if !defined(CONFIG_TICKSOURCE_GPTMR0)
-        struct irq_desc *timer_desc;
-#endif
         unsigned long ilat;
 
         bfin_write_IMASK(0);
@@ -360,17 +381,6 @@ static void __cpuinit setup_secondary(unsigned int cpu)
         bfin_irq_flags |= IMASK_IVG15 |
             IMASK_IVG14 | IMASK_IVG13 | IMASK_IVG12 | IMASK_IVG11 |
             IMASK_IVG10 | IMASK_IVG9 | IMASK_IVG8 | IMASK_IVG7 | IMASK_IVGHW;
-
-#if defined(CONFIG_TICKSOURCE_GPTMR0)
-        /* Power down the core timer, just to play safe. */
-        bfin_write_TCNTL(0);
-
-        /* system timer0 has been setup by CoreA. */
-#else
-        timer_desc = irq_desc + IRQ_CORETMR;
-        setup_core_timer();
-        timer_desc->chip->enable(IRQ_CORETMR);
-#endif
 }
 
 void __cpuinit secondary_start_kernel(void)
@@ -405,7 +415,6 @@ void __cpuinit secondary_start_kernel(void)
         atomic_inc(&mm->mm_users);
         atomic_inc(&mm->mm_count);
         current->active_mm = mm;
-        BUG_ON(current->mm);    /* Can't be, but better be safe than sorry. */
 
         preempt_disable();
 
@@ -413,6 +422,9 @@ void __cpuinit secondary_start_kernel(void)
 
         platform_secondary_init(cpu);
 
+        /* setup local core timer */
+        bfin_local_timer_setup();
+
         local_irq_enable();
 
         /*
@@ -462,25 +474,58 @@ void smp_icache_flush_range_others(unsigned long start, unsigned long end)
 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();
-        ++per_cpu(cpu_data, cpu).icache_invld_count;
+        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();
-        ++per_cpu(cpu_data, cpu).dcache_invld_count;
+        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