5 files changed, 67 insertions, 1 deletions

diff --git a/arch/mips/kernel/process.c b/arch/mips/kernel/process.c
index 8b393df460a..199a06e873c 100644
--- a/arch/mips/kernel/process.c
+++ b/arch/mips/kernel/process.c
@@ -185,6 +185,17 @@ int copy_thread(int nr, unsigned long clone_flags, unsigned long usp,
 	childregs->cp0_status &= ~(ST0_CU2|ST0_CU1);
 	clear_tsk_thread_flag(p, TIF_USEDFPU);
 
+#ifdef CONFIG_MIPS_MT_FPAFF
+	/*
+	 * FPU affinity support is cleaner if we track the
+	 * user-visible CPU affinity from the very beginning.
+	 * The generic cpus_allowed mask will already have
+	 * been copied from the parent before copy_thread
+	 * is invoked.
+	 */
+	p->thread.user_cpus_allowed = p->cpus_allowed;
+#endif /* CONFIG_MIPS_MT_FPAFF */
+
 	if (clone_flags & CLONE_SETTLS)
 		ti->tp_value = regs->regs[7];
 
diff --git a/arch/mips/kernel/scall32-o32.S b/arch/mips/kernel/scall32-o32.S
index 4e36b87be1e..a0ac0e5f61a 100644
--- a/arch/mips/kernel/scall32-o32.S
+++ b/arch/mips/kernel/scall32-o32.S
@@ -569,8 +569,19 @@ einval:	li	v0, -EINVAL
 	sys	sys_tkill		2
 	sys	sys_sendfile64		5
 	sys	sys_futex		6
+#ifdef CONFIG_MIPS_MT_FPAFF
+	/*
+	 * For FPU affinity scheduling on MIPS MT processors, we need to
+	 * intercept sys_sched_xxxaffinity() calls until we get a proper hook
+	 * in kernel/sched.c.  Considered only temporary we only support these
+	 * hooks for the 32-bit kernel - there is no MIPS64 MT processor atm.
+	 */
+	sys	mipsmt_sys_sched_setaffinity	3
+	sys	mipsmt_sys_sched_getaffinity	3
+#else
 	sys	sys_sched_setaffinity	3
 	sys	sys_sched_getaffinity	3	/* 4240 */
+#endif /* CONFIG_MIPS_MT_FPAFF */
 	sys	sys_io_setup		2
 	sys	sys_io_destroy		1
 	sys	sys_io_getevents	5
diff --git a/arch/mips/kernel/setup.c b/arch/mips/kernel/setup.c
index dcbfd27071f..bcf1b10e518 100644
--- a/arch/mips/kernel/setup.c
+++ b/arch/mips/kernel/setup.c
@@ -529,7 +529,10 @@ void __init setup_arch(char **cmdline_p)
 
 int __init fpu_disable(char *s)
 {
-	cpu_data[0].options &= ~MIPS_CPU_FPU;
+	int i;
+
+	for (i = 0; i < NR_CPUS; i++)
+		cpu_data[i].options &= ~MIPS_CPU_FPU;
 
 	return 1;
 }
diff --git a/arch/mips/kernel/smp-mt.c b/arch/mips/kernel/smp-mt.c
index 19b8e4b31b7..57770902b9a 100644
--- a/arch/mips/kernel/smp-mt.c
+++ b/arch/mips/kernel/smp-mt.c
@@ -150,6 +150,11 @@ void plat_smp_setup(void)
 	unsigned long val;
 	int i, num;
 
+#ifdef CONFIG_MIPS_MT_FPAFF
+	/* If we have an FPU, enroll ourselves in the FPU-full mask */
+	if (cpu_has_fpu)
+		cpu_set(0, mt_fpu_cpumask);
+#endif /* CONFIG_MIPS_MT_FPAFF */
 	if (!cpu_has_mipsmt)
 		return;
 
@@ -312,6 +317,12 @@ void prom_smp_finish(void)
 {
 	write_c0_compare(read_c0_count() + (8* mips_hpt_frequency/HZ));
 
+#ifdef CONFIG_MIPS_MT_FPAFF
+	/* If we have an FPU, enroll ourselves in the FPU-full mask */
+	if (cpu_has_fpu)
+		cpu_set(smp_processor_id(), mt_fpu_cpumask);
+#endif /* CONFIG_MIPS_MT_FPAFF */
+
 	local_irq_enable();
 }
 
diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index 6336fe8008e..e9902d89dc0 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -758,6 +758,36 @@ 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;