summaryrefslogtreecommitdiff
path: root/cleopatre/devkit/mt7601udrv/common/cmm_aes.c
diff options
context:
space:
mode:
Diffstat (limited to 'cleopatre/devkit/mt7601udrv/common/cmm_aes.c')
-rw-r--r--cleopatre/devkit/mt7601udrv/common/cmm_aes.c1142
1 files changed, 1142 insertions, 0 deletions
diff --git a/cleopatre/devkit/mt7601udrv/common/cmm_aes.c b/cleopatre/devkit/mt7601udrv/common/cmm_aes.c
new file mode 100644
index 0000000000..b254c63ce6
--- /dev/null
+++ b/cleopatre/devkit/mt7601udrv/common/cmm_aes.c
@@ -0,0 +1,1142 @@
+/*
+ ***************************************************************************
+ * Ralink Tech Inc.
+ * 4F, No. 2 Technology 5th Rd.
+ * Science-based Industrial Park
+ * Hsin-chu, Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2004, Ralink Technology, Inc.
+ *
+ * All rights reserved. Ralink's source code is an unpublished work and the
+ * use of a copyright notice does not imply otherwise. This source code
+ * contains confidential trade secret material of Ralink Tech. Any attemp
+ * or participation in deciphering, decoding, reverse engineering or in any
+ * way altering the source code is stricitly prohibited, unless the prior
+ * written consent of Ralink Technology, Inc. is obtained.
+ ***************************************************************************
+
+ Module Name:
+ cmm_aes.c
+
+ Abstract:
+
+ Revision History:
+ Who When What
+ -------- ---------- ----------------------------------------------
+ Paul Wu 02-25-02 Initial
+*/
+
+#include "rt_config.h"
+
+
+
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+UCHAR SboxTable[256] =
+{
+ 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+ 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+ 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+ 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+ 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+ 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+ 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+ 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+ 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+ 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+ 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+ 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+ 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+ 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+ 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+ 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+ 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+ 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+ 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+ 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+ 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+ 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+ 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+ 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+ 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+ 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+ 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+ 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+ 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+ 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+ 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+ 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+};
+
+VOID xor_32(
+ IN PUCHAR a,
+ IN PUCHAR b,
+ OUT PUCHAR out)
+{
+ INT i;
+
+ for (i=0;i<4; i++)
+ {
+ out[i] = a[i] ^ b[i];
+ }
+}
+
+VOID xor_128(
+ IN PUCHAR a,
+ IN PUCHAR b,
+ OUT PUCHAR out)
+{
+ INT i;
+
+ for (i=0;i<16; i++)
+ {
+ out[i] = a[i] ^ b[i];
+ }
+}
+
+UCHAR RTMPCkipSbox(
+ IN UCHAR a)
+{
+ return SboxTable[(int)a];
+}
+
+VOID next_key(
+ IN PUCHAR key,
+ IN INT round)
+{
+ UCHAR rcon;
+ UCHAR sbox_key[4];
+ UCHAR rcon_table[12] =
+ {
+ 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+ 0x1b, 0x36, 0x36, 0x36
+ };
+
+ sbox_key[0] = RTMPCkipSbox(key[13]);
+ sbox_key[1] = RTMPCkipSbox(key[14]);
+ sbox_key[2] = RTMPCkipSbox(key[15]);
+ sbox_key[3] = RTMPCkipSbox(key[12]);
+
+ rcon = rcon_table[round];
+
+ xor_32(&key[0], sbox_key, &key[0]);
+ key[0] = key[0] ^ rcon;
+
+ xor_32(&key[4], &key[0], &key[4]);
+ xor_32(&key[8], &key[4], &key[8]);
+ xor_32(&key[12], &key[8], &key[12]);
+}
+
+VOID byte_sub(
+ IN PUCHAR in,
+ OUT PUCHAR out)
+{
+ INT i;
+
+ for (i=0; i< 16; i++)
+ {
+ out[i] = RTMPCkipSbox(in[i]);
+ }
+}
+
+/************************************/
+/* bitwise_xor() */
+/* A 128 bit, bitwise exclusive or */
+/************************************/
+
+void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
+{
+ int i;
+ for (i=0; i<16; i++)
+ {
+ out[i] = ina[i] ^ inb[i];
+ }
+}
+
+VOID shift_row(
+ IN PUCHAR in,
+ OUT PUCHAR out)
+{
+ out[0] = in[0];
+ out[1] = in[5];
+ out[2] = in[10];
+ out[3] = in[15];
+ out[4] = in[4];
+ out[5] = in[9];
+ out[6] = in[14];
+ out[7] = in[3];
+ out[8] = in[8];
+ out[9] = in[13];
+ out[10] = in[2];
+ out[11] = in[7];
+ out[12] = in[12];
+ out[13] = in[1];
+ out[14] = in[6];
+ out[15] = in[11];
+}
+
+VOID mix_column(
+ IN PUCHAR in,
+ OUT PUCHAR out)
+{
+ INT i;
+ UCHAR add1b[4];
+ UCHAR add1bf7[4];
+ UCHAR rotl[4];
+ UCHAR swap_halfs[4];
+ UCHAR andf7[4];
+ UCHAR rotr[4];
+ UCHAR temp[4];
+ UCHAR tempb[4];
+
+ for (i=0 ; i<4; i++)
+ {
+ if ((in[i] & 0x80)== 0x80)
+ add1b[i] = 0x1b;
+ else
+ add1b[i] = 0x00;
+ }
+
+ swap_halfs[0] = in[2]; /* Swap halfs */
+ swap_halfs[1] = in[3];
+ swap_halfs[2] = in[0];
+ swap_halfs[3] = in[1];
+
+ rotl[0] = in[3]; /* Rotate left 8 bits */
+ rotl[1] = in[0];
+ rotl[2] = in[1];
+ rotl[3] = in[2];
+
+ andf7[0] = in[0] & 0x7f;
+ andf7[1] = in[1] & 0x7f;
+ andf7[2] = in[2] & 0x7f;
+ andf7[3] = in[3] & 0x7f;
+
+ for (i = 3; i>0; i--) /* logical shift left 1 bit */
+ {
+ andf7[i] = andf7[i] << 1;
+ if ((andf7[i-1] & 0x80) == 0x80)
+ {
+ andf7[i] = (andf7[i] | 0x01);
+ }
+ }
+ andf7[0] = andf7[0] << 1;
+ andf7[0] = andf7[0] & 0xfe;
+
+ xor_32(add1b, andf7, add1bf7);
+
+ xor_32(in, add1bf7, rotr);
+
+ temp[0] = rotr[0]; /* Rotate right 8 bits */
+ rotr[0] = rotr[1];
+ rotr[1] = rotr[2];
+ rotr[2] = rotr[3];
+ rotr[3] = temp[0];
+
+ xor_32(add1bf7, rotr, temp);
+ xor_32(swap_halfs, rotl,tempb);
+ xor_32(temp, tempb, out);
+}
+
+
+/************************************************/
+/* construct_mic_header1() */
+/* Builds the first MIC header block from */
+/* header fields. */
+/************************************************/
+
+void construct_mic_header1(
+ unsigned char *mic_header1,
+ int header_length,
+ unsigned char *mpdu)
+{
+ mic_header1[0] = (unsigned char)((header_length - 2) / 256);
+ mic_header1[1] = (unsigned char)((header_length - 2) % 256);
+ mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */
+ mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */
+ mic_header1[4] = mpdu[4]; /* A1 */
+ mic_header1[5] = mpdu[5];
+ mic_header1[6] = mpdu[6];
+ mic_header1[7] = mpdu[7];
+ mic_header1[8] = mpdu[8];
+ mic_header1[9] = mpdu[9];
+ mic_header1[10] = mpdu[10]; /* A2 */
+ mic_header1[11] = mpdu[11];
+ mic_header1[12] = mpdu[12];
+ mic_header1[13] = mpdu[13];
+ mic_header1[14] = mpdu[14];
+ mic_header1[15] = mpdu[15];
+}
+
+/************************************************/
+/* construct_mic_header2() */
+/* Builds the last MIC header block from */
+/* header fields. */
+/************************************************/
+
+void construct_mic_header2(
+ unsigned char *mic_header2,
+ unsigned char *mpdu,
+ int a4_exists,
+ int qc_exists)
+{
+ int i;
+
+ for (i = 0; i<16; i++) mic_header2[i]=0x00;
+
+ mic_header2[0] = mpdu[16]; /* A3 */
+ mic_header2[1] = mpdu[17];
+ mic_header2[2] = mpdu[18];
+ mic_header2[3] = mpdu[19];
+ mic_header2[4] = mpdu[20];
+ mic_header2[5] = mpdu[21];
+
+ /* In Sequence Control field, mute sequence numer bits (12-bit) */
+ mic_header2[6] = mpdu[22] & 0x0f; /* SC */
+ mic_header2[7] = 0x00; /* mpdu[23]; */
+
+ if ((!qc_exists) & a4_exists)
+ {
+ for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
+
+ }
+
+ if (qc_exists && (!a4_exists))
+ {
+ mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+ mic_header2[9] = mpdu[25] & 0x00;
+ }
+
+ if (qc_exists && a4_exists)
+ {
+ for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
+
+ mic_header2[14] = mpdu[30] & 0x0f;
+ mic_header2[15] = mpdu[31] & 0x00;
+ }
+}
+
+
+/************************************************/
+/* construct_mic_iv() */
+/* Builds the MIC IV from header fields and PN */
+/************************************************/
+
+void construct_mic_iv(
+ unsigned char *mic_iv,
+ int qc_exists,
+ int a4_exists,
+ unsigned char *mpdu,
+ unsigned int payload_length,
+ unsigned char *pn_vector)
+{
+ int i;
+
+ mic_iv[0] = 0x59;
+ if (qc_exists && a4_exists)
+ mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC */
+ if (qc_exists && !a4_exists)
+ mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */
+ if (!qc_exists)
+ mic_iv[1] = 0x00;
+ for (i = 2; i < 8; i++)
+ mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+ for (i = 8; i < 14; i++)
+ mic_iv[i] = pn_vector[i - 8]; /* mic_iv[8:13] = PN[0:5] */
+#else
+ for (i = 8; i < 14; i++)
+ mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
+#endif
+ i = (payload_length / 256);
+ i = (payload_length % 256);
+ mic_iv[14] = (unsigned char) (payload_length / 256);
+ mic_iv[15] = (unsigned char) (payload_length % 256);
+
+}
+
+/****************************************/
+/* aes128k128d() */
+/* Performs a 128 bit AES encrypt with */
+/* 128 bit data. */
+/****************************************/
+void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
+{
+ int round;
+ int i;
+ unsigned char intermediatea[16];
+ unsigned char intermediateb[16];
+ unsigned char round_key[16];
+
+ for(i=0; i<16; i++) round_key[i] = key[i];
+
+ for (round = 0; round < 11; round++)
+ {
+ if (round == 0)
+ {
+ xor_128(round_key, data, ciphertext);
+ next_key(round_key, round);
+ }
+ else if (round == 10)
+ {
+ byte_sub(ciphertext, intermediatea);
+ shift_row(intermediatea, intermediateb);
+ xor_128(intermediateb, round_key, ciphertext);
+ }
+ else /* 1 - 9 */
+ {
+ byte_sub(ciphertext, intermediatea);
+ shift_row(intermediatea, intermediateb);
+ mix_column(&intermediateb[0], &intermediatea[0]);
+ mix_column(&intermediateb[4], &intermediatea[4]);
+ mix_column(&intermediateb[8], &intermediatea[8]);
+ mix_column(&intermediateb[12], &intermediatea[12]);
+ xor_128(intermediatea, round_key, ciphertext);
+ next_key(round_key, round);
+ }
+ }
+
+}
+
+void construct_ctr_preload(
+ unsigned char *ctr_preload,
+ int a4_exists,
+ int qc_exists,
+ unsigned char *mpdu,
+ unsigned char *pn_vector,
+ int c)
+{
+
+ int i = 0;
+ for (i=0; i<16; i++) ctr_preload[i] = 0x00;
+ i = 0;
+
+ ctr_preload[0] = 0x01; /* flag */
+ if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */
+ if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
+
+ for (i = 2; i < 8; i++)
+ ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+ for (i = 8; i < 14; i++)
+ ctr_preload[i] = pn_vector[i - 8]; /* ctr_preload[8:13] = PN[0:5] */
+#else
+ for (i = 8; i < 14; i++)
+ ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */
+#endif
+ ctr_preload[14] = (unsigned char) (c / 256); /* Ctr */
+ ctr_preload[15] = (unsigned char) (c % 256);
+
+}
+
+BOOLEAN RTMPSoftDecryptAES(
+ IN PRTMP_ADAPTER pAd,
+ IN PUCHAR pData,
+ IN ULONG DataByteCnt,
+ IN PCIPHER_KEY pWpaKey)
+{
+ UINT HeaderLen;
+ UCHAR PN[6];
+ UINT payload_len;
+ UINT num_blocks;
+ UINT payload_remainder;
+ USHORT fc;
+ UCHAR fc0;
+ UCHAR fc1;
+ UINT frame_type;
+ UINT frame_subtype;
+ UINT from_ds;
+ UINT to_ds;
+ INT a4_exists;
+ INT qc_exists;
+ UCHAR aes_out[16];
+ int payload_index;
+ UINT i;
+ UCHAR ctr_preload[16];
+ UCHAR chain_buffer[16];
+ UCHAR padded_buffer[16];
+ UCHAR mic_iv[16];
+ UCHAR mic_header1[16];
+ UCHAR mic_header2[16];
+ UCHAR MIC[8];
+ UCHAR TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+ RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+ fc0 = *pData;
+ fc1 = *(pData + 1);
+
+ fc = *((PUSHORT)pData);
+
+ frame_type = ((fc0 >> 2) & 0x03);
+ frame_subtype = ((fc0 >> 4) & 0x0f);
+
+ from_ds = (fc1 & 0x2) >> 1;
+ to_ds = (fc1 & 0x1);
+
+ a4_exists = (from_ds & to_ds);
+ qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
+ (frame_subtype == 0x09) || /* Likely to change. */
+ (frame_subtype == 0x0a) ||
+ (frame_subtype == 0x0b)
+ );
+
+ HeaderLen = 24;
+
+ if (a4_exists)
+ HeaderLen += 6;
+
+ if (qc_exists)
+ HeaderLen += 2;
+
+ if (pWpaKey->KeyLen == 0)
+ {
+ DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(the Length can not be 0)\n"));
+ return FALSE;
+ }
+
+ PN[0] = *(pData+ HeaderLen);
+ PN[1] = *(pData+ HeaderLen + 1);
+ PN[2] = *(pData+ HeaderLen + 4);
+ PN[3] = *(pData+ HeaderLen + 5);
+ PN[4] = *(pData+ HeaderLen + 6);
+ PN[5] = *(pData+ HeaderLen + 7);
+
+ payload_len = DataByteCnt - HeaderLen - 8 - 8; /* 8 bytes for CCMP header , 8 bytes for MIC*/
+ payload_remainder = (payload_len) % 16;
+ num_blocks = (payload_len) / 16;
+
+
+
+ /* Find start of payload*/
+ payload_index = HeaderLen + 8; /*IV+EIV*/
+
+ for (i=0; i< num_blocks; i++)
+ {
+ construct_ctr_preload(ctr_preload,
+ a4_exists,
+ qc_exists,
+ pData,
+ PN,
+ i+1 );
+
+ aes128k128d(pWpaKey->Key, ctr_preload, aes_out);
+
+ bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+ NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
+ payload_index += 16;
+ }
+
+
+ /* If there is a short final block, then pad it*/
+ /* encrypt it and copy the unpadded part back */
+
+ if (payload_remainder > 0)
+ {
+ construct_ctr_preload(ctr_preload,
+ a4_exists,
+ qc_exists,
+ pData,
+ PN,
+ num_blocks + 1);
+
+ NdisZeroMemory(padded_buffer, 16);
+ NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+ aes128k128d(pWpaKey->Key, ctr_preload, aes_out);
+
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
+ payload_index += payload_remainder;
+ }
+
+
+ /* Descrypt the MIC*/
+ construct_ctr_preload(ctr_preload,
+ a4_exists,
+ qc_exists,
+ pData,
+ PN,
+ 0);
+ NdisZeroMemory(padded_buffer, 16);
+ NdisMoveMemory(padded_buffer, pData + payload_index, 8);
+
+ aes128k128d(pWpaKey->Key, ctr_preload, aes_out);
+
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+
+ NdisMoveMemory(TrailMIC, chain_buffer, 8);
+
+
+
+ /* Calculate MIC*/
+
+
+ /*Force the protected frame bit on*/
+ *(pData + 1) = *(pData + 1) | 0x40;
+
+ /* Find start of payload*/
+ /* Because the CCMP header has been removed*/
+ payload_index = HeaderLen;
+
+ construct_mic_iv(
+ mic_iv,
+ qc_exists,
+ a4_exists,
+ pData,
+ payload_len,
+ PN);
+
+ construct_mic_header1(
+ mic_header1,
+ HeaderLen,
+ pData);
+
+ construct_mic_header2(
+ mic_header2,
+ pData,
+ a4_exists,
+ qc_exists);
+
+ aes128k128d(pWpaKey->Key, mic_iv, aes_out);
+ bitwise_xor(aes_out, mic_header1, chain_buffer);
+ aes128k128d(pWpaKey->Key, chain_buffer, aes_out);
+ bitwise_xor(aes_out, mic_header2, chain_buffer);
+ aes128k128d(pWpaKey->Key, chain_buffer, aes_out);
+
+ /* iterate through each 16 byte payload block */
+ for (i = 0; i < num_blocks; i++)
+ {
+ bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+ payload_index += 16;
+ aes128k128d(pWpaKey->Key, chain_buffer, aes_out);
+ }
+
+ /* Add on the final payload block if it needs padding */
+ if (payload_remainder > 0)
+ {
+ NdisZeroMemory(padded_buffer, 16);
+ NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+ bitwise_xor(aes_out, padded_buffer, chain_buffer);
+ aes128k128d(pWpaKey->Key, chain_buffer, aes_out);
+ }
+
+ /* aes_out contains padded mic, discard most significant*/
+ /* 8 bytes to generate 64 bit MIC*/
+ for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
+
+ if (!NdisEqualMemory(MIC, TrailMIC, 8))
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n")); /* MIC error. */
+ return FALSE;
+ }
+
+#ifdef RT_BIG_ENDIAN
+ RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+ return TRUE;
+}
+
+
+/*
+ ========================================================================
+
+ Routine Description:
+ Construct AAD of CCMP.
+
+ Arguments:
+
+ Return Value:
+
+ Note:
+ It's described in IEEE Std 802.11-2007.
+ The AAD is constructed from the MPDU header.
+
+ ========================================================================
+*/
+VOID RTMPConstructCCMPAAD(
+ IN PUCHAR pHdr,
+ IN BOOLEAN isDataFrame,
+ IN UINT8 a4_exists,
+ IN UINT8 qc_exists,
+ OUT UCHAR *aad_hdr,
+ OUT UINT *aad_len)
+{
+ UINT len = 0;
+
+ /* Frame control -
+ Subtype bits (bits 4 5 6) in a Data MPDU masked to 0
+ Retry bit (bit 11) masked to 0
+ PwrMgt bit (bit 12) masked to 0
+ MoreData bit (bit 13) masked to 0
+ Protected Frame bit (bit 14) always set to 1 */
+ if (isDataFrame)
+ aad_hdr[0] = (*pHdr) & 0x8f;
+ else
+ aad_hdr[0] = (*pHdr);
+ aad_hdr[1] = (*(pHdr + 1)) & 0xc7;
+ aad_hdr[1] = aad_hdr[1] | 0x40;
+ len = 2;
+
+ /* Append Addr 1, 2 & 3 */
+ NdisMoveMemory(&aad_hdr[len], pHdr + 4, 3 * MAC_ADDR_LEN);
+ len += (3 * MAC_ADDR_LEN);
+
+ /* SC -
+ MPDU Sequence Control field, with the Sequence Number
+ subfield (bits 4-15 of the Sequence Control field)
+ masked to 0. The Fragment Number subfield is not modified. */
+ aad_hdr[len] = (*(pHdr + 22)) & 0x0f;
+ aad_hdr[len + 1] = 0x00;
+ len += 2;
+
+
+ /* Append the Addr4 field if present. */
+ if (a4_exists)
+ {
+ NdisMoveMemory(&aad_hdr[len], pHdr + 24, MAC_ADDR_LEN);
+ len += MAC_ADDR_LEN;
+ }
+
+ /* QC -
+ QoS Control field, if present, a 2-octet field that includes
+ the MSDU priority. The QC TID field is used in the
+ construction of the AAD and the remaining QC fields are
+ set to 0 for the AAD calculation (bits 4 to 15 are set to 0). */
+ if (qc_exists & a4_exists)
+ {
+ aad_hdr[len] = (*(pHdr + 30)) & 0x0f; /* Qos_TC */
+ aad_hdr[len + 1] = 0x00;
+ len += 2;
+ }
+ else if (qc_exists & !a4_exists)
+ {
+ aad_hdr[len] = (*(pHdr + 24)) & 0x0f; /* Qos_TC */
+ aad_hdr[len + 1] = 0x00;
+ len += 2;
+ }
+
+ *aad_len = len;
+}
+
+/*
+ ========================================================================
+
+ Routine Description:
+ Construct NONCE header of CCMP.
+
+ Arguments:
+
+ Return Value:
+
+ Note:
+
+ ========================================================================
+*/
+VOID RTMPConstructCCMPNonce(
+ IN PUCHAR pHdr,
+ IN UINT8 a4_exists,
+ IN UINT8 qc_exists,
+ IN BOOLEAN isMgmtFrame,
+ IN UCHAR *pn,
+ OUT UCHAR *nonce_hdr,
+ OUT UINT *nonce_hdr_len)
+{
+ UINT n_offset = 0;
+ INT i;
+
+ /* Decide the Priority Octet
+ The Priority sub-field of the Nonce Flags field shall
+ be set to the fixed value 0 when there is no QC field
+ present in the MPDU header. When the QC field is present,
+ bits 0 to 3 of the Priority field shall be set to the
+ value of the QC TID (bits 0 to 3 of the QC field).*/
+ if (qc_exists && a4_exists)
+ nonce_hdr[0] = (*(pHdr + 30)) & 0x0f;
+ if (qc_exists && !a4_exists)
+ nonce_hdr[0] = (*(pHdr + 24)) & 0x0f;
+
+ n_offset += 1;
+
+ /* Fill in MPDU Address A2 field */
+ NdisMoveMemory(&nonce_hdr[n_offset], pHdr + 10, MAC_ADDR_LEN);
+ n_offset += MAC_ADDR_LEN;
+
+ /* Fill in the PN. The PN field occupies octets 7¡V12.
+ The octets of PN shall be ordered so that PN0 is at octet index 12
+ and PN5 is at octet index 7. */
+ for (i = 0; i < 6; i++)
+ nonce_hdr[n_offset + i] = pn[5 - i];
+ n_offset += LEN_PN;
+
+ *nonce_hdr_len = n_offset;
+
+}
+
+/*
+ ========================================================================
+
+ Routine Description:
+ Construct CCMP header.
+
+ Arguments:
+
+ Return Value:
+
+ Note:
+ It's a 8-octets header.
+
+ ========================================================================
+*/
+VOID RTMPConstructCCMPHdr(
+ IN UINT8 key_idx,
+ IN UCHAR *pn,
+ OUT UCHAR *ccmp_hdr)
+{
+ NdisZeroMemory(ccmp_hdr, LEN_CCMP_HDR);
+
+ ccmp_hdr[0] = pn[0];
+ ccmp_hdr[1] = pn[1];
+ ccmp_hdr[3] = (key_idx <<6) | 0x20;
+ ccmp_hdr[4] = pn[2];
+ ccmp_hdr[5] = pn[3];
+ ccmp_hdr[6] = pn[4];
+ ccmp_hdr[7] = pn[5];
+}
+
+/*
+ ========================================================================
+
+ Routine Description:
+
+ Arguments:
+
+ Return Value:
+
+ Note:
+
+ ========================================================================
+*/
+BOOLEAN RTMPSoftEncryptCCMP(
+ IN PRTMP_ADAPTER pAd,
+ IN PUCHAR pHdr,
+ IN PUCHAR pIV,
+ IN PUCHAR pKey,
+ INOUT PUCHAR pData,
+ IN UINT32 DataLen)
+{
+ UINT8 frame_type, frame_subtype;
+ UINT8 from_ds, to_ds;
+ UINT8 a4_exists, qc_exists;
+ UINT8 aad_hdr[30];
+ UINT aad_len = 0;
+ UINT8 nonce_hdr[13];
+ UINT32 nonce_hdr_len = 0;
+ UINT32 out_len = DataLen + 8;
+
+#ifdef RT_BIG_ENDIAN
+ RTMPFrameEndianChange(pAd, (PUCHAR)pHdr, DIR_READ, FALSE);
+#endif
+
+ /* Initial variable */
+ NdisZeroMemory(aad_hdr, 30);
+ NdisZeroMemory(nonce_hdr, 13);
+
+ /* Indicate type and subtype of Frame Control field */
+ frame_type = (((*pHdr) >> 2) & 0x03);
+ frame_subtype = (((*pHdr) >> 4) & 0x0f);
+
+ /* Indicate the fromDS and ToDS */
+ from_ds = ((*(pHdr + 1)) & 0x2) >> 1;
+ to_ds = ((*(pHdr + 1)) & 0x1);
+
+ /* decide if the Address 4 exist or QoS exist */
+ a4_exists = (from_ds & to_ds);
+ qc_exists = 0;
+ if (frame_type == BTYPE_DATA)
+ {
+ qc_exists = ((frame_subtype == SUBTYPE_QDATA) ||
+ (frame_subtype == SUBTYPE_QDATA_CFACK) ||
+ (frame_subtype == SUBTYPE_QDATA_CFPOLL) ||
+ (frame_subtype == SUBTYPE_QDATA_CFACK_CFPOLL));
+ }
+
+ /* Construct AAD header */
+ RTMPConstructCCMPAAD(pHdr,
+ (frame_type == BTYPE_DATA),
+ a4_exists,
+ qc_exists,
+ aad_hdr,
+ &aad_len);
+
+ /* Construct NONCE header */
+ RTMPConstructCCMPNonce(pHdr,
+ a4_exists,
+ qc_exists,
+ (frame_type == BTYPE_MGMT),
+ pIV,
+ nonce_hdr,
+ &nonce_hdr_len);
+
+ /* CCM originator processing -
+ Use the temporal key, AAD, nonce, and MPDU data to
+ form the cipher text and MIC. */
+ if (AES_CCM_Encrypt(pData, DataLen,
+ pKey, 16,
+ nonce_hdr, nonce_hdr_len,
+ aad_hdr, aad_len, LEN_CCMP_MIC,
+ pData, &out_len))
+ return FALSE;
+
+#ifdef RT_BIG_ENDIAN
+ RTMPFrameEndianChange(pAd, (PUCHAR)pHdr, DIR_READ, FALSE);
+#endif
+
+ return TRUE;
+}
+
+/*
+ ========================================================================
+
+ Routine Description:
+ Decrypt data with CCMP.
+
+ Arguments:
+
+ Return Value:
+
+ Note:
+
+ ========================================================================
+*/
+BOOLEAN RTMPSoftDecryptCCMP(
+ IN PRTMP_ADAPTER pAd,
+ IN PUCHAR pHdr,
+ IN PCIPHER_KEY pKey,
+ INOUT PUCHAR pData,
+ INOUT UINT16 *DataLen)
+{
+ UINT8 frame_type, frame_subtype;
+ UINT8 from_ds, to_ds;
+ UINT8 a4_exists, qc_exists;
+ UINT8 aad_hdr[30];
+ UINT aad_len = 0;
+ UINT8 pn[LEN_PN];
+ PUCHAR cipherData_ptr;
+ UINT32 cipherData_len;
+ UINT8 nonce_hdr[13];
+ UINT32 nonce_hdr_len = 0;
+ UINT32 out_len = *DataLen;
+
+#ifdef RT_BIG_ENDIAN
+ RTMPFrameEndianChange(pAd, (PUCHAR)pHdr, DIR_READ, FALSE);
+#endif
+
+ /* Check the key is valid */
+ if (pKey->KeyLen == 0)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s : The key is not available !\n", __FUNCTION__));
+ return FALSE;
+ }
+
+ /* Initial variable */
+ NdisZeroMemory(aad_hdr, 30);
+ NdisZeroMemory(nonce_hdr, 13);
+
+ /* Indicate type and subtype of Frame Control field */
+ frame_type = (((*pHdr) >> 2) & 0x03);
+ frame_subtype = (((*pHdr) >> 4) & 0x0f);
+
+ /* Indicate the fromDS and ToDS */
+ from_ds = ((*(pHdr + 1)) & 0x2) >> 1;
+ to_ds = ((*(pHdr + 1)) & 0x1);
+
+ /* decide if the Address 4 exist or QoS exist */
+ a4_exists = (from_ds & to_ds);
+ qc_exists = 0;
+ if (frame_type == BTYPE_DATA)
+ {
+ qc_exists = ((frame_subtype == SUBTYPE_QDATA) ||
+ (frame_subtype == SUBTYPE_QDATA_CFACK) ||
+ (frame_subtype == SUBTYPE_QDATA_CFPOLL) ||
+ (frame_subtype == SUBTYPE_QDATA_CFACK_CFPOLL));
+ }
+
+ /* Extract PN and from CCMP header */
+ pn[0] = pData[0];
+ pn[1] = pData[1];
+ pn[2] = pData[4];
+ pn[3] = pData[5];
+ pn[4] = pData[6];
+ pn[5] = pData[7];
+
+ /* skip ccmp header */
+ cipherData_ptr = pData + LEN_CCMP_HDR;
+ cipherData_len = *DataLen - LEN_CCMP_HDR;
+
+ /* Construct AAD header */
+ RTMPConstructCCMPAAD(pHdr,
+ (frame_type == BTYPE_DATA),
+ a4_exists,
+ qc_exists,
+ aad_hdr,
+ &aad_len);
+
+ /* Construct NONCE header */
+ RTMPConstructCCMPNonce(pHdr,
+ a4_exists,
+ qc_exists,
+ (frame_type == BTYPE_MGMT),
+ pn,
+ nonce_hdr,
+ &nonce_hdr_len);
+
+ /* CCM recipient processing -
+ uses the temporal key, AAD, nonce, MIC,
+ and MPDU cipher text data */
+ if (AES_CCM_Decrypt(cipherData_ptr, cipherData_len,
+ pKey->Key, 16,
+ nonce_hdr, nonce_hdr_len,
+ aad_hdr, aad_len, LEN_CCMP_MIC,
+ pData, &out_len))
+ return FALSE;
+
+ *DataLen = out_len;
+
+#ifdef RT_BIG_ENDIAN
+ RTMPFrameEndianChange(pAd, (PUCHAR)pHdr, DIR_READ, FALSE);
+#endif
+
+ return TRUE;
+}
+
+/*
+ ========================================================================
+
+ Routine Description:
+ CCMP test vector
+
+ Arguments:
+
+ Return Value:
+
+ Note:
+
+ ========================================================================
+*/
+VOID CCMP_test_vector(
+ IN PRTMP_ADAPTER pAd,
+ IN INT input)
+{
+ UINT8 Key_ID = 0;
+ /*UINT8 A1[6] = {0x0f, 0xd2, 0xe1, 0x28, 0xa5, 0x7c};*/
+ /*UINT8 A2[6] = {0x50, 0x30, 0xf1, 0x84, 0x44, 0x08};*/
+ /*UINT8 A3[6] = {0xab, 0xae, 0xa5, 0xb8, 0xfc, 0xba};*/
+ UINT8 TK[16] = {0xc9, 0x7c, 0x1f, 0x67, 0xce, 0x37, 0x11, 0x85,
+ 0x51, 0x4a, 0x8a, 0x19, 0xf2, 0xbd, 0xd5, 0x2f};
+ UINT8 PN[6] = {0x0C, 0xE7, 0x76, 0x97, 0x03, 0xB5};
+ UINT8 HDR[24]= {0x08, 0x48, 0xc3, 0x2c, 0x0f, 0xd2, 0xe1, 0x28,
+ 0xa5, 0x7c, 0x50, 0x30, 0xf1, 0x84, 0x44, 0x08,
+ 0xab, 0xae, 0xa5, 0xb8, 0xfc, 0xba, 0x80, 0x33};
+ UINT8 AAD[22] = {0x08, 0x40, 0x0f, 0xd2, 0xe1, 0x28, 0xa5, 0x7c,
+ 0x50, 0x30, 0xf1, 0x84, 0x44, 0x08, 0xab, 0xae,
+ 0xa5, 0xb8, 0xfc, 0xba, 0x00, 0x00};
+ UINT8 CCMP_HDR[8] = {0x0c, 0xe7, 0x00, 0x20, 0x76, 0x97, 0x03, 0xb5};
+ UINT8 CCM_NONCE[13] = {0x00, 0x50, 0x30, 0xf1, 0x84, 0x44, 0x08, 0xb5,
+ 0x03, 0x97, 0x76, 0xe7, 0x0c};
+ UINT8 P_TEXT_DATA[20] = {0xf8, 0xba, 0x1a, 0x55, 0xd0, 0x2f, 0x85, 0xae,
+ 0x96, 0x7b, 0xb6, 0x2f, 0xb6, 0xcd, 0xa8, 0xeb,
+ 0x7e, 0x78, 0xa0, 0x50};
+ UINT8 C_TEXT_DATA[28] = {0xf3, 0xd0, 0xa2, 0xfe, 0x9a, 0x3d, 0xbf, 0x23,
+ 0x42, 0xa6, 0x43, 0xe4, 0x32, 0x46, 0xe8, 0x0c,
+ 0x3c, 0x04, 0xd0, 0x19, 0x78, 0x45, 0xce, 0x0b,
+ 0x16, 0xf9, 0x76, 0x23};
+ UINT8 res_buf[100];
+ UINT res_len = 0;
+
+ printk("== CCMP test vector == \n");
+
+ /* Check AAD */
+ NdisZeroMemory(res_buf, 100);
+ res_len = 0;
+ RTMPConstructCCMPAAD(HDR, TRUE, 0, 0, res_buf, &res_len);
+ if (res_len == 22 && NdisEqualMemory(res_buf, AAD, res_len))
+ printk("Construct AAD is OK!!!\n");
+ else
+ {
+ printk("\n!!!Construct AAD is FAILURE!!!\n\n");
+ hex_dump("Calculate AAD", res_buf, res_len);
+ }
+ /* Check NONCE */
+ NdisZeroMemory(res_buf, 100);
+ res_len = 0;
+ RTMPConstructCCMPNonce(HDR, 0, 0, FALSE, PN, res_buf, &res_len);
+ if (res_len == 13 && NdisEqualMemory(res_buf, CCM_NONCE, res_len))
+ printk("Construct NONCE is OK!!!\n");
+ else
+ {
+ printk("\n!!!Construct NONCE is FAILURE!!!\n\n");
+ hex_dump("Calculate NONCE", res_buf, res_len);
+ }
+ /* Check CCMP-Header */
+ NdisZeroMemory(res_buf, 100);
+ res_len = 0;
+ RTMPConstructCCMPHdr(Key_ID, PN, res_buf);
+ if (NdisEqualMemory(res_buf, CCMP_HDR, 8))
+ printk("Construct CCMP_HDR is OK!!!\n");
+ else
+ {
+ printk("\n!!!Construct CCMP_HDR is FAILURE!!!\n\n");
+ hex_dump("Calculate CCMP_HDR", res_buf, 8);
+ }
+
+ /* Encrypt action */
+ NdisZeroMemory(res_buf, 100);
+ NdisMoveMemory(res_buf, P_TEXT_DATA, sizeof(P_TEXT_DATA));
+ res_len = sizeof(C_TEXT_DATA);
+ if (AES_CCM_Encrypt(res_buf, sizeof(P_TEXT_DATA),
+ TK, sizeof(TK),
+ CCM_NONCE, sizeof(CCM_NONCE),
+ AAD, sizeof(AAD), 8,
+ res_buf, &res_len) == 0)
+ {
+ if (res_len == sizeof(C_TEXT_DATA) &&
+ NdisEqualMemory(res_buf, C_TEXT_DATA, res_len))
+ printk("CCM_Encrypt is OK!!!\n");
+ else
+ {
+ printk("\n!!!CCM_Encrypt is FAILURE!!!\n\n");
+ hex_dump("CCM_Encrypt", res_buf, res_len);
+ }
+ }
+
+ /* Decrypt action */
+ NdisZeroMemory(res_buf, 100);
+ NdisMoveMemory(res_buf, C_TEXT_DATA, sizeof(C_TEXT_DATA));
+ res_len = sizeof(P_TEXT_DATA);
+ if (AES_CCM_Decrypt(res_buf, sizeof(C_TEXT_DATA), TK, 16,
+ CCM_NONCE, sizeof(CCM_NONCE),
+ AAD, sizeof(AAD), 8,
+ res_buf, &res_len) == 0)
+ {
+ if (res_len == sizeof(P_TEXT_DATA) &&
+ NdisEqualMemory(res_buf, P_TEXT_DATA, res_len))
+ printk("CCM_Decrypt is OK!!!\n");
+ else
+ {
+ printk("\n!!!CCM_Decrypt is FAILURE!!!\n\n");
+ hex_dump("CCM_Decrypt", res_buf, res_len);
+ }
+ }
+
+ printk("== CCMP test vector == \n");
+
+ }
+
+