SMP for 32-bit kernel, support for Sibyte SB1. Patch from Justin

with minor changes by me.

8 files changed, 493 insertions, 23 deletions

diff --git a/arch/mips/kernel/Makefile b/arch/mips/kernel/Makefile
index ec2eae67e..10b8240aa 100644
--- a/arch/mips/kernel/Makefile
+++ b/arch/mips/kernel/Makefile
@@ -38,6 +38,7 @@ endif
 endif
 endif
 
+obj-$(CONFIG_SMP)               += smp.o
 obj-$(CONFIG_MIPS_FPE_MODULE)	+= fpe.o
 ifndef CONFIG_MIPS_FPU_EMULATOR
   obj-y				+= softfp.o
diff --git a/arch/mips/kernel/entry.S b/arch/mips/kernel/entry.S
index bc56db51b..59bc8c2c9 100644
--- a/arch/mips/kernel/entry.S
+++ b/arch/mips/kernel/entry.S
@@ -12,6 +12,7 @@
 #include <linux/config.h>
 #include <linux/sys.h>
 
+#include <asm/addrspace.h>
 #include <asm/asm.h>
 #include <asm/current.h>
 #include <asm/errno.h>
diff --git a/arch/mips/kernel/head.S b/arch/mips/kernel/head.S
index 529ebb57d..fb73d3db0 100644
--- a/arch/mips/kernel/head.S
+++ b/arch/mips/kernel/head.S
@@ -59,9 +59,19 @@
 	.set	noat
 	LEAF(except_vec0_r4000)
 	.set	mips3
+#ifdef CONFIG_SMP
+	mfc0	k1, CP0_CONTEXT
+	la	k0, current_pgd
+	srl	k1, 23
+	sll	k1, 2
+	addu	k1, k0, k1
+	lw	k1, (k1)
+#else 
+	lw	k1, current_pgd			# get pgd pointer
+#endif	
 	mfc0	k0, CP0_BADVADDR		# Get faulting address
 	srl	k0, k0, 22			# get pgd only bits
-	lw	k1, current_pgd			# get pgd pointer
+
 	sll	k0, k0, 2
 	addu	k1, k1, k0			# add in pgd offset
 	mfc0	k0, CP0_CONTEXT			# get context reg
@@ -442,9 +452,9 @@ NESTED(kernel_entry, 16, sp)
 	 */
 	la	$28, init_task_union
 	addiu	t0, $28, KERNEL_STACK_SIZE-32
-	sw	t0, kernelsp
 	subu	sp, t0, 4*SZREG
 
+	sw	t0, kernelsp
 	/* The firmware/bootloader passes argc/argp/envp
 	 * to us as arguments.  But clear bss first because
 	 * the romvec and other important info is stored there
@@ -462,6 +472,30 @@ NESTED(kernel_entry, 16, sp)
 	 nop
 	END(kernel_entry)
 
+
+#ifdef CONFIG_SMP
+
+/*
+ * SMP slave cpus entry point.  Board specific code
+ * for bootstrap calls this function after setting up
+ * the stack and gp registers. 
+ */
+	LEAF(smp_bootstrap)
+	.set push
+	.set noreorder
+	mtc0	zero, CP0_WIRED
+	CLI
+	mfc0	t0, CP0_STATUS
+	li	t1, ~(ST0_CU1|ST0_CU2|ST0_CU3|ST0_BEV); 
+	and	t0, t1
+	or	t0, (ST0_CU0|ST0_KX|ST0_SX|ST0_FR);
+	addiu	a0, zero, 0
+	jal	start_secondary
+         mtc0	t0, CP0_STATUS
+	.set pop
+	END(smp_bootstrap)
+#endif
+	
 /*
  * This buffer is reserved for the use of the cache error handler.
  */
@@ -469,12 +503,19 @@ NESTED(kernel_entry, 16, sp)
 		EXPORT(cache_error_buffer)
 		.fill	32*4,1,0
 
+#ifndef CONFIG_SMP
 EXPORT(kernelsp)
 		PTR	0
 EXPORT(current_pgd)
-		PTR	0
+		PTR     0
+#else
+		/* There's almost certainly a better way to do this with the macros...*/
+		.globl kernelsp
+		.comm  kernelsp,     NR_CPUS * 8, 8
+		.globl current_pgd
+		.comm	current_pgd, NR_CPUS * 8, 8
+#endif
 		.text
-
 		.org	0x1000
 EXPORT(swapper_pg_dir)
 
diff --git a/arch/mips/kernel/process.c b/arch/mips/kernel/process.c
index 6cda842fd..9284db514 100644
--- a/arch/mips/kernel/process.c
+++ b/arch/mips/kernel/process.c
@@ -162,21 +162,21 @@ int kernel_thread(int (*fn)(void *), void * arg, unsigned long flags)
 	long retval;
 
 	__asm__ __volatile__(
-		".set\tnoreorder\n\t"
-		"move\t$6,$sp\n\t"
-		"move\t$4,%5\n\t"
-		"li\t$2,%1\n\t"
-		"syscall\n\t"
-		"beq\t$6,$sp,1f\n\t"
-		"subu\t$sp,32\n\t"	/* delay slot */
-		"jalr\t%4\n\t"
-		"move\t$4,%3\n\t"	/* delay slot */
-		"move\t$4,$2\n\t"
-		"li\t$2,%2\n\t"
-		"syscall\n"
-		"1:\taddiu\t$sp,32\n\t"
-		"move\t%0,$2\n\t"
-		".set\treorder"
+		".set noreorder               \n"
+		"    move    $6,$sp           \n"
+		"    move    $4,%5            \n"
+		"    li      $2,%1            \n"
+		"    syscall                  \n"
+		"    beq     $6,$sp,1f        \n"
+		"    subu    $sp,32           \n"	/* delay slot */
+		"    jalr    %4               \n"
+		"    move    $4,%3            \n"	/* delay slot */
+		"    move    $4,$2            \n"
+		"    li      $2,%2            \n"
+		"    syscall                  \n"
+		"1:  addiu   $sp,32           \n"
+		"    move    %0,$2            \n"
+		".set reorder"
 		:"=r" (retval)
 		:"i" (__NR_clone), "i" (__NR_exit),
 		 "r" (arg), "r" (fn),
diff --git a/arch/mips/kernel/r4k_misc.S b/arch/mips/kernel/r4k_misc.S
index bfa3cb82c..e16820f88 100644
--- a/arch/mips/kernel/r4k_misc.S
+++ b/arch/mips/kernel/r4k_misc.S
@@ -35,10 +35,25 @@
 	 * in register PTE, a ptr into the table in which
 	 * the pte belongs is in PTR.
 	 */
+
+#ifdef CONFIG_SMP
+#define GET_PGD(scratch, ptr)        \
+	mfc0    ptr, CP0_CONTEXT;    \
+	la      scratch, current_pgd;\
+	srl     ptr, 23;             \
+	sll     ptr, 2;              \
+	addu    ptr, scratch, ptr;   \
+	lw      ptr, (ptr);          
+#else
+#define GET_PGD(scratch, ptr)    \
+	lw	ptr, current_pgd;
+#endif
+	
+	
 #define LOAD_PTE(pte, ptr) \
+	GET_PGD(pte, ptr)          \
 	mfc0	pte, CP0_BADVADDR; \
 	srl	pte, pte, 22; \
-	lw	ptr, current_pgd; \
 	sll	pte, pte, 2; \
 	addu	ptr, ptr, pte; \
 	mfc0	pte, CP0_BADVADDR; \
diff --git a/arch/mips/kernel/r4k_switch.S b/arch/mips/kernel/r4k_switch.S
index e1d10f92d..10328f4ad 100644
--- a/arch/mips/kernel/r4k_switch.S
+++ b/arch/mips/kernel/r4k_switch.S
@@ -1,5 +1,4 @@
-/* $Id: r4k_switch.S,v 1.9 1999/08/18 23:37:44 ralf Exp $
- *
+/*
  * 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.
@@ -50,7 +49,16 @@
 	move	$28, a1
 	CPU_RESTORE_NONSCRATCH($28)
 	addiu	t0, $28, KERNEL_STACK_SIZE-32
+#ifdef CONFIG_SMP	 
+	mfc0	a3, CP0_CONTEXT
+	la	t1, kernelsp
+	srl	a3, 23
+	sll	a3, 2
+	addu	t1, a3, t1
+	sw	t0, (t1)	
+#else
 	sw	t0, kernelsp
+#endif 
 	mfc0	t1, CP0_STATUS		/* Do we really need this? */
 	li	a3, 0xff00
 	and	t1, a3
diff --git a/arch/mips/kernel/smp.c b/arch/mips/kernel/smp.c
new file mode 100644
index 000000000..12653eb87
--- /dev/null
+++ b/arch/mips/kernel/smp.c
@@ -0,0 +1,403 @@
+/*
+ *
+ *  arch/mips/kernel/smp.c
+ *
+ *  Copyright (C) 2000 Sibyte
+ * 
+ *  Written by Justin Carlson (carlson@sibyte.com)
+ *
+ * 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 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.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ */ 
+
+
+#include <linux/config.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/threads.h>
+#include <linux/time.h>
+#include <linux/timex.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+
+#include <asm/atomic.h>
+#include <asm/processor.h>
+#include <asm/system.h>
+#include <asm/hardirq.h>
+#include <asm/softirq.h>
+#include <asm/mmu_context.h>
+#include <asm/delay.h>
+#include <asm/smp.h>
+
+/*
+ * This was written with the BRCM12500 MP SOC in mind, but tries to
+ * be generic.  It's modelled on the mips64 smp.c code, which is
+ * derived from Sparc, I'm guessing, which is derived from...
+ * 
+ * It's probably horribly designed for very large ccNUMA systems
+ * as it doesn't take any node clustering into account.  
+*/
+
+
+/* Ze Big Kernel Lock! */
+spinlock_t kernel_flag = SPIN_LOCK_UNLOCKED;
+int smp_threads_ready;  /* Not used */
+int smp_num_cpus;    
+int global_irq_holder = NO_PROC_ID;
+spinlock_t global_irq_lock = SPIN_LOCK_UNLOCKED;
+struct mips_cpuinfo cpu_data[NR_CPUS];
+
+struct smp_fn_call_struct smp_fn_call = 
+{ SPIN_LOCK_UNLOCKED, ATOMIC_INIT(0), NULL, NULL};
+
+static atomic_t cpus_booted = ATOMIC_INIT(0);
+
+
+/* These are defined by the board-specific code. */
+
+/* Cause the function described by smp_fn_call 
+   to be executed on the passed cpu.  When the function
+   has finished, increment the finished field of
+   smp_fn_call. */
+
+void core_call_function(int cpu);
+
+/*
+   Clear all undefined state in the cpu, set up sp and gp to the passed
+   values, and kick the cpu into smp_bootstrap(); 
+*/
+void prom_boot_secondary(int cpu, unsigned long sp, unsigned long gp);
+
+/*
+ *  After we've done initial boot, this function is called to allow the
+ *  board code to clean up state, if needed 
+ */
+
+void prom_init_secondary(void);
+
+
+void cpu_idle(void);
+
+/* Do whatever setup needs to be done for SMP at the board level.  Return
+   the number of cpus in the system, including this one */
+int prom_setup_smp(void);
+
+int start_secondary(void *unused)
+{
+	prom_init_secondary();
+	write_32bit_cp0_register(CP0_CONTEXT, smp_processor_id()<<23);
+	current_pgd[smp_processor_id()] = init_mm.pgd;
+	printk("Slave cpu booted successfully\n");
+	atomic_inc(&cpus_booted);
+	cpu_idle();
+	return 0;
+}
+
+void __init smp_boot_cpus(void)
+{
+	int i;
+	smp_num_cpus = prom_setup_smp();
+	init_new_context(current, &init_mm);
+	current->processor = 0;
+	atomic_set(&cpus_booted, 1);  /* Master CPU is already booted... */
+	init_idle();
+	for (i = 1; i < smp_num_cpus; i++) {
+		struct task_struct *p;
+		struct pt_regs regs;
+		printk("Starting CPU %d... ", i);
+
+		/* Spawn a new process normally.  Grab a pointer to
+		   its task struct so we can mess with it */
+		do_fork(CLONE_VM|CLONE_PID, 0, &regs, 0);
+		p = init_task.prev_task;
+
+		/* Schedule the first task manually */
+
+		p->processor = i;
+		p->has_cpu = 1;
+
+		/* Attach to the address space of init_task. */
+		atomic_inc(&init_mm.mm_count);
+		p->active_mm = &init_mm;
+		init_tasks[i] = p;
+		
+		del_from_runqueue(p);
+		unhash_process(p);
+
+		prom_boot_secondary(i,
+				    (unsigned long)p + KERNEL_STACK_SIZE - 32,
+				    (unsigned long)p);
+
+#if 0
+
+		/* This is copied from the ip-27 code in the mips64 tree */
+		
+		struct task_struct *p;
+		
+		/*
+		 * The following code is purely to make sure
+		 * Linux can schedule processes on this slave.
+		 */
+		kernel_thread(0, NULL, CLONE_PID);
+		p = init_task.prev_task;
+		sprintf(p->comm, "%s%d", "Idle", i);
+		init_tasks[i] = p;
+		p->processor = i;
+		p->has_cpu = 1; /* we schedule the first task manually */
+		del_from_runqueue(p);
+		unhash_process(p);
+		/* Attach to the address space of init_task. */
+		atomic_inc(&init_mm.mm_count);
+		p->active_mm = &init_mm;
+		prom_boot_secondary(i, 
+				    (unsigned long)p + KERNEL_STACK_SIZE - 32,
+				    (unsigned long)p);
+#endif
+	}
+	/* Wait for everyone to come up */
+	while (atomic_read(&cpus_booted) != smp_num_cpus) {}		
+}
+
+void __init smp_commence(void)
+{
+	/* Not sure what to do here yet */
+}
+
+static void reschedule_this_cpu(void *dummy)
+{
+	current->need_resched = 1;
+}
+
+void FASTCALL(smp_send_reschedule(int cpu))
+{
+	smp_call_function(reschedule_this_cpu, NULL, 0, 0);
+}
+
+
+/* The caller of this wants the passed function to run on every cpu.  If
+   wait is set, wait until all cpus have finished the function before
+   returning.  The lock is here to protect the call structure.  */
+int smp_call_function (void (*func) (void *info), void *info, int retry, 
+								int wait)
+{
+	int i;
+	int cpus = smp_num_cpus - 1;
+
+//	unsigned long flags;
+
+	if (smp_num_cpus < 2) {
+		return 0;
+	}
+
+	spin_lock_bh(&smp_fn_call.lock);
+
+	atomic_set(&smp_fn_call.finished, 0);
+	smp_fn_call.fn = func;
+	smp_fn_call.data = info;
+
+	for (i = 0; i < smp_num_cpus; i++) {
+		if (i != smp_processor_id()) {
+			/* Call the board specific routine */
+			core_call_function(i);
+		}
+	}
+
+	if (wait) {
+		while(atomic_read(&smp_fn_call.finished) != cpus) {}
+	}
+
+	spin_unlock_bh(&smp_fn_call.lock);
+	return 0;
+}
+
+void synchronize_irq(void)
+{
+	panic("synchronize_irq");
+}
+
+static void stop_this_cpu(void *dummy)
+{
+	printk("Cpu stopping\n");
+	for (;;);
+}
+
+void smp_send_stop(void)
+{
+	smp_call_function(stop_this_cpu, NULL, 1, 0);
+	smp_num_cpus = 1;
+}
+
+/* Not really SMP stuff ... */
+int setup_profiling_timer(unsigned int multiplier)
+{
+	return 0;
+}
+
+
+/* Most of this code is take from the mips64 tree (ip27-irq.c).  It's virtually identical
+   to the i386 implentation in arh/i386/irq.c, with translations for the interrupt enable bit */
+
+
+#define MAXCOUNT 		100000000
+#define SYNC_OTHER_CORES(x)	udelay(x+1)
+
+
+
+static inline void wait_on_irq(int cpu)
+{
+	int count = MAXCOUNT;
+
+	for (;;) {
+
+		/*
+		 * Wait until all interrupts are gone. Wait
+		 * for bottom half handlers unless we're
+		 * already executing in one..
+		 */
+		if (!irqs_running())
+			if (local_bh_count(cpu) || !spin_is_locked(&global_bh_lock))
+				break;
+
+		/* Duh, we have to loop. Release the lock to avoid deadlocks */
+		spin_unlock(&global_irq_lock);
+
+		for (;;) {
+			if (!--count) {
+				printk("Count spun out.  Huh?\n");
+				count = ~0;
+			}
+			__sti();
+			SYNC_OTHER_CORES(cpu);
+			__cli();
+			if (irqs_running())
+				continue;
+			if (spin_is_locked(&global_irq_lock))
+				continue;
+			if (!local_bh_count(cpu) && spin_is_locked(&global_bh_lock))
+				continue;
+			if (spin_trylock(&global_irq_lock))
+				break;
+		}
+	}
+}
+
+
+static inline void get_irqlock(int cpu)
+{
+	if (!spin_trylock(&global_irq_lock)) {
+		/* do we already hold the lock? */
+		if ((unsigned char) cpu == global_irq_holder)
+			return;
+		/* Uhhuh.. Somebody else got it. Wait.. */
+		spin_lock(&global_irq_lock);
+	}
+	/*
+	 * We also to make sure that nobody else is running
+	 * in an interrupt context.
+	 */
+	wait_on_irq(cpu);
+
+	/*
+	 * Ok, finally..
+	 */
+	global_irq_holder = cpu;
+}
+
+
+/*
+ * A global "cli()" while in an interrupt context
+ * turns into just a local cli(). Interrupts
+ * should use spinlocks for the (very unlikely)
+ * case that they ever want to protect against
+ * each other.
+ *
+ * If we already have local interrupts disabled,
+ * this will not turn a local disable into a
+ * global one (problems with spinlocks: this makes
+ * save_flags+cli+sti usable inside a spinlock).
+ */
+void __global_cli(void)
+{
+	unsigned int flags;
+
+	__save_flags(flags);
+	if (flags & ST0_IE) {
+		int cpu = smp_processor_id();
+		__cli();
+		if (!local_irq_count(cpu))
+			get_irqlock(cpu);
+	}
+}
+
+void __global_sti(void)
+{
+	int cpu = smp_processor_id();
+
+	if (!local_irq_count(cpu))
+		release_irqlock(cpu);
+	__sti();
+}
+
+/*
+ * SMP flags value to restore to:
+ * 0 - global cli
+ * 1 - global sti
+ * 2 - local cli
+ * 3 - local sti
+ */
+unsigned long __global_save_flags(void)
+{
+	int retval;
+	int local_enabled;
+	unsigned long flags;
+	int cpu = smp_processor_id();
+
+	__save_flags(flags);
+	local_enabled = (flags & ST0_IE);
+	/* default to local */
+	retval = 2 + local_enabled;
+
+	/* check for global flags if we're not in an interrupt */
+	if (!local_irq_count(cpu)) {
+		if (local_enabled)
+			retval = 1;
+		if (global_irq_holder == cpu)
+			retval = 0;
+	}
+	return retval;
+}
+
+void __global_restore_flags(unsigned long flags)
+{
+	switch (flags) {
+		case 0:
+			__global_cli();
+			break;
+		case 1:
+			__global_sti();
+			break;
+		case 2:
+			__cli();
+			break;
+		case 3:
+			__sti();
+			break;
+		default:
+			printk("global_restore_flags: %08lx\n", flags);
+	}
+}
+
+
+
diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index e2c3663cb..85767362e 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -938,5 +938,6 @@ void __init trap_init(void)
 
 	atomic_inc(&init_mm.mm_count);	/* XXX  UP?  */
 	current->active_mm = &init_mm;
-	current_pgd = init_mm.pgd;
+	write_32bit_cp0_register(CP0_CONTEXT, smp_processor_id()<<23);
+	current_pgd[0] = init_mm.pgd;
 }