6 files changed, 142 insertions, 99 deletions

diff --git a/arch/sh/kernel/cpu/init.c b/arch/sh/kernel/cpu/init.c
index fdc245b7b043..c217c4bf0085 100644
--- a/arch/sh/kernel/cpu/init.c
+++ b/arch/sh/kernel/cpu/init.c
@@ -22,6 +22,7 @@
 #include <asm/cache.h>
 #include <asm/io.h>
 #include <asm/ubc.h>
+#include <asm/smp.h>
 
 /*
  * Generic wrapper for command line arguments to disable on-chip
@@ -216,8 +217,11 @@ static void __init dsp_init(void)
  * Each processor family is still responsible for doing its own probing
  * and cache configuration in detect_cpu_and_cache_system().
  */
-asmlinkage void __init sh_cpu_init(void)
+
+asmlinkage void __cpuinit sh_cpu_init(void)
 {
+        current_thread_info()->cpu = hard_smp_processor_id();
+
         /* First, probe the CPU */
         detect_cpu_and_cache_system();
 
@@ -227,9 +231,10 @@ asmlinkage void __init sh_cpu_init(void)
         /* Init the cache */
         cache_init();
 
-        shm_align_mask = max_t(unsigned long,
-                               current_cpu_data.dcache.way_size - 1,
-                               PAGE_SIZE - 1);
+        if (raw_smp_processor_id() == 0)
+                shm_align_mask = max_t(unsigned long,
+                                       current_cpu_data.dcache.way_size - 1,
+                                       PAGE_SIZE - 1);
 
         /* Disable the FPU */
         if (fpu_disabled) {
@@ -268,6 +273,7 @@ asmlinkage void __init sh_cpu_init(void)
          * like PTRACE_SINGLESTEP or doing hardware watchpoints in GDB.  So ..
          * we wake it up and hope that all is well.
          */
-        ubc_wakeup();
+        if (raw_smp_processor_id() == 0)
+                ubc_wakeup();
         speculative_execution_init();
 }
diff --git a/arch/sh/kernel/head.S b/arch/sh/kernel/head.S
index 0bccc0ca5a0f..3338239717f1 100644
--- a/arch/sh/kernel/head.S
+++ b/arch/sh/kernel/head.S
@@ -54,8 +54,8 @@ ENTRY(_stext)
         mov.l   1f, r0          ! MD=1, RB=0, BL=0, IMASK=0xF
         ldc     r0, sr
         !                       Initialize global interrupt mask
-        mov     #0, r0
 #ifdef CONFIG_CPU_HAS_SR_RB
+        mov     #0, r0
         ldc     r0, r6_bank
 #endif
         
@@ -72,15 +72,18 @@ ENTRY(_stext)
         !
         mov.l   2f, r0
         mov     r0, r15         ! Set initial r15 (stack pointer)
-        mov     #(THREAD_SIZE >> 10), r1
-        shll8   r1              ! r1 = THREAD_SIZE
-        shll2   r1
-        sub     r1, r0          !
 #ifdef CONFIG_CPU_HAS_SR_RB
+        mov.l   7f, r0
         ldc     r0, r7_bank     ! ... and initial thread_info
 #endif
         
         !                       Clear BSS area
+#ifdef CONFIG_SMP       
+        mov.l   3f, r0
+        cmp/eq  #0, r0          ! skip clear if set to zero
+        bt      10f
+#endif
+        
         mov.l   3f, r1
         add     #4, r1
         mov.l   4f, r2
@@ -89,13 +92,14 @@ ENTRY(_stext)
         bf/s    9b              ! while (r1 < r2)
          mov.l  r0,@-r2
 
+10:             
         !                       Additional CPU initialization
         mov.l   6f, r0
         jsr     @r0
          nop
 
         SYNCO()                 ! Wait for pending instructions..
-
+        
         !                       Start kernel
         mov.l   5f, r0
         jmp     @r0
@@ -107,8 +111,10 @@ ENTRY(_stext)
 #else
 1:      .long   0x400080F0              ! MD=1, RB=0, BL=0, FD=1, IMASK=0xF
 #endif
+ENTRY(stack_start)
 2:      .long   init_thread_union+THREAD_SIZE
 3:      .long   __bss_start
 4:      .long   _end
 5:      .long   start_kernel
 6:      .long   sh_cpu_init
+7:      .long   init_thread_union
diff --git a/arch/sh/kernel/smp.c b/arch/sh/kernel/smp.c
index f93d5ffa9417..94075e1a1e61 100644
--- a/arch/sh/kernel/smp.c
+++ b/arch/sh/kernel/smp.c
@@ -3,68 +3,40 @@
  *
  * SMP support for the SuperH processors.
  *
- * Copyright (C) 2002, 2003 Paul Mundt
+ * Copyright (C) 2002 - 2007 Paul Mundt
+ * Copyright (C) 2006 - 2007 Akio Idehara
  *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation; either version 2 of the License, or (at your
- * option) any later version.
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
  */
-
 #include <linux/err.h>
 #include <linux/cache.h>
 #include <linux/cpumask.h>
 #include <linux/delay.h>
 #include <linux/init.h>
-#include <linux/interrupt.h>
 #include <linux/spinlock.h>
-#include <linux/threads.h>
+#include <linux/mm.h>
 #include <linux/module.h>
-#include <linux/time.h>
-#include <linux/timex.h>
-#include <linux/sched.h>
-#include <linux/module.h>
-
+#include <linux/interrupt.h>
 #include <asm/atomic.h>
 #include <asm/processor.h>
 #include <asm/system.h>
 #include <asm/mmu_context.h>
 #include <asm/smp.h>
+#include <asm/cacheflush.h>
+#include <asm/sections.h>
 
-/*
- * This was written with the Sega Saturn (SMP SH-2 7604) in mind,
- * but is designed to be usable regardless if there's an MMU
- * present or not.
- */
-struct sh_cpuinfo cpu_data[NR_CPUS];
-
-extern void per_cpu_trap_init(void);
+int __cpu_number_map[NR_CPUS];          /* Map physical to logical */
+int __cpu_logical_map[NR_CPUS];         /* Map logical to physical */
 
 cpumask_t cpu_possible_map;
 EXPORT_SYMBOL(cpu_possible_map);
 
 cpumask_t cpu_online_map;
 EXPORT_SYMBOL(cpu_online_map);
-static atomic_t cpus_booted = ATOMIC_INIT(0);
-
-/* These are defined by the board-specific code. */
 
-/*
- * Cause the function described by call_data to be executed on the passed
- * cpu.  When the function has finished, increment the finished field of
- * call_data.
- */
-void __smp_send_ipi(unsigned int cpu, unsigned int action);
-
-/*
- * Find the number of available processors
- */
-unsigned int __smp_probe_cpus(void);
-
-/*
- * Start a particular processor
- */
-void __smp_slave_init(unsigned int cpu);
+static atomic_t cpus_booted = ATOMIC_INIT(0);
 
 /*
  * Run specified function on a particular processor.
@@ -73,74 +45,123 @@ void __smp_call_function(unsigned int cpu);
 
 static inline void __init smp_store_cpu_info(unsigned int cpu)
 {
-        cpu_data[cpu].loops_per_jiffy = loops_per_jiffy;
+        struct sh_cpuinfo *c = cpu_data + cpu;
+
+        c->loops_per_jiffy = loops_per_jiffy;
 }
 
 void __init smp_prepare_cpus(unsigned int max_cpus)
 {
         unsigned int cpu = smp_processor_id();
-        int i;
 
-        atomic_set(&cpus_booted, 1);
-        smp_store_cpu_info(cpu);
-        
-        for (i = 0; i < __smp_probe_cpus(); i++)
-                cpu_set(i, cpu_possible_map);
+        init_new_context(current, &init_mm);
+        current_thread_info()->cpu = cpu;
+        plat_prepare_cpus(max_cpus);
+
+#ifndef CONFIG_HOTPLUG_CPU
+        cpu_present_map = cpu_possible_map;
+#endif
 }
 
 void __devinit smp_prepare_boot_cpu(void)
 {
         unsigned int cpu = smp_processor_id();
 
+        __cpu_number_map[0] = cpu;
+        __cpu_logical_map[0] = cpu;
+
         cpu_set(cpu, cpu_online_map);
         cpu_set(cpu, cpu_possible_map);
 }
 
-int __cpu_up(unsigned int cpu)
+asmlinkage void __cpuinit start_secondary(void)
 {
-        struct task_struct *tsk;
+        unsigned int cpu;
+        struct mm_struct *mm = &init_mm;
 
-        tsk = fork_idle(cpu);
+        atomic_inc(&mm->mm_count);
+        atomic_inc(&mm->mm_users);
+        current->active_mm = mm;
+        BUG_ON(current->mm);
+        enter_lazy_tlb(mm, current);
+
+        per_cpu_trap_init();
 
-        if (IS_ERR(tsk))
-                panic("Failed forking idle task for cpu %d\n", cpu);
-        
-        task_thread_info(tsk)->cpu = cpu;
+        preempt_disable();
+
+        local_irq_enable();
+
+        calibrate_delay();
+
+        cpu = smp_processor_id();
+        smp_store_cpu_info(cpu);
 
         cpu_set(cpu, cpu_online_map);
 
-        return 0;
+        cpu_idle();
 }
 
-int start_secondary(void *unused)
+extern struct {
+        unsigned long sp;
+        unsigned long bss_start;
+        unsigned long bss_end;
+        void *start_kernel_fn;
+        void *cpu_init_fn;
+        void *thread_info;
+} stack_start;
+
+int __cpuinit __cpu_up(unsigned int cpu)
 {
-        unsigned int cpu;
+        struct task_struct *tsk;
+        unsigned long timeout;
 
-        cpu = smp_processor_id();
+        tsk = fork_idle(cpu);
+        if (IS_ERR(tsk)) {
+                printk(KERN_ERR "Failed forking idle task for cpu %d\n", cpu);
+                return PTR_ERR(tsk);
+        }
 
-        atomic_inc(&init_mm.mm_count);
-        current->active_mm = &init_mm;
+        /* Fill in data in head.S for secondary cpus */
+        stack_start.sp = tsk->thread.sp;
+        stack_start.thread_info = tsk->stack;
+        stack_start.bss_start = 0; /* don't clear bss for secondary cpus */
+        stack_start.start_kernel_fn = start_secondary;
 
-        smp_store_cpu_info(cpu);
+        flush_cache_all();
 
-        __smp_slave_init(cpu);
-        preempt_disable();
-        per_cpu_trap_init();
-        
-        atomic_inc(&cpus_booted);
+        plat_start_cpu(cpu, (unsigned long)_stext);
 
-        cpu_idle();
-        return 0;
+        timeout = jiffies + HZ;
+        while (time_before(jiffies, timeout)) {
+                if (cpu_online(cpu))
+                        break;
+
+                udelay(10);
+        }
+
+        if (cpu_online(cpu))
+                return 0;
+
+        return -ENOENT;
 }
 
 void __init smp_cpus_done(unsigned int max_cpus)
 {
-        smp_mb();
+        unsigned long bogosum = 0;
+        int cpu;
+
+        for_each_online_cpu(cpu)
+                bogosum += cpu_data[cpu].loops_per_jiffy;
+
+        printk(KERN_INFO "SMP: Total of %d processors activated "
+               "(%lu.%02lu BogoMIPS).\n", num_online_cpus(),
+               bogosum / (500000/HZ),
+               (bogosum / (5000/HZ)) % 100);
 }
 
 void smp_send_reschedule(int cpu)
 {
-        __smp_send_ipi(cpu, SMP_MSG_RESCHEDULE);
+        plat_send_ipi(cpu, SMP_MSG_RESCHEDULE);
 }
 
 static void stop_this_cpu(void *unused)
@@ -157,7 +178,6 @@ void smp_send_stop(void)
         smp_call_function(stop_this_cpu, 0, 1, 0);
 }
 
-
 struct smp_fn_call_struct smp_fn_call = {
         .lock           = SPIN_LOCK_UNLOCKED,
         .finished       = ATOMIC_INIT(0),
@@ -175,9 +195,6 @@ int smp_call_function(void (*func)(void *info), void *info, int retry, int wait)
         unsigned int nr_cpus = atomic_read(&cpus_booted);
         int i;
 
-        if (nr_cpus < 2)
-                return 0;
-
         /* Can deadlock when called with interrupts disabled */
         WARN_ON(irqs_disabled());
 
@@ -189,7 +206,7 @@ int smp_call_function(void (*func)(void *info), void *info, int retry, int wait)
 
         for (i = 0; i < nr_cpus; i++)
                 if (i != smp_processor_id())
-                        __smp_call_function(i);
+                        plat_send_ipi(i, SMP_MSG_FUNCTION);
 
         if (wait)
                 while (atomic_read(&smp_fn_call.finished) != (nr_cpus - 1));
diff --git a/arch/sh/kernel/traps.c b/arch/sh/kernel/traps.c
index 67015044d74a..dcb46e71da1c 100644
--- a/arch/sh/kernel/traps.c
+++ b/arch/sh/kernel/traps.c
@@ -807,12 +807,13 @@ static inline void __init gdb_vbr_init(void)
 }
 #endif
 
-void __init per_cpu_trap_init(void)
+void __cpuinit per_cpu_trap_init(void)
 {
         extern void *vbr_base;
 
 #ifdef CONFIG_SH_STANDARD_BIOS
-        gdb_vbr_init();
+        if (raw_smp_processor_id() == 0)
+                gdb_vbr_init();
 #endif
 
         /* NOTE: The VBR value should be at P1
diff --git a/include/asm-sh/smp.h b/include/asm-sh/smp.h
index b99ca786c0c1..9c8d34b07ebf 100644
--- a/include/asm-sh/smp.h
+++ b/include/asm-sh/smp.h
@@ -1,12 +1,3 @@
-/*
- * include/asm-sh/smp.h
- *
- * Copyright (C) 2002, 2003  Paul Mundt
- *
- * This file is subject to the terms and conditions of the GNU General Public
- * License.  See the file "COPYING" in the main directory of this archive for
- * more details.
- */
 #ifndef __ASM_SH_SMP_H
 #define __ASM_SH_SMP_H
 
@@ -20,6 +11,15 @@
 #include <asm/current.h>
 
 #define raw_smp_processor_id()  (current_thread_info()->cpu)
+#define hard_smp_processor_id() plat_smp_processor_id()
+
+/* Map from cpu id to sequential logical cpu number. */
+extern int __cpu_number_map[NR_CPUS];
+#define cpu_number_map(cpu)  __cpu_number_map[cpu]
+
+/* The reverse map from sequential logical cpu number to cpu id.  */
+extern int __cpu_logical_map[NR_CPUS];
+#define cpu_logical_map(cpu)  __cpu_logical_map[cpu]
 
 /* I've no idea what the real meaning of this is */
 #define PROC_CHANGE_PENALTY     20
@@ -35,10 +35,22 @@ struct smp_fn_call_struct {
 
 extern struct smp_fn_call_struct smp_fn_call;
 
-#define SMP_MSG_RESCHEDULE      0x0001
+#define SMP_MSG_FUNCTION        0
+#define SMP_MSG_RESCHEDULE      1
+#define SMP_MSG_NR              2
 
-#endif /* CONFIG_SMP */
+void plat_smp_setup(void);
+void plat_prepare_cpus(unsigned int max_cpus);
+int plat_smp_processor_id(void);
+void plat_start_cpu(unsigned int cpu, unsigned long entry_point);
+void plat_send_ipi(unsigned int cpu, unsigned int message);
+int plat_register_ipi_handler(unsigned int message,
+                              void (*handler)(void *), void *arg);
+
+#else
 
 #define hard_smp_processor_id() (0)
 
+#endif /* CONFIG_SMP */
+
 #endif /* __ASM_SH_SMP_H */
diff --git a/include/asm-sh/system.h b/include/asm-sh/system.h
index 245042537205..9d849e6df268 100644
--- a/include/asm-sh/system.h
+++ b/include/asm-sh/system.h
@@ -266,6 +266,7 @@ void disable_hlt(void);
 void enable_hlt(void);
 
 void default_idle(void);
+void per_cpu_trap_init(void);
 
 asmlinkage void break_point_trap(void);
 asmlinkage void debug_trap_handler(unsigned long r4, unsigned long r5,