- 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.

17 files changed, 1231 insertions, 111 deletions

diff --git a/sys/mips/cavium/cryptocteon/cavium_crypto.c b/sys/mips/cavium/cryptocteon/cavium_crypto.c
index f983cfa..ea01b8d 100644
--- a/sys/mips/cavium/cryptocteon/cavium_crypto.c
+++ b/sys/mips/cavium/cryptocteon/cavium_crypto.c
@@ -87,22 +87,6 @@ __FBSDID("$FreeBSD$");
 	    }								\
 	} while (0)
 
-static inline unsigned long octeon_crypto_enable(void)
-{
-    register_t s;
-    
-    s = intr_disable();
-    mips_wr_status(mips_rd_status() | MIPS_SR_COP_2_BIT);
-
-    return (s);
-}
-
-static inline void octeon_crypto_disable(register_t s)
-{
-    mips_wr_status(mips_rd_status() & ~MIPS_SR_COP_2_BIT);
-    intr_restore(s);
-}
-
 #define ESP_HEADER_LENGTH     8
 #define DES_CBC_IV_LENGTH     8
 #define AES_CBC_IV_LENGTH     16
@@ -252,14 +236,12 @@ octo_calc_hash(uint8_t auth, unsigned char *key, uint64_t *inner, uint64_t *oute
     uint64_t *key1;
     register uint64_t xor1 = 0x3636363636363636ULL;
     register uint64_t xor2 = 0x5c5c5c5c5c5c5c5cULL;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
     memset(hash_key, 0, sizeof(hash_key));
     memcpy(hash_key, (uint8_t *) key, (auth ? 20 : 16));
     key1 = (uint64_t *) hash_key;
-    s = octeon_crypto_enable();
     if (auth) {
        CVMX_MT_HSH_IV(0x67452301EFCDAB89ULL, 0);
        CVMX_MT_HSH_IV(0x98BADCFE10325476ULL, 1);
@@ -332,7 +314,6 @@ octo_calc_hash(uint8_t auth, unsigned char *key, uint64_t *inner, uint64_t *oute
       outer[2] = 0;
       CVMX_MF_HSH_IV(outer[2], 2);
     }
-    octeon_crypto_disable(s);
     return;
 }
 
@@ -349,7 +330,6 @@ octo_des_cbc_encrypt(
 {
     uint64_t *data;
     int data_i, data_l;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -367,7 +347,6 @@ octo_des_cbc_encrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
 
     /* load 3DES Key */
     CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
@@ -378,7 +357,6 @@ octo_des_cbc_encrypt(
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 1);
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 2);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -397,7 +375,6 @@ octo_des_cbc_encrypt(
 	crypt_len -= 8;
     }
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -412,7 +389,6 @@ octo_des_cbc_decrypt(
 {
     uint64_t *data;
     int data_i, data_l;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -430,8 +406,6 @@ octo_des_cbc_decrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load 3DES Key */
     CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     if (od->octo_encklen == 24) {
@@ -441,7 +415,6 @@ octo_des_cbc_decrypt(
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 1);
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 2);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -460,7 +433,6 @@ octo_des_cbc_decrypt(
 	crypt_len -= 8;
     }
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -477,7 +449,6 @@ octo_aes_cbc_encrypt(
 {
     uint64_t *data, *pdata;
     int data_i, data_l;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -495,8 +466,6 @@ octo_aes_cbc_encrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load AES Key */
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[1], 1);
@@ -511,7 +480,6 @@ octo_aes_cbc_encrypt(
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[2], 2);
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[3], 3);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -536,7 +504,6 @@ octo_aes_cbc_encrypt(
 	crypt_len -= 16;
     }
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -551,7 +518,6 @@ octo_aes_cbc_decrypt(
 {
     uint64_t *data, *pdata;
     int data_i, data_l;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -569,8 +535,6 @@ octo_aes_cbc_decrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load AES Key */
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[1], 1);
@@ -585,7 +549,6 @@ octo_aes_cbc_decrypt(
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[2], 2);
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[3], 3);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -610,7 +573,6 @@ octo_aes_cbc_decrypt(
 	crypt_len -= 16;
     }
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -629,7 +591,6 @@ octo_null_md5_encrypt(
     uint64_t *data;
     uint64_t tmp1, tmp2;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -644,8 +605,6 @@ octo_null_md5_encrypt(
 
     IOV_INIT(iov, data, data_i, data_l);
 
-    s = octeon_crypto_enable();
-
     /* Load MD5 IV */
     CVMX_MT_HSH_IV(od->octo_hminner[0], 0);
     CVMX_MT_HSH_IV(od->octo_hminner[1], 1);
@@ -716,7 +675,6 @@ octo_null_md5_encrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *(uint32_t *)data = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -735,7 +693,6 @@ octo_null_sha1_encrypt(
     uint64_t *data;
     uint64_t tmp1, tmp2, tmp3;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -750,8 +707,6 @@ octo_null_sha1_encrypt(
 
     IOV_INIT(iov, data, data_i, data_l);
 
-    s = octeon_crypto_enable();
-
     /* Load SHA1 IV */
     CVMX_MT_HSH_IV(od->octo_hminner[0], 0);
     CVMX_MT_HSH_IV(od->octo_hminner[1], 1);
@@ -825,7 +780,6 @@ octo_null_sha1_encrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *(uint32_t *)data = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -849,7 +803,6 @@ octo_des_cbc_md5_encrypt(
     uint32_t *data32;
     uint64_t tmp1, tmp2;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -870,8 +823,6 @@ octo_des_cbc_md5_encrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load 3DES Key */
     CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     if (od->octo_encklen == 24) {
@@ -881,7 +832,6 @@ octo_des_cbc_md5_encrypt(
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 1);
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 2);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -981,7 +931,6 @@ octo_des_cbc_md5_encrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *data32 = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -1002,7 +951,6 @@ octo_des_cbc_md5_decrypt(
     uint32_t *data32;
     uint64_t tmp1, tmp2;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -1023,8 +971,6 @@ octo_des_cbc_md5_decrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load 3DES Key */
     CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     if (od->octo_encklen == 24) {
@@ -1034,7 +980,6 @@ octo_des_cbc_md5_decrypt(
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 1);
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 2);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -1134,7 +1079,6 @@ octo_des_cbc_md5_decrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *data32 = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -1158,7 +1102,6 @@ octo_des_cbc_sha1_encrypt(
     uint32_t *data32;
     uint64_t tmp1, tmp2, tmp3;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -1179,8 +1122,6 @@ octo_des_cbc_sha1_encrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load 3DES Key */
     CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     if (od->octo_encklen == 24) {
@@ -1190,7 +1131,6 @@ octo_des_cbc_sha1_encrypt(
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 1);
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 2);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -1293,7 +1233,6 @@ octo_des_cbc_sha1_encrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *data32 = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -1314,7 +1253,6 @@ octo_des_cbc_sha1_decrypt(
     uint32_t *data32;
     uint64_t tmp1, tmp2, tmp3;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -1335,8 +1273,6 @@ octo_des_cbc_sha1_decrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load 3DES Key */
     CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     if (od->octo_encklen == 24) {
@@ -1346,7 +1282,6 @@ octo_des_cbc_sha1_decrypt(
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 1);
 	CVMX_MT_3DES_KEY(((uint64_t *) od->octo_enckey)[0], 2);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -1448,7 +1383,6 @@ octo_des_cbc_sha1_decrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *data32 = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -1473,7 +1407,6 @@ octo_aes_cbc_md5_encrypt(
     uint32_t *data32;
     uint64_t tmp1, tmp2;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -1494,8 +1427,6 @@ octo_aes_cbc_md5_encrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load AES Key */
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[1], 1);
@@ -1510,7 +1441,6 @@ octo_aes_cbc_md5_encrypt(
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[2], 2);
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[3], 3);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -1633,7 +1563,6 @@ octo_aes_cbc_md5_encrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *data32 = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -1655,7 +1584,6 @@ octo_aes_cbc_md5_decrypt(
     uint32_t *data32;
     uint64_t tmp1, tmp2;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -1676,8 +1604,6 @@ octo_aes_cbc_md5_decrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load AES Key */
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[1], 1);
@@ -1692,7 +1618,6 @@ octo_aes_cbc_md5_decrypt(
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[2], 2);
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[3], 3);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -1811,7 +1736,6 @@ octo_aes_cbc_md5_decrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *data32 = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -1836,7 +1760,6 @@ octo_aes_cbc_sha1_encrypt(
     uint32_t *data32;
     uint64_t tmp1, tmp2, tmp3;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -1857,8 +1780,6 @@ octo_aes_cbc_sha1_encrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load AES Key */
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[1], 1);
@@ -1873,7 +1794,6 @@ octo_aes_cbc_sha1_encrypt(
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[2], 2);
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[3], 3);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -2015,7 +1935,6 @@ octo_aes_cbc_sha1_encrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *data32 = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
@@ -2037,7 +1956,6 @@ octo_aes_cbc_sha1_decrypt(
     uint32_t *data32;
     uint64_t tmp1, tmp2, tmp3;
     int data_i, data_l, alen = auth_len;
-    register_t s;
 
     dprintf("%s()\n", __func__);
 
@@ -2058,8 +1976,6 @@ octo_aes_cbc_sha1_decrypt(
     CVMX_PREFETCH0(ivp);
     CVMX_PREFETCH0(od->octo_enckey);
 
-    s = octeon_crypto_enable();
-
     /* load AES Key */
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[0], 0);
     CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[1], 1);
@@ -2074,7 +1990,6 @@ octo_aes_cbc_sha1_decrypt(
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[2], 2);
 	CVMX_MT_AES_KEY(((uint64_t *) od->octo_enckey)[3], 3);
     } else {
-	octeon_crypto_disable(s);
 	dprintf("%s: Bad key length %d\n", __func__, od->octo_encklen);
 	return -EINVAL;
     }
@@ -2215,7 +2130,6 @@ octo_aes_cbc_sha1_decrypt(
     CVMX_MF_HSH_IV(tmp1, 1);
     *data32 = (uint32_t) (tmp1 >> 32);
 
-    octeon_crypto_disable(s);
     return 0;
 }
 
diff --git a/sys/mips/cavium/files.octeon1 b/sys/mips/cavium/files.octeon1
index acf8275..1e4fdb0 100644
--- a/sys/mips/cavium/files.octeon1
+++ b/sys/mips/cavium/files.octeon1
@@ -21,6 +21,8 @@ mips/cavium/octeon_nmi.S			optional octeon_wdog
 
 mips/cavium/cryptocteon/cavium_crypto.c		optional cryptocteon
 mips/cavium/cryptocteon/cryptocteon.c		optional cryptocteon
+mips/mips/octeon_cop2_swtch.S			standard
+mips/mips/octeon_cop2.c				standard
 
 mips/cavium/octe/ethernet.c			optional octe
 mips/cavium/octe/ethernet-mv88e61xx.c		optional octe octeon_vendor_lanner
diff --git a/sys/mips/cavium/octeon_cop2.S b/sys/mips/cavium/octeon_cop2.S
new file mode 100644
index 0000000..0a983f9
--- /dev/null
+++ b/sys/mips/cavium/octeon_cop2.S
@@ -0,0 +1,225 @@
+/*-
+ * 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 <mips/cavium/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)
+	/* 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)
+	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:	
+	jr ra
+	nop
+END(octeon_cop2_save)
+
+LEAF(octeon_cop2_restore)
+	/* 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)
+	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:	
+	jr ra
+	nop
+END(octeon_cop2_restore)
diff --git a/sys/mips/cavium/octeon_cop2.h b/sys/mips/cavium/octeon_cop2.h
new file mode 100644
index 0000000..b7aa031
--- /dev/null
+++ b/sys/mips/cavium/octeon_cop2.h
@@ -0,0 +1,210 @@
+/*-
+ * 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.
+ *
+ * $FreeBSD$
+ *
+ */
+
+#ifndef __OCTEON_COP2_H__
+#define __OCTEON_COP2_H__
+
+/*
+ * COP2 registers of interest
+ */
+#define	COP2_CRC_IV		0x201
+#define	COP2_CRC_IV_SET		COP2_CRC_IV
+#define	COP2_CRC_LENGTH		0x202
+#define	COP2_CRC_LENGTH_SET	0x1202
+#define	COP2_CRC_POLY		0x200
+#define	COP2_CRC_POLY_SET	0x4200
+#define	COP2_LLM_DAT0		0x402
+#define	COP2_LLM_DAT0_SET	COP2_LLM_DAT0
+#define	COP2_LLM_DAT1		0x40A
+#define	COP2_LLM_DAT1_SET	COP2_LLM_DAT1
+#define	COP2_3DES_IV		0x084
+#define	COP2_3DES_IV_SET	COP2_3DES_IV
+#define	COP2_3DES_KEY0		0x080
+#define	COP2_3DES_KEY0_SET	COP2_3DES_KEY0
+#define	COP2_3DES_KEY1		0x081
+#define	COP2_3DES_KEY1_SET	COP2_3DES_KEY1
+#define	COP2_3DES_KEY2		0x082
+#define	COP2_3DES_KEY2_SET	COP2_3DES_KEY2
+#define	COP2_3DES_RESULT	0x088
+#define	COP2_3DES_RESULT_SET	0x098
+#define	COP2_AES_INP0		0x111
+#define	COP2_AES_INP0_SET	COP2_AES_INP0
+#define	COP2_AES_IV0		0x102
+#define	COP2_AES_IV0_SET	COP2_AES_IV0
+#define	COP2_AES_IV1		0x103
+#define	COP2_AES_IV1_SET	COP2_AES_IV1
+#define	COP2_AES_KEY0		0x104
+#define	COP2_AES_KEY0_SET	COP2_AES_KEY0
+#define	COP2_AES_KEY1		0x105
+#define	COP2_AES_KEY1_SET	COP2_AES_KEY1
+#define	COP2_AES_KEY2		0x106
+#define	COP2_AES_KEY2_SET	COP2_AES_KEY2
+#define	COP2_AES_KEY3		0x107
+#define	COP2_AES_KEY3_SET	COP2_AES_KEY3
+#define	COP2_AES_KEYLEN		0x110
+#define	COP2_AES_KEYLEN_SET	COP2_AES_KEYLEN
+#define	COP2_AES_RESULT0	0x100
+#define	COP2_AES_RESULT0_SET	COP2_AES_RESULT0
+#define	COP2_AES_RESULT1	0x101
+#define	COP2_AES_RESULT1_SET	COP2_AES_RESULT1
+#define	COP2_HSH_DATW0		0x240
+#define	COP2_HSH_DATW0_SET	COP2_HSH_DATW0
+#define	COP2_HSH_DATW1		0x241
+#define	COP2_HSH_DATW1_SET	COP2_HSH_DATW1
+#define	COP2_HSH_DATW2		0x242
+#define	COP2_HSH_DATW2_SET	COP2_HSH_DATW2
+#define	COP2_HSH_DATW3		0x243
+#define	COP2_HSH_DATW3_SET	COP2_HSH_DATW3
+#define	COP2_HSH_DATW4		0x244
+#define	COP2_HSH_DATW4_SET	COP2_HSH_DATW4
+#define	COP2_HSH_DATW5		0x245
+#define	COP2_HSH_DATW5_SET	COP2_HSH_DATW5
+#define	COP2_HSH_DATW6		0x246
+#define	COP2_HSH_DATW6_SET	COP2_HSH_DATW6
+#define	COP2_HSH_DATW7		0x247
+#define	COP2_HSH_DATW7_SET	COP2_HSH_DATW7
+#define	COP2_HSH_DATW8		0x248
+#define	COP2_HSH_DATW8_SET	COP2_HSH_DATW8
+#define	COP2_HSH_DATW9		0x249
+#define	COP2_HSH_DATW9_SET	COP2_HSH_DATW9
+#define	COP2_HSH_DATW10		0x24A
+#define	COP2_HSH_DATW10_SET	COP2_HSH_DATW10
+#define	COP2_HSH_DATW11		0x24B
+#define	COP2_HSH_DATW11_SET	COP2_HSH_DATW11
+#define	COP2_HSH_DATW12		0x24C
+#define	COP2_HSH_DATW12_SET	COP2_HSH_DATW12
+#define	COP2_HSH_DATW13		0x24D
+#define	COP2_HSH_DATW13_SET	COP2_HSH_DATW13
+#define	COP2_HSH_DATW14		0x24E
+#define	COP2_HSH_DATW14_SET	COP2_HSH_DATW14
+#define	COP2_HSH_IVW0		0x250
+#define	COP2_HSH_IVW0_SET	COP2_HSH_IVW0
+#define	COP2_HSH_IVW1		0x251
+#define	COP2_HSH_IVW1_SET	COP2_HSH_IVW1
+#define	COP2_HSH_IVW2		0x252
+#define	COP2_HSH_IVW2_SET	COP2_HSH_IVW2
+#define	COP2_HSH_IVW3		0x253
+#define	COP2_HSH_IVW3_SET	COP2_HSH_IVW3
+#define	COP2_HSH_IVW4		0x254
+#define	COP2_HSH_IVW4_SET	COP2_HSH_IVW4
+#define	COP2_HSH_IVW5		0x255
+#define	COP2_HSH_IVW5_SET	COP2_HSH_IVW5
+#define	COP2_HSH_IVW6		0x256
+#define	COP2_HSH_IVW6_SET	COP2_HSH_IVW6
+#define	COP2_HSH_IVW7		0x257
+#define	COP2_HSH_IVW7_SET	COP2_HSH_IVW7
+#define	COP2_GFM_MULT0		0x258
+#define	COP2_GFM_MULT0_SET	COP2_GFM_MULT0
+#define	COP2_GFM_MULT1		0x259
+#define	COP2_GFM_MULT1_SET	COP2_GFM_MULT1
+#define	COP2_GFM_POLY		0x25E
+#define	COP2_GFM_POLY_SET	COP2_GFM_POLY
+#define	COP2_GFM_RESULT0	0x25A
+#define	COP2_GFM_RESULT0_SET	COP2_GFM_RESULT0
+#define	COP2_GFM_RESULT1	0x25B
+#define	COP2_GFM_RESULT1_SET	COP2_GFM_RESULT1
+#define	COP2_HSH_DATW0_PASS1	0x040
+#define	COP2_HSH_DATW0_PASS1_SET	COP2_HSH_DATW0_PASS1
+#define	COP2_HSH_DATW1_PASS1	0x041
+#define	COP2_HSH_DATW1_PASS1_SET	COP2_HSH_DATW1_PASS1
+#define	COP2_HSH_DATW2_PASS1	0x042
+#define	COP2_HSH_DATW2_PASS1_SET	COP2_HSH_DATW2_PASS1
+#define	COP2_HSH_DATW3_PASS1	0x043
+#define	COP2_HSH_DATW3_PASS1_SET	COP2_HSH_DATW3_PASS1
+#define	COP2_HSH_DATW4_PASS1	0x044
+#define	COP2_HSH_DATW4_PASS1_SET	COP2_HSH_DATW4_PASS1
+#define	COP2_HSH_DATW5_PASS1	0x045
+#define	COP2_HSH_DATW5_PASS1_SET	COP2_HSH_DATW5_PASS1
+#define	COP2_HSH_DATW6_PASS1	0x046
+#define	COP2_HSH_DATW6_PASS1_SET	COP2_HSH_DATW6_PASS1
+#define	COP2_HSH_IVW0_PASS1	0x048
+#define	COP2_HSH_IVW0_PASS1_SET	COP2_HSH_IVW0_PASS1
+#define	COP2_HSH_IVW1_PASS1	0x049
+#define	COP2_HSH_IVW1_PASS1_SET	COP2_HSH_IVW1_PASS1
+#define	COP2_HSH_IVW2_PASS1	0x04A
+#define	COP2_HSH_IVW2_PASS1_SET	COP2_HSH_IVW2_PASS1
+
+#ifndef LOCORE
+
+struct octeon_cop2_state {
+	/* 3DES */
+        /* 0x0084 */
+        unsigned long   _3des_iv;
+        /* 0x0080..0x0082 */
+        unsigned long   _3des_key[3];
+        /* 0x0088, set: 0x0098 */
+        unsigned long   _3des_result;
+
+	/* AES */
+        /* 0x0111 */
+        unsigned long   aes_inp0;
+        /* 0x0102..0x0103 */
+        unsigned long   aes_iv[2];
+        /* 0x0104..0x0107 */
+        unsigned long   aes_key[4];
+        /* 0x0110 */
+        unsigned long   aes_keylen;
+        /* 0x0100..0x0101 */
+        unsigned long   aes_result[2];
+
+	/* CRC */
+        /*  0x0201 */
+        unsigned long   crc_iv;
+        /* 0x0202, set: 0x1202 */
+        unsigned long   crc_length;
+        /* 0x0200, set: 0x4200 */
+        unsigned long   crc_poly;
+
+	/* Low-latency memory stuff */
+        /* 0x0402, 0x040A */
+        unsigned long   llm_dat[2];
+
+	/* SHA & MD5 */
+        /* 0x0240..0x024E */
+        unsigned long   hsh_datw[15];
+        /* 0x0250..0x0257 */
+        unsigned long   hsh_ivw[8];
+
+	/* GFM */
+        /*  0x0258..0x0259 */
+        unsigned long   gfm_mult[2];
+        /* 0x025E */
+        unsigned long   gfm_poly;
+        /* 0x025A..0x025B */
+        unsigned long   gfm_result[2];
+};
+
+/* Prototypes */
+
+void octeon_cop2_save(struct octeon_cop2_state *);
+void octeon_cop2_restore(struct octeon_cop2_state *);
+
+#endif /* LOCORE */
+#endif /* __OCTEON_COP2_H__ */
diff --git a/sys/mips/include/cpuregs.h b/sys/mips/include/cpuregs.h
index b47b264..eec9621 100644
--- a/sys/mips/include/cpuregs.h
+++ b/sys/mips/include/cpuregs.h
@@ -233,6 +233,7 @@
 #define	MIPS3_CR_EXC_CODE	0x0000007C	/* five bits */
 #define	MIPS_CR_IP		0x0000FF00
 #define	MIPS_CR_EXC_CODE_SHIFT	2
+#define	MIPS_CR_COP_ERR_SHIFT	28
 
 /*
  * The bits in the status register.  All bits are active when set to 1.
diff --git a/sys/mips/include/frame.h b/sys/mips/include/frame.h
index b08d88c..de7f94c 100644
--- a/sys/mips/include/frame.h
+++ b/sys/mips/include/frame.h
@@ -122,11 +122,6 @@ struct trapframe {
 	f_register_t	f31;
 	register_t	fsr;
         register_t   fdummy;
-	/*
-	 * COP2 registers may need to be saved here based on the CPU, and those
-	 * might need to be per process, or even for the kernel, so we need
-	 * some thought here.
-	 */
 };
 
 /* REVISIT */
diff --git a/sys/mips/include/octeon_cop2.h b/sys/mips/include/octeon_cop2.h
new file mode 100644
index 0000000..d5161dc
--- /dev/null
+++ b/sys/mips/include/octeon_cop2.h
@@ -0,0 +1,215 @@
+/*-
+ * 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.
+ *
+ * $FreeBSD$
+ *
+ */
+
+#ifndef __OCTEON_COP2_H__
+#define __OCTEON_COP2_H__
+
+/*
+ * COP2 registers of interest
+ */
+#define	COP2_CRC_IV		0x201
+#define	COP2_CRC_IV_SET		COP2_CRC_IV
+#define	COP2_CRC_LENGTH		0x202
+#define	COP2_CRC_LENGTH_SET	0x1202
+#define	COP2_CRC_POLY		0x200
+#define	COP2_CRC_POLY_SET	0x4200
+#define	COP2_LLM_DAT0		0x402
+#define	COP2_LLM_DAT0_SET	COP2_LLM_DAT0
+#define	COP2_LLM_DAT1		0x40A
+#define	COP2_LLM_DAT1_SET	COP2_LLM_DAT1
+#define	COP2_3DES_IV		0x084
+#define	COP2_3DES_IV_SET	COP2_3DES_IV
+#define	COP2_3DES_KEY0		0x080
+#define	COP2_3DES_KEY0_SET	COP2_3DES_KEY0
+#define	COP2_3DES_KEY1		0x081
+#define	COP2_3DES_KEY1_SET	COP2_3DES_KEY1
+#define	COP2_3DES_KEY2		0x082
+#define	COP2_3DES_KEY2_SET	COP2_3DES_KEY2
+#define	COP2_3DES_RESULT	0x088
+#define	COP2_3DES_RESULT_SET	0x098
+#define	COP2_AES_INP0		0x111
+#define	COP2_AES_INP0_SET	COP2_AES_INP0
+#define	COP2_AES_IV0		0x102
+#define	COP2_AES_IV0_SET	COP2_AES_IV0
+#define	COP2_AES_IV1		0x103
+#define	COP2_AES_IV1_SET	COP2_AES_IV1
+#define	COP2_AES_KEY0		0x104
+#define	COP2_AES_KEY0_SET	COP2_AES_KEY0
+#define	COP2_AES_KEY1		0x105
+#define	COP2_AES_KEY1_SET	COP2_AES_KEY1
+#define	COP2_AES_KEY2		0x106
+#define	COP2_AES_KEY2_SET	COP2_AES_KEY2
+#define	COP2_AES_KEY3		0x107
+#define	COP2_AES_KEY3_SET	COP2_AES_KEY3
+#define	COP2_AES_KEYLEN		0x110
+#define	COP2_AES_KEYLEN_SET	COP2_AES_KEYLEN
+#define	COP2_AES_RESULT0	0x100
+#define	COP2_AES_RESULT0_SET	COP2_AES_RESULT0
+#define	COP2_AES_RESULT1	0x101
+#define	COP2_AES_RESULT1_SET	COP2_AES_RESULT1
+#define	COP2_HSH_DATW0		0x240
+#define	COP2_HSH_DATW0_SET	COP2_HSH_DATW0
+#define	COP2_HSH_DATW1		0x241
+#define	COP2_HSH_DATW1_SET	COP2_HSH_DATW1
+#define	COP2_HSH_DATW2		0x242
+#define	COP2_HSH_DATW2_SET	COP2_HSH_DATW2
+#define	COP2_HSH_DATW3		0x243
+#define	COP2_HSH_DATW3_SET	COP2_HSH_DATW3
+#define	COP2_HSH_DATW4		0x244
+#define	COP2_HSH_DATW4_SET	COP2_HSH_DATW4
+#define	COP2_HSH_DATW5		0x245
+#define	COP2_HSH_DATW5_SET	COP2_HSH_DATW5
+#define	COP2_HSH_DATW6		0x246
+#define	COP2_HSH_DATW6_SET	COP2_HSH_DATW6
+#define	COP2_HSH_DATW7		0x247
+#define	COP2_HSH_DATW7_SET	COP2_HSH_DATW7
+#define	COP2_HSH_DATW8		0x248
+#define	COP2_HSH_DATW8_SET	COP2_HSH_DATW8
+#define	COP2_HSH_DATW9		0x249
+#define	COP2_HSH_DATW9_SET	COP2_HSH_DATW9
+#define	COP2_HSH_DATW10		0x24A
+#define	COP2_HSH_DATW10_SET	COP2_HSH_DATW10
+#define	COP2_HSH_DATW11		0x24B
+#define	COP2_HSH_DATW11_SET	COP2_HSH_DATW11
+#define	COP2_HSH_DATW12		0x24C
+#define	COP2_HSH_DATW12_SET	COP2_HSH_DATW12
+#define	COP2_HSH_DATW13		0x24D
+#define	COP2_HSH_DATW13_SET	COP2_HSH_DATW13
+#define	COP2_HSH_DATW14		0x24E
+#define	COP2_HSH_DATW14_SET	COP2_HSH_DATW14
+#define	COP2_HSH_IVW0		0x250
+#define	COP2_HSH_IVW0_SET	COP2_HSH_IVW0
+#define	COP2_HSH_IVW1		0x251
+#define	COP2_HSH_IVW1_SET	COP2_HSH_IVW1
+#define	COP2_HSH_IVW2		0x252
+#define	COP2_HSH_IVW2_SET	COP2_HSH_IVW2
+#define	COP2_HSH_IVW3		0x253
+#define	COP2_HSH_IVW3_SET	COP2_HSH_IVW3
+#define	COP2_HSH_IVW4		0x254
+#define	COP2_HSH_IVW4_SET	COP2_HSH_IVW4
+#define	COP2_HSH_IVW5		0x255
+#define	COP2_HSH_IVW5_SET	COP2_HSH_IVW5
+#define	COP2_HSH_IVW6		0x256
+#define	COP2_HSH_IVW6_SET	COP2_HSH_IVW6
+#define	COP2_HSH_IVW7		0x257
+#define	COP2_HSH_IVW7_SET	COP2_HSH_IVW7
+#define	COP2_GFM_MULT0		0x258
+#define	COP2_GFM_MULT0_SET	COP2_GFM_MULT0
+#define	COP2_GFM_MULT1		0x259
+#define	COP2_GFM_MULT1_SET	COP2_GFM_MULT1
+#define	COP2_GFM_POLY		0x25E
+#define	COP2_GFM_POLY_SET	COP2_GFM_POLY
+#define	COP2_GFM_RESULT0	0x25A
+#define	COP2_GFM_RESULT0_SET	COP2_GFM_RESULT0
+#define	COP2_GFM_RESULT1	0x25B
+#define	COP2_GFM_RESULT1_SET	COP2_GFM_RESULT1
+#define	COP2_HSH_DATW0_PASS1	0x040
+#define	COP2_HSH_DATW0_PASS1_SET	COP2_HSH_DATW0_PASS1
+#define	COP2_HSH_DATW1_PASS1	0x041
+#define	COP2_HSH_DATW1_PASS1_SET	COP2_HSH_DATW1_PASS1
+#define	COP2_HSH_DATW2_PASS1	0x042
+#define	COP2_HSH_DATW2_PASS1_SET	COP2_HSH_DATW2_PASS1
+#define	COP2_HSH_DATW3_PASS1	0x043
+#define	COP2_HSH_DATW3_PASS1_SET	COP2_HSH_DATW3_PASS1
+#define	COP2_HSH_DATW4_PASS1	0x044
+#define	COP2_HSH_DATW4_PASS1_SET	COP2_HSH_DATW4_PASS1
+#define	COP2_HSH_DATW5_PASS1	0x045
+#define	COP2_HSH_DATW5_PASS1_SET	COP2_HSH_DATW5_PASS1
+#define	COP2_HSH_DATW6_PASS1	0x046
+#define	COP2_HSH_DATW6_PASS1_SET	COP2_HSH_DATW6_PASS1
+#define	COP2_HSH_IVW0_PASS1	0x048
+#define	COP2_HSH_IVW0_PASS1_SET	COP2_HSH_IVW0_PASS1
+#define	COP2_HSH_IVW1_PASS1	0x049
+#define	COP2_HSH_IVW1_PASS1_SET	COP2_HSH_IVW1_PASS1
+#define	COP2_HSH_IVW2_PASS1	0x04A
+#define	COP2_HSH_IVW2_PASS1_SET	COP2_HSH_IVW2_PASS1
+
+#ifndef LOCORE
+
+struct octeon_cop2_state {
+	/* 3DES */
+	/* 0x0084 */
+	unsigned long   _3des_iv;
+	/* 0x0080..0x0082 */
+	unsigned long   _3des_key[3];
+	/* 0x0088, set: 0x0098 */
+	unsigned long   _3des_result;
+
+	/* AES */
+	/* 0x0111 */
+	unsigned long   aes_inp0;
+	/* 0x0102..0x0103 */
+	unsigned long   aes_iv[2];
+	/* 0x0104..0x0107 */
+	unsigned long   aes_key[4];
+	/* 0x0110 */
+	unsigned long   aes_keylen;
+	/* 0x0100..0x0101 */
+	unsigned long   aes_result[2];
+
+	/* CRC */
+	/*  0x0201 */
+	unsigned long   crc_iv;
+	/* 0x0202, set: 0x1202 */
+	unsigned long   crc_length;
+	/* 0x0200, set: 0x4200 */
+	unsigned long   crc_poly;
+
+	/* Low-latency memory stuff */
+	/* 0x0402, 0x040A */
+	unsigned long   llm_dat[2];
+
+	/* SHA & MD5 */
+	/* 0x0240..0x024E */
+	unsigned long   hsh_datw[15];
+	/* 0x0250..0x0257 */
+	unsigned long   hsh_ivw[8];
+
+	/* GFM */
+	/*  0x0258..0x0259 */
+	unsigned long   gfm_mult[2];
+	/* 0x025E */
+	unsigned long   gfm_poly;
+	/* 0x025A..0x025B */
+	unsigned long   gfm_result[2];
+};
+
+/* Prototypes */
+
+struct octeon_cop2_state* octeon_cop2_alloc_ctx(void);
+void octeon_cop2_free_ctx(struct octeon_cop2_state *);
+/*
+ * Save/restore part
+ */
+void octeon_cop2_save(struct octeon_cop2_state *);
+void octeon_cop2_restore(struct octeon_cop2_state *);
+
+#endif /* LOCORE */
+#endif /* __OCTEON_COP2_H__ */
diff --git a/sys/mips/include/proc.h b/sys/mips/include/proc.h
index 8b575dd..beba9167 100644
--- a/sys/mips/include/proc.h
+++ b/sys/mips/include/proc.h
@@ -39,6 +39,10 @@
 #ifndef _MACHINE_PROC_H_
 #define	_MACHINE_PROC_H_
 
+#ifdef	CPU_CNMIPS
+#include <machine/octeon_cop2.h>
+#endif
+
 /*
  * Machine-dependent part of the proc structure.
  */
@@ -58,10 +62,18 @@ struct mdthread {
 	int		md_pc_count;	/* performance counter */
 	int		md_pc_spill;	/* performance counter spill */
 	void		*md_tls;
+#ifdef	CPU_CNMIPS
+	struct octeon_cop2_state	*md_cop2; /* kernel context */
+	struct octeon_cop2_state	*md_ucop2; /* userland context */
+#define	COP2_OWNER_USERLAND	0x0000		/* Userland owns COP2 */
+#define	COP2_OWNER_KERNEL	0x0001		/* Kernel owns COP2 */
+	int		md_cop2owner;
+#endif
 };
 
 /* md_flags */
 #define	MDTD_FPUSED	0x0001		/* Process used the FPU */
+#define	MDTD_COP2USED	0x0002		/* Process used the COP2 */
 
 struct mdproc {
 	/* empty */
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