[RFC PATCH 0/5] Cop2 save/restore for XLR/XLP

[RFC PATCH 0/5] Cop2 save/restore for XLR/XLP

[Jayachandran C]

This patchset is to generalize the COP2 save/restore implemented for
Octeon and use it for Netlogic XLR/XLP.

Patch 1 and 2 of the series affects the Octeon platform as well, so please
let me know if there are any suggestions/comments.

Thanks,
JC.

Jayachandran C (5):
  MIPS: Move cop2 save/restore to switch_to()
  MIPS: Allow kernel to use coprocessor 2
  MIPS: Netlogic: Fix nlm_read_c2_status() definition
  MIPS: Netlogic: rename nlm_cop2_save/restore
  MIPS: Netlogic: COP2 save/restore code

 arch/mips/include/asm/cop2.h             |   29 ++++++++
 arch/mips/include/asm/netlogic/xlr/fmn.h |   13 ++--
 arch/mips/include/asm/processor.h        |   30 +++++---
 arch/mips/include/asm/switch_to.h        |   19 ++++-
 arch/mips/kernel/octeon_switch.S         |   27 -------
 arch/mips/kernel/traps.c                 |   15 ++--
 arch/mips/netlogic/common/Makefile       |    1 +
 arch/mips/netlogic/common/cop2-ex.c      |  113 ++++++++++++++++++++++++++++++
 arch/mips/netlogic/xlr/fmn.c             |   18 ++---
 9 files changed, 202 insertions(+), 63 deletions(-)
 create mode 100644 arch/mips/netlogic/common/cop2-ex.c

-- 
1.7.9.5

[PATCH 1/5] MIPS: Move cop2 save/restore to switch_to()

[Jayachandran C]

Move the common code in saving and restoring platform specific COP2
registers to switch_to. This will make supporting new platforms (like
Netlogic XLR/XLP) easier.

The platform specific COP2 definitions are to be specified in
asm/processor.h and in asm/cop2.h.

Signed-off-by: Jayachandran C <jchandra@broadcom.com>
---
 arch/mips/include/asm/cop2.h      |   19 +++++++++++++++++++
 arch/mips/include/asm/processor.h |   18 +++++++-----------
 arch/mips/include/asm/switch_to.h |   19 ++++++++++++++++++-
 arch/mips/kernel/octeon_switch.S  |   27 ---------------------------
 4 files changed, 44 insertions(+), 39 deletions(-)

diff --git a/arch/mips/include/asm/cop2.h b/arch/mips/include/asm/cop2.h
index 3532e2c..b17f38e 100644
--- a/arch/mips/include/asm/cop2.h
+++ b/arch/mips/include/asm/cop2.h
@@ -11,6 +11,25 @@
 
 #include <linux/notifier.h>
 
+#if defined(CONFIG_CPU_CAVIUM_OCTEON)
+
+extern void octeon_cop2_save(struct octeon_cop2_state *);
+extern void octeon_cop2_restore(struct octeon_cop2_state *);
+
+#define cop2_save(r)		octeon_cop2_save(r)
+#define cop2_restore(r)		octeon_cop2_restore(r)
+
+#define cop2_present		1
+#define cop2_lazy_restore	1
+
+#else
+
+#define cop2_present		0
+#define cop2_lazy_restore	0
+#define cop2_save(r)
+#define cop2_restore(r)
+#endif
+
 enum cu2_ops {
 	CU2_EXCEPTION,
 	CU2_LWC2_OP,
diff --git a/arch/mips/include/asm/processor.h b/arch/mips/include/asm/processor.h
index 2a5fa7a..131c78f 100644
--- a/arch/mips/include/asm/processor.h
+++ b/arch/mips/include/asm/processor.h
@@ -133,7 +133,7 @@ union mips_watch_reg_state {
 	struct mips3264_watch_reg_state mips3264;
 };
 
-#ifdef CONFIG_CPU_CAVIUM_OCTEON
+#if defined(CONFIG_CPU_CAVIUM_OCTEON)
 
 struct octeon_cop2_state {
 	/* DMFC2 rt, 0x0201 */
@@ -178,13 +178,16 @@ struct octeon_cop2_state {
 	/* DMFC2 rt, 0x025A; DMFC2 rt, 0x025B - Pass2 */
 	unsigned long	cop2_gfm_result[2];
 };
-#define INIT_OCTEON_COP2 {0,}
+#define COP2_INIT						\
+	.cp2			= {0,},
 
 struct octeon_cvmseg_state {
 	unsigned long cvmseg[CONFIG_CAVIUM_OCTEON_CVMSEG_SIZE]
 			    [cpu_dcache_line_size() / sizeof(unsigned long)];
 };
 
+#else
+#define COP2_INIT
 #endif
 
 typedef struct {
@@ -241,13 +244,6 @@ struct thread_struct {
 #define FPAFF_INIT
 #endif /* CONFIG_MIPS_MT_FPAFF */
 
-#ifdef CONFIG_CPU_CAVIUM_OCTEON
-#define OCTEON_INIT						\
-	.cp2			= INIT_OCTEON_COP2,
-#else
-#define OCTEON_INIT
-#endif /* CONFIG_CPU_CAVIUM_OCTEON */
-
 #define INIT_THREAD  {						\
 	/*							\
 	 * Saved main processor registers			\
@@ -296,9 +292,9 @@ struct thread_struct {
 	.cp0_baduaddr		= 0,				\
 	.error_code		= 0,				\
 	/*							\
-	 * Cavium Octeon specifics (null if not Octeon)		\
+	 * Platform specific cop2 registers(null if no COP2)	\
 	 */							\
-	OCTEON_INIT						\
+	COP2_INIT						\
 }
 
 struct task_struct;
diff --git a/arch/mips/include/asm/switch_to.h b/arch/mips/include/asm/switch_to.h
index fd16bcb..eb0af15 100644
--- a/arch/mips/include/asm/switch_to.h
+++ b/arch/mips/include/asm/switch_to.h
@@ -15,6 +15,7 @@
 #include <asm/cpu-features.h>
 #include <asm/watch.h>
 #include <asm/dsp.h>
+#include <asm/cop2.h>
 
 struct task_struct;
 
@@ -66,10 +67,18 @@ do {									\
 
 #define switch_to(prev, next, last)					\
 do {									\
-	u32 __usedfpu;							\
+	u32 __usedfpu, __c0_stat;					\
 	__mips_mt_fpaff_switch_to(prev);				\
 	if (cpu_has_dsp)						\
 		__save_dsp(prev);					\
+	if (cop2_present && (KSTK_STATUS(prev) & ST0_CU2)) {		\
+		if (cop2_lazy_restore)					\
+			KSTK_STATUS(prev) &= ~ST0_CU2;			\
+		__c0_stat = read_c0_status();				\
+		write_c0_status(__c0_stat | ST0_CU2);			\
+		cop2_save(&prev->thread.cp2);				\
+		write_c0_status(__c0_stat & ~ST0_CU2);			\
+	}								\
 	__clear_software_ll_bit();					\
 	__usedfpu = test_and_clear_tsk_thread_flag(prev, TIF_USEDFPU);	\
 	(last) = resume(prev, next, task_thread_info(next), __usedfpu); \
@@ -77,6 +86,14 @@ do {									\
 
 #define finish_arch_switch(prev)					\
 do {									\
+	u32 __c0_stat;							\
+	if (cop2_present && !cop2_lazy_restore &&			\
+			(KSTK_STATUS(current) & ST0_CU2)) {		\
+		__c0_stat = read_c0_status();				\
+		write_c0_status(__c0_stat | ST0_CU2);			\
+		cop2_restore(&current->thread.cp2);			\
+		write_c0_status(__c0_stat & ~ST0_CU2);			\
+	}								\
 	if (cpu_has_dsp)						\
 		__restore_dsp(current);					\
 	if (cpu_has_userlocal)						\
diff --git a/arch/mips/kernel/octeon_switch.S b/arch/mips/kernel/octeon_switch.S
index 0e23343..22e2aa1 100644
--- a/arch/mips/kernel/octeon_switch.S
+++ b/arch/mips/kernel/octeon_switch.S
@@ -40,33 +40,6 @@
 	cpu_save_nonscratch a0
 	LONG_S	ra, THREAD_REG31(a0)
 
-	/* check if we need to save COP2 registers */
-	PTR_L	t2, TASK_THREAD_INFO(a0)
-	LONG_L	t0, ST_OFF(t2)
-	bbit0	t0, 30, 1f
-
-	/* Disable COP2 in the stored process state */
-	li	t1, ST0_CU2
-	xor	t0, t1
-	LONG_S	t0, ST_OFF(t2)
-
-	/* Enable COP2 so we can save it */
-	mfc0	t0, CP0_STATUS
-	or	t0, t1
-	mtc0	t0, CP0_STATUS
-
-	/* Save COP2 */
-	daddu	a0, THREAD_CP2
-	jal octeon_cop2_save
-	dsubu	a0, THREAD_CP2
-
-	/* Disable COP2 now that we are done */
-	mfc0	t0, CP0_STATUS
-	li	t1, ST0_CU2
-	xor	t0, t1
-	mtc0	t0, CP0_STATUS
-
-1:
 #if CONFIG_CAVIUM_OCTEON_CVMSEG_SIZE > 0
 	/* Check if we need to store CVMSEG state */
 	mfc0	t0, $11,7	/* CvmMemCtl */
-- 
1.7.9.5

[PATCH 2/5] MIPS: Allow kernel to use coprocessor 2

[Jayachandran C]

Kernel threads should be able to use COP2 if the platform needs it.
Do not call die_if_kernel() for a coprocessor unusable exception if
the exception due to COP2 usage.  Instead, the default notifier for
COP2 exceptions is updated to call die_if_kernel.

Signed-off-by: Jayachandran C <jchandra@broadcom.com>
---
 arch/mips/kernel/traps.c |   15 +++++----------
 1 file changed, 5 insertions(+), 10 deletions(-)

diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index a200b5b..36c242f 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -970,15 +970,9 @@ static int default_cu2_call(struct notifier_block *nfb, unsigned long action,
 {
 	struct pt_regs *regs = data;
 
-	switch (action) {
-	default:
-		die_if_kernel("Unhandled kernel unaligned access or invalid "
+	die_if_kernel("COP2: Unhandled kernel unaligned access or invalid "
 			      "instruction", regs);
-		/* Fall through	 */
-
-	case CU2_EXCEPTION:
-		force_sig(SIGILL, current);
-	}
+	force_sig(SIGILL, current);
 
 	return NOTIFY_OK;
 }
@@ -992,10 +986,11 @@ asmlinkage void do_cpu(struct pt_regs *regs)
 	int status;
 	unsigned long __maybe_unused flags;
 
-	die_if_kernel("do_cpu invoked from kernel context!", regs);
-
 	cpid = (regs->cp0_cause >> CAUSEB_CE) & 3;
 
+	if (cpid != 2)
+		die_if_kernel("do_cpu invoked from kernel context!", regs);
+
 	switch (cpid) {
 	case 0:
 		epc = (unsigned int __user *)exception_epc(regs);
-- 
1.7.9.5

[PATCH 3/5] MIPS: Netlogic: Fix nlm_read_c2_status() definition

[Jayachandran C]

The sel argument os nlm_read_c2_status() was not used and the macro
returned the sel 0 in all cases. Fix this by defining two macros:
nlm_read_c2_status0() and nlm_read_c2_status1() to return the two
status registers.

Add functions to write to the status registers as well.

Signed-off-by: Jayachandran C <jchandra@broadcom.com>
---
 arch/mips/include/asm/netlogic/xlr/fmn.h |    9 ++++++---
 arch/mips/netlogic/xlr/fmn.c             |    2 +-
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/arch/mips/include/asm/netlogic/xlr/fmn.h b/arch/mips/include/asm/netlogic/xlr/fmn.h
index 2a78929..90e1126 100644
--- a/arch/mips/include/asm/netlogic/xlr/fmn.h
+++ b/arch/mips/include/asm/netlogic/xlr/fmn.h
@@ -175,7 +175,10 @@
 #define nlm_write_c2_cc14(s, v)		__write_32bit_c2_register($30, s, v)
 #define nlm_write_c2_cc15(s, v)		__write_32bit_c2_register($31, s, v)
 
-#define nlm_read_c2_status(sel)		__read_32bit_c2_register($2, 0)
+#define nlm_read_c2_status0()		__read_32bit_c2_register($2, 0)
+#define nlm_write_c2_status0(v)		__write_32bit_c2_register($2, 0, v)
+#define nlm_read_c2_status1()		__read_32bit_c2_register($2, 1)
+#define nlm_write_c2_status1(v)		__write_32bit_c2_register($2, 1, v)
 #define nlm_read_c2_config()		__read_32bit_c2_register($3, 0)
 #define nlm_write_c2_config(v)		__write_32bit_c2_register($3, 0, v)
 #define nlm_read_c2_bucksize(b)		__read_32bit_c2_register($4, b)
@@ -296,7 +299,7 @@ static inline int nlm_fmn_send(unsigned int size, unsigned int code,
 	 */
 	for (i = 0; i < 8; i++) {
 		nlm_msgsnd(dest);
-		status = nlm_read_c2_status(0);
+		status = nlm_read_c2_status0();
 		if ((status & 0x2) == 1)
 			pr_info("Send pending fail!\n");
 		if ((status & 0x4) == 0)
@@ -316,7 +319,7 @@ static inline int nlm_fmn_receive(int bucket, int *size, int *code, int *stid,
 
 	/* wait for load pending to clear */
 	do {
-		status = nlm_read_c2_status(1);
+		status = nlm_read_c2_status0();
 	} while ((status & 0x08) != 0);
 
 	/* receive error bits */
diff --git a/arch/mips/netlogic/xlr/fmn.c b/arch/mips/netlogic/xlr/fmn.c
index 4d74f03..0fdce61 100644
--- a/arch/mips/netlogic/xlr/fmn.c
+++ b/arch/mips/netlogic/xlr/fmn.c
@@ -80,7 +80,7 @@ static irqreturn_t fmn_message_handler(int irq, void *data)
 	while (1) {
 		/* 8 bkts per core, [24:31] each bit represents one bucket
 		 * Bit is Zero if bucket is not empty */
-		bkt_status = (nlm_read_c2_status() >> 24) & 0xff;
+		bkt_status = (nlm_read_c2_status0() >> 24) & 0xff;
 		if (bkt_status == 0xff)
 			break;
 		for (bucket = 0; bucket < 8; bucket++) {
-- 
1.7.9.5

[PATCH 4/5] MIPS: Netlogic: rename nlm_cop2_save/restore

[Jayachandran C]

Rename macro nlm_cop2_enable() to nlm_cop2_enable_irqsave() and the macro
nlm_cop2_restore to nlm_cop2_disable_irqrestore(). The new names will
reflect the functionality better, and will make nlm_cop2_restore()
available to be used later in COP2 save/restore patch.

Signed-off-by: Jayachandran C <jchandra@broadcom.com>
---
 arch/mips/include/asm/netlogic/xlr/fmn.h |    4 ++--
 arch/mips/netlogic/xlr/fmn.c             |   16 ++++++++--------
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/arch/mips/include/asm/netlogic/xlr/fmn.h b/arch/mips/include/asm/netlogic/xlr/fmn.h
index 90e1126..c4d6014 100644
--- a/arch/mips/include/asm/netlogic/xlr/fmn.h
+++ b/arch/mips/include/asm/netlogic/xlr/fmn.h
@@ -240,7 +240,7 @@ static inline void nlm_msgwait(unsigned int mask)
 /*
  * Disable interrupts and enable COP2 access
  */
-static inline uint32_t nlm_cop2_enable(void)
+static inline uint32_t nlm_cop2_enable_irqsave(void)
 {
 	uint32_t sr = read_c0_status();
 
@@ -248,7 +248,7 @@ static inline uint32_t nlm_cop2_enable(void)
 	return sr;
 }
 
-static inline void nlm_cop2_restore(uint32_t sr)
+static inline void nlm_cop2_disable_irq_restore(uint32_t sr)
 {
 	write_c0_status(sr);
 }
diff --git a/arch/mips/netlogic/xlr/fmn.c b/arch/mips/netlogic/xlr/fmn.c
index 0fdce61..ba3d6b8 100644
--- a/arch/mips/netlogic/xlr/fmn.c
+++ b/arch/mips/netlogic/xlr/fmn.c
@@ -74,7 +74,7 @@ static irqreturn_t fmn_message_handler(int irq, void *data)
 	struct nlm_fmn_msg msg;
 	uint32_t mflags, bkt_status;
 
-	mflags = nlm_cop2_enable();
+	mflags = nlm_cop2_enable_irqsave();
 	/* Disable message ring interrupt */
 	nlm_fmn_setup_intr(irq, 0);
 	while (1) {
@@ -97,16 +97,16 @@ static irqreturn_t fmn_message_handler(int irq, void *data)
 				pr_warn("No msgring handler for stnid %d\n",
 						src_stnid);
 			else {
-				nlm_cop2_restore(mflags);
+				nlm_cop2_disable_irq_restore(mflags);
 				hndlr->action(bucket, src_stnid, size, code,
 					&msg, hndlr->arg);
-				mflags = nlm_cop2_enable();
+				mflags = nlm_cop2_enable_irqsave();
 			}
 		}
 	};
 	/* Enable message ring intr, to any thread in core */
 	nlm_fmn_setup_intr(irq, (1 << nlm_threads_per_core) - 1);
-	nlm_cop2_restore(mflags);
+	nlm_cop2_disable_irq_restore(mflags);
 	return IRQ_HANDLED;
 }
 
@@ -128,7 +128,7 @@ void xlr_percpu_fmn_init(void)
 
 	bucket_sizes = xlr_board_fmn_config.bucket_size;
 	cpu_fmn_info = &xlr_board_fmn_config.cpu[id];
-	flags = nlm_cop2_enable();
+	flags = nlm_cop2_enable_irqsave();
 
 	/* Setup bucket sizes for the core. */
 	nlm_write_c2_bucksize(0, bucket_sizes[id * 8 + 0]);
@@ -166,7 +166,7 @@ void xlr_percpu_fmn_init(void)
 
 	/* enable FMN interrupts on this CPU */
 	nlm_fmn_setup_intr(IRQ_FMN, (1 << nlm_threads_per_core) - 1);
-	nlm_cop2_restore(flags);
+	nlm_cop2_disable_irq_restore(flags);
 }
 
 
@@ -198,7 +198,7 @@ void nlm_setup_fmn_irq(void)
 	/* setup irq only once */
 	setup_irq(IRQ_FMN, &fmn_irqaction);
 
-	flags = nlm_cop2_enable();
+	flags = nlm_cop2_enable_irqsave();
 	nlm_fmn_setup_intr(IRQ_FMN, (1 << nlm_threads_per_core) - 1);
-	nlm_cop2_restore(flags);
+	nlm_cop2_disable_irq_restore(flags);
 }
-- 
1.7.9.5

[PATCH 5/5] MIPS: Netlogic: COP2 save/restore code

[Jayachandran C]

Add COP2 register state struct, and add functions to save and
restore COP2 at context switch.

Signed-off-by: Jayachandran C <jchandra@broadcom.com>
---
 arch/mips/include/asm/cop2.h        |   10 ++++
 arch/mips/include/asm/processor.h   |   12 ++++
 arch/mips/netlogic/common/Makefile  |    1 +
 arch/mips/netlogic/common/cop2-ex.c |  113 +++++++++++++++++++++++++++++++++++
 4 files changed, 136 insertions(+)
 create mode 100644 arch/mips/netlogic/common/cop2-ex.c

diff --git a/arch/mips/include/asm/cop2.h b/arch/mips/include/asm/cop2.h
index b17f38e..39a752c 100644
--- a/arch/mips/include/asm/cop2.h
+++ b/arch/mips/include/asm/cop2.h
@@ -22,6 +22,16 @@ extern void octeon_cop2_restore(struct octeon_cop2_state *);
 #define cop2_present		1
 #define cop2_lazy_restore	1
 
+#elif defined(CONFIG_CPU_XLR) || defined(CONFIG_CPU_XLP)
+
+extern void nlm_cop2_save(struct nlm_cop2_state *);
+extern void nlm_cop2_restore(struct nlm_cop2_state *);
+#define cop2_save(r)		nlm_cop2_save(r)
+#define cop2_restore(r)		nlm_cop2_restore(r)
+
+#define cop2_present		1
+#define cop2_lazy_restore	0
+
 #else
 
 #define cop2_present		0
diff --git a/arch/mips/include/asm/processor.h b/arch/mips/include/asm/processor.h
index 131c78f..06e2f88 100644
--- a/arch/mips/include/asm/processor.h
+++ b/arch/mips/include/asm/processor.h
@@ -186,6 +186,15 @@ struct octeon_cvmseg_state {
 			    [cpu_dcache_line_size() / sizeof(unsigned long)];
 };
 
+#elif defined(CONFIG_CPU_XLR) || defined(CONFIG_CPU_XLP)
+struct nlm_cop2_state {
+	u64	rx[4];
+	u64	tx[4];
+	u32	status0;
+};
+
+#define COP2_INIT						\
+	.cp2			= {{0}, {0}, 0},
 #else
 #define COP2_INIT
 #endif
@@ -233,6 +242,9 @@ struct thread_struct {
     struct octeon_cop2_state cp2 __attribute__ ((__aligned__(128)));
     struct octeon_cvmseg_state cvmseg __attribute__ ((__aligned__(128)));
 #endif
+#if defined(CONFIG_CPU_XLR) || defined(CONFIG_CPU_XLP)
+	struct nlm_cop2_state cp2;
+#endif
 	struct mips_abi *abi;
 };
 
diff --git a/arch/mips/netlogic/common/Makefile b/arch/mips/netlogic/common/Makefile
index 291372a..fbd34d3 100644
--- a/arch/mips/netlogic/common/Makefile
+++ b/arch/mips/netlogic/common/Makefile
@@ -1,3 +1,4 @@
 obj-y				+= irq.o time.o
 obj-$(CONFIG_SMP)		+= smp.o smpboot.o
 obj-$(CONFIG_EARLY_PRINTK)	+= earlycons.o
+obj-y				+= cop2-ex.o
diff --git a/arch/mips/netlogic/common/cop2-ex.c b/arch/mips/netlogic/common/cop2-ex.c
new file mode 100644
index 0000000..73e77e5
--- /dev/null
+++ b/arch/mips/netlogic/common/cop2-ex.c
@@ -0,0 +1,113 @@
+/*
+ * 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.
+ *
+ * Copyright (C) 2013 Broadcom Corporation.
+ *
+ * based on arch/mips/cavium-octeon/cpu.c
+ * Copyright (C) 2009 Wind River Systems,
+ *   written by Ralf Baechle <ralf@linux-mips.org>
+ */
+#include <linux/init.h>
+#include <linux/irqflags.h>
+#include <linux/notifier.h>
+#include <linux/prefetch.h>
+#include <linux/sched.h>
+
+#include <asm/cop2.h>
+#include <asm/current.h>
+#include <asm/mipsregs.h>
+#include <asm/page.h>
+
+#include <asm/netlogic/xlr/fmn.h>
+
+/*
+ * 64 bit ops are done in inline assembly to support 32 bit
+ * compilation
+ */
+void nlm_cop2_save(struct nlm_cop2_state *r)
+{
+	asm volatile(
+		".set	push\n"
+		".set	noat\n"
+		"dmfc2	$1, $0, 0\n"
+		"sd	$1, 0(%1)\n"
+		"dmfc2	$1, $0, 1\n"
+		"sd	$1, 8(%1)\n"
+		"dmfc2	$1, $0, 2\n"
+		"sd	$1, 16(%1)\n"
+		"dmfc2	$1, $0, 3\n"
+		"sd	$1, 24(%1)\n"
+		"dmfc2	$1, $1, 0\n"
+		"sd	$1, 0(%2)\n"
+		"dmfc2	$1, $1, 1\n"
+		"sd	$1, 8(%2)\n"
+		"dmfc2	$1, $1, 2\n"
+		"sd	$1, 16(%2)\n"
+		"dmfc2	$1, $1, 3\n"
+		"sd	$1, 24(%2)\n"
+		".set	pop\n"
+		: "=m"(*r)
+		: "r"(r->rx), "r"(r->tx));
+
+	r->status0 = nlm_read_c2_status0() & 0x3f;
+}
+
+void nlm_cop2_restore(struct nlm_cop2_state *r)
+{
+	asm volatile(
+		".set	push\n"
+		".set	noat\n"
+		"ld	$1, 0(%1)\n"
+		"dmtc2	$1, $0, 0\n"
+		"ld	$1, 8(%1)\n"
+		"dmtc2	$1, $0, 1\n"
+		"ld	$1, 16(%1)\n"
+		"dmtc2	$1, $0, 2\n"
+		"ld	$1, 24(%1)\n"
+		"dmtc2	$1, $0, 3\n"
+		"ld	$1, 0(%2)\n"
+		"dmtc2	$1, $1, 0\n"
+		"ld	$1, 8(%2)\n"
+		"dmtc2	$1, $1, 1\n"
+		"ld	$1, 16(%2)\n"
+		"dmtc2	$1, $1, 2\n"
+		"ld	$1, 24(%2)\n"
+		"dmtc2	$1, $1, 3\n"
+		".set	pop\n"
+		: : "m"(*r), "r"(r->rx), "r"(r->tx));
+
+	nlm_write_c2_status0(r->status0);
+}
+
+static int nlm_cu2_call(struct notifier_block *nfb, unsigned long action,
+	void *data)
+{
+	unsigned long flags;
+	unsigned int status;
+
+	switch (action) {
+	case CU2_EXCEPTION:
+		if (!capable(CAP_SYS_ADMIN) || !capable(CAP_SYS_RAWIO))
+			break;
+		local_irq_save(flags);
+		KSTK_STATUS(current) |= ST0_CU2;
+		status = read_c0_status();
+		write_c0_status(status | ST0_CU2);
+		nlm_cop2_restore(&(current->thread.cp2));
+		write_c0_status(status & ~ST0_CU2);
+		local_irq_restore(flags);
+		pr_info("COP2 access enabled for pid %d (%s)\n",
+					current->pid, current->comm);
+		return NOTIFY_BAD;	/* Don't call default notifier */
+	}
+
+	return NOTIFY_OK;		/* Let default notifier send signals */
+}
+
+static int __init nlm_cu2_setup(void)
+{
+	return cu2_notifier(nlm_cu2_call, 0);
+}
+early_initcall(nlm_cu2_setup);
-- 
1.7.9.5