Move to rsdk 3.2.4. Compiles cleanly.

Signed-off-by: Roman Yeryomin <roman@advem.lv>

1 files changed, 953 insertions, 0 deletions

diff --git a/target/linux/realtek/files/drivers/net/wireless/rtl8192e/8192cd_aes.c b/target/linux/realtek/files/drivers/net/wireless/rtl8192e/8192cd_aes.c
new file mode 100644
index 000000000..5f76b8974
--- /dev/null
+++ b/target/linux/realtek/files/drivers/net/wireless/rtl8192e/8192cd_aes.c
@@ -0,0 +1,953 @@
+/*
+ *  Software AES encryption/descryption routines
+ *
+ *  $Id: 8192cd_aes.c,v 1.1.4.2 2010/11/29 12:23:52 button Exp $
+ *
+ *  Copyright (c) 2009 Realtek Semiconductor Corp.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#define _8192CD_AES_C_
+
+#ifdef __KERNEL__
+#include <asm/byteorder.h>
+#elif defined(__ECOS)
+#include <cyg/io/eth/rltk/819x/wrapper/sys_support.h>
+#include <cyg/io/eth/rltk/819x/wrapper/skbuff.h>
+#include <cyg/io/eth/rltk/819x/wrapper/timer.h>
+#include <cyg/io/eth/rltk/819x/wrapper/wrapper.h>
+#endif
+
+#include "./8192cd_cfg.h"
+
+#if !defined(__KERNEL__) && !defined(__ECOS)
+#include "./sys-support.h"
+#endif
+
+#include "./8192cd.h"
+#ifdef __KERNEL__
+#include "./ieee802_mib.h"
+#elif defined(__ECOS)
+#include <cyg/io/eth/rltk/819x/wlan/ieee802_mib.h>
+#endif
+#include "./8192cd_util.h"
+#include "./8192cd_headers.h"
+#include "./8192cd_debug.h"
+
+#define SHOW_INIT_BLOCKS
+#define SHOW_HEADER_FIELDS
+#define SHOW_CTR_PRELOAD
+
+#define MAX_MSG_SIZE	2048
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+    unsigned char sbox_table[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
+    };
+
+/*****************************/
+/**** Function Prototypes ****/
+/*****************************/
+
+void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out);
+void construct_mic_iv(
+                        unsigned char *mic_header1,
+                        int qc_exists,
+                        int a4_exists,
+                        unsigned char *mpdu,
+                        unsigned int payload_length,
+                        unsigned char * pn_vector);
+void construct_mic_header1(
+                        unsigned char *mic_header1,
+                        int header_length,
+                        unsigned char *mpdu);
+void construct_mic_header2(
+                    unsigned char *mic_header2,
+                    unsigned char *mpdu,
+                    int a4_exists,
+                    int qc_exists);
+void construct_ctr_preload(
+                        unsigned char *ctr_preload,
+                        int a4_exists,
+                        int qc_exists,
+                        unsigned char *mpdu,
+                        unsigned char *pn_vector,
+                        int c);
+void xor_128(unsigned char *a, unsigned char *b, unsigned char *out);
+void xor_32(unsigned char *a, unsigned char *b, unsigned char *out);
+unsigned char sbox(unsigned char a);
+void next_key(unsigned char *key, int round);
+void byte_sub(unsigned char *in, unsigned char *out);
+void shift_row(unsigned char *in, unsigned char *out);
+void mix_column(unsigned char *in, unsigned char *out);
+void add_round_key( unsigned char *shiftrow_in,
+                    unsigned char *mcol_in,
+                    unsigned char *block_in,
+                    int round,
+                    unsigned char *out);
+void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext);
+
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+void xor_128(unsigned char *a, unsigned char *b, unsigned char *out)
+{
+    int i;
+    for (i=0;i<16; i++)
+    {
+        out[i] = a[i] ^ b[i];
+    }
+}
+
+
+void xor_32(unsigned char *a, unsigned char *b, unsigned char *out)
+{
+    int i;
+    for (i=0;i<4; i++)
+    {
+        out[i] = a[i] ^ b[i];
+    }
+}
+
+
+unsigned char sbox(unsigned char a)
+{
+    return sbox_table[(int)a];
+}
+
+
+void next_key(unsigned char *key, int round)
+{
+    unsigned char rcon;
+    unsigned char sbox_key[4];
+    unsigned char rcon_table[12] =
+    {
+        0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+        0x1b, 0x36, 0x36, 0x36
+    };
+
+    sbox_key[0] = sbox(key[13]);
+    sbox_key[1] = sbox(key[14]);
+    sbox_key[2] = sbox(key[15]);
+    sbox_key[3] = sbox(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(unsigned char *in, unsigned char *out)
+{
+    int i;
+    for (i=0; i< 16; i++)
+    {
+        out[i] = sbox(in[i]);
+    }
+}
+
+
+void shift_row(unsigned char *in, unsigned char *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(unsigned char *in, unsigned char *out)
+{
+    int i;
+    unsigned char add1b[4];
+    unsigned char add1bf7[4];
+    unsigned char rotl[4];
+    unsigned char swap_halfs[4];
+    unsigned char andf7[4];
+    unsigned char rotr[4];
+    unsigned char temp[4];
+    unsigned char 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);
+}
+
+
+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);
+        }
+    }
+
+}
+
+
+/************************************************/
+/* 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
+    mic_iv[14] = (unsigned char) (payload_length / 256);
+    mic_iv[15] = (unsigned char) (payload_length % 256);
+}
+
+
+/************************************************/
+/* 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];
+
+    //mic_header2[6] = mpdu[22] & 0xf0;   /* SC */
+    mic_header2[6] = 0x00;
+    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_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+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);
+}
+
+
+/************************************/
+/* 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];
+    }
+}
+
+
+/*-----------------------------------------------------------------------------
+hdr: wlanhdr
+llc: llc_snap
+pframe: raw data payload
+plen: length of raw data payload
+mic: mic of AES
+------------------------------------------------------------------------------*/
+static void aes_tx(struct rtl8192cd_priv *priv, UINT8 *key, UINT8 keyid,
+			union PN48 *pn48, UINT8 *hdr, UINT8 *llc,
+			UINT8 *pframe, UINT32 plen, UINT8* txmic)
+{
+	static UINT8	message[MAX_MSG_SIZE];
+	UINT32	qc_exists, a4_exists, i, j, payload_remainder,
+			num_blocks,payload_length, payload_index;
+
+	UINT8 pn_vector[6];
+	UINT8 mic_iv[16];
+    UINT8 mic_header1[16];
+    UINT8 mic_header2[16];
+    UINT8 ctr_preload[16];
+
+    /* Intermediate Buffers */
+    UINT8 chain_buffer[16];
+    UINT8 aes_out[16];
+    UINT8 padded_buffer[16];
+    UINT8 mic[8];
+
+	UINT32	offset = 0;
+	UINT32	hdrlen  = get_hdrlen(priv, hdr);
+
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)padded_buffer, 0, 16);
+
+	if (get_tofr_ds(hdr) != 0x03)
+		a4_exists = 0;
+	else
+		a4_exists = 1;
+
+	if (is_qos_data(hdr)) {
+		qc_exists = 1;
+		//hdrlen += 2;	// these 2 bytes has already added
+	}
+	else
+		qc_exists = 0;
+
+	// below is to collecting each frag(hdr, llc, pay, and mic into single message buf)
+
+	// extiv (8 bytes long) should have been appended
+	pn_vector[0]  = hdr[hdrlen]   = pn48->_byte_.TSC0;
+	pn_vector[1]  = hdr[hdrlen+1] = pn48->_byte_.TSC1;
+	hdr[hdrlen+2] =  0x00;
+	hdr[hdrlen+3] = (0x20 | (keyid << 6));
+	pn_vector[2]  = hdr[hdrlen+4] = pn48->_byte_.TSC2;
+	pn_vector[3]  = hdr[hdrlen+5] = pn48->_byte_.TSC3;
+	pn_vector[4]  = hdr[hdrlen+6] = pn48->_byte_.TSC4;
+	pn_vector[5]  = hdr[hdrlen+7] = pn48->_byte_.TSC5;
+
+	memcpy((void *)message, hdr, (hdrlen + 8)); //8 is for ext iv len
+	offset = (hdrlen + 8);
+
+	if (llc)
+	{
+		memcpy((void *)(message + offset), (void *)llc, 8);
+		offset += 8;
+	}
+	memcpy((void *)(message + offset), (void *)pframe, plen);
+	offset += plen;
+
+	// now we have collecting all the bytes into single message buf
+
+	payload_length = plen; // 8 is for llc
+
+	if (llc)
+		payload_length += 8;
+
+	construct_mic_iv(
+                        mic_iv,
+                        qc_exists,
+                        a4_exists,
+                        message,
+                        (payload_length),
+                        pn_vector
+                        );
+
+    construct_mic_header1(
+                            mic_header1,
+                            hdrlen,
+                            message
+                            );
+    construct_mic_header2(
+                            mic_header2,
+                            message,
+                            a4_exists,
+                            qc_exists
+                            );
+
+
+	payload_remainder = (payload_length) % 16;
+    num_blocks = (payload_length) / 16;
+
+    /* Find start of payload */
+    payload_index = (hdrlen + 8);
+
+    /* Calculate MIC */
+    aes128k128d(key, mic_iv, aes_out);
+    bitwise_xor(aes_out, mic_header1, chain_buffer);
+    aes128k128d(key, chain_buffer, aes_out);
+    bitwise_xor(aes_out, mic_header2, chain_buffer);
+    aes128k128d(key, chain_buffer, aes_out);
+
+	for (i = 0; i < num_blocks; i++)
+    {
+        bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+
+        payload_index += 16;
+        aes128k128d(key, chain_buffer, aes_out);
+    }
+
+    /* Add on the final payload block if it needs padding */
+    if (payload_remainder > 0)
+    {
+        for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+        for (j = 0; j < payload_remainder; j++)
+        {
+            padded_buffer[j] = message[payload_index++];
+        }
+        bitwise_xor(aes_out, padded_buffer, chain_buffer);
+        aes128k128d(key, chain_buffer, aes_out);
+
+    }
+
+    for (j = 0 ; j < 8; j++) mic[j] = aes_out[j];
+
+    /* Insert MIC into payload */
+    for (j = 0; j < 8; j++)
+    	message[payload_index+j] = mic[j];
+
+	payload_index = hdrlen + 8;
+	for (i=0; i< num_blocks; i++)
+    {
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                message,
+                                pn_vector,
+                                i+1);
+        aes128k128d(key, ctr_preload, aes_out);
+        bitwise_xor(aes_out, &message[payload_index], chain_buffer);
+        for (j=0; j<16;j++) message[payload_index++] = chain_buffer[j];
+    }
+
+    if (payload_remainder > 0)          /* If there is a short final block, then pad it,*/
+    {                                   /* encrypt it and copy the unpadded part back   */
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                message,
+                                pn_vector,
+                                num_blocks+1);
+
+        for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+        for (j = 0; j < payload_remainder; j++)
+        {
+            padded_buffer[j] = message[payload_index+j];
+        }
+        aes128k128d(key, ctr_preload, aes_out);
+        bitwise_xor(aes_out, padded_buffer, chain_buffer);
+        for (j=0; j<payload_remainder;j++) message[payload_index++] = chain_buffer[j];
+    }
+
+    /* Encrypt the MIC */
+    construct_ctr_preload(
+                        ctr_preload,
+                        a4_exists,
+                        qc_exists,
+                        message,
+                        pn_vector,
+                        0);
+
+    for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+    for (j = 0; j < 8; j++)
+    {
+        padded_buffer[j] = message[j+hdrlen+8+payload_length];
+    }
+
+    aes128k128d(key, ctr_preload, aes_out);
+    bitwise_xor(aes_out, padded_buffer, chain_buffer);
+    for (j=0; j<8;j++) message[payload_index++] = chain_buffer[j];
+
+	// now, going to copy the final result back to the input buf...
+	offset =0;
+
+	//if (llc)
+	{
+		memcpy((void *)hdr, (void *)(&message[offset]), (hdrlen + 8 )); //8 is for ext iv
+    	offset += (hdrlen + 8);
+    }
+
+    if (llc)
+    {
+    	memcpy((void *)llc, (void *)(&message[offset]),  8 ); //8 is for llc
+    	offset +=  (8);
+    }
+
+
+    memcpy((void *)pframe, (void *)(&message[offset]), (plen)); //now is for plen
+    offset += (plen);
+
+
+    memcpy((void *)txmic, (void *)(&message[offset]), 8); //now is for mic
+    offset += 8;
+
+	rtl_cache_sync_wback(priv, (unsigned long)hdr, hdrlen + 8, PCI_DMA_TODEVICE);
+	if (llc)
+		rtl_cache_sync_wback(priv, (unsigned long)llc, 8, PCI_DMA_TODEVICE);
+	rtl_cache_sync_wback(priv, (unsigned long)pframe, plen, PCI_DMA_TODEVICE);
+	rtl_cache_sync_wback(priv, (unsigned long)txmic, 8, PCI_DMA_TODEVICE);
+
+    _DEBUG_INFO("--txmic=%X %X %X %X %X %X %X %X\n",
+    txmic[0], txmic[1], txmic[2], txmic[3],
+    txmic[4], txmic[5], txmic[6], txmic[7]);
+
+   	if (pn48->val48 == 0xffffffffffffULL)
+		pn48->val48 = 0;
+	else
+		pn48->val48++;
+}
+
+
+void aesccmp_encrypt(struct rtl8192cd_priv *priv, unsigned char *pwlhdr,
+				unsigned char *frag1,
+				unsigned char *frag2, unsigned int frag2_len,
+				unsigned char *frag3)
+{
+	unsigned char *da;
+	unsigned char *ttkey = NULL;
+	union PN48 	  *pn48 = NULL;
+	unsigned int	keyid = 0;
+	struct stat_info	*pstat = NULL;
+
+	da = get_da(pwlhdr);
+
+	if (OPMODE & WIFI_AP_STATE)
+	{
+#if defined(WDS) || defined(CONFIG_RTK_MESH) || defined(A4_STA)
+		unsigned int to_fr_ds = (GetToDs(pwlhdr) << 1) | GetFrDs(pwlhdr);
+		if (to_fr_ds == 3)
+			da = GetAddr1Ptr(pwlhdr);
+#endif
+
+		if (IS_MCAST(da))
+		{
+			ttkey = GET_GROUP_ENCRYP_KEY;
+			pn48 = GET_GROUP_ENCRYP_PN;
+			keyid = 1;
+		}
+		else
+		{
+			pstat = get_stainfo(priv, da);
+			if (pstat == NULL) {
+				DEBUG_ERR("tx aes pstat == NULL\n");
+				return;
+			}
+			ttkey = GET_UNICAST_ENCRYP_KEY;
+			pn48 = GET_UNICAST_ENCRYP_PN;
+			keyid = 0;
+		}
+	}
+#ifdef CLIENT_MODE
+	else if (OPMODE & WIFI_STATION_STATE)
+	{
+		pstat = get_stainfo(priv, BSSID);
+		if (pstat == NULL) {
+			DEBUG_ERR("tx aes pstat == NULL\n");
+			return;
+		}
+		ttkey = GET_UNICAST_ENCRYP_KEY;
+		pn48 = GET_UNICAST_ENCRYP_PN;
+		keyid = 0;
+	}
+	else if (OPMODE & WIFI_ADHOC_STATE)
+	{
+		ttkey = GET_GROUP_ENCRYP_KEY;
+		pn48 = GET_GROUP_ENCRYP_PN;
+		keyid = 0;
+	}
+#endif
+
+	if ((ttkey == NULL) || (pn48 == NULL)) {
+		DEBUG_ERR("no encrypt key for AES due to ttkey=NULL or pn48=NULL\n");
+		return;
+	}
+
+	aes_tx(priv, ttkey, keyid, pn48, pwlhdr, frag1, frag2, frag2_len, frag3);
+}
+
+
+static void aes_rx(UINT8 *ttkey, UINT8 qc_exists, UINT8 a4_exists,
+				UINT8 *pframe, UINT32 hdrlen, UINT32 plen)
+{
+	UINT32	i, j, payload_remainder,
+			num_blocks, payload_index;
+
+	UINT8 pn_vector[6];
+	UINT8 mic_iv[16];
+    UINT8 mic_header1[16];
+    UINT8 mic_header2[16];
+    UINT8 ctr_preload[16];
+
+    UINT8 chain_buffer[16];
+    UINT8 aes_out[16];
+    UINT8 padded_buffer[16];
+    UINT8 mic[8];
+
+	memset((void *)mic_iv, 0, 16);
+	memset((void *)mic_header1, 0, 16);
+	memset((void *)mic_header2, 0, 16);
+	memset((void *)ctr_preload, 0, 16);
+	memset((void *)chain_buffer, 0, 16);
+	memset((void *)aes_out, 0, 16);
+	memset((void *)mic, 0, 8);
+
+	num_blocks = plen / 16; //(plen including llc, payload_length and mic )
+
+	payload_remainder = plen % 16;
+
+	pn_vector[0]  = pframe[hdrlen];
+	pn_vector[1]  = pframe[hdrlen+1];
+	pn_vector[2]  = pframe[hdrlen+4];
+	pn_vector[3]  = pframe[hdrlen+5];
+	pn_vector[4]  = pframe[hdrlen+6];
+	pn_vector[5]  = pframe[hdrlen+7];
+
+	// now, decrypt pframe with hdrlen offset and plen long
+
+	payload_index = hdrlen + 8; // 8 is for extiv
+	for (i=0; i< num_blocks; i++)
+    {
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                pframe,
+                                pn_vector,
+                                i+1
+                            );
+
+        aes128k128d(ttkey, ctr_preload, aes_out);
+        bitwise_xor(aes_out, &pframe[payload_index], chain_buffer);
+
+        for (j=0; j<16;j++) pframe[payload_index++] = chain_buffer[j];
+    }
+
+    if (payload_remainder > 0)          /* If there is a short final block, then pad it,*/
+    {                                   /* encrypt it and copy the unpadded part back   */
+        construct_ctr_preload(
+                                ctr_preload,
+                                a4_exists,
+                                qc_exists,
+                                pframe,
+                                pn_vector,
+                                num_blocks+1
+                            );
+
+        for (j = 0; j < 16; j++) padded_buffer[j] = 0x00;
+        for (j = 0; j < payload_remainder; j++)
+        {
+            padded_buffer[j] = pframe[payload_index+j];
+        }
+        aes128k128d(ttkey, ctr_preload, aes_out);
+        bitwise_xor(aes_out, padded_buffer, chain_buffer);
+        for (j=0; j<payload_remainder;j++) pframe[payload_index++] = chain_buffer[j];
+    }
+}
+
+
+unsigned int aesccmp_decrypt(struct rtl8192cd_priv *priv, struct rx_frinfo *pfrinfo)
+{
+	UINT8	*ttkey = NULL;
+	UINT32	hdrlen, keylen = 0;
+	UINT8	a4_exists;
+	UINT8	qc_exists;
+	UINT8	*sa	= pfrinfo->sa;
+	UINT8	*pframe = get_pframe(pfrinfo);
+	UINT8	to_fr_ds = pfrinfo->to_fr_ds;
+	struct stat_info	*pstat = NULL;
+
+	if (to_fr_ds != 0x03) {
+		hdrlen = WLAN_HDR_A3_LEN;
+		a4_exists = 0;
+	}
+	else {
+		hdrlen = WLAN_HDR_A4_LEN;
+		a4_exists = 1;
+	}
+
+	if (is_qos_data(pframe)) {
+		qc_exists = 1;
+		hdrlen += 2;
+	}
+	else
+		qc_exists = 0;
+
+	if (OPMODE & WIFI_AP_STATE)
+	{
+#if defined(WDS) || defined(CONFIG_RTK_MESH)
+		if (to_fr_ds == 3)
+			pstat = get_stainfo (priv, GetAddr2Ptr(pframe));
+		else
+#endif
+
+#ifdef A4_STA
+		if (to_fr_ds == 3)
+			pstat = get_stainfo (priv, GetAddr2Ptr(pframe));
+		else			
+#endif
+		pstat = get_stainfo (priv, sa);
+
+		{
+		if (pstat == NULL)
+		{
+			DEBUG_ERR("AES Rx fails! sa=%02X%02X%02X%02X%02X%02X\n",
+				sa[0], sa[1], sa[2], sa[3], sa[4], sa[5]);
+			return FALSE;
+		}
+
+		keylen = GET_UNICAST_ENCRYP_KEYLEN;
+		ttkey  = GET_UNICAST_ENCRYP_KEY;
+		}
+	}
+#ifdef CLIENT_MODE
+	else if (OPMODE & WIFI_STATION_STATE)
+	{
+		if (IS_MCAST(pfrinfo->da))
+		{
+			keylen = GET_GROUP_ENCRYP_KEYLEN;
+			ttkey  = GET_GROUP_ENCRYP_KEY;
+		}
+		else
+		{
+			pstat = get_stainfo(priv, BSSID);
+			if (pstat == NULL) {
+				DEBUG_ERR("rx aes pstat == NULL\n");
+				return FALSE;
+			}
+			keylen = GET_UNICAST_ENCRYP_KEYLEN;
+			ttkey  = GET_UNICAST_ENCRYP_KEY;
+		}
+	}
+	else if (OPMODE & WIFI_ADHOC_STATE)
+	{
+		keylen = GET_GROUP_ENCRYP_KEYLEN;
+		ttkey  = GET_GROUP_ENCRYP_KEY;
+	}
+#endif
+
+	if (keylen == 0) {
+		DEBUG_ERR("no descrypt key for AES due to keylen=0\n");
+		return FALSE;
+	}
+
+	aes_rx(ttkey, qc_exists, a4_exists, pframe, hdrlen, pfrinfo->pktlen-hdrlen-8);
+	return TRUE;
+}
+