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 b6c7e79..bf3e2b9 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/include/cpuregs.h b/sys/mips/include/cpuregs.h
index 01d710d..173867a 100644
--- a/sys/mips/include/cpuregs.h
+++ b/sys/mips/include/cpuregs.h
@@ -231,6 +231,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/octeon_cop2.h b/sys/mips/include/octeon_cop2.h
new file mode 100644
index 0000000..6be7484
--- /dev/null
+++ b/sys/mips/include/octeon_cop2.h
@@ -0,0 +1,212 @@
+/*-
+ * 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.
+ */
+
+#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 11a1f8e..54aa732 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,6 +62,7 @@ struct mdthread {
 	int		md_pc_count;	/* performance counter */
 	int		md_pc_spill;	/* performance counter spill */
 	void		*md_tls;
+	struct octeon_cop2_state	*md_cop2;
 };
 
 /* md_flags */
diff --git a/sys/mips/mips/genassym.c b/sys/mips/mips/genassym.c
index fd168bc..fb83754 100644
--- a/sys/mips/mips/genassym.c
+++ b/sys/mips/mips/genassym.c
@@ -59,11 +59,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));
@@ -105,3 +108,65 @@ 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_COP2, offsetof(struct thread, td_md.md_cop2));
+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 9f685eb..4bea15f 100644
--- a/sys/mips/mips/locore.S
+++ b/sys/mips/mips/locore.S
@@ -95,7 +95,7 @@ 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)
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..2292ced
--- /dev/null
+++ b/sys/mips/mips/octeon_cop2_swtch.S
@@ -0,0 +1,226 @@
+/*-
+ * 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/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/mips/pm_machdep.c b/sys/mips/mips/pm_machdep.c
index 7e80e2f..ccb9a01 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..fa96935 100644
--- a/sys/mips/mips/swtch.S
+++ b/sys/mips/mips/swtch.S
@@ -249,6 +249,32 @@ NON_LEAF(cpu_switch, CALLFRAME_SIZ, ra)
 	nop
 getpc:
 	SAVE_U_PCB_CONTEXT(ra, PREG_PC, a0)		# save return address
+	/*
+	 * FREEBSD_DEVELOPERS_FIXME:
+	 * In case there are CPU-specific registers that need
+	 * to be saved with the other registers do so here.
+	 */
+#ifdef CPU_CNMIPS
+	and	t1, t0, MIPS_SR_COP_2_BIT
+	beqz	t1, cop2_disabled
+	nop
+	move	s0, a0
+	move	s1, a1
+	move	s2, a2
+	move	s3, a3
+	PTR_L	a0, TD_COP2(a3)
+	beqz	a0, no_cop2_context
+	nop
+	jal	octeon_cop2_save
+	nop
+	move	a3, s3
+	move	a2, s2
+	move	a1, s1
+no_cop2_context:
+	move	a0, s0
+
+cop2_disabled:
+#endif
 
 	PTR_S	a2, TD_LOCK(a3)			# Switchout td_lock 
 
@@ -339,7 +365,19 @@ sw2:
 	RESTORE_U_PCB_CONTEXT(s6, PREG_S6, a0)
 	RESTORE_U_PCB_CONTEXT(s7, PREG_S7, a0)
 	RESTORE_U_PCB_CONTEXT(s8, PREG_S8, a0)
-
+	/*
+	 * FREEBSD_DEVELOPERS_FIXME:
+	 * In case there are CPU-specific registers that need
+	 * to be restored with the other registers do so here.
+	 */
+#ifdef CPU_CNMIPS
+	/*
+	 * Disable COP2, it will be enabled in trap handler
+	 * triggered by Coprocessor Unusable exception if
+	 * required
+	 */
+	and	v0, v0, ~MIPS_SR_COP_2_BIT
+#endif
 	mfc0	t0, MIPS_COP_0_STATUS
 	and	t0, t0, MIPS_SR_INT_MASK
 	and	v0, v0, ~MIPS_SR_INT_MASK
diff --git a/sys/mips/mips/trap.c b/sys/mips/mips/trap.c
index 35b8030..aff9071 100644
--- a/sys/mips/mips/trap.c
+++ b/sys/mips/mips/trap.c
@@ -94,6 +94,9 @@ __FBSDID("$FreeBSD$");
 #include <sys/cdefs.h>
 #include <sys/syslog.h>
 
+#ifdef	CPU_CNMIPS
+#include <machine/octeon_cop2.h>
+#endif
 
 #ifdef TRAP_DEBUG
 int trap_debug = 0;
@@ -287,6 +290,7 @@ trap(struct trapframe *trapframe)
 	char *msg = NULL;
 	intptr_t addr = 0;
 	register_t pc;
+	int cop;
 
 	trapdebug_enter(trapframe, 0);
 	
@@ -833,18 +837,39 @@ 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;
+
+		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");
+		}
+
+		mips_wr_status(mips_rd_status() | MIPS_SR_COP_2_BIT);
+		octeon_cop2_restore(td->td_md.md_cop2);
+		mips_wr_status(mips_rd_status() & ~MIPS_SR_COP_2_BIT);
+		trapframe->sr |= 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 !defined(CPU_HAVEFPU)
 		/* FP (COP1) instruction */
-		if ((trapframe->cause & MIPS_CR_COP_ERR) == 0x10000000) {
+		if (cop == 1) {
 			log_illegal_instruction("COP1_UNUSABLE", trapframe);
 			i = SIGILL;
 			break;
 		}
 #endif
-		if ((trapframe->cause & MIPS_CR_COP_ERR) != 0x10000000) {
+		if (cop != 1) {
 			log_illegal_instruction("COPn_UNUSABLE", trapframe);
 			i = SIGILL;	/* only FPU instructions allowed */
 			break;
diff --git a/sys/mips/mips/vm_machdep.c b/sys/mips/mips/vm_machdep.c
index 47058ee..c9d3d01 100644
--- a/sys/mips/mips/vm_machdep.c
+++ b/sys/mips/mips/vm_machdep.c
@@ -163,7 +163,7 @@ 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;
+	pcb2->pcb_context[PCB_REG_SR] |= MIPS_SR_PX | MIPS_SR_UX | MIPS_SR_KX | MIPS_SR_SX;
 #endif
 }
 
@@ -200,6 +200,10 @@ cpu_thread_exit(struct thread *td)
 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);
+#endif
 }
 
 void
@@ -357,7 +361,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