- Add better COP2 (crypto coprocessor) context handler for Octeon. Keep

    COP2 disabled and lazily allocate COP2 context structure in exception
    handler. Keep kernel and userland contexts separated.

9 files changed, 566 insertions, 20 deletions

diff --git a/sys/mips/mips/exception.S b/sys/mips/mips/exception.S
index 8b7307c..b73b3d3 100644
--- a/sys/mips/mips/exception.S
+++ b/sys/mips/mips/exception.S
@@ -375,15 +375,15 @@ NNON_LEAF(MipsKernGenException, KERN_EXC_FRAME_SIZE, ra)
 	REG_S	a3, CALLFRAME_RA + KERN_REG_SIZE(sp)		# for debugging
 
 	/*
-	 * Update interrupt mask in saved status register
+	 * Update interrupt and CPU mask in saved status register
 	 * Some of interrupts could be disabled by
 	 * intr filters if interrupts are enabled later
 	 * in trap handler
 	 */
 	mfc0	a0, MIPS_COP_0_STATUS
-	and	a0, a0, MIPS_SR_INT_MASK
+	and	a0, a0, (MIPS_SR_INT_MASK|MIPS_SR_COP_USABILITY)
 	RESTORE_REG(a1, SR, sp)
-	and	a1, a1, ~MIPS_SR_INT_MASK
+	and	a1, a1, ~(MIPS_SR_INT_MASK|MIPS_SR_COP_USABILITY)
 	or	a1, a1, a0
 	SAVE_REG(a1, SR, sp)
 	RESTORE_CPU			# v0 contains the return address.
@@ -469,6 +469,7 @@ NNON_LEAF(MipsUserGenException, CALLFRAME_SIZ, ra)
 # Turn off fpu and enter kernel mode
 	and	t0, a0, ~(MIPS_SR_COP_1_BIT | MIPS_SR_EXL | MIPS3_SR_KSU_MASK | MIPS_SR_INT_IE)
 #if defined(CPU_CNMIPS)
+	and	t0, t0, ~(MIPS_SR_COP_2_BIT)
 	or      t0, t0, (MIPS_SR_KX | MIPS_SR_SX | MIPS_SR_UX | MIPS_SR_PX)
 #elif defined(CPU_RMI)	|| defined(CPU_NLM)
 	or      t0, t0, (MIPS_SR_KX | MIPS_SR_UX | MIPS_SR_COP_2_BIT)
@@ -631,15 +632,15 @@ NNON_LEAF(MipsKernIntr, KERN_EXC_FRAME_SIZE, ra)
 	REG_S	a3, CALLFRAME_RA + KERN_REG_SIZE(sp)		# for debugging
 
 	/*
-	 * Update interrupt mask in saved status register
+	 * Update interrupt and CPU mask in saved status register
 	 * Some of interrupts could be disabled by
 	 * intr filters if interrupts are enabled later
 	 * in trap handler
 	 */
 	mfc0	a0, MIPS_COP_0_STATUS
-	and	a0, a0, MIPS_SR_INT_MASK
+	and	a0, a0, (MIPS_SR_INT_MASK|MIPS_SR_COP_USABILITY)
 	RESTORE_REG(a1, SR, sp)
-	and	a1, a1, ~MIPS_SR_INT_MASK
+	and	a1, a1, ~(MIPS_SR_INT_MASK|MIPS_SR_COP_USABILITY)
 	or	a1, a1, a0
 	SAVE_REG(a1, SR, sp)
 	REG_L	v0, CALLFRAME_RA + KERN_REG_SIZE(sp)
@@ -727,6 +728,7 @@ NNON_LEAF(MipsUserIntr, CALLFRAME_SIZ, ra)
 # Turn off fpu, disable interrupts, set kernel mode kernel mode, clear exception level.
 	and	t0, a0, ~(MIPS_SR_COP_1_BIT | MIPS_SR_EXL | MIPS_SR_INT_IE | MIPS3_SR_KSU_MASK)
 #ifdef CPU_CNMIPS
+	and	t0, t0, ~(MIPS_SR_COP_2_BIT)
 	or      t0, t0, (MIPS_SR_KX | MIPS_SR_SX | MIPS_SR_UX | MIPS_SR_PX)
 #elif defined(CPU_RMI)	|| defined(CPU_NLM)
 	or      t0, t0, (MIPS_SR_KX | MIPS_SR_UX | MIPS_SR_COP_2_BIT)
diff --git a/sys/mips/mips/genassym.c b/sys/mips/mips/genassym.c
index 2cb046d..58b1d86 100644
--- a/sys/mips/mips/genassym.c
+++ b/sys/mips/mips/genassym.c
@@ -58,11 +58,14 @@ __FBSDID("$FreeBSD$");
 #include <machine/sigframe.h>
 #include <machine/proc.h>
 
+#ifdef	CPU_CNMIPS
+#include <machine/octeon_cop2.h>
+#endif
+
 #ifndef offsetof
 #define	offsetof(t,m) (int)((&((t *)0L)->m))
 #endif
 
-
 ASSYM(TD_PCB, offsetof(struct thread, td_pcb));
 ASSYM(TD_UPTE, offsetof(struct thread, td_md.md_upte));
 ASSYM(TD_KSTACK, offsetof(struct thread, td_kstack));
@@ -70,6 +73,7 @@ ASSYM(TD_FLAGS, offsetof(struct thread, td_flags));
 ASSYM(TD_LOCK, offsetof(struct thread, td_lock));
 ASSYM(TD_FRAME, offsetof(struct thread, td_frame));
 ASSYM(TD_TLS, offsetof(struct thread, td_md.md_tls));
+ASSYM(TD_MDFLAGS, offsetof(struct thread, td_md.md_flags));
 
 ASSYM(TF_REG_SR, offsetof(struct trapframe, sr));
 
@@ -99,8 +103,73 @@ ASSYM(TDF_NEEDRESCHED, TDF_NEEDRESCHED);
 ASSYM(TDF_ASTPENDING, TDF_ASTPENDING);
 ASSYM(PCPU_SIZE, sizeof(struct pcpu));
 ASSYM(MAXCOMLEN, MAXCOMLEN);
+ASSYM(MDTD_COP2USED, MDTD_COP2USED);
 
 ASSYM(MIPS_KSEG0_START, MIPS_KSEG0_START);
 ASSYM(MIPS_KSEG1_START, MIPS_KSEG1_START);
 ASSYM(MIPS_KSEG2_START, MIPS_KSEG2_START);
 ASSYM(MIPS_XKSEG_START, MIPS_XKSEG_START);
+
+#ifdef	CPU_CNMIPS
+ASSYM(TD_COP2OWNER, offsetof(struct thread, td_md.md_cop2owner));
+ASSYM(TD_COP2, offsetof(struct thread, td_md.md_cop2));
+ASSYM(TD_UCOP2, offsetof(struct thread, td_md.md_ucop2));
+ASSYM(COP2_CRC_IV_OFFSET, offsetof(struct octeon_cop2_state, crc_iv));
+ASSYM(COP2_CRC_LENGTH_OFFSET, offsetof(struct octeon_cop2_state, crc_length));
+ASSYM(COP2_CRC_POLY_OFFSET, offsetof(struct octeon_cop2_state, crc_poly));
+ASSYM(COP2_LLM_DAT0_OFFSET, offsetof(struct octeon_cop2_state, llm_dat));
+ASSYM(COP2_LLM_DAT1_OFFSET, offsetof(struct octeon_cop2_state, llm_dat) + 8);
+ASSYM(COP2_3DES_IV_OFFSET, offsetof(struct octeon_cop2_state, _3des_iv));
+ASSYM(COP2_3DES_KEY0_OFFSET, offsetof(struct octeon_cop2_state, _3des_key));
+ASSYM(COP2_3DES_KEY1_OFFSET, offsetof(struct octeon_cop2_state, _3des_key) + 8);
+ASSYM(COP2_3DES_KEY2_OFFSET, offsetof(struct octeon_cop2_state, _3des_key) + 16);
+ASSYM(COP2_3DES_RESULT_OFFSET, offsetof(struct octeon_cop2_state, _3des_result));
+ASSYM(COP2_AES_INP0_OFFSET, offsetof(struct octeon_cop2_state, aes_inp0));
+ASSYM(COP2_AES_IV0_OFFSET, offsetof(struct octeon_cop2_state, aes_iv));
+ASSYM(COP2_AES_IV1_OFFSET, offsetof(struct octeon_cop2_state, aes_iv) + 8);
+ASSYM(COP2_AES_KEY0_OFFSET, offsetof(struct octeon_cop2_state, aes_key));
+ASSYM(COP2_AES_KEY1_OFFSET, offsetof(struct octeon_cop2_state, aes_key) + 8);
+ASSYM(COP2_AES_KEY2_OFFSET, offsetof(struct octeon_cop2_state, aes_key) + 16);
+ASSYM(COP2_AES_KEY3_OFFSET, offsetof(struct octeon_cop2_state, aes_key) + 24);
+ASSYM(COP2_AES_KEYLEN_OFFSET, offsetof(struct octeon_cop2_state, aes_keylen));
+ASSYM(COP2_AES_RESULT0_OFFSET, offsetof(struct octeon_cop2_state, aes_result));
+ASSYM(COP2_AES_RESULT1_OFFSET, offsetof(struct octeon_cop2_state, aes_result) + 8);
+ASSYM(COP2_HSH_DATW0_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw));
+ASSYM(COP2_HSH_DATW1_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 8);
+ASSYM(COP2_HSH_DATW2_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 16);
+ASSYM(COP2_HSH_DATW3_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 24);
+ASSYM(COP2_HSH_DATW4_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 32);
+ASSYM(COP2_HSH_DATW5_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 40);
+ASSYM(COP2_HSH_DATW6_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 48);
+ASSYM(COP2_HSH_DATW7_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 56);
+ASSYM(COP2_HSH_DATW8_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 64);
+ASSYM(COP2_HSH_DATW9_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 72);
+ASSYM(COP2_HSH_DATW10_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 80);
+ASSYM(COP2_HSH_DATW11_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 88);
+ASSYM(COP2_HSH_DATW12_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 96);
+ASSYM(COP2_HSH_DATW13_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 104);
+ASSYM(COP2_HSH_DATW14_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 112);
+ASSYM(COP2_HSH_IVW0_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw));
+ASSYM(COP2_HSH_IVW1_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 8);
+ASSYM(COP2_HSH_IVW2_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 16);
+ASSYM(COP2_HSH_IVW3_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 24);
+ASSYM(COP2_HSH_IVW4_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 32);
+ASSYM(COP2_HSH_IVW5_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 40);
+ASSYM(COP2_HSH_IVW6_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 48);
+ASSYM(COP2_HSH_IVW7_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 56);
+ASSYM(COP2_GFM_MULT0_OFFSET, offsetof(struct octeon_cop2_state, gfm_mult));
+ASSYM(COP2_GFM_MULT1_OFFSET, offsetof(struct octeon_cop2_state, gfm_mult) + 8);
+ASSYM(COP2_GFM_POLY_OFFSET, offsetof(struct octeon_cop2_state, gfm_poly));
+ASSYM(COP2_GFM_RESULT0_OFFSET, offsetof(struct octeon_cop2_state, gfm_result));
+ASSYM(COP2_GFM_RESULT1_OFFSET, offsetof(struct octeon_cop2_state, gfm_result) + 8);
+ASSYM(COP2_HSH_DATW0_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw));
+ASSYM(COP2_HSH_DATW1_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 8);
+ASSYM(COP2_HSH_DATW2_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 16);
+ASSYM(COP2_HSH_DATW3_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 24);
+ASSYM(COP2_HSH_DATW4_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 32);
+ASSYM(COP2_HSH_DATW5_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 40);
+ASSYM(COP2_HSH_DATW6_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_datw) + 48);
+ASSYM(COP2_HSH_IVW0_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw));
+ASSYM(COP2_HSH_IVW1_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 8);
+ASSYM(COP2_HSH_IVW2_PASS1_OFFSET, offsetof(struct octeon_cop2_state, hsh_ivw) + 16);
+#endif
diff --git a/sys/mips/mips/locore.S b/sys/mips/mips/locore.S
index 4b28f19..c6e78a2 100644
--- a/sys/mips/mips/locore.S
+++ b/sys/mips/mips/locore.S
@@ -95,10 +95,10 @@ VECTOR(_locore, unknown)
 	 */
 
 	/* Set these bits */
-        li	t1, (MIPS_SR_COP_2_BIT | MIPS_SR_COP_0_BIT | MIPS_SR_PX | MIPS_SR_KX | MIPS_SR_UX | MIPS_SR_SX | MIPS_SR_BEV)
+        li	t1, (MIPS_SR_COP_0_BIT | MIPS_SR_PX | MIPS_SR_KX | MIPS_SR_UX | MIPS_SR_SX | MIPS_SR_BEV)
 
 	/* Reset these bits */
-        li	t0, ~(MIPS_SR_DE | MIPS_SR_SOFT_RESET | MIPS_SR_ERL | MIPS_SR_EXL | MIPS_SR_INT_IE)
+        li	t0, ~(MIPS_SR_DE | MIPS_SR_SOFT_RESET | MIPS_SR_ERL | MIPS_SR_EXL | MIPS_SR_INT_IE | MIPS_SR_COP_2_BIT)
 #elif defined (CPU_RMI) || defined (CPU_NLM) 
 	/* Set these bits */
         li	t1, (MIPS_SR_COP_2_BIT | MIPS_SR_COP_0_BIT | MIPS_SR_KX | MIPS_SR_UX)
diff --git a/sys/mips/mips/octeon_cop2.c b/sys/mips/mips/octeon_cop2.c
new file mode 100644
index 0000000..c85522d
--- /dev/null
+++ b/sys/mips/mips/octeon_cop2.c
@@ -0,0 +1,62 @@
+/*-
+ * Copyright (c) 2011, Oleksandr Tymoshenko <gonzo@FreeBSD.org>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice unmodified, this list of conditions, and the following
+ *    disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/types.h>
+#include <sys/systm.h>
+#include <sys/param.h>
+#include <sys/kernel.h>
+#include <vm/uma.h>
+
+#include <machine/octeon_cop2.h>
+
+static uma_zone_t ctxzone;
+
+static void 
+octeon_cop2_init(void* dummy)
+{
+	printf("Create COP2 context zone\n");
+	ctxzone = uma_zcreate("COP2 context",
+	                        sizeof(struct octeon_cop2_state), 
+				NULL, NULL, NULL, NULL, 8, 0);
+}
+
+struct octeon_cop2_state *
+octeon_cop2_alloc_ctx()
+{
+	return uma_zalloc(ctxzone, M_NOWAIT);
+}
+
+void
+octeon_cop2_free_ctx(struct octeon_cop2_state *ctx)
+{
+	uma_zfree(ctxzone, ctx);
+}
+
+SYSINIT(octeon_cop2, SI_SUB_CPU, SI_ORDER_FIRST, octeon_cop2_init, NULL);
diff --git a/sys/mips/mips/octeon_cop2_swtch.S b/sys/mips/mips/octeon_cop2_swtch.S
new file mode 100644
index 0000000..688db6b
--- /dev/null
+++ b/sys/mips/mips/octeon_cop2_swtch.S
@@ -0,0 +1,246 @@
+/*-
+ * Copyright (c) 2011 Oleksandr Tymoshenko
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#include <machine/asm.h>
+#include <machine/cpuregs.h>
+#include <machine/octeon_cop2.h>
+
+#include "assym.s"
+
+.set noreorder
+
+#define SAVE_COP2_REGISTER(reg)				\
+	dmfc2	t1, reg; sd	t1, reg##_OFFSET(a0)
+
+
+#define RESTORE_COP2_REGISTER(reg)			\
+	ld	t1, reg##_OFFSET(a0); dmtc2	t1, reg##_SET
+ 
+LEAF(octeon_cop2_save)
+
+	/* save original cop2 status in t2*/
+	mfc0	t2, MIPS_COP_0_STATUS
+	or	t0, t2, MIPS_SR_COP_2_BIT
+	and	t0, t0, ~MIPS_SR_INT_IE
+	mtc0	t0, MIPS_COP_0_STATUS
+
+	/* Get CvmCtl register */
+	dmfc0	t0, $9, 7
+
+	/* CRC state */
+	SAVE_COP2_REGISTER(COP2_CRC_IV)
+	SAVE_COP2_REGISTER(COP2_CRC_LENGTH)
+	SAVE_COP2_REGISTER(COP2_CRC_POLY)
+
+	/* if CvmCtl[NODFA_CP2] -> save_nodfa */
+	bbit1	t0, 28, save_nodfa
+	nop
+
+	/* LLM state */
+	SAVE_COP2_REGISTER(COP2_LLM_DAT0)
+	SAVE_COP2_REGISTER(COP2_LLM_DAT1)
+
+save_nodfa:
+	/* crypto stuff is irrelevant if CvmCtl[NOCRYPTO]  */
+	bbit1	t0, 26, save_done
+	nop
+
+	SAVE_COP2_REGISTER(COP2_3DES_IV)
+	SAVE_COP2_REGISTER(COP2_3DES_KEY0)
+	SAVE_COP2_REGISTER(COP2_3DES_KEY1)
+	SAVE_COP2_REGISTER(COP2_3DES_KEY2)
+	SAVE_COP2_REGISTER(COP2_3DES_RESULT)
+
+	SAVE_COP2_REGISTER(COP2_AES_INP0)
+	SAVE_COP2_REGISTER(COP2_AES_IV0)
+	SAVE_COP2_REGISTER(COP2_AES_IV1)
+	SAVE_COP2_REGISTER(COP2_AES_KEY0)
+	SAVE_COP2_REGISTER(COP2_AES_KEY1)
+	SAVE_COP2_REGISTER(COP2_AES_KEY2)
+	SAVE_COP2_REGISTER(COP2_AES_KEY3)
+	SAVE_COP2_REGISTER(COP2_AES_KEYLEN)
+	SAVE_COP2_REGISTER(COP2_AES_RESULT0)
+	SAVE_COP2_REGISTER(COP2_AES_RESULT1)
+
+	dmfc0	t0, $15
+	li	t1, 0x000d0000 /* Octeon Pass1 */
+	beq	t0, t1, save_pass1
+	nop
+
+	SAVE_COP2_REGISTER(COP2_HSH_DATW0)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW1)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW2)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW3)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW4)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW5)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW6)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW7)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW8)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW9)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW10)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW11)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW12)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW13)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW14)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW0)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW1)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW2)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW3)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW4)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW5)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW6)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW7)
+	SAVE_COP2_REGISTER(COP2_GFM_MULT0)
+	SAVE_COP2_REGISTER(COP2_GFM_MULT1)
+	SAVE_COP2_REGISTER(COP2_GFM_POLY)
+	SAVE_COP2_REGISTER(COP2_GFM_RESULT0)
+	SAVE_COP2_REGISTER(COP2_GFM_RESULT1)
+	/* restore saved COP2 status */
+	mtc0	t2, MIPS_COP_0_STATUS
+	jr ra
+	nop
+
+save_pass1:
+	SAVE_COP2_REGISTER(COP2_HSH_DATW0_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW1_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW2_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW3_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW4_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW5_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_DATW6_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW0_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW1_PASS1)
+	SAVE_COP2_REGISTER(COP2_HSH_IVW2_PASS1)
+
+save_done:	
+	/* restore saved COP2 status */
+	mtc0	t2, MIPS_COP_0_STATUS
+	jr ra
+	nop
+END(octeon_cop2_save)
+
+LEAF(octeon_cop2_restore)
+	/* save original cop2 status in t2*/
+	mfc0	t2, MIPS_COP_0_STATUS
+	or	t0, t2, MIPS_SR_COP_2_BIT
+	and	t0, t0, ~MIPS_SR_INT_IE
+	mtc0	t0, MIPS_COP_0_STATUS
+	/* Get CvmCtl register */
+	dmfc0	t0, $9, 7
+
+	/* CRC state */
+	RESTORE_COP2_REGISTER(COP2_CRC_IV)
+	RESTORE_COP2_REGISTER(COP2_CRC_LENGTH)
+	RESTORE_COP2_REGISTER(COP2_CRC_POLY)
+
+	/* if CvmCtl[NODFA_CP2] -> save_nodfa */
+	bbit1	t0, 28, restore_nodfa
+	nop
+
+	/* LLM state */
+	RESTORE_COP2_REGISTER(COP2_LLM_DAT0)
+	RESTORE_COP2_REGISTER(COP2_LLM_DAT1)
+
+restore_nodfa:
+	/* crypto stuff is irrelevant if CvmCtl[NOCRYPTO]  */
+	bbit1	t0, 26, restore_done
+	nop
+
+	RESTORE_COP2_REGISTER(COP2_3DES_IV)
+	RESTORE_COP2_REGISTER(COP2_3DES_KEY0)
+	RESTORE_COP2_REGISTER(COP2_3DES_KEY1)
+	RESTORE_COP2_REGISTER(COP2_3DES_KEY2)
+	RESTORE_COP2_REGISTER(COP2_3DES_RESULT)
+
+	RESTORE_COP2_REGISTER(COP2_AES_INP0)
+	RESTORE_COP2_REGISTER(COP2_AES_IV0)
+	RESTORE_COP2_REGISTER(COP2_AES_IV1)
+	RESTORE_COP2_REGISTER(COP2_AES_KEY0)
+	RESTORE_COP2_REGISTER(COP2_AES_KEY1)
+	RESTORE_COP2_REGISTER(COP2_AES_KEY2)
+	RESTORE_COP2_REGISTER(COP2_AES_KEY3)
+	RESTORE_COP2_REGISTER(COP2_AES_KEYLEN)
+	RESTORE_COP2_REGISTER(COP2_AES_RESULT0)
+	RESTORE_COP2_REGISTER(COP2_AES_RESULT1)
+
+	dmfc0	t0, $15
+	li	t1, 0x000d0000 /* Octeon Pass1 */
+	beq	t0, t1, restore_pass1
+	nop
+
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW0)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW1)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW2)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW3)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW4)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW5)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW6)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW7)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW8)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW9)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW10)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW11)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW12)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW13)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW14)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW0)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW1)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW2)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW3)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW4)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW5)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW6)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW7)
+	RESTORE_COP2_REGISTER(COP2_GFM_MULT0)
+	RESTORE_COP2_REGISTER(COP2_GFM_MULT1)
+	RESTORE_COP2_REGISTER(COP2_GFM_POLY)
+	RESTORE_COP2_REGISTER(COP2_GFM_RESULT0)
+	RESTORE_COP2_REGISTER(COP2_GFM_RESULT1)
+	/* restore saved COP2 status */
+	mtc0	t2, MIPS_COP_0_STATUS
+	jr ra
+	nop
+
+restore_pass1:
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW0_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW1_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW2_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW3_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW4_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW5_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_DATW6_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW0_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW1_PASS1)
+	RESTORE_COP2_REGISTER(COP2_HSH_IVW2_PASS1)
+
+restore_done:	
+	/* restore saved COP2 status */
+	mtc0	t2, MIPS_COP_0_STATUS
+	jr ra
+	nop
+END(octeon_cop2_restore)
diff --git a/sys/mips/mips/pm_machdep.c b/sys/mips/mips/pm_machdep.c
index d730ccb..d136f5f 100644
--- a/sys/mips/mips/pm_machdep.c
+++ b/sys/mips/mips/pm_machdep.c
@@ -520,7 +520,7 @@ exec_setregs(struct thread *td, struct image_params *imgp, u_long stack)
 	td->td_frame->sr |= MIPS_SR_PX | MIPS_SR_UX | MIPS_SR_KX;
 #endif
 #ifdef CPU_CNMIPS
-	td->td_frame->sr |= MIPS_SR_COP_2_BIT | MIPS_SR_PX | MIPS_SR_UX |
+	td->td_frame->sr |= MIPS_SR_PX | MIPS_SR_UX |
 	    MIPS_SR_KX | MIPS_SR_SX;
 #endif
 	/*
diff --git a/sys/mips/mips/swtch.S b/sys/mips/mips/swtch.S
index 699c206..8b285dd 100644
--- a/sys/mips/mips/swtch.S
+++ b/sys/mips/mips/swtch.S
@@ -250,6 +250,61 @@ NON_LEAF(cpu_switch, CALLFRAME_SIZ, ra)
 getpc:
 	SAVE_U_PCB_CONTEXT(ra, PREG_PC, a0)		# save return address
 
+#ifdef CPU_CNMIPS
+
+	lw	t2, TD_MDFLAGS(a3)		# get md_flags
+	and	t1, t2, MDTD_COP2USED
+	beqz	t1, cop2_untouched
+	nop
+
+	/* Clear cop2used flag */
+	and	t2, t2, ~MDTD_COP2USED
+	sw	t2, TD_MDFLAGS(a3)
+
+	and	t2, t0, ~MIPS_SR_COP_2_BIT	# clear COP_2 enable bit
+	SAVE_U_PCB_CONTEXT(t2, PREG_SR, a0)	# save status register
+
+	RESTORE_U_PCB_REG(t0, PS, a0)		# get CPU status register
+	and	t2, t0, ~MIPS_SR_COP_2_BIT	# clear COP_2 enable bit
+	SAVE_U_PCB_REG(t2, PS, a0)		# save stratus register
+
+	/* preserve a0..a3 */
+	move	s0, a0
+	move	s1, a1
+	move	s2, a2
+	move	s3, a3
+
+	/* does kernel own COP2 context? */
+	lw	t1, TD_COP2OWNER(a3)		# get md_cop2owner
+	beqz	t1, userland_cop2		# 0 - it's userland context
+	nop
+
+	PTR_L	a0, TD_COP2(a3)
+	beqz	a0, no_cop2_context
+	nop
+
+	j	do_cop2_save
+	nop
+
+userland_cop2:
+
+	PTR_L	a0, TD_UCOP2(a3)
+	beqz	a0, no_cop2_context
+	nop
+
+do_cop2_save:
+	jal	octeon_cop2_save
+	nop
+
+no_cop2_context:
+	move	a3, s3
+	move	a2, s2
+	move	a1, s1
+	move	a0, s0
+
+cop2_untouched:
+#endif
+
 	PTR_S	a2, TD_LOCK(a3)			# Switchout td_lock 
 
 mips_sw1:
diff --git a/sys/mips/mips/trap.c b/sys/mips/mips/trap.c
index 97374a7..3b632d0 100644
--- a/sys/mips/mips/trap.c
+++ b/sys/mips/mips/trap.c
@@ -251,6 +251,9 @@ char *access_name[] = {
 	"Store Doubleword"
 };
 
+#ifdef	CPU_CNMIPS
+#include <machine/octeon_cop2.h>
+#endif
 
 static int allow_unaligned_acc = 1;
 
@@ -410,6 +413,7 @@ trap(struct trapframe *trapframe)
 	char *msg = NULL;
 	intptr_t addr = 0;
 	register_t pc;
+	int cop;
 
 	trapdebug_enter(trapframe, 0);
 	
@@ -767,28 +771,91 @@ dofault:
 		goto err;
 		break;
 	case T_COP_UNUSABLE:
+#ifdef	CPU_CNMIPS
+		cop = (trapframe->cause & MIPS_CR_COP_ERR) >> MIPS_CR_COP_ERR_SHIFT;
+		/* Handle only COP2 exception */
+		if (cop != 2)
+			goto err;
+
+		addr = trapframe->pc;
+		/* save userland cop2 context if it has been touched */
+		if ((td->td_md.md_flags & MDTD_COP2USED) &&
+		    (td->td_md.md_cop2owner == COP2_OWNER_USERLAND)) {
+			if (td->td_md.md_ucop2)
+				octeon_cop2_save(td->td_md.md_ucop2);
+			else
+				panic("COP2 was used in user mode but md_ucop2 is NULL");
+		}
+
+		if (td->td_md.md_cop2 == NULL) {
+			td->td_md.md_cop2 = octeon_cop2_alloc_ctx();
+			if (td->td_md.md_cop2 == NULL)
+				panic("Failed to allocate COP2 context");
+			memset(td->td_md.md_cop2, 0, sizeof(*td->td_md.md_cop2));
+		}
+
+		octeon_cop2_restore(td->td_md.md_cop2);
+		
+		/* Make userland re-request its context */
+		td->td_frame->sr &= ~MIPS_SR_COP_2_BIT;
+		td->td_md.md_flags |= MDTD_COP2USED;
+		td->td_md.md_cop2owner = COP2_OWNER_KERNEL;
+		/* Enable COP2, it will be disabled in cpu_switch */
+		mips_wr_status(mips_rd_status() | MIPS_SR_COP_2_BIT);
+		return (trapframe->pc);
+#else
 		goto err;
 		break;
+#endif
+
 	case T_COP_UNUSABLE + T_USER:
+		cop = (trapframe->cause & MIPS_CR_COP_ERR) >> MIPS_CR_COP_ERR_SHIFT;
+		if (cop == 1) {
 #if !defined(CPU_HAVEFPU)
 		/* FP (COP1) instruction */
-		if ((trapframe->cause & MIPS_CR_COP_ERR) == 0x10000000) {
 			log_illegal_instruction("COP1_UNUSABLE", trapframe);
 			i = SIGILL;
 			break;
+#else
+			addr = trapframe->pc;
+			MipsSwitchFPState(PCPU_GET(fpcurthread), td->td_frame);
+			PCPU_SET(fpcurthread, td);
+			td->td_frame->sr |= MIPS_SR_COP_1_BIT;
+			td->td_md.md_flags |= MDTD_FPUSED;
+			goto out;
+#endif
+		}
+#ifdef	CPU_CNMIPS
+		else  if (cop == 2) {
+			addr = trapframe->pc;
+			if ((td->td_md.md_flags & MDTD_COP2USED) &&
+			    (td->td_md.md_cop2owner == COP2_OWNER_KERNEL)) {
+				if (td->td_md.md_cop2)
+					octeon_cop2_save(td->td_md.md_cop2);
+				else
+					panic("COP2 was used in kernel mode but md_cop2 is NULL");
+			}
+
+			if (td->td_md.md_ucop2 == NULL) {
+				td->td_md.md_ucop2 = octeon_cop2_alloc_ctx();
+				if (td->td_md.md_ucop2 == NULL)
+					panic("Failed to allocate userland COP2 context");
+				memset(td->td_md.md_ucop2, 0, sizeof(*td->td_md.md_ucop2));
+			}
+
+			octeon_cop2_restore(td->td_md.md_ucop2);
+
+			td->td_frame->sr |= MIPS_SR_COP_2_BIT;
+			td->td_md.md_flags |= MDTD_COP2USED;
+			td->td_md.md_cop2owner = COP2_OWNER_USERLAND;
+			goto out;
 		}
 #endif
-		if ((trapframe->cause & MIPS_CR_COP_ERR) != 0x10000000) {
+		else {
 			log_illegal_instruction("COPn_UNUSABLE", trapframe);
 			i = SIGILL;	/* only FPU instructions allowed */
 			break;
 		}
-		addr = trapframe->pc;
-		MipsSwitchFPState(PCPU_GET(fpcurthread), td->td_frame);
-		PCPU_SET(fpcurthread, td);
-		td->td_frame->sr |= MIPS_SR_COP_1_BIT;
-		td->td_md.md_flags |= MDTD_FPUSED;
-		goto out;
 
 	case T_FPE:
 #if !defined(SMP) && (defined(DDB) || defined(DEBUG))
diff --git a/sys/mips/mips/vm_machdep.c b/sys/mips/mips/vm_machdep.c
index 47058ee..bdd73e7 100644
--- a/sys/mips/mips/vm_machdep.c
+++ b/sys/mips/mips/vm_machdep.c
@@ -163,7 +163,36 @@ cpu_fork(register struct thread *td1,register struct proc *p2,
 	td2->td_md.md_saved_intr = MIPS_SR_INT_IE;
 	td2->td_md.md_spinlock_count = 1;
 #ifdef CPU_CNMIPS
-	pcb2->pcb_context[PCB_REG_SR] |= MIPS_SR_COP_2_BIT | MIPS_SR_PX | MIPS_SR_UX | MIPS_SR_KX | MIPS_SR_SX;
+	if (td1->td_md.md_flags & MDTD_COP2USED) {
+		if (td1->td_md.md_cop2owner == COP2_OWNER_USERLAND) {
+			if (td1->td_md.md_ucop2)
+				octeon_cop2_save(td1->td_md.md_ucop2);
+			else
+				panic("cpu_fork: ucop2 is NULL but COP2 is enabled");
+		}
+		else {
+			if (td1->td_md.md_cop2)
+				octeon_cop2_save(td1->td_md.md_cop2);
+			else
+				panic("cpu_fork: cop2 is NULL but COP2 is enabled");
+		}
+	}
+
+	if (td1->td_md.md_cop2) {
+		td2->td_md.md_cop2 = octeon_cop2_alloc_ctx();
+		memcpy(td2->td_md.md_cop2, td1->td_md.md_cop2, 
+			sizeof(*td1->td_md.md_cop2));
+	}
+	if (td1->td_md.md_ucop2) {
+		td2->td_md.md_ucop2 = octeon_cop2_alloc_ctx();
+		memcpy(td2->td_md.md_ucop2, td1->td_md.md_ucop2, 
+			sizeof(*td1->td_md.md_ucop2));
+	}
+	td2->td_md.md_cop2owner = td1->td_md.md_cop2owner;
+	pcb2->pcb_context[PCB_REG_SR] |= MIPS_SR_PX | MIPS_SR_UX | MIPS_SR_KX | MIPS_SR_SX;
+	/* Clear COP2 bits for userland & kernel */
+	td2->td_frame->sr &= ~MIPS_SR_COP_2_BIT;
+	pcb2->pcb_context[PCB_REG_SR] &= ~MIPS_SR_COP_2_BIT;
 #endif
 }
 
@@ -195,11 +224,27 @@ cpu_thread_exit(struct thread *td)
 
 	if (PCPU_GET(fpcurthread) == td)
 		PCPU_GET(fpcurthread) = (struct thread *)0;
+#ifdef  CPU_CNMIPS
+	if (td->td_md.md_cop2)
+		memset(td->td_md.md_cop2, 0,
+			sizeof(*td->td_md.md_cop2));
+	if (td->td_md.md_ucop2)
+		memset(td->td_md.md_ucop2, 0,
+			sizeof(*td->td_md.md_ucop2));
+#endif
 }
 
 void
 cpu_thread_free(struct thread *td)
 {
+#ifdef  CPU_CNMIPS
+	if (td->td_md.md_cop2)
+		octeon_cop2_free_ctx(td->td_md.md_cop2);
+	if (td->td_md.md_ucop2)
+		octeon_cop2_free_ctx(td->td_md.md_ucop2);
+	td->td_md.md_cop2 = NULL;
+	td->td_md.md_ucop2 = NULL;
+#endif
 }
 
 void
@@ -357,7 +402,7 @@ cpu_set_upcall(struct thread *td, struct thread *td0)
 	    (MIPS_SR_KX | MIPS_SR_UX | MIPS_SR_INT_MASK);
 
 #ifdef CPU_CNMIPS
-	pcb2->pcb_context[PCB_REG_SR] |= MIPS_SR_COP_2_BIT | MIPS_SR_COP_0_BIT |
+	pcb2->pcb_context[PCB_REG_SR] |= MIPS_SR_COP_0_BIT |
 	  MIPS_SR_PX | MIPS_SR_UX | MIPS_SR_KX | MIPS_SR_SX;
 #endif