MN10300: And Panasonic AM34 subarch and implement SMP

Implement the Panasonic MN10300 AM34 CPU subarch and implement SMP support for
MN10300.  Also implement support for the MN2WS0060 processor and the ASB2364
evaluation board which are AM34 based.

Signed-off-by: Akira Takeuchi <takeuchi.akr@jp.panasonic.com>
Signed-off-by: Kiyoshi Owada <owada.kiyoshi@jp.panasonic.com>
Signed-off-by: David Howells <dhowells@redhat.com>

23 files changed, 2224 insertions, 240 deletions

diff --git a/arch/mn10300/kernel/Makefile b/arch/mn10300/kernel/Makefile
index 99022351717a..5b41192f496b 100644
--- a/arch/mn10300/kernel/Makefile
+++ b/arch/mn10300/kernel/Makefile
@@ -10,8 +10,9 @@ obj-y   := process.o signal.o entry.o traps.o irq.o \
            ptrace.o setup.o time.o sys_mn10300.o io.o kthread.o \
            switch_to.o mn10300_ksyms.o kernel_execve.o $(fpu-obj-y)
 
-obj-$(CONFIG_MN10300_WD_TIMER) += mn10300-watchdog.o mn10300-watchdog-low.o
+obj-$(CONFIG_SMP) += smp.o smp-low.o
 
+obj-$(CONFIG_MN10300_WD_TIMER) += mn10300-watchdog.o mn10300-watchdog-low.o
 
 obj-$(CONFIG_MN10300_TTYSM) += mn10300-serial.o mn10300-serial-low.o \
                                mn10300-debug.o
diff --git a/arch/mn10300/kernel/asm-offsets.c b/arch/mn10300/kernel/asm-offsets.c
index 78e290e342fc..54cc5b6b13f2 100644
--- a/arch/mn10300/kernel/asm-offsets.c
+++ b/arch/mn10300/kernel/asm-offsets.c
@@ -66,7 +66,7 @@ void foo(void)
         OFFSET(THREAD_SP,               thread_struct, sp);
         OFFSET(THREAD_A3,               thread_struct, a3);
         OFFSET(THREAD_USP,              thread_struct, usp);
-        OFFSET(THREAD_FRAME,            thread_struct, __frame);
+        OFFSET(THREAD_FRAME,            thread_struct, frame);
 #ifdef CONFIG_FPU
         OFFSET(THREAD_FPU_FLAGS,        thread_struct, fpu_flags);
         OFFSET(THREAD_FPU_STATE,        thread_struct, fpu_state);
diff --git a/arch/mn10300/kernel/entry.S b/arch/mn10300/kernel/entry.S
index 355f68176771..f00b9bafcd3e 100644
--- a/arch/mn10300/kernel/entry.S
+++ b/arch/mn10300/kernel/entry.S
@@ -28,25 +28,17 @@
 #include <asm/asm-offsets.h>
 #include <asm/frame.inc>
 
+#if defined(CONFIG_SMP) && defined(CONFIG_GDBSTUB)
+#include <asm/gdb-stub.h>
+#endif /* CONFIG_SMP && CONFIG_GDBSTUB */
+
 #ifdef CONFIG_PREEMPT
-#define preempt_stop            __cli
+#define preempt_stop            LOCAL_IRQ_DISABLE
 #else
 #define preempt_stop
 #define resume_kernel           restore_all
 #endif
 
-        .macro __cli
-        and     ~EPSW_IM,epsw
-        or      EPSW_IE|MN10300_CLI_LEVEL,epsw
-        nop
-        nop
-        nop
-        .endm
-        .macro __sti
-        or      EPSW_IE|EPSW_IM_7,epsw
-        .endm
-
-
         .am33_2
 
 ###############################################################################
@@ -88,7 +80,7 @@ syscall_call:
 syscall_exit:
         # make sure we don't miss an interrupt setting need_resched or
         # sigpending between sampling and the rti
-        __cli
+        LOCAL_IRQ_DISABLE
         mov     (TI_flags,a2),d2
         btst    _TIF_ALLWORK_MASK,d2
         bne     syscall_exit_work
@@ -105,7 +97,7 @@ restore_all:
 syscall_exit_work:
         btst    _TIF_SYSCALL_TRACE,d2
         beq     work_pending
-        __sti                           # could let syscall_trace_exit() call
+        LOCAL_IRQ_ENABLE                # could let syscall_trace_exit() call
                                         # schedule() instead
         mov     fp,d0
         call    syscall_trace_exit[],0  # do_syscall_trace(regs)
@@ -121,7 +113,7 @@ work_resched:
 
         # make sure we don't miss an interrupt setting need_resched or
         # sigpending between sampling and the rti
-        __cli
+        LOCAL_IRQ_DISABLE
 
         # is there any work to be done other than syscall tracing?
         mov     (TI_flags,a2),d2
@@ -168,7 +160,7 @@ ret_from_intr:
 ENTRY(resume_userspace)
         # make sure we don't miss an interrupt setting need_resched or
         # sigpending between sampling and the rti
-        __cli
+        LOCAL_IRQ_DISABLE
 
         # is there any work to be done on int/exception return?
         mov     (TI_flags,a2),d2
@@ -178,7 +170,7 @@ ENTRY(resume_userspace)
 
 #ifdef CONFIG_PREEMPT
 ENTRY(resume_kernel)
-        __cli
+        LOCAL_IRQ_DISABLE
         mov     (TI_preempt_count,a2),d0        # non-zero preempt_count ?
         cmp     0,d0
         bne     restore_all
@@ -281,6 +273,79 @@ ENTRY(nmi_handler)
         add     -4,sp
         mov     d0,(sp)
         mov     (TBR),d0
+
+#ifdef CONFIG_SMP
+        add     -4,sp
+        mov     d0,(sp)                 # save d0(TBR)
+        movhu   (NMIAGR),d0
+        and     NMIAGR_GN,d0
+        lsr     0x2,d0
+        cmp     CALL_FUNCTION_NMI_IPI,d0
+        bne     5f                      # if not call function, jump
+
+        # function call nmi ipi
+        add     4,sp                    # no need to store TBR
+        mov     GxICR_DETECT,d0         # clear NMI request
+        movbu   d0,(GxICR(CALL_FUNCTION_NMI_IPI))
+        movhu   (GxICR(CALL_FUNCTION_NMI_IPI)),d0
+        and     ~EPSW_NMID,epsw         # enable NMI
+
+        mov     (sp),d0                 # restore d0
+        SAVE_ALL
+        call    smp_nmi_call_function_interrupt[],0
+        RESTORE_ALL
+
+5:
+#ifdef CONFIG_GDBSTUB
+        cmp     GDB_NMI_IPI,d0
+        bne     3f                      # if not gdb nmi ipi, jump
+
+        # gdb nmi ipi
+        add     4,sp                    # no need to store TBR
+        mov     GxICR_DETECT,d0         # clear NMI
+        movbu   d0,(GxICR(GDB_NMI_IPI))
+        movhu   (GxICR(GDB_NMI_IPI)),d0
+        and     ~EPSW_NMID,epsw         # enable NMI
+#ifdef CONFIG_MN10300_CACHE_ENABLED
+        mov     (gdbstub_nmi_opr_type),d0
+        cmp     GDBSTUB_NMI_CACHE_PURGE,d0
+        bne     4f                      # if not gdb cache purge, jump
+
+        # gdb cache purge nmi ipi
+        add     -20,sp
+        mov     d1,(4,sp)
+        mov     a0,(8,sp)
+        mov     a1,(12,sp)
+        mov     mdr,d0
+        mov     d0,(16,sp)
+        call    gdbstub_local_purge_cache[],0
+        mov     0x1,d0
+        mov     (CPUID),d1
+        asl     d1,d0
+        mov     gdbstub_nmi_cpumask,a0
+        bclr    d0,(a0)
+        mov     (4,sp),d1
+        mov     (8,sp),a0
+        mov     (12,sp),a1
+        mov     (16,sp),d0
+        mov     d0,mdr
+        add     20,sp
+        mov     (sp),d0
+        add     4,sp
+        rti
+4:
+#endif /* CONFIG_MN10300_CACHE_ENABLED */
+        # gdb wait nmi ipi
+        mov     (sp),d0
+        SAVE_ALL
+        call    gdbstub_nmi_wait[],0
+        RESTORE_ALL
+3:
+#endif /* CONFIG_GDBSTUB */
+        mov     (sp),d0                 # restore TBR to d0
+        add     4,sp
+#endif /* CONFIG_SMP */
+
         bra     __common_exception_nonmi
 
 ENTRY(__common_exception)
@@ -314,15 +379,21 @@ __common_exception_nonmi:
         mov     d0,(REG_ORIG_D0,fp)
 
 #ifdef CONFIG_GDBSTUB
+#ifdef CONFIG_SMP
+        call    gdbstub_busy_check[],0
+        and     d0,d0                   # check return value
+        beq     2f
+#else  /* CONFIG_SMP */
         btst    0x01,(gdbstub_busy)
         beq     2f
+#endif /* CONFIG_SMP */
         and     ~EPSW_IE,epsw
         mov     fp,d0
         mov     a2,d1
         call    gdbstub_exception[],0   # gdbstub itself caused an exception
         bra     restore_all
 2:
-#endif
+#endif /* CONFIG_GDBSTUB */
 
         mov     fp,d0                   # arg 0: stacked register file
         mov     a2,d1                   # arg 1: exception number
@@ -357,11 +428,7 @@ ENTRY(set_excp_vector)
         add     exception_table,d0
         mov     d1,(d0)
         mov     4,d1
-#if defined(CONFIG_MN10300_CACHE_WBACK)
-        jmp     mn10300_dcache_flush_inv_range2
-#else
         ret     [],0
-#endif
 
 ###############################################################################
 #
diff --git a/arch/mn10300/kernel/gdb-io-serial-low.S b/arch/mn10300/kernel/gdb-io-serial-low.S
index 4998b24f5d3a..b1d0152e96cb 100644
--- a/arch/mn10300/kernel/gdb-io-serial-low.S
+++ b/arch/mn10300/kernel/gdb-io-serial-low.S
@@ -18,6 +18,7 @@
 #include <asm/thread_info.h>
 #include <asm/frame.inc>
 #include <asm/intctl-regs.h>
+#include <asm/irqflags.h>
 #include <unit/serial.h>
 
         .text
@@ -69,7 +70,7 @@ gdbstub_io_rx_overflow:
         bra     gdbstub_io_rx_done
 
 gdbstub_io_rx_enter:
-        or      EPSW_IE|EPSW_IM_1,epsw
+        LOCAL_CHANGE_INTR_MASK_LEVEL(NUM2EPSW_IM(CONFIG_GDBSTUB_IRQ_LEVEL+1))
         add     -4,sp
         SAVE_ALL
 
@@ -80,7 +81,7 @@ gdbstub_io_rx_enter:
         mov     fp,d0
         call    gdbstub_rx_irq[],0      # gdbstub_rx_irq(regs,excep)
 
-        and     ~EPSW_IE,epsw
+        LOCAL_CLI
         bclr    0x01,(gdbstub_busy)
 
         .globl gdbstub_return
diff --git a/arch/mn10300/kernel/gdb-io-serial.c b/arch/mn10300/kernel/gdb-io-serial.c
index ae663dc717e9..0d5d63c91dc3 100644
--- a/arch/mn10300/kernel/gdb-io-serial.c
+++ b/arch/mn10300/kernel/gdb-io-serial.c
@@ -23,6 +23,7 @@
 #include <asm/exceptions.h>
 #include <asm/serial-regs.h>
 #include <unit/serial.h>
+#include <asm/smp.h>
 
 /*
  * initialise the GDB stub
@@ -45,22 +46,34 @@ void gdbstub_io_init(void)
         XIRQxICR(GDBPORT_SERIAL_IRQ) = 0;
         tmp = XIRQxICR(GDBPORT_SERIAL_IRQ);
 
+#if   CONFIG_GDBSTUB_IRQ_LEVEL == 0
         IVAR0 = EXCEP_IRQ_LEVEL0;
-        set_intr_stub(EXCEP_IRQ_LEVEL0, gdbstub_io_rx_handler);
+#elif CONFIG_GDBSTUB_IRQ_LEVEL == 1
+        IVAR1 = EXCEP_IRQ_LEVEL1;
+#elif CONFIG_GDBSTUB_IRQ_LEVEL == 2
+        IVAR2 = EXCEP_IRQ_LEVEL2;
+#elif CONFIG_GDBSTUB_IRQ_LEVEL == 3
+        IVAR3 = EXCEP_IRQ_LEVEL3;
+#elif CONFIG_GDBSTUB_IRQ_LEVEL == 4
+        IVAR4 = EXCEP_IRQ_LEVEL4;
+#elif CONFIG_GDBSTUB_IRQ_LEVEL == 5
+        IVAR5 = EXCEP_IRQ_LEVEL5;
+#else
+#error "Unknown irq level for gdbstub."
+#endif
+
+        set_intr_stub(NUM2EXCEP_IRQ_LEVEL(CONFIG_GDBSTUB_IRQ_LEVEL),
+                gdbstub_io_rx_handler);
 
         XIRQxICR(GDBPORT_SERIAL_IRQ) &= ~GxICR_REQUEST;
-        XIRQxICR(GDBPORT_SERIAL_IRQ) = GxICR_ENABLE | GxICR_LEVEL_0;
+        XIRQxICR(GDBPORT_SERIAL_IRQ) =
+                GxICR_ENABLE | NUM2GxICR_LEVEL(CONFIG_GDBSTUB_IRQ_LEVEL);
         tmp = XIRQxICR(GDBPORT_SERIAL_IRQ);
 
         GDBPORT_SERIAL_IER = UART_IER_RDI | UART_IER_RLSI;
 
         /* permit level 0 IRQs to take place */
-        asm volatile(
-                "       and %0,epsw     \n"
-                "       or %1,epsw      \n"
-                :
-                : "i"(~EPSW_IM), "i"(EPSW_IE | EPSW_IM_1)
-                );
+        local_change_intr_mask_level(NUM2EPSW_IM(CONFIG_GDBSTUB_IRQ_LEVEL + 1));
 }
 
 /*
@@ -87,6 +100,9 @@ int gdbstub_io_rx_char(unsigned char *_ch, int nonblock)
 {
         unsigned ix;
         u8 ch, st;
+#if defined(CONFIG_MN10300_WD_TIMER)
+        int cpu;
+#endif
 
         *_ch = 0xff;
 
@@ -104,8 +120,9 @@ int gdbstub_io_rx_char(unsigned char *_ch, int nonblock)
                 if (nonblock)
                         return -EAGAIN;
 #ifdef CONFIG_MN10300_WD_TIMER
-                watchdog_alert_counter = 0;
-#endif /* CONFIG_MN10300_WD_TIMER */
+        for (cpu = 0; cpu < NR_CPUS; cpu++)
+                watchdog_alert_counter[cpu] = 0;
+#endif
                 goto try_again;
         }
 
diff --git a/arch/mn10300/kernel/gdb-io-ttysm.c b/arch/mn10300/kernel/gdb-io-ttysm.c
index a560bbc3137d..97dfda23342c 100644
--- a/arch/mn10300/kernel/gdb-io-ttysm.c
+++ b/arch/mn10300/kernel/gdb-io-ttysm.c
@@ -58,9 +58,12 @@ void __init gdbstub_io_init(void)
         gdbstub_io_set_baud(115200);
 
         /* we want to get serial receive interrupts */
-        set_intr_level(gdbstub_port->rx_irq, GxICR_LEVEL_0);
-        set_intr_level(gdbstub_port->tx_irq, GxICR_LEVEL_0);
-        set_intr_stub(EXCEP_IRQ_LEVEL0, gdbstub_io_rx_handler);
+        set_intr_level(gdbstub_port->rx_irq,
+                NUM2GxICR_LEVEL(CONFIG_GDBSTUB_IRQ_LEVEL));
+        set_intr_level(gdbstub_port->tx_irq,
+                NUM2GxICR_LEVEL(CONFIG_GDBSTUB_IRQ_LEVEL));
+        set_intr_stub(NUM2EXCEP_IRQ_LEVEL(CONFIG_GDBSTUB_IRQ_LEVEL),
+                gdbstub_io_rx_handler);
 
         *gdbstub_port->rx_icr |= GxICR_ENABLE;
         tmp = *gdbstub_port->rx_icr;
@@ -84,12 +87,7 @@ void __init gdbstub_io_init(void)
         tmp = *gdbstub_port->_control;
 
         /* permit level 0 IRQs only */
-        asm volatile(
-                "       and %0,epsw     \n"
-                "       or %1,epsw      \n"
-                :
-                : "i"(~EPSW_IM), "i"(EPSW_IE|EPSW_IM_1)
-                );
+        local_change_intr_mask_level(NUM2EPSW_IM(CONFIG_GDBSTUB_IRQ_LEVEL + 1));
 }
 
 /*
@@ -184,6 +182,9 @@ int gdbstub_io_rx_char(unsigned char *_ch, int nonblock)
 {
         unsigned ix;
         u8 ch, st;
+#if defined(CONFIG_MN10300_WD_TIMER)
+        int cpu;
+#endif
 
         *_ch = 0xff;
 
@@ -201,8 +202,9 @@ try_again:
                 if (nonblock)
                         return -EAGAIN;
 #ifdef CONFIG_MN10300_WD_TIMER
-                watchdog_alert_counter = 0;
-#endif /* CONFIG_MN10300_WD_TIMER */
+        for (cpu = 0; cpu < NR_CPUS; cpu++)
+                watchdog_alert_counter[cpu] = 0;
+#endif
                 goto try_again;
         }
 
diff --git a/arch/mn10300/kernel/gdb-stub.c b/arch/mn10300/kernel/gdb-stub.c
index 41b11706c8ed..a5fc3f05309b 100644
--- a/arch/mn10300/kernel/gdb-stub.c
+++ b/arch/mn10300/kernel/gdb-stub.c
@@ -440,15 +440,11 @@ static const unsigned char gdbstub_insn_sizes[256] =
 
 static int __gdbstub_mark_bp(u8 *addr, int ix)
 {
-        if (addr < (u8 *) 0x70000000UL)
-                return 0;
-        /* 70000000-7fffffff: vmalloc area */
-        if (addr < (u8 *) 0x80000000UL)
+        /* vmalloc area */
+        if (((u8 *) VMALLOC_START <= addr) && (addr < (u8 *) VMALLOC_END))
                 goto okay;
-        if (addr < (u8 *) 0x8c000000UL)
-                return 0;
-        /* 8c000000-93ffffff: SRAM, SDRAM */
-        if (addr < (u8 *) 0x94000000UL)
+        /* SRAM, SDRAM */
+        if (((u8 *) 0x80000000UL <= addr) && (addr < (u8 *) 0xa0000000UL))
                 goto okay;
         return 0;
 
@@ -1197,9 +1193,8 @@ static int gdbstub(struct pt_regs *regs, enum exception_code excep)
         mn10300_set_gdbleds(1);
 
         asm volatile("mov mdr,%0" : "=d"(mdr));
-        asm volatile("mov epsw,%0" : "=d"(epsw));
-        asm volatile("mov %0,epsw"
-                     :: "d"((epsw & ~EPSW_IM) | EPSW_IE | EPSW_IM_1));
+        local_save_flags(epsw);
+        local_change_intr_mask_level(NUM2EPSW_IM(CONFIG_GDBSTUB_IRQ_LEVEL + 1));
 
         gdbstub_store_fpu();
 
diff --git a/arch/mn10300/kernel/head.S b/arch/mn10300/kernel/head.S
index a81e34fba651..73e00fc78072 100644
--- a/arch/mn10300/kernel/head.S
+++ b/arch/mn10300/kernel/head.S
@@ -19,6 +19,12 @@
 #include <asm/frame.inc>
 #include <asm/param.h>
 #include <unit/serial.h>
+#ifdef CONFIG_SMP
+#include <asm/smp.h>
+#include <asm/intctl-regs.h>
+#include <asm/cpu-regs.h>
+#include <proc/smp-regs.h>
+#endif /* CONFIG_SMP */
 
         __HEAD
 
@@ -30,17 +36,51 @@
         .globl  _start
         .type   _start,@function
 _start:
+#ifdef CONFIG_SMP
+        #
+        # If this is a secondary CPU (AP), then deal with that elsewhere
+        #
+        mov     (CPUID),d3
+        and     CPUID_MASK,d3
+        bne     startup_secondary
+
+        #
+        # We're dealing with the primary CPU (BP) here, then.
+        # Keep BP's D0,D1,D2 register for boot check.
+        #
+
+        # Set up the Boot IPI for each secondary CPU
+        mov     0x1,a0
+loop_set_secondary_icr:
+        mov     a0,a1
+        asl     CROSS_ICR_CPU_SHIFT,a1
+        add     CROSS_GxICR(SMP_BOOT_IRQ,0),a1
+        movhu   (a1),d3
+        or      GxICR_ENABLE|GxICR_LEVEL_0,d3
+        movhu   d3,(a1)
+        movhu   (a1),d3                         # flush
+        inc     a0
+        cmp     NR_CPUS,a0
+        bne     loop_set_secondary_icr
+#endif /* CONFIG_SMP */
+
         # save commandline pointer
         mov     d0,a3
 
         # preload the PGD pointer register
         mov     swapper_pg_dir,d0
         mov     d0,(PTBR)
+        clr     d0
+        movbu   d0,(PIDR)
 
         # turn on the TLBs
         mov     MMUCTR_IIV|MMUCTR_DIV,d0
         mov     d0,(MMUCTR)
+#ifdef CONFIG_AM34_2
+        mov     MMUCTR_ITE|MMUCTR_DTE|MMUCTR_CE|MMUCTR_WTE,d0
+#else
         mov     MMUCTR_ITE|MMUCTR_DTE|MMUCTR_CE,d0
+#endif
         mov     d0,(MMUCTR)
 
         # turn on AM33v2 exception handling mode and set the trap table base
@@ -51,6 +91,11 @@ _start:
         mov     d0,(TBR)
 
         # invalidate and enable both of the caches
+#ifdef CONFIG_SMP
+        mov     ECHCTR,a0
+        clr     d0
+        mov     d0,(a0)
+#endif
         mov     CHCTR,a0
         clr     d0
         movhu   d0,(a0)                                 # turn off first
@@ -206,6 +251,44 @@ __no_parameters:
         call    processor_init[],0
         call    unit_init[],0
 
+#ifdef CONFIG_SMP
+        # mark the primary CPU in cpu_boot_map
+        mov     cpu_boot_map,a0
+        mov     0x1,d0
+        mov     d0,(a0)
+
+        # signal each secondary CPU to begin booting
+        mov     0x1,d2                          # CPU ID
+
+loop_request_boot_secondary:
+        mov     d2,a0
+        # send SMP_BOOT_IPI to secondary CPU
+        asl     CROSS_ICR_CPU_SHIFT,a0
+        add     CROSS_GxICR(SMP_BOOT_IRQ,0),a0
+        movhu   (a0),d0
+        or      GxICR_REQUEST|GxICR_DETECT,d0
+        movhu   d0,(a0)
+        movhu   (a0),d0                         # flush
+
+        # wait up to 100ms for AP's IPI to be received
+        clr     d3
+wait_on_secondary_boot:
+        mov     DELAY_TIME_BOOT_IPI,d0
+        call    __delay[],0
+        inc     d3
+        mov     cpu_boot_map,a0
+        mov     (a0),d0
+        lsr     d2,d0
+        btst    0x1,d0
+        bne     1f
+        cmp     TIME_OUT_COUNT_BOOT_IPI,d3
+        bne     wait_on_secondary_boot
+1:
+        inc     d2
+        cmp     NR_CPUS,d2
+        bne     loop_request_boot_secondary
+#endif /* CONFIG_SMP */
+
 #ifdef CONFIG_GDBSTUB
         call    gdbstub_init[],0
 
@@ -217,7 +300,118 @@ __gdbstub_pause:
 #endif
 
         jmp     start_kernel
-        .size   _start, _start-.
+        .size   _start,.-_start
+
+###############################################################################
+#
+# Secondary CPU boot point
+#
+###############################################################################
+#ifdef CONFIG_SMP
+startup_secondary:
+        # preload the PGD pointer register
+        mov     swapper_pg_dir,d0
+        mov     d0,(PTBR)
+        clr     d0
+        movbu   d0,(PIDR)
+
+        # turn on the TLBs
+        mov     MMUCTR_IIV|MMUCTR_DIV,d0
+        mov     d0,(MMUCTR)
+#ifdef CONFIG_AM34_2
+        mov     MMUCTR_ITE|MMUCTR_DTE|MMUCTR_CE|MMUCTR_WTE,d0
+#else
+        mov     MMUCTR_ITE|MMUCTR_DTE|MMUCTR_CE,d0
+#endif
+        mov     d0,(MMUCTR)
+
+        # turn on AM33v2 exception handling mode and set the trap table base
+        movhu   (CPUP),d0
+        or      CPUP_EXM_AM33V2,d0
+        movhu   d0,(CPUP)
+
+        # set the interrupt vector table
+        mov     CONFIG_INTERRUPT_VECTOR_BASE,d0
+        mov     d0,(TBR)
+
+        # invalidate and enable both of the caches
+        mov     ECHCTR,a0
+        clr     d0
+        mov     d0,(a0)
+        mov     CHCTR,a0
+        clr     d0
+        movhu   d0,(a0)                                 # turn off first
+        mov     CHCTR_ICINV|CHCTR_DCINV,d0
+        movhu   d0,(a0)
+        setlb
+        mov     (a0),d0
+        btst    CHCTR_ICBUSY|CHCTR_DCBUSY,d0            # wait till not busy (use CPU loop buffer)
+        lne
+
+#ifdef CONFIG_MN10300_CACHE_ENABLED
+#ifdef  CONFIG_MN10300_CACHE_WBACK
+#ifndef CONFIG_MN10300_CACHE_WBACK_NOWRALLOC
+        mov     CHCTR_ICEN|CHCTR_DCEN|CHCTR_DCWTMD_WRBACK,d0
+#else
+        mov     CHCTR_ICEN|CHCTR_DCEN|CHCTR_DCWTMD_WRBACK|CHCTR_DCALMD,d0
+#endif  /* !NOWRALLOC */
+#else
+        mov     CHCTR_ICEN|CHCTR_DCEN|CHCTR_DCWTMD_WRTHROUGH,d0
+#endif  /* WBACK */
+        movhu   d0,(a0)                                 # enable
+#endif  /* ENABLED */
+
+        # Clear the boot IPI interrupt for this CPU
+        movhu   (GxICR(SMP_BOOT_IRQ)),d0
+        and     ~GxICR_REQUEST,d0
+        movhu   d0,(GxICR(SMP_BOOT_IRQ))
+        movhu   (GxICR(SMP_BOOT_IRQ)),d0                # flush
+
+        /* get stack */
+        mov     CONFIG_INTERRUPT_VECTOR_BASE + CONFIG_BOOT_STACK_OFFSET,a0
+        mov     (CPUID),d0
+        and     CPUID_MASK,d0
+        mulu    CONFIG_BOOT_STACK_SIZE,d0
+        sub     d0,a0
+        mov     a0,sp
+
+        # init interrupt for AP
+        call    smp_prepare_cpu_init[],0
+
+        # mark this secondary CPU in cpu_boot_map
+        mov     (CPUID),d0
+        mov     0x1,d1
+        asl     d0,d1
+        mov     cpu_boot_map,a0
+        bset    d1,(a0)
+
+        or      EPSW_IE|EPSW_IM_1,epsw  # permit level 0 interrupts
+        nop
+        nop
+#ifdef  CONFIG_MN10300_CACHE_WBACK
+        # flush the local cache if it's in writeback mode
+        call    mn10300_local_dcache_flush_inv[],0
+        setlb
+        mov     (CHCTR),d0
+        btst    CHCTR_DCBUSY,d0         # wait till not busy (use CPU loop buffer)
+        lne
+#endif
+
+        # now sleep waiting for further instructions
+secondary_sleep:
+        mov     CPUM_SLEEP,d0
+        movhu   d0,(CPUM)
+        nop
+        nop
+        bra     secondary_sleep
+        .size   startup_secondary,.-startup_secondary
+#endif /* CONFIG_SMP */
+
+###############################################################################
+#
+#
+#
+###############################################################################
 ENTRY(__head_end)
 
 /*
diff --git a/arch/mn10300/kernel/internal.h b/arch/mn10300/kernel/internal.h
index eee2eee86267..3b1f48b7e7f4 100644
--- a/arch/mn10300/kernel/internal.h
+++ b/arch/mn10300/kernel/internal.h
@@ -18,3 +18,15 @@ extern int kernel_thread_helper(int);
  * entry.S
  */
 extern void ret_from_fork(struct task_struct *) __attribute__((noreturn));
+
+/*
+ * smp-low.S
+ */
+#ifdef CONFIG_SMP
+extern void mn10300_low_ipi_handler(void);
+#endif
+
+/*
+ * time.c
+ */
+extern irqreturn_t local_timer_interrupt(void);
diff --git a/arch/mn10300/kernel/irq.c b/arch/mn10300/kernel/irq.c
index b5b970d2954a..80f15725ecad 100644
--- a/arch/mn10300/kernel/irq.c
+++ b/arch/mn10300/kernel/irq.c
@@ -12,11 +12,34 @@
 #include <linux/interrupt.h>
 #include <linux/kernel_stat.h>
 #include <linux/seq_file.h>
+#include <linux/cpumask.h>
 #include <asm/setup.h>
+#include <asm/serial-regs.h>
 
-unsigned long __mn10300_irq_enabled_epsw = EPSW_IE | EPSW_IM_7;
+#ifdef CONFIG_SMP
+#undef  GxICR
+#define GxICR(X) CROSS_GxICR(X, irq_affinity_online[X])
+
+#undef  GxICR_u8
+#define GxICR_u8(X) CROSS_GxICR_u8(X, irq_affinity_online[X])
+#endif /* CONFIG_SMP */
+
+unsigned long __mn10300_irq_enabled_epsw[NR_CPUS] __cacheline_aligned_in_smp = {
+        [0 ... NR_CPUS - 1] = EPSW_IE | EPSW_IM_7
+};
 EXPORT_SYMBOL(__mn10300_irq_enabled_epsw);
 
+#ifdef CONFIG_SMP
+static char irq_affinity_online[NR_IRQS] = {
+        [0 ... NR_IRQS - 1] = 0
+};
+
+#define NR_IRQ_WORDS    ((NR_IRQS + 31) / 32)
+static unsigned long irq_affinity_request[NR_IRQ_WORDS] = {
+        [0 ... NR_IRQ_WORDS - 1] = 0
+};
+#endif  /* CONFIG_SMP */
+
 atomic_t irq_err_count;
 
 /*
@@ -24,30 +47,65 @@ atomic_t irq_err_count;
  */
 static void mn10300_cpupic_ack(unsigned int irq)
 {
+        unsigned long flags;
+        u16 tmp;
+
+        flags = arch_local_cli_save();
+        GxICR_u8(irq) = GxICR_DETECT;
+        tmp = GxICR(irq);
+        arch_local_irq_restore(flags);
+}
+
+static void __mask_and_set_icr(unsigned int irq,
+                               unsigned int mask, unsigned int set)
+{
+        unsigned long flags;
         u16 tmp;
-        *(volatile u8 *) &GxICR(irq) = GxICR_DETECT;
+
+        flags = arch_local_cli_save();
         tmp = GxICR(irq);
+        GxICR(irq) = (tmp & mask) | set;
+        tmp = GxICR(irq);
+        arch_local_irq_restore(flags);
 }
 
 static void mn10300_cpupic_mask(unsigned int irq)
 {
-        u16 tmp = GxICR(irq);
-        GxICR(irq) = (tmp & GxICR_LEVEL);
-        tmp = GxICR(irq);
+        __mask_and_set_icr(irq, GxICR_LEVEL, 0);
 }
 
 static void mn10300_cpupic_mask_ack(unsigned int irq)
 {
-        u16 tmp = GxICR(irq);
-        GxICR(irq) = (tmp & GxICR_LEVEL) | GxICR_DETECT;
-        tmp = GxICR(irq);
+#ifdef CONFIG_SMP
+        unsigned long flags;
+        u16 tmp;
+
+        flags = arch_local_cli_save();
+
+        if (!test_and_clear_bit(irq, irq_affinity_request)) {
+                tmp = GxICR(irq);
+                GxICR(irq) = (tmp & GxICR_LEVEL) | GxICR_DETECT;
+                tmp = GxICR(irq);
+        } else {
+                u16 tmp2;
+                tmp = GxICR(irq);
+                GxICR(irq) = (tmp & GxICR_LEVEL);
+                tmp2 = GxICR(irq);
+
+                irq_affinity_online[irq] = any_online_cpu(*irq_desc[irq].affinity);
+                GxICR(irq) = (tmp & (GxICR_LEVEL | GxICR_ENABLE)) | GxICR_DETECT;
+                tmp = GxICR(irq);
+        }
+
+        arch_local_irq_restore(flags);
+#else  /* CONFIG_SMP */
+        __mask_and_set_icr(irq, GxICR_LEVEL, GxICR_DETECT);
+#endif /* CONFIG_SMP */
 }
 
 static void mn10300_cpupic_unmask(unsigned int irq)
 {
-        u16 tmp = GxICR(irq);
-        GxICR(irq) = (tmp & GxICR_LEVEL) | GxICR_ENABLE;
-        tmp = GxICR(irq);
+        __mask_and_set_icr(irq, GxICR_LEVEL, GxICR_ENABLE);
 }
 
 static void mn10300_cpupic_unmask_clear(unsigned int irq)
@@ -56,11 +114,89 @@ static void mn10300_cpupic_unmask_clear(unsigned int irq)
          * device has ceased to assert its interrupt line and the interrupt
          * channel has been disabled in the PIC, so for level-triggered
          * interrupts we need to clear the request bit when we re-enable */
-        u16 tmp = GxICR(irq);
-        GxICR(irq) = (tmp & GxICR_LEVEL) | GxICR_ENABLE | GxICR_DETECT;
-        tmp = GxICR(irq);
+#ifdef CONFIG_SMP
+        unsigned long flags;
+        u16 tmp;
+
+        flags = arch_local_cli_save();
+
+        if (!test_and_clear_bit(irq, irq_affinity_request)) {
+                tmp = GxICR(irq);
+                GxICR(irq) = (tmp & GxICR_LEVEL) | GxICR_ENABLE | GxICR_DETECT;
+                tmp = GxICR(irq);
+        } else {
+                tmp = GxICR(irq);
+
+                irq_affinity_online[irq] = any_online_cpu(*irq_desc[irq].affinity);
+                GxICR(irq) = (tmp & GxICR_LEVEL) | GxICR_ENABLE | GxICR_DETECT;
+                tmp = GxICR(irq);
+        }
+
+        arch_local_irq_restore(flags);
+#else  /* CONFIG_SMP */
+        __mask_and_set_icr(irq, GxICR_LEVEL, GxICR_ENABLE | GxICR_DETECT);
+#endif /* CONFIG_SMP */
 }
 
+#ifdef CONFIG_SMP
+static int
+mn10300_cpupic_setaffinity(unsigned int irq, const struct cpumask *mask)
+{
+        unsigned long flags;
+        int err;
+
+        flags = arch_local_cli_save();
+
+        /* check irq no */
+        switch (irq) {
+        case TMJCIRQ:
+        case RESCHEDULE_IPI:
+        case CALL_FUNC_SINGLE_IPI:
+        case LOCAL_TIMER_IPI:
+        case FLUSH_CACHE_IPI:
+        case CALL_FUNCTION_NMI_IPI:
+        case GDB_NMI_IPI:
+#ifdef CONFIG_MN10300_TTYSM0
+        case SC0RXIRQ:
+        case SC0TXIRQ:
+#ifdef CONFIG_MN10300_TTYSM0_TIMER8
+        case TM8IRQ:
+#elif CONFIG_MN10300_TTYSM0_TIMER2
+        case TM2IRQ:
+#endif /* CONFIG_MN10300_TTYSM0_TIMER8 */
+#endif /* CONFIG_MN10300_TTYSM0 */
+
+#ifdef CONFIG_MN10300_TTYSM1
+        case SC1RXIRQ:
+        case SC1TXIRQ:
+#ifdef CONFIG_MN10300_TTYSM1_TIMER12
+        case TM12IRQ:
+#elif CONFIG_MN10300_TTYSM1_TIMER9
+        case TM9IRQ:
+#elif CONFIG_MN10300_TTYSM1_TIMER3
+        case TM3IRQ:
+#endif /* CONFIG_MN10300_TTYSM1_TIMER12 */
+#endif /* CONFIG_MN10300_TTYSM1 */
+
+#ifdef CONFIG_MN10300_TTYSM2
+        case SC2RXIRQ:
+        case SC2TXIRQ:
+        case TM10IRQ:
+#endif /* CONFIG_MN10300_TTYSM2 */
+                err = -1;
+                break;
+
+        default:
+                set_bit(irq, irq_affinity_request);
+                err = 0;
+                break;
+        }
+
+        arch_local_irq_restore(flags);
+        return err;
+}
+#endif /* CONFIG_SMP */
+
 /*
  * MN10300 PIC level-triggered IRQ handling.
  *
@@ -79,6 +215,9 @@ static struct irq_chip mn10300_cpu_pic_level = {
         .mask           = mn10300_cpupic_mask,
         .mask_ack       = mn10300_cpupic_mask,
         .unmask         = mn10300_cpupic_unmask_clear,
+#ifdef CONFIG_SMP
+        .set_affinity   = mn10300_cpupic_setaffinity,
+#endif /* CONFIG_SMP */
 };
 
 /*
@@ -94,6 +233,9 @@ static struct irq_chip mn10300_cpu_pic_edge = {
         .mask           = mn10300_cpupic_mask,
         .mask_ack       = mn10300_cpupic_mask_ack,
         .unmask         = mn10300_cpupic_unmask,
+#ifdef CONFIG_SMP
+        .set_affinity   = mn10300_cpupic_setaffinity,
+#endif /* CONFIG_SMP */
 };
 
 /*
@@ -111,14 +253,34 @@ void ack_bad_irq(int irq)
  */
 void set_intr_level(int irq, u16 level)
 {
-        u16 tmp;
+        BUG_ON(in_interrupt());
 
-        if (in_interrupt())
-                BUG();
+        __mask_and_set_icr(irq, GxICR_ENABLE, level);
+}
 
-        tmp = GxICR(irq);
-        GxICR(irq) = (tmp & GxICR_ENABLE) | level;
-        tmp = GxICR(irq);
+void mn10300_intc_set_level(unsigned int irq, unsigned int level)
+{
+        set_intr_level(irq, NUM2GxICR_LEVEL(level) & GxICR_LEVEL);
+}
+
+void mn10300_intc_clear(unsigned int irq)
+{
+        __mask_and_set_icr(irq, GxICR_LEVEL | GxICR_ENABLE, GxICR_DETECT);
+}
+
+void mn10300_intc_set(unsigned int irq)
+{
+        __mask_and_set_icr(irq, 0, GxICR_REQUEST | GxICR_DETECT);
+}
+
+void mn10300_intc_enable(unsigned int irq)
+{
+        mn10300_cpupic_unmask(irq);
+}
+
+void mn10300_intc_disable(unsigned int irq)
+{
+        mn10300_cpupic_mask(irq);
 }
 
 /*
@@ -126,7 +288,7 @@ void set_intr_level(int irq, u16 level)
  * than before
  * - see Documentation/mn10300/features.txt
  */
-void set_intr_postackable(int irq)
+void mn10300_set_lateack_irq_type(int irq)
 {
         set_irq_chip_and_handler(irq, &mn10300_cpu_pic_level,
                                  handle_level_irq);
@@ -147,6 +309,7 @@ void __init init_IRQ(void)
                          * interrupts */
                         set_irq_chip_and_handler(irq, &mn10300_cpu_pic_edge,
                                                  handle_level_irq);
+
         unit_init_IRQ();
 }
 
@@ -156,6 +319,7 @@ void __init init_IRQ(void)
 asmlinkage void do_IRQ(void)
 {
         unsigned long sp, epsw, irq_disabled_epsw, old_irq_enabled_epsw;
+        unsigned int cpu_id = smp_processor_id();
         int irq;
 
         sp = current_stack_pointer();
@@ -163,12 +327,14 @@ asmlinkage void do_IRQ(void)
 
         /* make sure local_irq_enable() doesn't muck up the interrupt priority
          * setting in EPSW */
-        old_irq_enabled_epsw = __mn10300_irq_enabled_epsw;
+        old_irq_enabled_epsw = __mn10300_irq_enabled_epsw[cpu_id];
         local_save_flags(epsw);
-        __mn10300_irq_enabled_epsw = EPSW_IE | (EPSW_IM & epsw);
+        __mn10300_irq_enabled_epsw[cpu_id] = EPSW_IE | (EPSW_IM & epsw);
         irq_disabled_epsw = EPSW_IE | MN10300_CLI_LEVEL;
 
-        __IRQ_STAT(smp_processor_id(), __irq_count)++;
+#ifdef CONFIG_MN10300_WD_TIMER
+        __IRQ_STAT(cpu_id, __irq_count)++;
+#endif
 
         irq_enter();
 
@@ -188,7 +354,7 @@ asmlinkage void do_IRQ(void)
                 local_irq_restore(epsw);
         }
 
-        __mn10300_irq_enabled_epsw = old_irq_enabled_epsw;
+        __mn10300_irq_enabled_epsw[cpu_id] = old_irq_enabled_epsw;
 
         irq_exit();
 }
@@ -239,11 +405,13 @@ int show_interrupts(struct seq_file *p, void *v)
 
                 /* polish off with NMI and error counters */
         case NR_IRQS:
+#ifdef CONFIG_MN10300_WD_TIMER
                 seq_printf(p, "NMI: ");
                 for (j = 0; j < NR_CPUS; j++)
                         if (cpu_online(j))
                                 seq_printf(p, "%10u ", nmi_count(j));
                 seq_putc(p, '\n');
+#endif
 
                 seq_printf(p, "ERR: %10u\n", atomic_read(&irq_err_count));
                 break;
@@ -251,3 +419,51 @@ int show_interrupts(struct seq_file *p, void *v)
 
         return 0;
 }
+
+#ifdef CONFIG_HOTPLUG_CPU
+void migrate_irqs(void)
+{
+        irq_desc_t *desc;
+        int irq;
+        unsigned int self, new;
+        unsigned long flags;
+
+        self = smp_processor_id();
+        for (irq = 0; irq < NR_IRQS; irq++) {
+                desc = irq_desc + irq;
+
+                if (desc->status == IRQ_PER_CPU)
+                        continue;
+
+                if (cpu_isset(self, irq_desc[irq].affinity) &&
+                    !cpus_intersects(irq_affinity[irq], cpu_online_map)) {
+                        int cpu_id;
+                        cpu_id = first_cpu(cpu_online_map);
+                        cpu_set(cpu_id, irq_desc[irq].affinity);
+                }
+                /* We need to operate irq_affinity_online atomically. */
+                arch_local_cli_save(flags);
+                if (irq_affinity_online[irq] == self) {
+                        u16 x, tmp;
+
+                        x = CROSS_GxICR(irq, self);
+                        CROSS_GxICR(irq, self) = x & GxICR_LEVEL;
+                        tmp = CROSS_GxICR(irq, self);
+
+                        new = any_online_cpu(irq_desc[irq].affinity);
+                        irq_affinity_online[irq] = new;
+
+                        CROSS_GxICR(irq, new) =
+                                (x & GxICR_LEVEL) | GxICR_DETECT;
+                        tmp = CROSS_GxICR(irq, new);
+
+                        x &= GxICR_LEVEL | GxICR_ENABLE;
+                        if (CROSS_GxICR(irq, self) & GxICR_REQUEST)
+                                x |= GxICR_REQUEST | GxICR_DETECT;
+                        CROSS_GxICR(irq, new) = x;
+                        tmp = CROSS_GxICR(irq, new);
+                }
+                arch_local_irq_restore(flags);
+        }
+}
+#endif /* CONFIG_HOTPLUG_CPU */
diff --git a/arch/mn10300/kernel/mn10300-serial-low.S b/arch/mn10300/kernel/mn10300-serial-low.S
index 66702d256610..dfc1b6f2fa9a 100644
--- a/arch/mn10300/kernel/mn10300-serial-low.S
+++ b/arch/mn10300/kernel/mn10300-serial-low.S
@@ -39,7 +39,7 @@
 ###############################################################################
         .balign L1_CACHE_BYTES
 ENTRY(mn10300_serial_vdma_interrupt)
-        or      EPSW_IE,psw                     # permit overriding by
+#       or      EPSW_IE,psw                     # permit overriding by
                                                 # debugging interrupts
         movm    [d2,d3,a2,a3,exreg0],(sp)
 
@@ -164,7 +164,7 @@ mnsc_vdma_tx_noint:
         rti
 
 mnsc_vdma_tx_empty:
-        mov     +(GxICR_LEVEL_1|GxICR_DETECT),d2
+        mov     +(NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL)|GxICR_DETECT),d2
         movhu   d2,(e3)                 # disable the interrupt
         movhu   (e3),d2                 # flush
 
@@ -175,7 +175,7 @@ mnsc_vdma_tx_break:
         movhu   (SCxCTR,e2),d2          # turn on break mode
         or      SC01CTR_BKE,d2
         movhu   d2,(SCxCTR,e2)
-        mov     +(GxICR_LEVEL_1|GxICR_DETECT),d2
+        mov     +(NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL)|GxICR_DETECT),d2
         movhu   d2,(e3)                 # disable transmit interrupts on this
                                         # channel
         movhu   (e3),d2                 # flush
diff --git a/arch/mn10300/kernel/mn10300-serial.c b/arch/mn10300/kernel/mn10300-serial.c
index db509dd80565..996384dba45d 100644
--- a/arch/mn10300/kernel/mn10300-serial.c
+++ b/arch/mn10300/kernel/mn10300-serial.c
@@ -44,6 +44,11 @@ static const char serial_revdate[] = "2007-11-06";
 #include <unit/timex.h>
 #include "mn10300-serial.h"
 
+#ifdef CONFIG_SMP
+#undef  GxICR
+#define GxICR(X) CROSS_GxICR(X, 0)
+#endif /* CONFIG_SMP */
+
 #define kenter(FMT, ...) \
         printk(KERN_DEBUG "-->%s(" FMT ")\n", __func__, ##__VA_ARGS__)
 #define _enter(FMT, ...) \
@@ -57,6 +62,11 @@ static const char serial_revdate[] = "2007-11-06";
 #define _proto(FMT, ...) \
         no_printk(KERN_DEBUG "### MNSERIAL " FMT " ###\n", ##__VA_ARGS__)
 
+#ifndef CODMSB
+/* c_cflag bit meaning */
+#define CODMSB  004000000000    /* change Transfer bit-order */
+#endif
+
 #define NR_UARTS 3
 
 #ifdef CONFIG_MN10300_TTYSM_CONSOLE
@@ -152,26 +162,35 @@ struct mn10300_serial_port mn10300_serial_port_sif0 = {
         .name           = "ttySM0",
         ._iobase        = &SC0CTR,
         ._control       = &SC0CTR,
-        ._status        = (volatile u8 *) &SC0STR,
+        ._status        = (volatile u8 *)&SC0STR,
         ._intr          = &SC0ICR,
         ._rxb           = &SC0RXB,
         ._txb           = &SC0TXB,
         .rx_name        = "ttySM0:Rx",
         .tx_name        = "ttySM0:Tx",
-#ifdef CONFIG_MN10300_TTYSM0_TIMER8
+#if defined(CONFIG_MN10300_TTYSM0_TIMER8)
         .tm_name        = "ttySM0:Timer8",
         ._tmxmd         = &TM8MD,
         ._tmxbr         = &TM8BR,
         ._tmicr         = &TM8ICR,
         .tm_irq         = TM8IRQ,
         .div_timer      = MNSCx_DIV_TIMER_16BIT,
-#else /* CONFIG_MN10300_TTYSM0_TIMER2 */
+#elif defined(CONFIG_MN10300_TTYSM0_TIMER0)
+        .tm_name        = "ttySM0:Timer0",
+        ._tmxmd         = &TM0MD,
+        ._tmxbr         = (volatile u16 *)&TM0BR,
+        ._tmicr         = &TM0ICR,
+        .tm_irq         = TM0IRQ,
+        .div_timer      = MNSCx_DIV_TIMER_8BIT,
+#elif defined(CONFIG_MN10300_TTYSM0_TIMER2)
         .tm_name        = "ttySM0:Timer2",
         ._tmxmd         = &TM2MD,
-        ._tmxbr         = (volatile u16 *) &TM2BR,
+        ._tmxbr         = (volatile u16 *)&TM2BR,
         ._tmicr         = &TM2ICR,
         .tm_irq         = TM2IRQ,
         .div_timer      = MNSCx_DIV_TIMER_8BIT,
+#else
+#error "Unknown config for ttySM0"
 #endif
         .rx_irq         = SC0RXIRQ,
         .tx_irq         = SC0TXIRQ,
@@ -205,26 +224,35 @@ struct mn10300_serial_port mn10300_serial_port_sif1 = {
         .name           = "ttySM1",
         ._iobase        = &SC1CTR,
         ._control       = &SC1CTR,
-        ._status        = (volatile u8 *) &SC1STR,
+        ._status        = (volatile u8 *)&SC1STR,
         ._intr          = &SC1ICR,
         ._rxb           = &SC1RXB,
         ._txb           = &SC1TXB,
         .rx_name        = "ttySM1:Rx",
         .tx_name        = "ttySM1:Tx",
-#ifdef CONFIG_MN10300_TTYSM1_TIMER9
+#if defined(CONFIG_MN10300_TTYSM1_TIMER9)
         .tm_name        = "ttySM1:Timer9",
         ._tmxmd         = &TM9MD,
         ._tmxbr         = &TM9BR,
         ._tmicr         = &TM9ICR,
         .tm_irq         = TM9IRQ,
         .div_timer      = MNSCx_DIV_TIMER_16BIT,
-#else /* CONFIG_MN10300_TTYSM1_TIMER3 */
+#elif defined(CONFIG_MN10300_TTYSM1_TIMER3)
         .tm_name        = "ttySM1:Timer3",
         ._tmxmd         = &TM3MD,
-        ._tmxbr         = (volatile u16 *) &TM3BR,
+        ._tmxbr         = (volatile u16 *)&TM3BR,
         ._tmicr         = &TM3ICR,
         .tm_irq         = TM3IRQ,
         .div_timer      = MNSCx_DIV_TIMER_8BIT,
+#elif defined(CONFIG_MN10300_TTYSM1_TIMER12)
+        .tm_name        = "ttySM1/Timer12",
+        ._tmxmd         = &TM12MD,
+        ._tmxbr         = &TM12BR,
+        ._tmicr         = &TM12ICR,
+        .tm_irq         = TM12IRQ,
+        .div_timer      = MNSCx_DIV_TIMER_16BIT,
+#else
+#error "Unknown config for ttySM1"
 #endif
         .rx_irq         = SC1RXIRQ,
         .tx_irq         = SC1TXIRQ,
@@ -260,20 +288,45 @@ struct mn10300_serial_port mn10300_serial_port_sif2 = {
         .uart.lock      =
         __SPIN_LOCK_UNLOCKED(mn10300_serial_port_sif2.uart.lock),
         .name           = "ttySM2",
-        .rx_name        = "ttySM2:Rx",
-        .tx_name        = "ttySM2:Tx",
-        .tm_name        = "ttySM2:Timer10",
         ._iobase        = &SC2CTR,
         ._control       = &SC2CTR,
-        ._status        = &SC2STR,
+        ._status        = (volatile u8 *)&SC2STR,
         ._intr          = &SC2ICR,
         ._rxb           = &SC2RXB,
         ._txb           = &SC2TXB,
+        .rx_name        = "ttySM2:Rx",
+        .tx_name        = "ttySM2:Tx",
+#if defined(CONFIG_MN10300_TTYSM2_TIMER10)
+        .tm_name        = "ttySM2/Timer10",
         ._tmxmd         = &TM10MD,
         ._tmxbr         = &TM10BR,
         ._tmicr         = &TM10ICR,
         .tm_irq         = TM10IRQ,
         .div_timer      = MNSCx_DIV_TIMER_16BIT,
+#elif defined(CONFIG_MN10300_TTYSM2_TIMER9)
+        .tm_name        = "ttySM2/Timer9",
+        ._tmxmd         = &TM9MD,
+        ._tmxbr         = &TM9BR,
+        ._tmicr         = &TM9ICR,
+        .tm_irq         = TM9IRQ,
+        .div_timer      = MNSCx_DIV_TIMER_16BIT,
+#elif defined(CONFIG_MN10300_TTYSM2_TIMER1)
+        .tm_name        = "ttySM2/Timer1",
+        ._tmxmd         = &TM1MD,
+        ._tmxbr         = (volatile u16 *)&TM1BR,
+        ._tmicr         = &TM1ICR,
+        .tm_irq         = TM1IRQ,
+        .div_timer      = MNSCx_DIV_TIMER_8BIT,
+#elif defined(CONFIG_MN10300_TTYSM2_TIMER3)
+        .tm_name        = "ttySM2/Timer3",
+        ._tmxmd         = &TM3MD,
+        ._tmxbr         = (volatile u16 *)&TM3BR,
+        ._tmicr         = &TM3ICR,
+        .tm_irq         = TM3IRQ,
+        .div_timer      = MNSCx_DIV_TIMER_8BIT,
+#else
+#error "Unknown config for ttySM2"
+#endif
         .rx_irq         = SC2RXIRQ,
         .tx_irq         = SC2TXIRQ,
         .rx_icr         = &GxICR(SC2RXIRQ),
@@ -322,9 +375,13 @@ struct mn10300_serial_port *mn10300_serial_ports[NR_UARTS + 1] = {
  */
 static void mn10300_serial_mask_ack(unsigned int irq)
 {
+        unsigned long flags;
         u16 tmp;
+
+        flags = arch_local_cli_save();
         GxICR(irq) = GxICR_LEVEL_6;
         tmp = GxICR(irq); /* flush write buffer */
+        arch_local_irq_restore(flags);
 }
 
 static void mn10300_serial_nop(unsigned int irq)
@@ -348,23 +405,36 @@ struct mn10300_serial_int mn10300_serial_int_tbl[NR_IRQS];
 
 static void mn10300_serial_dis_tx_intr(struct mn10300_serial_port *port)
 {
+        unsigned long flags;
         u16 x;
-        *port->tx_icr = GxICR_LEVEL_1 | GxICR_DETECT;
+
+        flags = arch_local_cli_save();
+        *port->tx_icr = NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL);
         x = *port->tx_icr;
+        arch_local_irq_restore(flags);
 }
 
 static void mn10300_serial_en_tx_intr(struct mn10300_serial_port *port)
 {
+        unsigned long flags;
         u16 x;
-        *port->tx_icr = GxICR_LEVEL_1 | GxICR_ENABLE;
+
+        flags = arch_local_cli_save();
+        *port->tx_icr =
+                NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL) | GxICR_ENABLE;
         x = *port->tx_icr;
+        arch_local_irq_restore(flags);
 }
 
 static void mn10300_serial_dis_rx_intr(struct mn10300_serial_port *port)
 {
+        unsigned long flags;
         u16 x;
-        *port->rx_icr = GxICR_LEVEL_1 | GxICR_DETECT;
+
+        flags = arch_local_cli_save();
+        *port->rx_icr = NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL);
         x = *port->rx_icr;
+        arch_local_irq_restore(flags);
 }
 
 /*
@@ -650,7 +720,7 @@ static unsigned int mn10300_serial_tx_empty(struct uart_port *_port)
 static void mn10300_serial_set_mctrl(struct uart_port *_port,
                                      unsigned int mctrl)
 {
-        struct mn10300_serial_port *port =
+        struct mn10300_serial_port *port __attribute__ ((unused)) =
                 container_of(_port, struct mn10300_serial_port, uart);
 
         _enter("%s,%x", port->name, mctrl);
@@ -706,6 +776,7 @@ static void mn10300_serial_start_tx(struct uart_port *_port)
                         UART_XMIT_SIZE));
 
         /* kick the virtual DMA controller */
+        arch_local_cli();
         x = *port->tx_icr;
         x |= GxICR_ENABLE;
 
@@ -716,10 +787,14 @@ static void mn10300_serial_start_tx(struct uart_port *_port)
 
         _debug("CTR=%04hx ICR=%02hx STR=%04x TMD=%02hx TBR=%04hx ICR=%04hx",
                *port->_control, *port->_intr, *port->_status,
-               *port->_tmxmd, *port->_tmxbr, *port->tx_icr);
+               *port->_tmxmd,
+               (port->div_timer == MNSCx_DIV_TIMER_8BIT) ?
+                   *(volatile u8 *)port->_tmxbr : *port->_tmxbr,
+               *port->tx_icr);
 
         *port->tx_icr = x;
         x = *port->tx_icr;
+        arch_local_sti();
 }
 
 /*
@@ -842,8 +917,10 @@ static int mn10300_serial_startup(struct uart_port *_port)
         pint->port = port;
         pint->vdma = mn10300_serial_vdma_tx_handler;
 
-        set_intr_level(port->rx_irq, GxICR_LEVEL_1);
-        set_intr_level(port->tx_irq, GxICR_LEVEL_1);
+        set_intr_level(port->rx_irq,
+                NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL));
+        set_intr_level(port->tx_irq,
+                NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL));
         set_irq_chip(port->tm_irq, &mn10300_serial_pic);
 
         if (request_irq(port->rx_irq, mn10300_serial_interrupt,
@@ -876,6 +953,7 @@ error:
  */
 static void mn10300_serial_shutdown(struct uart_port *_port)
 {
+        u16 x;
         struct mn10300_serial_port *port =
                 container_of(_port, struct mn10300_serial_port, uart);
 
@@ -897,8 +975,12 @@ static void mn10300_serial_shutdown(struct uart_port *_port)
         free_irq(port->rx_irq, port);
         free_irq(port->tx_irq, port);
 
-        *port->rx_icr = GxICR_LEVEL_1;
-        *port->tx_icr = GxICR_LEVEL_1;
+        arch_local_cli();
+        *port->rx_icr = NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL);
+        x = *port->rx_icr;
+        *port->tx_icr = NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL);
+        x = *port->tx_icr;
+        arch_local_sti();
 }
 
 /*
@@ -947,11 +1029,66 @@ static void mn10300_serial_change_speed(struct mn10300_serial_port *port,
         /* Determine divisor based on baud rate */
         battempt = 0;
 
-        if (div_timer == MNSCx_DIV_TIMER_16BIT)
-                scxctr |= SC0CTR_CK_TM8UFLOW_8; /* ( == SC1CTR_CK_TM9UFLOW_8
-                                                 *   == SC2CTR_CK_TM10UFLOW) */
-        else if (div_timer == MNSCx_DIV_TIMER_8BIT)
+        switch (port->uart.line) {
+#ifdef CONFIG_MN10300_TTYSM0
+        case 0: /* ttySM0 */
+#if   defined(CONFIG_MN10300_TTYSM0_TIMER8)
+                scxctr |= SC0CTR_CK_TM8UFLOW_8;
+#elif defined(CONFIG_MN10300_TTYSM0_TIMER0)
+                scxctr |= SC0CTR_CK_TM0UFLOW_8;
+#elif defined(CONFIG_MN10300_TTYSM0_TIMER2)
                 scxctr |= SC0CTR_CK_TM2UFLOW_8;
+#else
+#error "Unknown config for ttySM0"
+#endif
+                break;
+#endif /* CONFIG_MN10300_TTYSM0 */
+
+#ifdef CONFIG_MN10300_TTYSM1
+        case 1: /* ttySM1 */
+#if defined(CONFIG_AM33_2) || defined(CONFIG_AM33_3)
+#if   defined(CONFIG_MN10300_TTYSM1_TIMER9)
+                scxctr |= SC1CTR_CK_TM9UFLOW_8;
+#elif defined(CONFIG_MN10300_TTYSM1_TIMER3)
+                scxctr |= SC1CTR_CK_TM3UFLOW_8;
+#else
+#error "Unknown config for ttySM1"
+#endif
+#else /* CONFIG_AM33_2 || CONFIG_AM33_3 */
+#if defined(CONFIG_MN10300_TTYSM1_TIMER12)
+                scxctr |= SC1CTR_CK_TM12UFLOW_8;
+#else
+#error "Unknown config for ttySM1"
+#endif
+#endif /* CONFIG_AM33_2 || CONFIG_AM33_3 */
+                break;
+#endif /* CONFIG_MN10300_TTYSM1 */
+
+#ifdef CONFIG_MN10300_TTYSM2
+        case 2: /* ttySM2 */
+#if defined(CONFIG_AM33_2)
+#if   defined(CONFIG_MN10300_TTYSM2_TIMER10)
+                scxctr |= SC2CTR_CK_TM10UFLOW;
+#else
+#error "Unknown config for ttySM2"
+#endif
+#else /* CONFIG_AM33_2 */
+#if   defined(CONFIG_MN10300_TTYSM2_TIMER9)
+                scxctr |= SC2CTR_CK_TM9UFLOW_8;
+#elif defined(CONFIG_MN10300_TTYSM2_TIMER1)
+                scxctr |= SC2CTR_CK_TM1UFLOW_8;
+#elif defined(CONFIG_MN10300_TTYSM2_TIMER3)
+                scxctr |= SC2CTR_CK_TM3UFLOW_8;
+#else
+#error "Unknown config for ttySM2"
+#endif
+#endif /* CONFIG_AM33_2 */
+                break;
+#endif /* CONFIG_MN10300_TTYSM2 */
+
+        default:
+                break;
+        }
 
 try_alternative:
         baud = uart_get_baud_rate(&port->uart, new, old, 0,
@@ -1195,6 +1332,12 @@ static void mn10300_serial_set_termios(struct uart_port *_port,
                 ctr &= ~SC2CTR_TWE;
                 *port->_control = ctr;
         }
+
+        /* change Transfer bit-order (LSB/MSB) */
+        if (new->c_cflag & CODMSB)
+                *port->_control |= SC01CTR_OD_MSBFIRST; /* MSB MODE */
+        else
+                *port->_control &= ~SC01CTR_OD_MSBFIRST; /* LSB MODE */
 }
 
 /*
@@ -1302,11 +1445,16 @@ static int __init mn10300_serial_init(void)
         printk(KERN_INFO "%s version %s (%s)\n",
                serial_name, serial_version, serial_revdate);
 
-#ifdef CONFIG_MN10300_TTYSM2
-        SC2TIM = 8; /* make the baud base of timer 2 IOCLK/8 */
+#if defined(CONFIG_MN10300_TTYSM2) && defined(CONFIG_AM33_2)
+        {
+                int tmp;
+                SC2TIM = 8; /* make the baud base of timer 2 IOCLK/8 */
+                tmp = SC2TIM;
+        }
 #endif
 
-        set_intr_stub(EXCEP_IRQ_LEVEL1, mn10300_serial_vdma_interrupt);
+        set_intr_stub(NUM2EXCEP_IRQ_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL),
+                mn10300_serial_vdma_interrupt);
 
         ret = uart_register_driver(&mn10300_serial_driver);
         if (!ret) {
@@ -1366,9 +1514,11 @@ static void mn10300_serial_console_write(struct console *co,
         port = mn10300_serial_ports[co->index];
 
         /* firstly hijack the serial port from the "virtual DMA" controller */
+        arch_local_cli();
         txicr = *port->tx_icr;
-        *port->tx_icr = GxICR_LEVEL_1;
+        *port->tx_icr = NUM2GxICR_LEVEL(CONFIG_MN10300_SERIAL_IRQ_LEVEL);
         tmp = *port->tx_icr;
+        arch_local_sti();
 
         /* the transmitter may be disabled */
         scxctr = *port->_control;
@@ -1422,8 +1572,10 @@ static void mn10300_serial_console_write(struct console *co,
         if (!(scxctr & SC01CTR_TXE))
                 *port->_control = scxctr;
 
+        arch_local_cli();
         *port->tx_icr = txicr;
         tmp = *port->tx_icr;
+        arch_local_sti();
 }
 
 /*
diff --git a/arch/mn10300/kernel/mn10300-watchdog-low.S b/arch/mn10300/kernel/mn10300-watchdog-low.S
index 996244745cca..f2f5c9cfaabd 100644
--- a/arch/mn10300/kernel/mn10300-watchdog-low.S
+++ b/arch/mn10300/kernel/mn10300-watchdog-low.S
@@ -16,6 +16,7 @@
 #include <asm/intctl-regs.h>
 #include <asm/timer-regs.h>
 #include <asm/frame.inc>
+#include <linux/threads.h>
 
         .text
 
@@ -53,7 +54,13 @@ watchdog_handler:
         .type   touch_nmi_watchdog,@function
 touch_nmi_watchdog:
         clr     d0
-        mov     d0,(watchdog_alert_counter)
+        clr     d1
+        mov     watchdog_alert_counter, a0
+        setlb
+        mov     d0, (a0+)
+        inc     d1
+        cmp     NR_CPUS, d1
+        lne
         ret     [],0
 
         .size   touch_nmi_watchdog,.-touch_nmi_watchdog
diff --git a/arch/mn10300/kernel/mn10300-watchdog.c b/arch/mn10300/kernel/mn10300-watchdog.c
index f362d9d138f1..965dd61656c3 100644
--- a/arch/mn10300/kernel/mn10300-watchdog.c
+++ b/arch/mn10300/kernel/mn10300-watchdog.c
@@ -30,7 +30,7 @@
 static DEFINE_SPINLOCK(watchdog_print_lock);
 static unsigned int watchdog;
 static unsigned int watchdog_hz = 1;
-unsigned int watchdog_alert_counter;
+unsigned int watchdog_alert_counter[NR_CPUS];
 
 EXPORT_SYMBOL(touch_nmi_watchdog);
 
@@ -39,9 +39,6 @@ EXPORT_SYMBOL(touch_nmi_watchdog);
  * is to check its timer makes IRQ counts. If they are not
  * changing then that CPU has some problem.
  *
- * as these watchdog NMI IRQs are generated on every CPU, we only
- * have to check the current processor.
- *
  * since NMIs dont listen to _any_ locks, we have to be extremely
  * careful not to rely on unsafe variables. The printk might lock
  * up though, so we have to break up any console locks first ...
@@ -69,8 +66,8 @@ int __init check_watchdog(void)
 
         printk(KERN_INFO "OK.\n");
 
-        /* now that we know it works we can reduce NMI frequency to
-         * something more reasonable; makes a difference in some configs
+        /* now that we know it works we can reduce NMI frequency to something
+         * more reasonable; makes a difference in some configs
          */
         watchdog_hz = 1;
 
@@ -121,15 +118,22 @@ void __init watchdog_go(void)
         }
 }
 
+#ifdef CONFIG_SMP
+static void watchdog_dump_register(void *dummy)
+{
+        printk(KERN_ERR "--- Register Dump (CPU%d) ---\n", CPUID);
+        show_registers(__frame);
+}
+#endif
+
 asmlinkage
 void watchdog_interrupt(struct pt_regs *regs, enum exception_code excep)
 {
-
         /*
          * Since current-> is always on the stack, and we always switch
          * the stack NMI-atomically, it's safe to use smp_processor_id().
          */
-        int sum, cpu = smp_processor_id();
+        int sum, cpu;
         int irq = NMIIRQ;
         u8 wdt, tmp;
 
@@ -138,43 +142,61 @@ void watchdog_interrupt(struct pt_regs *regs, enum exception_code excep)
         tmp = WDCTR;
         NMICR = NMICR_WDIF;
 
-        nmi_count(cpu)++;
+        nmi_count(smp_processor_id())++;
         kstat_incr_irqs_this_cpu(irq, irq_to_desc(irq));
-        sum = irq_stat[cpu].__irq_count;
-
-        if (last_irq_sums[cpu] == sum) {
-                /*
-                 * Ayiee, looks like this CPU is stuck ...
-                 * wait a few IRQs (5 seconds) before doing the oops ...
-                 */
-                watchdog_alert_counter++;
-                if (watchdog_alert_counter == 5 * watchdog_hz) {
-                        spin_lock(&watchdog_print_lock);
+
+        for_each_online_cpu(cpu) {
+
+                sum = irq_stat[cpu].__irq_count;
+
+                if ((last_irq_sums[cpu] == sum)
+#if defined(CONFIG_GDBSTUB) && defined(CONFIG_SMP)
+                        && !(CHK_GDBSTUB_BUSY()
+                             || atomic_read(&cpu_doing_single_step))
+#endif
+                        ) {
                         /*
-                         * We are in trouble anyway, lets at least try
-                         * to get a message out.
+                         * Ayiee, looks like this CPU is stuck ...
+                         * wait a few IRQs (5 seconds) before doing the oops ...
                          */
-                        bust_spinlocks(1);
-                        printk(KERN_ERR
-                               "NMI Watchdog detected LOCKUP on CPU%d,"
-                               " pc %08lx, registers:\n",
-                               cpu, regs->pc);
-                        show_registers(regs);
-                        printk("console shuts up ...\n");
-                        console_silent();
-                        spin_unlock(&watchdog_print_lock);
-                        bust_spinlocks(0);
+                        watchdog_alert_counter[cpu]++;
+                        if (watchdog_alert_counter[cpu] == 5 * watchdog_hz) {
+                                spin_lock(&watchdog_print_lock);
+                                /*
+                                 * We are in trouble anyway, lets at least try
+                                 * to get a message out.
+                                 */
+                                bust_spinlocks(1);
+                                printk(KERN_ERR
+                                       "NMI Watchdog detected LOCKUP on CPU%d,"
+                                       " pc %08lx, registers:\n",
+                                       cpu, regs->pc);
+#ifdef CONFIG_SMP
+                                printk(KERN_ERR
+                                       "--- Register Dump (CPU%d) ---\n",
+                                       CPUID);
+#endif
+                                show_registers(regs);
+#ifdef CONFIG_SMP
+                                smp_nmi_call_function(watchdog_dump_register,
+                                        NULL, 1);
+#endif
+                                printk(KERN_NOTICE "console shuts up ...\n");
+                                console_silent();
+                                spin_unlock(&watchdog_print_lock);
+                                bust_spinlocks(0);
 #ifdef CONFIG_GDBSTUB
-                        if (gdbstub_busy)
-                                gdbstub_exception(regs, excep);
-                        else
-                                gdbstub_intercept(regs, excep);
+                                if (CHK_GDBSTUB_BUSY_AND_ACTIVE())
+                                        gdbstub_exception(regs, excep);
+                                else
+                                        gdbstub_intercept(regs, excep);
 #endif
-                        do_exit(SIGSEGV);
+                                do_exit(SIGSEGV);
+                        }
+                } else {
+                        last_irq_sums[cpu] = sum;
+                        watchdog_alert_counter[cpu] = 0;
                 }
-        } else {
-                last_irq_sums[cpu] = sum;
-                watchdog_alert_counter = 0;
         }
 
         WDCTR = wdt | WDCTR_WDRST;
diff --git a/arch/mn10300/kernel/process.c b/arch/mn10300/kernel/process.c
index 243e33cd874b..b2e85ed73a54 100644
--- a/arch/mn10300/kernel/process.c
+++ b/arch/mn10300/kernel/process.c
@@ -57,6 +57,7 @@ unsigned long thread_saved_pc(struct task_struct *tsk)
 void (*pm_power_off)(void);
 EXPORT_SYMBOL(pm_power_off);
 
+#if !defined(CONFIG_SMP) || defined(CONFIG_HOTPLUG_CPU)
 /*
  * we use this if we don't have any better idle routine
  */
@@ -69,6 +70,35 @@ static void default_idle(void)
                 local_irq_enable();
 }
 
+#else /* !CONFIG_SMP || CONFIG_HOTPLUG_CPU  */
+/*
+ * On SMP it's slightly faster (but much more power-consuming!)
+ * to poll the ->work.need_resched flag instead of waiting for the
+ * cross-CPU IPI to arrive. Use this option with caution.
+ */
+static inline void poll_idle(void)
+{
+        int oldval;
+
+        local_irq_enable();
+
+        /*
+         * Deal with another CPU just having chosen a thread to
+         * run here:
+         */
+        oldval = test_and_clear_thread_flag(TIF_NEED_RESCHED);
+
+        if (!oldval) {
+                set_thread_flag(TIF_POLLING_NRFLAG);
+                while (!need_resched())
+                        cpu_relax();
+                clear_thread_flag(TIF_POLLING_NRFLAG);
+        } else {
+                set_need_resched();
+        }
+}
+#endif /* !CONFIG_SMP || CONFIG_HOTPLUG_CPU */
+
 /*
  * the idle thread
  * - there's no useful work to be done, so just try to conserve power and have
@@ -77,8 +107,6 @@ static void default_idle(void)
  */
 void cpu_idle(void)
 {
-        int cpu = smp_processor_id();
-
         /* endless idle loop with no priority at all */
         for (;;) {
                 while (!need_resched()) {
@@ -86,8 +114,13 @@ void cpu_idle(void)
 
                         smp_rmb();
                         idle = pm_idle;
-                        if (!idle)
+                        if (!idle) {
+#if defined(CONFIG_SMP) && !defined(CONFIG_HOTPLUG_CPU)
+                                idle = poll_idle;
+#else  /* CONFIG_SMP && !CONFIG_HOTPLUG_CPU */
                                 idle = default_idle;
+#endif /* CONFIG_SMP && !CONFIG_HOTPLUG_CPU */
+                        }
                         idle();
                 }
 
@@ -233,7 +266,7 @@ int copy_thread(unsigned long clone_flags,
         }
 
         /* set up things up so the scheduler can start the new task */
-        p->thread.__frame = c_kregs;
+        p->thread.frame = c_kregs;
         p->thread.a3    = (unsigned long) c_kregs;
         p->thread.sp    = c_ksp;
         p->thread.pc    = (unsigned long) ret_from_fork;
diff --git a/arch/mn10300/kernel/profile.c b/arch/mn10300/kernel/profile.c
index 20d7d0306b16..4f342f75d00c 100644
--- a/arch/mn10300/kernel/profile.c
+++ b/arch/mn10300/kernel/profile.c
@@ -41,7 +41,7 @@ static __init int profile_init(void)
         tmp = TM11ICR;
 
         printk(KERN_INFO "Profiling initiated on timer 11, priority 0, %uHz\n",
-               mn10300_ioclk / 8 / (TM11BR + 1));
+               MN10300_IOCLK / 8 / (TM11BR + 1));
         printk(KERN_INFO "Profile histogram stored %p-%p\n",
                prof_buffer, (u8 *)(prof_buffer + prof_len) - 1);
 
diff --git a/arch/mn10300/kernel/rtc.c b/arch/mn10300/kernel/rtc.c
index 4eef0e7224f6..e9e20f9a4dd3 100644
--- a/arch/mn10300/kernel/rtc.c
+++ b/arch/mn10300/kernel/rtc.c
@@ -20,18 +20,22 @@
 DEFINE_SPINLOCK(rtc_lock);
 EXPORT_SYMBOL(rtc_lock);
 
-/* time for RTC to update itself in ioclks */
-static unsigned long mn10300_rtc_update_period;
-
+/*
+ * Read the current RTC time
+ */
 void read_persistent_clock(struct timespec *ts)
 {
         struct rtc_time tm;
 
         get_rtc_time(&tm);
 
-        ts->tv_sec = mktime(tm.tm_year, tm.tm_mon, tm.tm_mday,
-                      tm.tm_hour, tm.tm_min, tm.tm_sec);
         ts->tv_nsec = 0;
+        ts->tv_sec = mktime(tm.tm_year, tm.tm_mon, tm.tm_mday,
+                            tm.tm_hour, tm.tm_min, tm.tm_sec);
+
+        /* if rtc is way off in the past, set something reasonable */
+        if (ts->tv_sec < 0)
+                ts->tv_sec = mktime(2009, 1, 1, 12, 0, 0);
 }
 
 /*
@@ -115,39 +119,14 @@ int update_persistent_clock(struct timespec now)
  */
 void __init calibrate_clock(void)
 {
-        unsigned long count0, counth, count1;
         unsigned char status;
 
         /* make sure the RTC is running and is set to operate in 24hr mode */
         status = RTSRC;
         RTCRB |= RTCRB_SET;
         RTCRB |= RTCRB_TM_24HR;
+        RTCRB &= ~RTCRB_DM_BINARY;
         RTCRA |= RTCRA_DVR;
         RTCRA &= ~RTCRA_DVR;
         RTCRB &= ~RTCRB_SET;
-
-        /* work out the clock speed by counting clock cycles between ends of
-         * the RTC update cycle - track the RTC through one complete update
-         * cycle (1 second)
-         */
-        startup_timestamp_counter();
-
-        while (!(RTCRA & RTCRA_UIP)) {}
-        while ((RTCRA & RTCRA_UIP)) {}
-
-        count0 = TMTSCBC;
-
-        while (!(RTCRA & RTCRA_UIP)) {}
-
-        counth = TMTSCBC;
-
-        while ((RTCRA & RTCRA_UIP)) {}
-
-        count1 = TMTSCBC;
-
-        shutdown_timestamp_counter();
-
-        MN10300_TSCCLK = count0 - count1; /* the timers count down */
-        mn10300_rtc_update_period = counth - count1;
-        MN10300_TSC_PER_HZ = MN10300_TSCCLK / HZ;
 }
diff --git a/arch/mn10300/kernel/setup.c b/arch/mn10300/kernel/setup.c
index d464affcba0e..12514570ed5d 100644
--- a/arch/mn10300/kernel/setup.c
+++ b/arch/mn10300/kernel/setup.c
@@ -22,6 +22,7 @@
 #include <linux/init.h>
 #include <linux/bootmem.h>
 #include <linux/seq_file.h>
+#include <linux/cpu.h>
 #include <asm/processor.h>
 #include <linux/console.h>
 #include <asm/uaccess.h>
@@ -30,7 +31,6 @@
 #include <asm/io.h>
 #include <asm/smp.h>
 #include <proc/proc.h>
-#include <asm/busctl-regs.h>
 #include <asm/fpu.h>
 #include <asm/sections.h>
 
@@ -64,11 +64,13 @@ unsigned long memory_size;
 struct thread_info *__current_ti = &init_thread_union.thread_info;
 struct task_struct *__current = &init_task;
 
-#define mn10300_known_cpus 3
+#define mn10300_known_cpus 5
 static const char *const mn10300_cputypes[] = {
-        "am33v1",
-        "am33v2",
-        "am34v1",
+        "am33-1",
+        "am33-2",
+        "am34-1",
+        "am33-3",
+        "am34-2",
         "unknown"
 };
 
@@ -123,6 +125,7 @@ void __init setup_arch(char **cmdline_p)
 
         cpu_init();
         unit_setup();
+        smp_init_cpus();
         parse_mem_cmdline(cmdline_p);
 
         init_mm.start_code = (unsigned long)&_text;
@@ -179,7 +182,6 @@ void __init setup_arch(char **cmdline_p)
 void __init cpu_init(void)
 {
         unsigned long cpurev = CPUREV, type;
-        unsigned long base, size;
 
         type = (CPUREV & CPUREV_TYPE) >> CPUREV_TYPE_S;
         if (type > mn10300_known_cpus)
@@ -189,47 +191,46 @@ void __init cpu_init(void)
                mn10300_cputypes[type],
                (cpurev & CPUREV_REVISION) >> CPUREV_REVISION_S);
 
-        /* determine the memory size and base from the memory controller regs */
-        memory_size = 0;
-
-        base = SDBASE(0);
-        if (base & SDBASE_CE) {
-                size = (base & SDBASE_CBAM) << SDBASE_CBAM_SHIFT;
-                size = ~size + 1;
-                base &= SDBASE_CBA;
+        get_mem_info(&phys_memory_base, &memory_size);
+        phys_memory_end = phys_memory_base + memory_size;
 
-                printk(KERN_INFO "SDRAM[0]: %luMb @%08lx\n", size >> 20, base);
-                memory_size += size;
-                phys_memory_base = base;
-        }
+        fpu_init_state();
+}
 
-        base = SDBASE(1);
-        if (base & SDBASE_CE) {
-                size = (base & SDBASE_CBAM) << SDBASE_CBAM_SHIFT;
-                size = ~size + 1;
-                base &= SDBASE_CBA;
+static struct cpu cpu_devices[NR_CPUS];
 
-                printk(KERN_INFO "SDRAM[1]: %luMb @%08lx\n", size >> 20, base);
-                memory_size += size;
-                if (phys_memory_base == 0)
-                        phys_memory_base = base;
-        }
+static int __init topology_init(void)
+{
+        int i;
 
-        phys_memory_end = phys_memory_base + memory_size;
+        for_each_present_cpu(i)
+                register_cpu(&cpu_devices[i], i);
 
-#ifdef CONFIG_FPU
-        fpu_init_state();
-#endif
+        return 0;
 }
 
+subsys_initcall(topology_init);
+
 /*
  * Get CPU information for use by the procfs.
  */
 static int show_cpuinfo(struct seq_file *m, void *v)
 {
+#ifdef CONFIG_SMP
+        struct mn10300_cpuinfo *c = v;
+        unsigned long cpu_id = c - cpu_data;
+        unsigned long cpurev = c->type, type, icachesz, dcachesz;
+#else  /* CONFIG_SMP */
+        unsigned long cpu_id = 0;
         unsigned long cpurev = CPUREV, type, icachesz, dcachesz;
+#endif /* CONFIG_SMP */
 
-        type = (CPUREV & CPUREV_TYPE) >> CPUREV_TYPE_S;
+#ifdef CONFIG_SMP
+        if (!cpu_online(cpu_id))
+                return 0;
+#endif
+
+        type = (cpurev & CPUREV_TYPE) >> CPUREV_TYPE_S;
         if (type > mn10300_known_cpus)
                 type = mn10300_known_cpus;
 
@@ -244,13 +245,14 @@ static int show_cpuinfo(struct seq_file *m, void *v)
                 1024;
 
         seq_printf(m,
-                   "processor  : 0\n"
+                   "processor  : %ld\n"
                    "vendor_id  : Matsushita\n"
                    "cpu core   : %s\n"
                    "cpu rev    : %lu\n"
                    "model name : " PROCESSOR_MODEL_NAME         "\n"
                    "icache size: %lu\n"
                    "dcache size: %lu\n",
+                   cpu_id,
                    mn10300_cputypes[type],
                    (cpurev & CPUREV_REVISION) >> CPUREV_REVISION_S,
                    icachesz,
@@ -262,8 +264,13 @@ static int show_cpuinfo(struct seq_file *m, void *v)
                    "bogomips   : %lu.%02lu\n\n",
                    MN10300_IOCLK / 1000000,
                    (MN10300_IOCLK / 10000) % 100,
+#ifdef CONFIG_SMP
+                   c->loops_per_jiffy / (500000 / HZ),
+                   (c->loops_per_jiffy / (5000 / HZ)) % 100
+#else  /* CONFIG_SMP */
                    loops_per_jiffy / (500000 / HZ),
                    (loops_per_jiffy / (5000 / HZ)) % 100
+#endif /* CONFIG_SMP */
                    );
 
         return 0;
diff --git a/arch/mn10300/kernel/smp-low.S b/arch/mn10300/kernel/smp-low.S
new file mode 100644
index 000000000000..72938cefc05e
--- /dev/null
+++ b/arch/mn10300/kernel/smp-low.S
@@ -0,0 +1,97 @@
+/* SMP IPI low-level handler
+ *
+ * Copyright (C) 2006-2007 Matsushita Electric Industrial Co., Ltd.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ *
+ */
+
+#include <linux/sys.h>
+#include <linux/linkage.h>
+#include <asm/smp.h>
+#include <asm/system.h>
+#include <asm/thread_info.h>
+#include <asm/cpu-regs.h>
+#include <proc/smp-regs.h>
+#include <asm/asm-offsets.h>
+#include <asm/frame.inc>
+
+        .am33_2
+
+###############################################################################
+#
+# IPI interrupt handler
+#
+###############################################################################
+        .globl mn10300_low_ipi_handler
+mn10300_low_ipi_handler:
+        add     -4,sp
+        mov     d0,(sp)
+        movhu   (IAGR),d0
+        and     IAGR_GN,d0
+        lsr     0x2,d0
+#ifdef CONFIG_MN10300_CACHE_ENABLED
+        cmp     FLUSH_CACHE_IPI,d0
+        beq     mn10300_flush_cache_ipi
+#endif
+        cmp     SMP_BOOT_IRQ,d0
+        beq     mn10300_smp_boot_ipi
+        /* OTHERS */
+        mov     (sp),d0
+        add     4,sp
+#ifdef CONFIG_GDBSTUB
+        jmp     gdbstub_io_rx_handler
+#else
+        jmp     end
+#endif
+
+###############################################################################
+#
+# Cache flush IPI interrupt handler
+#
+###############################################################################
+#ifdef CONFIG_MN10300_CACHE_ENABLED
+mn10300_flush_cache_ipi:
+        mov     (sp),d0
+        add     4,sp
+
+        /* FLUSH_CACHE_IPI */
+        add     -4,sp
+        SAVE_ALL
+        mov     GxICR_DETECT,d2
+        movbu   d2,(GxICR(FLUSH_CACHE_IPI))     # ACK the interrupt
+        movhu   (GxICR(FLUSH_CACHE_IPI)),d2
+        call    smp_cache_interrupt[],0
+        RESTORE_ALL
+        jmp     end
+#endif
+
+###############################################################################
+#
+# SMP boot CPU IPI interrupt handler
+#
+###############################################################################
+mn10300_smp_boot_ipi:
+        /* clear interrupt */
+        movhu   (GxICR(SMP_BOOT_IRQ)),d0
+        and     ~GxICR_REQUEST,d0
+        movhu   d0,(GxICR(SMP_BOOT_IRQ))
+        mov     (sp),d0
+        add     4,sp
+
+        # get stack
+        mov     (CPUID),a0
+        add     -1,a0
+        add     a0,a0
+        add     a0,a0
+        mov     (start_stack,a0),a0
+        mov     a0,sp
+        jmp     initialize_secondary
+
+
+# Jump here after RTI to suppress the icache lookahead
+end:
diff --git a/arch/mn10300/kernel/smp.c b/arch/mn10300/kernel/smp.c
new file mode 100644
index 000000000000..b80234c28e0d
--- /dev/null
+++ b/arch/mn10300/kernel/smp.c
@@ -0,0 +1,1141 @@
+/* SMP support routines.
+ *
+ * Copyright (C) 2006-2008 Panasonic Corporation
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/cpumask.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/profile.h>
+#include <linux/smp.h>
+#include <asm/tlbflush.h>
+#include <asm/system.h>
+#include <asm/bitops.h>
+#include <asm/processor.h>
+#include <asm/bug.h>
+#include <asm/exceptions.h>
+#include <asm/hardirq.h>
+#include <asm/fpu.h>
+#include <asm/mmu_context.h>
+#include <asm/thread_info.h>
+#include <asm/cpu-regs.h>
+#include <asm/intctl-regs.h>
+#include "internal.h"
+
+#ifdef CONFIG_HOTPLUG_CPU
+#include <linux/cpu.h>
+#include <asm/cacheflush.h>
+
+static unsigned long sleep_mode[NR_CPUS];
+
+static void run_sleep_cpu(unsigned int cpu);
+static void run_wakeup_cpu(unsigned int cpu);
+#endif /* CONFIG_HOTPLUG_CPU */
+
+/*
+ * Debug Message function
+ */
+
+#undef DEBUG_SMP
+#ifdef DEBUG_SMP
+#define Dprintk(fmt, ...) printk(KERN_DEBUG fmt, ##__VA_ARGS__)
+#else
+#define Dprintk(fmt, ...) no_printk(KERN_DEBUG fmt, ##__VA_ARGS__)
+#endif
+
+/* timeout value in msec for smp_nmi_call_function. zero is no timeout. */
+#define CALL_FUNCTION_NMI_IPI_TIMEOUT   0
+
+/*
+ * Structure and data for smp_nmi_call_function().
+ */
+struct nmi_call_data_struct {
+        smp_call_func_t func;
+        void            *info;
+        cpumask_t       started;
+        cpumask_t       finished;
+        int             wait;
+        char            size_alignment[0]
+        __attribute__ ((__aligned__(SMP_CACHE_BYTES)));
+} __attribute__ ((__aligned__(SMP_CACHE_BYTES)));
+
+static DEFINE_SPINLOCK(smp_nmi_call_lock);
+static struct nmi_call_data_struct *nmi_call_data;
+
+/*
+ * Data structures and variables
+ */
+static cpumask_t cpu_callin_map;        /* Bitmask of callin CPUs */
+static cpumask_t cpu_callout_map;       /* Bitmask of callout CPUs */
+cpumask_t cpu_boot_map;                 /* Bitmask of boot APs */
+unsigned long start_stack[NR_CPUS - 1];
+
+/*
+ * Per CPU parameters
+ */
+struct mn10300_cpuinfo cpu_data[NR_CPUS] __cacheline_aligned;
+
+static int cpucount;                    /* The count of boot CPUs */
+static cpumask_t smp_commenced_mask;
+cpumask_t cpu_initialized __initdata = CPU_MASK_NONE;
+
+/*
+ * Function Prototypes
+ */
+static int do_boot_cpu(int);
+static void smp_show_cpu_info(int cpu_id);
+static void smp_callin(void);
+static void smp_online(void);
+static void smp_store_cpu_info(int);
+static void smp_cpu_init(void);
+static void smp_tune_scheduling(void);
+static void send_IPI_mask(const cpumask_t *cpumask, int irq);
+static void init_ipi(void);
+
+/*
+ * IPI Initialization interrupt definitions
+ */
+static void mn10300_ipi_disable(unsigned int irq);
+static void mn10300_ipi_enable(unsigned int irq);
+static void mn10300_ipi_ack(unsigned int irq);
+static void mn10300_ipi_nop(unsigned int irq);
+
+static struct irq_chip mn10300_ipi_type = {
+        .name           = "cpu_ipi",
+        .disable        = mn10300_ipi_disable,
+        .enable         = mn10300_ipi_enable,
+        .ack            = mn10300_ipi_ack,
+        .eoi            = mn10300_ipi_nop
+};
+
+static irqreturn_t smp_reschedule_interrupt(int irq, void *dev_id);
+static irqreturn_t smp_call_function_interrupt(int irq, void *dev_id);
+static irqreturn_t smp_ipi_timer_interrupt(int irq, void *dev_id);
+
+static struct irqaction reschedule_ipi = {
+        .handler        = smp_reschedule_interrupt,
+        .name           = "smp reschedule IPI"
+};
+static struct irqaction call_function_ipi = {
+        .handler        = smp_call_function_interrupt,
+        .name           = "smp call function IPI"
+};
+static struct irqaction local_timer_ipi = {
+        .handler        = smp_ipi_timer_interrupt,
+        .flags          = IRQF_DISABLED,
+        .name           = "smp local timer IPI"
+};
+
+/**
+ * init_ipi - Initialise the IPI mechanism
+ */
+static void init_ipi(void)
+{
+        unsigned long flags;
+        u16 tmp16;
+
+        /* set up the reschedule IPI */
+        set_irq_chip_and_handler(RESCHEDULE_IPI,
+                                 &mn10300_ipi_type, handle_percpu_irq);
+        setup_irq(RESCHEDULE_IPI, &reschedule_ipi);
+        set_intr_level(RESCHEDULE_IPI, RESCHEDULE_GxICR_LV);
+        mn10300_ipi_enable(RESCHEDULE_IPI);
+
+        /* set up the call function IPI */
+        set_irq_chip_and_handler(CALL_FUNC_SINGLE_IPI,
+                                 &mn10300_ipi_type, handle_percpu_irq);
+        setup_irq(CALL_FUNC_SINGLE_IPI, &call_function_ipi);
+        set_intr_level(CALL_FUNC_SINGLE_IPI, CALL_FUNCTION_GxICR_LV);
+        mn10300_ipi_enable(CALL_FUNC_SINGLE_IPI);
+
+        /* set up the local timer IPI */
+        set_irq_chip_and_handler(LOCAL_TIMER_IPI,
+                                 &mn10300_ipi_type, handle_percpu_irq);
+        setup_irq(LOCAL_TIMER_IPI, &local_timer_ipi);
+        set_intr_level(LOCAL_TIMER_IPI, LOCAL_TIMER_GxICR_LV);
+        mn10300_ipi_enable(LOCAL_TIMER_IPI);
+
+#ifdef CONFIG_MN10300_CACHE_ENABLED
+        /* set up the cache flush IPI */
+        flags = arch_local_cli_save();
+        __set_intr_stub(NUM2EXCEP_IRQ_LEVEL(FLUSH_CACHE_GxICR_LV),
+                        mn10300_low_ipi_handler);
+        GxICR(FLUSH_CACHE_IPI) = FLUSH_CACHE_GxICR_LV | GxICR_DETECT;
+        mn10300_ipi_enable(FLUSH_CACHE_IPI);
+        arch_local_irq_restore(flags);
+#endif
+
+        /* set up the NMI call function IPI */
+        flags = arch_local_cli_save();
+        GxICR(CALL_FUNCTION_NMI_IPI) = GxICR_NMI | GxICR_ENABLE | GxICR_DETECT;
+        tmp16 = GxICR(CALL_FUNCTION_NMI_IPI);
+        arch_local_irq_restore(flags);
+
+        /* set up the SMP boot IPI */
+        flags = arch_local_cli_save();
+        __set_intr_stub(NUM2EXCEP_IRQ_LEVEL(SMP_BOOT_GxICR_LV),
+                        mn10300_low_ipi_handler);
+        arch_local_irq_restore(flags);
+}
+
+/**
+ * mn10300_ipi_shutdown - Shut down handling of an IPI
+ * @irq: The IPI to be shut down.
+ */
+static void mn10300_ipi_shutdown(unsigned int irq)
+{
+        unsigned long flags;
+        u16 tmp;
+
+        flags = arch_local_cli_save();
+
+        tmp = GxICR(irq);
+        GxICR(irq) = (tmp & GxICR_LEVEL) | GxICR_DETECT;
+        tmp = GxICR(irq);
+
+        arch_local_irq_restore(flags);
+}
+
+/**
+ * mn10300_ipi_enable - Enable an IPI
+ * @irq: The IPI to be enabled.
+ */
+static void mn10300_ipi_enable(unsigned int irq)
+{
+        unsigned long flags;
+        u16 tmp;
+
+        flags = arch_local_cli_save();
+
+        tmp = GxICR(irq);
+        GxICR(irq) = (tmp & GxICR_LEVEL) | GxICR_ENABLE;
+        tmp = GxICR(irq);
+
+        arch_local_irq_restore(flags);
+}
+
+/**
+ * mn10300_ipi_disable - Disable an IPI
+ * @irq: The IPI to be disabled.
+ */
+static void mn10300_ipi_disable(unsigned int irq)
+{
+        unsigned long flags;
+        u16 tmp;
+
+        flags = arch_local_cli_save();
+
+        tmp = GxICR(irq);
+        GxICR(irq) = tmp & GxICR_LEVEL;
+        tmp = GxICR(irq);
+
+        arch_local_irq_restore(flags);
+}
+
+/**
+ * mn10300_ipi_ack - Acknowledge an IPI interrupt in the PIC
+ * @irq: The IPI to be acknowledged.
+ *
+ * Clear the interrupt detection flag for the IPI on the appropriate interrupt
+ * channel in the PIC.
+ */
+static void mn10300_ipi_ack(unsigned int irq)
+{
+        unsigned long flags;
+        u16 tmp;
+
+        flags = arch_local_cli_save();
+        GxICR_u8(irq) = GxICR_DETECT;
+        tmp = GxICR(irq);
+        arch_local_irq_restore(flags);
+}
+
+/**
+ * mn10300_ipi_nop - Dummy IPI action
+ * @irq: The IPI to be acted upon.
+ */
+static void mn10300_ipi_nop(unsigned int irq)
+{
+}
+
+/**
+ * send_IPI_mask - Send IPIs to all CPUs in list
+ * @cpumask: The list of CPUs to target.
+ * @irq: The IPI request to be sent.
+ *
+ * Send the specified IPI to all the CPUs in the list, not waiting for them to
+ * finish before returning.  The caller is responsible for synchronisation if
+ * that is needed.
+ */
+static void send_IPI_mask(const cpumask_t *cpumask, int irq)
+{
+        int i;
+        u16 tmp;
+
+        for (i = 0; i < NR_CPUS; i++) {
+                if (cpu_isset(i, *cpumask)) {
+                        /* send IPI */
+                        tmp = CROSS_GxICR(irq, i);
+                        CROSS_GxICR(irq, i) =
+                                tmp | GxICR_REQUEST | GxICR_DETECT;
+                        tmp = CROSS_GxICR(irq, i); /* flush write buffer */
+                }
+        }
+}
+
+/**
+ * send_IPI_self - Send an IPI to this CPU.
+ * @irq: The IPI request to be sent.
+ *
+ * Send the specified IPI to the current CPU.
+ */
+void send_IPI_self(int irq)
+{
+        send_IPI_mask(cpumask_of(smp_processor_id()), irq);
+}
+
+/**
+ * send_IPI_allbutself - Send IPIs to all the other CPUs.
+ * @irq: The IPI request to be sent.
+ *
+ * Send the specified IPI to all CPUs in the system barring the current one,
+ * not waiting for them to finish before returning.  The caller is responsible
+ * for synchronisation if that is needed.
+ */
+void send_IPI_allbutself(int irq)
+{
+        cpumask_t cpumask;
+
+        cpumask = cpu_online_map;
+        cpu_clear(smp_processor_id(), cpumask);
+        send_IPI_mask(&cpumask, irq);
+}
+
+void arch_send_call_function_ipi_mask(const struct cpumask *mask)
+{
+        BUG();
+        /*send_IPI_mask(mask, CALL_FUNCTION_IPI);*/
+}
+
+void arch_send_call_function_single_ipi(int cpu)
+{
+        send_IPI_mask(cpumask_of(cpu), CALL_FUNC_SINGLE_IPI);
+}
+
+/**
+ * smp_send_reschedule - Send reschedule IPI to a CPU
+ * @cpu: The CPU to target.
+ */
+void smp_send_reschedule(int cpu)
+{
+        send_IPI_mask(cpumask_of(cpu), RESCHEDULE_IPI);
+}
+
+/**
+ * smp_nmi_call_function - Send a call function NMI IPI to all CPUs
+ * @func: The function to ask to be run.
+ * @info: The context data to pass to that function.
+ * @wait: If true, wait (atomically) until function is run on all CPUs.
+ *
+ * Send a non-maskable request to all CPUs in the system, requesting them to
+ * run the specified function with the given context data, and, potentially, to
+ * wait for completion of that function on all CPUs.
+ *
+ * Returns 0 if successful, -ETIMEDOUT if we were asked to wait, but hit the
+ * timeout.
+ */
+int smp_nmi_call_function(smp_call_func_t func, void *info, int wait)
+{
+        struct nmi_call_data_struct data;
+        unsigned long flags;
+        unsigned int cnt;
+        int cpus, ret = 0;
+
+        cpus = num_online_cpus() - 1;
+        if (cpus < 1)
+                return 0;
+
+        data.func = func;
+        data.info = info;
+        data.started = cpu_online_map;
+        cpu_clear(smp_processor_id(), data.started);
+        data.wait = wait;
+        if (wait)
+                data.finished = data.started;
+
+        spin_lock_irqsave(&smp_nmi_call_lock, flags);
+        nmi_call_data = &data;
+        smp_mb();
+
+        /* Send a message to all other CPUs and wait for them to respond */
+        send_IPI_allbutself(CALL_FUNCTION_NMI_IPI);
+
+        /* Wait for response */
+        if (CALL_FUNCTION_NMI_IPI_TIMEOUT > 0) {
+                for (cnt = 0;
+                     cnt < CALL_FUNCTION_NMI_IPI_TIMEOUT &&
+                             !cpus_empty(data.started);
+                     cnt++)
+                        mdelay(1);
+
+                if (wait && cnt < CALL_FUNCTION_NMI_IPI_TIMEOUT) {
+                        for (cnt = 0;
+                             cnt < CALL_FUNCTION_NMI_IPI_TIMEOUT &&
+                                     !cpus_empty(data.finished);
+                             cnt++)
+                                mdelay(1);
+                }
+
+                if (cnt >= CALL_FUNCTION_NMI_IPI_TIMEOUT)
+                        ret = -ETIMEDOUT;
+
+        } else {
+                /* If timeout value is zero, wait until cpumask has been
+                 * cleared */
+                while (!cpus_empty(data.started))
+                        barrier();
+                if (wait)
+                        while (!cpus_empty(data.finished))
+                                barrier();
+        }
+
+        spin_unlock_irqrestore(&smp_nmi_call_lock, flags);
+        return ret;
+}
+
+/**
+ * stop_this_cpu - Callback to stop a CPU.
+ * @unused: Callback context (ignored).
+ */
+void stop_this_cpu(void *unused)
+{
+        static volatile int stopflag;
+        unsigned long flags;
+
+#ifdef CONFIG_GDBSTUB
+        /* In case of single stepping smp_send_stop by other CPU,
+         * clear procindebug to avoid deadlock.
+         */
+        atomic_set(&procindebug[smp_processor_id()], 0);
+#endif  /* CONFIG_GDBSTUB */
+
+        flags = arch_local_cli_save();
+        cpu_clear(smp_processor_id(), cpu_online_map);
+
+        while (!stopflag)
+                cpu_relax();
+
+        cpu_set(smp_processor_id(), cpu_online_map);
+        arch_local_irq_restore(flags);
+}
+
+/**
+ * smp_send_stop - Send a stop request to all CPUs.
+ */
+void smp_send_stop(void)
+{
+        smp_nmi_call_function(stop_this_cpu, NULL, 0);
+}
+
+/**
+ * smp_reschedule_interrupt - Reschedule IPI handler
+ * @irq: The interrupt number.
+ * @dev_id: The device ID.
+ *
+ * We need do nothing here, since the scheduling will be effected on our way
+ * back through entry.S.
+ *
+ * Returns IRQ_HANDLED to indicate we handled the interrupt successfully.
+ */
+static irqreturn_t smp_reschedule_interrupt(int irq, void *dev_id)
+{
+        /* do nothing */
+        return IRQ_HANDLED;
+}
+
+/**
+ * smp_call_function_interrupt - Call function IPI handler
+ * @irq: The interrupt number.
+ * @dev_id: The device ID.
+ *
+ * Returns IRQ_HANDLED to indicate we handled the interrupt successfully.
+ */
+static irqreturn_t smp_call_function_interrupt(int irq, void *dev_id)
+{
+        /* generic_smp_call_function_interrupt(); */
+        generic_smp_call_function_single_interrupt();
+        return IRQ_HANDLED;
+}
+
+/**
+ * smp_nmi_call_function_interrupt - Non-maskable call function IPI handler
+ */
+void smp_nmi_call_function_interrupt(void)
+{
+        smp_call_func_t func = nmi_call_data->func;
+        void *info = nmi_call_data->info;
+        int wait = nmi_call_data->wait;
+
+        /* Notify the initiating CPU that I've grabbed the data and am about to
+         * execute the function
+         */
+        smp_mb();
+        cpu_clear(smp_processor_id(), nmi_call_data->started);
+        (*func)(info);
+
+        if (wait) {
+                smp_mb();
+                cpu_clear(smp_processor_id(), nmi_call_data->finished);
+        }
+}
+
+/**
+ * smp_ipi_timer_interrupt - Local timer IPI handler
+ * @irq: The interrupt number.
+ * @dev_id: The device ID.
+ *
+ * Returns IRQ_HANDLED to indicate we handled the interrupt successfully.
+ */
+static irqreturn_t smp_ipi_timer_interrupt(int irq, void *dev_id)
+{
+        return local_timer_interrupt();
+}
+
+void __init smp_init_cpus(void)
+{
+        int i;
+        for (i = 0; i < NR_CPUS; i++) {
+                set_cpu_possible(i, true);
+                set_cpu_present(i, true);
+        }
+}
+
+/**
+ * smp_cpu_init - Initialise AP in start_secondary.
+ *
+ * For this Application Processor, set up init_mm, initialise FPU and set
+ * interrupt level 0-6 setting.
+ */
+static void __init smp_cpu_init(void)
+{
+        unsigned long flags;
+        int cpu_id = smp_processor_id();
+        u16 tmp16;
+
+        if (test_and_set_bit(cpu_id, &cpu_initialized)) {
+                printk(KERN_WARNING "CPU#%d already initialized!\n", cpu_id);
+                for (;;)
+                        local_irq_enable();
+        }
+        printk(KERN_INFO "Initializing CPU#%d\n", cpu_id);
+
+        atomic_inc(&init_mm.mm_count);
+        current->active_mm = &init_mm;
+        BUG_ON(current->mm);
+
+        enter_lazy_tlb(&init_mm, current);
+
+        /* Force FPU initialization */
+        clear_using_fpu(current);
+
+        GxICR(CALL_FUNC_SINGLE_IPI) = CALL_FUNCTION_GxICR_LV | GxICR_DETECT;
+        mn10300_ipi_enable(CALL_FUNC_SINGLE_IPI);
+
+        GxICR(LOCAL_TIMER_IPI) = LOCAL_TIMER_GxICR_LV | GxICR_DETECT;
+        mn10300_ipi_enable(LOCAL_TIMER_IPI);
+
+        GxICR(RESCHEDULE_IPI) = RESCHEDULE_GxICR_LV | GxICR_DETECT;
+        mn10300_ipi_enable(RESCHEDULE_IPI);
+
+#ifdef CONFIG_MN10300_CACHE_ENABLED
+        GxICR(FLUSH_CACHE_IPI) = FLUSH_CACHE_GxICR_LV | GxICR_DETECT;
+        mn10300_ipi_enable(FLUSH_CACHE_IPI);
+#endif
+
+        mn10300_ipi_shutdown(SMP_BOOT_IRQ);
+
+        /* Set up the non-maskable call function IPI */
+        flags = arch_local_cli_save();
+        GxICR(CALL_FUNCTION_NMI_IPI) = GxICR_NMI | GxICR_ENABLE | GxICR_DETECT;
+        tmp16 = GxICR(CALL_FUNCTION_NMI_IPI);
+        arch_local_irq_restore(flags);
+}
+
+/**
+ * smp_prepare_cpu_init - Initialise CPU in startup_secondary
+ *
+ * Set interrupt level 0-6 setting and init ICR of gdbstub.
+ */
+void smp_prepare_cpu_init(void)
+{
+        int loop;
+
+        /* Set the interrupt vector registers */
+        IVAR0 = EXCEP_IRQ_LEVEL0;
+        IVAR1 = EXCEP_IRQ_LEVEL1;
+        IVAR2 = EXCEP_IRQ_LEVEL2;
+        IVAR3 = EXCEP_IRQ_LEVEL3;
+        IVAR4 = EXCEP_IRQ_LEVEL4;
+        IVAR5 = EXCEP_IRQ_LEVEL5;
+        IVAR6 = EXCEP_IRQ_LEVEL6;
+
+        /* Disable all interrupts and set to priority 6 (lowest) */
+        for (loop = 0; loop < GxICR_NUM_IRQS; loop++)
+                GxICR(loop) = GxICR_LEVEL_6 | GxICR_DETECT;
+
+#ifdef CONFIG_GDBSTUB
+        /* initialise GDB-stub */
+        do {
+                unsigned long flags;
+                u16 tmp16;
+
+                flags = arch_local_cli_save();
+                GxICR(GDB_NMI_IPI) = GxICR_NMI | GxICR_ENABLE | GxICR_DETECT;
+                tmp16 = GxICR(GDB_NMI_IPI);
+                arch_local_irq_restore(flags);
+        } while (0);
+#endif
+}
+
+/**
+ * start_secondary - Activate a secondary CPU (AP)
+ * @unused: Thread parameter (ignored).
+ */
+int __init start_secondary(void *unused)
+{
+        smp_cpu_init();
+
+        smp_callin();
+        while (!cpu_isset(smp_processor_id(), smp_commenced_mask))
+                cpu_relax();
+
+        local_flush_tlb();
+        preempt_disable();
+        smp_online();
+
+        cpu_idle();
+        return 0;
+}
+
+/**
+ * smp_prepare_cpus - Boot up secondary CPUs (APs)
+ * @max_cpus: Maximum number of CPUs to boot.
+ *
+ * Call do_boot_cpu, and boot up APs.
+ */
+void __init smp_prepare_cpus(unsigned int max_cpus)
+{
+        int phy_id;
+
+        /* Setup boot CPU information */
+        smp_store_cpu_info(0);
+        smp_tune_scheduling();
+
+        init_ipi();
+
+        /* If SMP should be disabled, then finish */
+        if (max_cpus == 0) {
+                printk(KERN_INFO "SMP mode deactivated.\n");
+                goto smp_done;
+        }
+
+        /* Boot secondary CPUs (for which phy_id > 0) */
+        for (phy_id = 0; phy_id < NR_CPUS; phy_id++) {
+                /* Don't boot primary CPU */
+                if (max_cpus <= cpucount + 1)
+                        continue;
+                if (phy_id != 0)
+                        do_boot_cpu(phy_id);
+                set_cpu_possible(phy_id, true);
+                smp_show_cpu_info(phy_id);
+        }
+
+smp_done:
+        Dprintk("Boot done.\n");
+}
+
+/**
+ * smp_store_cpu_info - Save a CPU's information
+ * @cpu: The CPU to save for.
+ *
+ * Save boot_cpu_data and jiffy for the specified CPU.
+ */
+static void __init smp_store_cpu_info(int cpu)
+{
+        struct mn10300_cpuinfo *ci = &cpu_data[cpu];
+
+        *ci = boot_cpu_data;
+        ci->loops_per_jiffy = loops_per_jiffy;
+        ci->type = CPUREV;
+}
+
+/**
+ * smp_tune_scheduling - Set time slice value
+ *
+ * Nothing to do here.
+ */
+static void __init smp_tune_scheduling(void)
+{
+}
+
+/**
+ * do_boot_cpu: Boot up one CPU
+ * @phy_id: Physical ID of CPU to boot.
+ *
+ * Send an IPI to a secondary CPU to boot it.  Returns 0 on success, 1
+ * otherwise.
+ */
+static int __init do_boot_cpu(int phy_id)
+{
+        struct task_struct *idle;
+        unsigned long send_status, callin_status;
+        int timeout, cpu_id;
+
+        send_status = GxICR_REQUEST;
+        callin_status = 0;
+        timeout = 0;
+        cpu_id = phy_id;
+
+        cpucount++;
+
+        /* Create idle thread for this CPU */
+        idle = fork_idle(cpu_id);
+        if (IS_ERR(idle))
+                panic("Failed fork for CPU#%d.", cpu_id);
+
+        idle->thread.pc = (unsigned long)start_secondary;
+
+        printk(KERN_NOTICE "Booting CPU#%d\n", cpu_id);
+        start_stack[cpu_id - 1] = idle->thread.sp;
+
+        task_thread_info(idle)->cpu = cpu_id;
+
+        /* Send boot IPI to AP */
+        send_IPI_mask(cpumask_of(phy_id), SMP_BOOT_IRQ);
+
+        Dprintk("Waiting for send to finish...\n");
+
+        /* Wait for AP's IPI receive in 100[ms] */
+        do {
+                udelay(1000);
+                send_status =
+                        CROSS_GxICR(SMP_BOOT_IRQ, phy_id) & GxICR_REQUEST;
+        } while (send_status == GxICR_REQUEST && timeout++ < 100);
+
+        Dprintk("Waiting for cpu_callin_map.\n");
+
+        if (send_status == 0) {
+                /* Allow AP to start initializing */
+                cpu_set(cpu_id, cpu_callout_map);
+
+                /* Wait for setting cpu_callin_map */
+                timeout = 0;
+                do {
+                        udelay(1000);
+                        callin_status = cpu_isset(cpu_id, cpu_callin_map);
+                } while (callin_status == 0 && timeout++ < 5000);
+
+                if (callin_status == 0)
+                        Dprintk("Not responding.\n");
+        } else {
+                printk(KERN_WARNING "IPI not delivered.\n");
+        }
+
+        if (send_status == GxICR_REQUEST || callin_status == 0) {
+                cpu_clear(cpu_id, cpu_callout_map);
+                cpu_clear(cpu_id, cpu_callin_map);
+                cpu_clear(cpu_id, cpu_initialized);
+                cpucount--;
+                return 1;
+        }
+        return 0;
+}
+
+/**
+ * smp_show_cpu_info - Show SMP CPU information
+ * @cpu: The CPU of interest.
+ */
+static void __init smp_show_cpu_info(int cpu)
+{
+        struct mn10300_cpuinfo *ci = &cpu_data[cpu];
+
+        printk(KERN_INFO
+               "CPU#%d : ioclk speed: %lu.%02luMHz : bogomips : %lu.%02lu\n",
+               cpu,
+               MN10300_IOCLK / 1000000,
+               (MN10300_IOCLK / 10000) % 100,
+               ci->loops_per_jiffy / (500000 / HZ),
+               (ci->loops_per_jiffy / (5000 / HZ)) % 100);
+}
+
+/**
+ * smp_callin - Set cpu_callin_map of the current CPU ID
+ */
+static void __init smp_callin(void)
+{
+        unsigned long timeout;
+        int cpu;
+
+        cpu = smp_processor_id();
+        timeout = jiffies + (2 * HZ);
+
+        if (cpu_isset(cpu, cpu_callin_map)) {
+                printk(KERN_ERR "CPU#%d already present.\n", cpu);
+                BUG();
+        }
+        Dprintk("CPU#%d waiting for CALLOUT\n", cpu);
+
+        /* Wait for AP startup 2s total */
+        while (time_before(jiffies, timeout)) {
+                if (cpu_isset(cpu, cpu_callout_map))
+                        break;
+                cpu_relax();
+        }
+
+        if (!time_before(jiffies, timeout)) {
+                printk(KERN_ERR
+                       "BUG: CPU#%d started up but did not get a callout!\n",
+                       cpu);
+                BUG();
+        }
+
+#ifdef CONFIG_CALIBRATE_DELAY
+        calibrate_delay();              /* Get our bogomips */
+#endif
+
+        /* Save our processor parameters */
+        smp_store_cpu_info(cpu);
+
+        /* Allow the boot processor to continue */
+        cpu_set(cpu, cpu_callin_map);
+}
+
+/**
+ * smp_online - Set cpu_online_map
+ */
+static void __init smp_online(void)
+{
+        int cpu;
+
+        cpu = smp_processor_id();
+
+        local_irq_enable();
+
+        cpu_set(cpu, cpu_online_map);
+        smp_wmb();
+}
+
+/**
+ * smp_cpus_done -
+ * @max_cpus: Maximum CPU count.
+ *
+ * Do nothing.
+ */
+void __init smp_cpus_done(unsigned int max_cpus)
+{
+}
+
+/*
+ * smp_prepare_boot_cpu - Set up stuff for the boot processor.
+ *
+ * Set up the cpu_online_map, cpu_callout_map and cpu_callin_map of the boot
+ * processor (CPU 0).
+ */
+void __devinit smp_prepare_boot_cpu(void)
+{
+        cpu_set(0, cpu_callout_map);
+        cpu_set(0, cpu_callin_map);
+        current_thread_info()->cpu = 0;
+}
+
+/*
+ * initialize_secondary - Initialise a secondary CPU (Application Processor).
+ *
+ * Set SP register and jump to thread's PC address.
+ */
+void initialize_secondary(void)
+{
+        asm volatile (
+                "mov    %0,sp   \n"
+                "jmp    (%1)    \n"
+                :
+                : "a"(current->thread.sp), "a"(current->thread.pc));
+}
+
+/**
+ * __cpu_up - Set smp_commenced_mask for the nominated CPU
+ * @cpu: The target CPU.
+ */
+int __devinit __cpu_up(unsigned int cpu)
+{
+        int timeout;
+
+#ifdef CONFIG_HOTPLUG_CPU
+        if (num_online_cpus() == 1)
+                disable_hlt();
+        if (sleep_mode[cpu])
+                run_wakeup_cpu(cpu);
+#endif /* CONFIG_HOTPLUG_CPU */
+
+        cpu_set(cpu, smp_commenced_mask);
+
+        /* Wait 5s total for a response */
+        for (timeout = 0 ; timeout < 5000 ; timeout++) {
+                if (cpu_isset(cpu, cpu_online_map))
+                        break;
+                udelay(1000);
+        }
+
+        BUG_ON(!cpu_isset(cpu, cpu_online_map));
+        return 0;
+}
+
+/**
+ * setup_profiling_timer - Set up the profiling timer
+ * @multiplier - The frequency multiplier to use
+ *
+ * The frequency of the profiling timer can be changed by writing a multiplier
+ * value into /proc/profile.
+ */
+int setup_profiling_timer(unsigned int multiplier)
+{
+        return -EINVAL;
+}
+
+/*
+ * CPU hotplug routines
+ */
+#ifdef CONFIG_HOTPLUG_CPU
+
+static DEFINE_PER_CPU(struct cpu, cpu_devices);
+
+static int __init topology_init(void)
+{
+        int cpu, ret;
+
+        for_each_cpu(cpu) {
+                ret = register_cpu(&per_cpu(cpu_devices, cpu), cpu, NULL);
+                if (ret)
+                        printk(KERN_WARNING
+                               "topology_init: register_cpu %d failed (%d)\n",
+                               cpu, ret);
+        }
+        return 0;
+}
+
+subsys_initcall(topology_init);
+
+int __cpu_disable(void)
+{
+        int cpu = smp_processor_id();
+        if (cpu == 0)
+                return -EBUSY;
+
+        migrate_irqs();
+        cpu_clear(cpu, current->active_mm->cpu_vm_mask);
+        return 0;
+}
+
+void __cpu_die(unsigned int cpu)
+{
+        run_sleep_cpu(cpu);
+
+        if (num_online_cpus() == 1)
+                enable_hlt();
+}
+
+#ifdef CONFIG_MN10300_CACHE_ENABLED
+static inline void hotplug_cpu_disable_cache(void)
+{
+        int tmp;
+        asm volatile(
+                "       movhu   (%1),%0 \n"
+                "       and     %2,%0   \n"
+                "       movhu   %0,(%1) \n"
+                "1:     movhu   (%1),%0 \n"
+                "       btst    %3,%0   \n"
+                "       bne     1b      \n"
+                : "=&r"(tmp)
+                : "a"(&CHCTR),
+                  "i"(~(CHCTR_ICEN | CHCTR_DCEN)),
+                  "i"(CHCTR_ICBUSY | CHCTR_DCBUSY)
+                : "memory", "cc");
+}
+
+static inline void hotplug_cpu_enable_cache(void)
+{
+        int tmp;
+        asm volatile(
+                "movhu  (%1),%0 \n"
+                "or     %2,%0   \n"
+                "movhu  %0,(%1) \n"
+                : "=&r"(tmp)
+                : "a"(&CHCTR),
+                  "i"(CHCTR_ICEN | CHCTR_DCEN)
+                : "memory", "cc");
+}
+
+static inline void hotplug_cpu_invalidate_cache(void)
+{
+        int tmp;
+        asm volatile (
+                "movhu  (%1),%0 \n"
+                "or     %2,%0   \n"
+                "movhu  %0,(%1) \n"
+                : "=&r"(tmp)
+                : "a"(&CHCTR),
+                  "i"(CHCTR_ICINV | CHCTR_DCINV)
+                : "cc");
+}
+
+#else /* CONFIG_MN10300_CACHE_ENABLED */
+#define hotplug_cpu_disable_cache()     do {} while (0)
+#define hotplug_cpu_enable_cache()      do {} while (0)
+#define hotplug_cpu_invalidate_cache()  do {} while (0)
+#endif /* CONFIG_MN10300_CACHE_ENABLED */
+
+/**
+ * hotplug_cpu_nmi_call_function - Call a function on other CPUs for hotplug
+ * @cpumask: List of target CPUs.
+ * @func: The function to call on those CPUs.
+ * @info: The context data for the function to be called.
+ * @wait: Whether to wait for the calls to complete.
+ *
+ * Non-maskably call a function on another CPU for hotplug purposes.
+ *
+ * This function must be called with maskable interrupts disabled.
+ */
+static int hotplug_cpu_nmi_call_function(cpumask_t cpumask,
+                                         smp_call_func_t func, void *info,
+                                         int wait)
+{
+        /*
+         * The address and the size of nmi_call_func_mask_data
+         * need to be aligned on L1_CACHE_BYTES.
+         */
+        static struct nmi_call_data_struct nmi_call_func_mask_data
+                __cacheline_aligned;
+        unsigned long start, end;
+
+        start = (unsigned long)&nmi_call_func_mask_data;
+        end = start + sizeof(struct nmi_call_data_struct);
+
+        nmi_call_func_mask_data.func = func;
+        nmi_call_func_mask_data.info = info;
+        nmi_call_func_mask_data.started = cpumask;
+        nmi_call_func_mask_data.wait = wait;
+        if (wait)
+                nmi_call_func_mask_data.finished = cpumask;
+
+        spin_lock(&smp_nmi_call_lock);
+        nmi_call_data = &nmi_call_func_mask_data;
+        mn10300_local_dcache_flush_range(start, end);
+        smp_wmb();
+
+        send_IPI_mask(cpumask, CALL_FUNCTION_NMI_IPI);
+
+        do {
+                mn10300_local_dcache_inv_range(start, end);
+                barrier();
+        } while (!cpus_empty(nmi_call_func_mask_data.started));
+
+        if (wait) {
+                do {
+                        mn10300_local_dcache_inv_range(start, end);
+                        barrier();
+                } while (!cpus_empty(nmi_call_func_mask_data.finished));
+        }
+
+        spin_unlock(&smp_nmi_call_lock);
+        return 0;
+}
+
+static void restart_wakeup_cpu(void)
+{
+        unsigned int cpu = smp_processor_id();
+
+        cpu_set(cpu, cpu_callin_map);
+        local_flush_tlb();
+        cpu_set(cpu, cpu_online_map);
+        smp_wmb();
+}
+
+static void prepare_sleep_cpu(void *unused)
+{
+        sleep_mode[smp_processor_id()] = 1;
+        smp_mb();
+        mn10300_local_dcache_flush_inv();
+        hotplug_cpu_disable_cache();
+        hotplug_cpu_invalidate_cache();
+}
+
+/* when this function called, IE=0, NMID=0. */
+static void sleep_cpu(void *unused)
+{
+        unsigned int cpu_id = smp_processor_id();
+        /*
+         * CALL_FUNCTION_NMI_IPI for wakeup_cpu() shall not be requested,
+         * before this cpu goes in SLEEP mode.
+         */
+        do {
+                smp_mb();
+                __sleep_cpu();
+        } while (sleep_mode[cpu_id]);
+        restart_wakeup_cpu();
+}
+
+static void run_sleep_cpu(unsigned int cpu)
+{
+        unsigned long flags;
+        cpumask_t cpumask = cpumask_of(cpu);
+
+        flags = arch_local_cli_save();
+        hotplug_cpu_nmi_call_function(cpumask, prepare_sleep_cpu, NULL, 1);
+        hotplug_cpu_nmi_call_function(cpumask, sleep_cpu, NULL, 0);
+        udelay(1);              /* delay for the cpu to sleep. */
+        arch_local_irq_restore(flags);
+}
+
+static void wakeup_cpu(void)
+{
+        hotplug_cpu_invalidate_cache();
+        hotplug_cpu_enable_cache();
+        smp_mb();
+        sleep_mode[smp_processor_id()] = 0;
+}
+
+static void run_wakeup_cpu(unsigned int cpu)
+{
+        unsigned long flags;
+
+        flags = arch_local_cli_save();
+#if NR_CPUS == 2
+        mn10300_local_dcache_flush_inv();
+#else
+        /*
+         * Before waking up the cpu,
+         * all online cpus should stop and flush D-Cache for global data.
+         */
+#error not support NR_CPUS > 2, when CONFIG_HOTPLUG_CPU=y.
+#endif
+        hotplug_cpu_nmi_call_function(cpumask_of(cpu), wakeup_cpu, NULL, 1);
+        arch_local_irq_restore(flags);
+}
+
+#endif /* CONFIG_HOTPLUG_CPU */
diff --git a/arch/mn10300/kernel/switch_to.S b/arch/mn10300/kernel/switch_to.S
index 630aad71b946..b08cb2e3aebd 100644
--- a/arch/mn10300/kernel/switch_to.S
+++ b/arch/mn10300/kernel/switch_to.S
@@ -15,6 +15,9 @@
 #include <linux/linkage.h>
 #include <asm/thread_info.h>
 #include <asm/cpu-regs.h>
+#ifdef CONFIG_SMP
+#include <proc/smp-regs.h>
+#endif /* CONFIG_SMP */
 
         .text
 
@@ -35,7 +38,14 @@ ENTRY(__switch_to)
         mov     d1,a1
 
         # save prev context
+#ifdef CONFIG_SMP
+        mov     (CPUID),a2
+        add     a2,a2
+        add     a2,a2
+        mov     (___frame,a2),d0
+#else  /* CONFIG_SMP */
         mov     (__frame),d0
+#endif /* CONFIG_SMP */
         mov     d0,(THREAD_FRAME,a0)
         mov     __switch_back,d0
         mov     d0,(THREAD_PC,a0)
@@ -59,7 +69,14 @@ ENTRY(__switch_to)
 #endif
 
         mov     (THREAD_FRAME,a1),a2
+#ifdef CONFIG_SMP
+        mov     (CPUID),a0
+        add     a0,a0
+        add     a0,a0
+        mov     a2,(___frame,a0)
+#else  /* CONFIG_SMP */
         mov     a2,(__frame)
+#endif /* CONFIG_SMP */
         mov     (THREAD_PC,a1),a2
         mov     d2,d0                   # for ret_from_fork
         mov     d0,a0                   # for __switch_to
diff --git a/arch/mn10300/kernel/time.c b/arch/mn10300/kernel/time.c
index 0b5c856b4266..0cb9bdb3b6bd 100644
--- a/arch/mn10300/kernel/time.c
+++ b/arch/mn10300/kernel/time.c
@@ -22,12 +22,7 @@
 #include <asm/processor.h>
 #include <asm/intctl-regs.h>
 #include <asm/rtc.h>
-
-#ifdef CONFIG_MN10300_RTC
-unsigned long mn10300_ioclk;            /* system I/O clock frequency */
-unsigned long mn10300_iobclk;           /* system I/O clock frequency */
-unsigned long mn10300_tsc_per_HZ;       /* number of ioclks per jiffy */
-#endif /* CONFIG_MN10300_RTC */
+#include "internal.h"
 
 static unsigned long mn10300_last_tsc;  /* time-stamp counter at last time
                                          * interrupt occurred */
@@ -95,6 +90,19 @@ static void __init mn10300_sched_clock_init(void)
                 __muldiv64u(NSEC_PER_SEC, 1 << 16, MN10300_TSCCLK);
 }
 
+/**
+ * local_timer_interrupt - Local timer interrupt handler
+ *
+ * Handle local timer interrupts for this CPU.  They may have been propagated
+ * to this CPU from the CPU that actually gets them by way of an IPI.
+ */
+irqreturn_t local_timer_interrupt(void)
+{
+        profile_tick(CPU_PROFILING);
+        update_process_times(user_mode(get_irq_regs()));
+        return IRQ_HANDLED;
+}
+
 /*
  * advance the kernel's time keeping clocks (xtime and jiffies)
  * - we use Timer 0 & 1 cascaded as a clock to nudge us the next time
@@ -103,6 +111,7 @@ static void __init mn10300_sched_clock_init(void)
 static irqreturn_t timer_interrupt(int irq, void *dev_id)
 {
         unsigned tsc, elapse;
+        irqreturn_t ret;
 
         write_seqlock(&xtime_lock);
 
@@ -114,15 +123,16 @@ static irqreturn_t timer_interrupt(int irq, void *dev_id)
                 mn10300_last_tsc -= MN10300_TSC_PER_HZ;
 
                 /* advance the kernel's time tracking system */
-                profile_tick(CPU_PROFILING);
                 do_timer(1);
         }
 
         write_sequnlock(&xtime_lock);
 
-        update_process_times(user_mode(get_irq_regs()));
-
-        return IRQ_HANDLED;
+        ret = local_timer_interrupt();
+#ifdef CONFIG_SMP
+        send_IPI_allbutself(LOCAL_TIMER_IPI);
+#endif
+        return ret;
 }
 
 /*
@@ -148,7 +158,7 @@ void __init time_init(void)
         /* use timer 0 & 1 cascaded to tick at as close to HZ as possible */
         setup_irq(TMJCIRQ, &timer_irq);
 
-        set_intr_level(TMJCIRQ, TMJCICR_LEVEL);
+        set_intr_level(TMJCIRQ, NUM2GxICR_LEVEL(CONFIG_TIMER_IRQ_LEVEL));
 
         startup_jiffies_counter();
 
diff --git a/arch/mn10300/kernel/traps.c b/arch/mn10300/kernel/traps.c
index 716a221df2f9..c924a1dd3323 100644
--- a/arch/mn10300/kernel/traps.c
+++ b/arch/mn10300/kernel/traps.c
@@ -45,8 +45,13 @@
 #error "INTERRUPT_VECTOR_BASE not aligned to 16MiB boundary!"
 #endif
 
+#ifdef CONFIG_SMP
+struct pt_regs *___frame[NR_CPUS]; /* current frame pointer */
+EXPORT_SYMBOL(___frame);
+#else  /* CONFIG_SMP */
 struct pt_regs *__frame; /* current frame pointer */
 EXPORT_SYMBOL(__frame);
+#endif /* CONFIG_SMP */
 
 int kstack_depth_to_print = 24;
 
@@ -221,11 +226,14 @@ void show_registers_only(struct pt_regs *regs)
         printk(KERN_EMERG "threadinfo=%p task=%p)\n",
                current_thread_info(), current);
 
-        if ((unsigned long) current >= 0x90000000UL &&
-            (unsigned long) current < 0x94000000UL)
+        if ((unsigned long) current >= PAGE_OFFSET &&
+            (unsigned long) current < (unsigned long)high_memory)
                 printk(KERN_EMERG "Process %s (pid: %d)\n",
                        current->comm, current->pid);
 
+#ifdef CONFIG_SMP
+        printk(KERN_EMERG "CPUID:  %08x\n", CPUID);
+#endif
         printk(KERN_EMERG "CPUP:   %04hx\n", CPUP);
         printk(KERN_EMERG "TBR:    %08x\n", TBR);
         printk(KERN_EMERG "DEAR:   %08x\n", DEAR);
@@ -521,8 +529,12 @@ void __init set_intr_stub(enum exception_code code, void *handler)
 {
         unsigned long addr;
         u8 *vector = (u8 *)(CONFIG_INTERRUPT_VECTOR_BASE + code);
+        unsigned long flags;
 
         addr = (unsigned long) handler - (unsigned long) vector;
+
+        flags = arch_local_cli_save();
+
         vector[0] = 0xdc;               /* JMP handler */
         vector[1] = addr;
         vector[2] = addr >> 8;
@@ -532,6 +544,8 @@ void __init set_intr_stub(enum exception_code code, void *handler)
         vector[6] = 0xcb;
         vector[7] = 0xcb;
 
+        arch_local_irq_restore(flags);
+
 #ifndef CONFIG_MN10300_CACHE_SNOOP
         mn10300_dcache_flush_inv();
         mn10300_icache_inv();