1 files changed, 173 insertions, 39 deletions

diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index bed0eb6cf55d..4901f0a37fca 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -42,6 +42,7 @@
 #include <asm/watch.h>
 #include <asm/types.h>
 
+extern asmlinkage void handle_int(void);
 extern asmlinkage void handle_tlbm(void);
 extern asmlinkage void handle_tlbl(void);
 extern asmlinkage void handle_tlbs(void);
@@ -279,9 +280,16 @@ static DEFINE_SPINLOCK(die_lock);
 NORET_TYPE void ATTRIB_NORET die(const char * str, struct pt_regs * regs)
 {
 	static int die_counter;
+#ifdef CONFIG_MIPS_MT_SMTC
+	unsigned long dvpret = dvpe();
+#endif /* CONFIG_MIPS_MT_SMTC */
 
 	console_verbose();
 	spin_lock_irq(&die_lock);
+	bust_spinlocks(1);
+#ifdef CONFIG_MIPS_MT_SMTC
+	mips_mt_regdump(dvpret);
+#endif /* CONFIG_MIPS_MT_SMTC */
 	printk("%s[#%d]:\n", str, ++die_counter);
 	show_registers(regs);
 	spin_unlock_irq(&die_lock);
@@ -750,12 +758,43 @@ asmlinkage void do_cpu(struct pt_regs *regs)
 						&current->thread.fpu.soft);
 			if (sig)
 				force_sig(sig, current);
+#ifdef CONFIG_MIPS_MT_FPAFF
+			else {
+			/*
+			 * MIPS MT processors may have fewer FPU contexts
+			 * than CPU threads. If we've emulated more than
+			 * some threshold number of instructions, force
+			 * migration to a "CPU" that has FP support.
+			 */
+			 if(mt_fpemul_threshold > 0
+			 && ((current->thread.emulated_fp++
+			    > mt_fpemul_threshold))) {
+			  /*
+			   * If there's no FPU present, or if the
+			   * application has already restricted
+			   * the allowed set to exclude any CPUs
+			   * with FPUs, we'll skip the procedure.
+			   */
+			  if (cpus_intersects(current->cpus_allowed,
+			  			mt_fpu_cpumask)) {
+			    cpumask_t tmask;
+
+			    cpus_and(tmask,
+					current->thread.user_cpus_allowed,
+					mt_fpu_cpumask);
+			    set_cpus_allowed(current, tmask);
+			    current->thread.mflags |= MF_FPUBOUND;
+			  }
+			 }
+			}
+#endif /* CONFIG_MIPS_MT_FPAFF */
 		}
 
 		return;
 
 	case 2:
 	case 3:
+		die_if_kernel("do_cpu invoked from kernel context!", regs);
 		break;
 	}
 
@@ -793,6 +832,36 @@ asmlinkage void do_mcheck(struct pt_regs *regs)
 
 asmlinkage void do_mt(struct pt_regs *regs)
 {
+	int subcode;
+
+	die_if_kernel("MIPS MT Thread exception in kernel", regs);
+
+	subcode = (read_vpe_c0_vpecontrol() & VPECONTROL_EXCPT)
+			>> VPECONTROL_EXCPT_SHIFT;
+	switch (subcode) {
+	case 0:
+		printk(KERN_ERR "Thread Underflow\n");
+		break;
+	case 1:
+		printk(KERN_ERR "Thread Overflow\n");
+		break;
+	case 2:
+		printk(KERN_ERR "Invalid YIELD Qualifier\n");
+		break;
+	case 3:
+		printk(KERN_ERR "Gating Storage Exception\n");
+		break;
+	case 4:
+		printk(KERN_ERR "YIELD Scheduler Exception\n");
+		break;
+	case 5:
+		printk(KERN_ERR "Gating Storage Schedulier Exception\n");
+		break;
+	default:
+		printk(KERN_ERR "*** UNKNOWN THREAD EXCEPTION %d ***\n",
+			subcode);
+		break;
+	}
 	die_if_kernel("MIPS MT Thread exception in kernel", regs);
 
 	force_sig(SIGILL, current);
@@ -928,7 +997,15 @@ void ejtag_exception_handler(struct pt_regs *regs)
  */
 void nmi_exception_handler(struct pt_regs *regs)
 {
+#ifdef CONFIG_MIPS_MT_SMTC
+	unsigned long dvpret = dvpe();
+	bust_spinlocks(1);
+	printk("NMI taken!!!!\n");
+	mips_mt_regdump(dvpret);
+#else
+	bust_spinlocks(1);
 	printk("NMI taken!!!!\n");
+#endif /* CONFIG_MIPS_MT_SMTC */
 	die("NMI", regs);
 	while(1) ;
 }
@@ -960,27 +1037,29 @@ void *set_except_vector(int n, void *addr)
 
 #ifdef CONFIG_CPU_MIPSR2
 /*
- * Shadow register allocation
+ * MIPSR2 shadow register set allocation
  * FIXME: SMP...
  */
 
-/* MIPSR2 shadow register sets */
-struct shadow_registers {
-	spinlock_t sr_lock;	/*  */
-	int sr_supported;	/* Number of shadow register sets supported */
-	int sr_allocated;	/* Bitmap of allocated shadow registers */
+static struct shadow_registers {
+	/*
+	 * Number of shadow register sets supported
+	 */
+	unsigned long sr_supported;
+	/*
+	 * Bitmap of allocated shadow registers
+	 */
+	unsigned long sr_allocated;
 } shadow_registers;
 
-void mips_srs_init(void)
+static void mips_srs_init(void)
 {
 #ifdef CONFIG_CPU_MIPSR2_SRS
 	shadow_registers.sr_supported = ((read_c0_srsctl() >> 26) & 0x0f) + 1;
-	printk ("%d MIPSR2 register sets available\n", shadow_registers.sr_supported);
-#else
-	shadow_registers.sr_supported = 1;
+	printk(KERN_INFO "%d MIPSR2 register sets available\n",
+	       shadow_registers.sr_supported);
 #endif
 	shadow_registers.sr_allocated = 1;	/* Set 0 used by kernel */
-	spin_lock_init(&shadow_registers.sr_lock);
 }
 
 int mips_srs_max(void)
@@ -988,38 +1067,30 @@ int mips_srs_max(void)
 	return shadow_registers.sr_supported;
 }
 
-int mips_srs_alloc (void)
+int mips_srs_alloc(void)
 {
 	struct shadow_registers *sr = &shadow_registers;
-	unsigned long flags;
 	int set;
 
-	spin_lock_irqsave(&sr->sr_lock, flags);
+again:
+	set = find_first_zero_bit(&sr->sr_allocated, sr->sr_supported);
+	if (set >= sr->sr_supported)
+		return -1;
 
-	for (set = 0; set < sr->sr_supported; set++) {
-		if ((sr->sr_allocated & (1 << set)) == 0) {
-			sr->sr_allocated |= 1 << set;
-			spin_unlock_irqrestore(&sr->sr_lock, flags);
-			return set;
-		}
-	}
+	if (test_and_set_bit(set, &sr->sr_allocated))
+		goto again;
 
-	/* None available */
-	spin_unlock_irqrestore(&sr->sr_lock, flags);
-	return -1;
+	return set;
 }
 
-void mips_srs_free (int set)
+void mips_srs_free(int set)
 {
 	struct shadow_registers *sr = &shadow_registers;
-	unsigned long flags;
 
-	spin_lock_irqsave(&sr->sr_lock, flags);
-	sr->sr_allocated &= ~(1 << set);
-	spin_unlock_irqrestore(&sr->sr_lock, flags);
+	clear_bit(set, &sr->sr_allocated);
 }
 
-void *set_vi_srs_handler (int n, void *addr, int srs)
+static void *set_vi_srs_handler(int n, void *addr, int srs)
 {
 	unsigned long handler;
 	unsigned long old_handler = vi_handlers[n];
@@ -1032,8 +1103,7 @@ void *set_vi_srs_handler (int n, void *addr, int srs)
 	if (addr == NULL) {
 		handler = (unsigned long) do_default_vi;
 		srs = 0;
-	}
-	else
+	} else
 		handler = (unsigned long) addr;
 	vi_handlers[n] = (unsigned long) addr;
 
@@ -1045,8 +1115,7 @@ void *set_vi_srs_handler (int n, void *addr, int srs)
 	if (cpu_has_veic) {
 		if (board_bind_eic_interrupt)
 			board_bind_eic_interrupt (n, srs);
-	}
-	else if (cpu_has_vint) {
+	} else if (cpu_has_vint) {
 		/* SRSMap is only defined if shadow sets are implemented */
 		if (mips_srs_max() > 1)
 			change_c0_srsmap (0xf << n*4, srs << n*4);
@@ -1060,6 +1129,15 @@ void *set_vi_srs_handler (int n, void *addr, int srs)
 
 		extern char except_vec_vi, except_vec_vi_lui;
 		extern char except_vec_vi_ori, except_vec_vi_end;
+#ifdef CONFIG_MIPS_MT_SMTC
+		/*
+		 * We need to provide the SMTC vectored interrupt handler
+		 * not only with the address of the handler, but with the
+		 * Status.IM bit to be masked before going there.
+		 */
+		extern char except_vec_vi_mori;
+		const int mori_offset = &except_vec_vi_mori - &except_vec_vi;
+#endif /* CONFIG_MIPS_MT_SMTC */
 		const int handler_len = &except_vec_vi_end - &except_vec_vi;
 		const int lui_offset = &except_vec_vi_lui - &except_vec_vi;
 		const int ori_offset = &except_vec_vi_ori - &except_vec_vi;
@@ -1073,6 +1151,12 @@ void *set_vi_srs_handler (int n, void *addr, int srs)
 		}
 
 		memcpy (b, &except_vec_vi, handler_len);
+#ifdef CONFIG_MIPS_MT_SMTC
+		if (n > 7)
+			printk("Vector index %d exceeds SMTC maximum\n", n);
+		w = (u32 *)(b + mori_offset);
+		*w = (*w & 0xffff0000) | (0x100 << n);
+#endif /* CONFIG_MIPS_MT_SMTC */
 		w = (u32 *)(b + lui_offset);
 		*w = (*w & 0xffff0000) | (((u32)handler >> 16) & 0xffff);
 		w = (u32 *)(b + ori_offset);
@@ -1095,9 +1179,9 @@ void *set_vi_srs_handler (int n, void *addr, int srs)
 	return (void *)old_handler;
 }
 
-void *set_vi_handler (int n, void *addr)
+void *set_vi_handler(int n, void *addr)
 {
-	return set_vi_srs_handler (n, addr, 0);
+	return set_vi_srs_handler(n, addr, 0);
 }
 #endif
 
@@ -1113,8 +1197,29 @@ extern asmlinkage int _restore_fp_context(struct sigcontext *sc);
 extern asmlinkage int fpu_emulator_save_context(struct sigcontext *sc);
 extern asmlinkage int fpu_emulator_restore_context(struct sigcontext *sc);
 
+#ifdef CONFIG_SMP
+static int smp_save_fp_context(struct sigcontext *sc)
+{
+	return cpu_has_fpu
+	       ? _save_fp_context(sc)
+	       : fpu_emulator_save_context(sc);
+}
+
+static int smp_restore_fp_context(struct sigcontext *sc)
+{
+	return cpu_has_fpu
+	       ? _restore_fp_context(sc)
+	       : fpu_emulator_restore_context(sc);
+}
+#endif
+
 static inline void signal_init(void)
 {
+#ifdef CONFIG_SMP
+	/* For now just do the cpu_has_fpu check when the functions are invoked */
+	save_fp_context = smp_save_fp_context;
+	restore_fp_context = smp_restore_fp_context;
+#else
 	if (cpu_has_fpu) {
 		save_fp_context = _save_fp_context;
 		restore_fp_context = _restore_fp_context;
@@ -1122,6 +1227,7 @@ static inline void signal_init(void)
 		save_fp_context = fpu_emulator_save_context;
 		restore_fp_context = fpu_emulator_restore_context;
 	}
+#endif
 }
 
 #ifdef CONFIG_MIPS32_COMPAT
@@ -1158,6 +1264,20 @@ void __init per_cpu_trap_init(void)
 {
 	unsigned int cpu = smp_processor_id();
 	unsigned int status_set = ST0_CU0;
+#ifdef CONFIG_MIPS_MT_SMTC
+	int secondaryTC = 0;
+	int bootTC = (cpu == 0);
+
+	/*
+	 * Only do per_cpu_trap_init() for first TC of Each VPE.
+	 * Note that this hack assumes that the SMTC init code
+	 * assigns TCs consecutively and in ascending order.
+	 */
+
+	if (((read_c0_tcbind() & TCBIND_CURTC) != 0) &&
+	    ((read_c0_tcbind() & TCBIND_CURVPE) == cpu_data[cpu - 1].vpe_id))
+		secondaryTC = 1;
+#endif /* CONFIG_MIPS_MT_SMTC */
 
 	/*
 	 * Disable coprocessors and select 32-bit or 64-bit addressing
@@ -1180,6 +1300,10 @@ void __init per_cpu_trap_init(void)
 	write_c0_hwrena (0x0000000f); /* Allow rdhwr to all registers */
 #endif
 
+#ifdef CONFIG_MIPS_MT_SMTC
+	if (!secondaryTC) {
+#endif /* CONFIG_MIPS_MT_SMTC */
+
 	/*
 	 * Interrupt handling.
 	 */
@@ -1196,6 +1320,9 @@ void __init per_cpu_trap_init(void)
 		} else
 			set_c0_cause(CAUSEF_IV);
 	}
+#ifdef CONFIG_MIPS_MT_SMTC
+	}
+#endif /* CONFIG_MIPS_MT_SMTC */
 
 	cpu_data[cpu].asid_cache = ASID_FIRST_VERSION;
 	TLBMISS_HANDLER_SETUP();
@@ -1205,8 +1332,14 @@ void __init per_cpu_trap_init(void)
 	BUG_ON(current->mm);
 	enter_lazy_tlb(&init_mm, current);
 
-	cpu_cache_init();
-	tlb_init();
+#ifdef CONFIG_MIPS_MT_SMTC
+	if (bootTC) {
+#endif /* CONFIG_MIPS_MT_SMTC */
+		cpu_cache_init();
+		tlb_init();
+#ifdef CONFIG_MIPS_MT_SMTC
+	}
+#endif /* CONFIG_MIPS_MT_SMTC */
 }
 
 /* Install CPU exception handler */
@@ -1278,7 +1411,7 @@ void __init trap_init(void)
 	if (cpu_has_veic || cpu_has_vint) {
 		int nvec = cpu_has_veic ? 64 : 8;
 		for (i = 0; i < nvec; i++)
-			set_vi_handler (i, NULL);
+			set_vi_handler(i, NULL);
 	}
 	else if (cpu_has_divec)
 		set_handler(0x200, &except_vec4, 0x8);
@@ -1297,6 +1430,7 @@ void __init trap_init(void)
 	if (board_be_init)
 		board_be_init();
 
+	set_except_vector(0, handle_int);
 	set_except_vector(1, handle_tlbm);
 	set_except_vector(2, handle_tlbl);
 	set_except_vector(3, handle_tlbs);