git.oblomov.eu Git - linux-2.6/blob - drivers/staging/rt3070/common/rtmp_tkip.c

   1 /*
   2  *************************************************************************
   3  * Ralink Tech Inc.
   4  * 5F., No.36, Taiyuan St., Jhubei City,
   5  * Hsinchu County 302,
   6  * Taiwan, R.O.C.
   7  *
   8  * (c) Copyright 2002-2007, Ralink Technology, Inc.
   9  *
  10  * This program is free software; you can redistribute it and/or modify  *
  11  * it under the terms of the GNU General Public License as published by  *
  12  * the Free Software Foundation; either version 2 of the License, or     *
  13  * (at your option) any later version.                                   *
  14  *                                                                       *
  15  * This program is distributed in the hope that it will be useful,       *
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  18  * GNU General Public License for more details.                          *
  19  *                                                                       *
  20  * You should have received a copy of the GNU General Public License     *
  21  * along with this program; if not, write to the                         *
  22  * Free Software Foundation, Inc.,                                       *
  23  * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  24  *                                                                       *
  25  *************************************************************************
  26
  27         Module Name:
  28         rtmp_tkip.c
  29
  30         Abstract:
  31
  32         Revision History:
  33         Who                     When                    What
  34         --------        ----------              ----------------------------------------------
  35         Paul Wu         02-25-02                Initial
  36 */
  37
  38 #include        "../rt_config.h"
  39
  40 // Rotation functions on 32 bit values
  41 #define ROL32( A, n ) \
  42         ( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
  43 #define ROR32( A, n ) ROL32( (A), 32-(n) )
  44
  45 UINT Tkip_Sbox_Lower[256] =
  46 {
  47         0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
  48         0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
  49         0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
  50         0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
  51         0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
  52         0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
  53         0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
  54         0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
  55         0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
  56         0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
  57         0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
  58         0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
  59         0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
  60         0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
  61         0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
  62         0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
  63         0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
  64         0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
  65         0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
  66         0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
  67         0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
  68         0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
  69         0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
  70         0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
  71         0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
  72         0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
  73         0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
  74         0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
  75         0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
  76         0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
  77         0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
  78         0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
  79 };
  80
  81 UINT Tkip_Sbox_Upper[256] =
  82 {
  83         0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
  84         0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
  85         0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
  86         0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
  87         0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
  88         0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
  89         0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
  90         0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
  91         0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
  92         0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
  93         0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
  94         0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
  95         0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
  96         0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
  97         0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
  98         0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
  99         0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
 100         0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
 101         0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
 102         0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
 103         0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
 104         0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
 105         0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
 106         0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
 107         0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
 108         0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
 109         0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
 110         0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
 111         0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
 112         0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
 113         0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
 114         0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
 115 };
 116
 117 /*****************************/
 118 /******** SBOX Table *********/
 119 /*****************************/
 120
 121 UCHAR SboxTable[256] =
 122 {
 123         0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
 124         0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
 125         0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
 126         0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
 127         0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
 128         0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
 129         0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
 130         0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
 131         0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
 132         0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
 133         0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
 134         0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
 135         0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
 136         0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
 137         0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
 138         0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
 139         0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
 140         0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
 141         0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
 142         0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
 143         0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
 144         0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
 145         0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
 146         0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
 147         0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
 148         0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
 149         0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
 150         0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
 151         0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
 152         0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
 153         0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
 154         0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
 155 };
 156
 157 VOID xor_32(
 158         IN  PUCHAR              a,
 159         IN  PUCHAR              b,
 160         OUT PUCHAR              out);
 161
 162 VOID xor_128(
 163         IN  PUCHAR              a,
 164         IN  PUCHAR              b,
 165         OUT PUCHAR              out);
 166
 167 VOID next_key(
 168         IN  PUCHAR              key,
 169         IN  INT                 round);
 170
 171 VOID byte_sub(
 172         IN  PUCHAR              in,
 173         OUT PUCHAR              out);
 174
 175 VOID shift_row(
 176         IN  PUCHAR              in,
 177         OUT PUCHAR              out);
 178
 179 VOID mix_column(
 180         IN  PUCHAR              in,
 181         OUT PUCHAR              out);
 182
 183 UCHAR RTMPCkipSbox(
 184         IN  UCHAR               a);
 185 //
 186 // Expanded IV for TKIP function.
 187 //
 188 typedef struct  PACKED _IV_CONTROL_
 189 {
 190         union PACKED
 191         {
 192                 struct PACKED
 193                 {
 194                         UCHAR           rc0;
 195                         UCHAR           rc1;
 196                         UCHAR           rc2;
 197
 198                         union PACKED
 199                         {
 200                                 struct PACKED
 201                                 {
 202                                         UCHAR   Rsvd:5;
 203                                         UCHAR   ExtIV:1;
 204                                         UCHAR   KeyID:2;
 205                                 }       field;
 206                                 UCHAR           Byte;
 207                         }       CONTROL;
 208                 }       field;
 209
 210                 ULONG   word;
 211         }       IV16;
 212
 213         ULONG   IV32;
 214 }       TKIP_IV, *PTKIP_IV;
 215
 216
 217 /*
 218         ========================================================================
 219
 220         Routine Description:
 221                 Convert from UCHAR[] to ULONG in a portable way
 222
 223         Arguments:
 224       pMICKey           pointer to MIC Key
 225
 226         Return Value:
 227                 None
 228
 229         Note:
 230
 231         ========================================================================
 232 */
 233 ULONG   RTMPTkipGetUInt32(
 234         IN      PUCHAR  pMICKey)
 235 {
 236         ULONG   res = 0;
 237         INT             i;
 238
 239         for (i = 0; i < 4; i++)
 240         {
 241                 res |= (*pMICKey++) << (8 * i);
 242         }
 243
 244         return res;
 245 }
 246
 247 /*
 248         ========================================================================
 249
 250         Routine Description:
 251                 Convert from ULONG to UCHAR[] in a portable way
 252
 253         Arguments:
 254       pDst                      pointer to destination for convert ULONG to UCHAR[]
 255       val                       the value for convert
 256
 257         Return Value:
 258                 None
 259
 260         IRQL = DISPATCH_LEVEL
 261
 262         Note:
 263
 264         ========================================================================
 265 */
 266 VOID    RTMPTkipPutUInt32(
 267         IN OUT  PUCHAR          pDst,
 268         IN              ULONG           val)
 269 {
 270         INT i;
 271
 272         for(i = 0; i < 4; i++)
 273         {
 274                 *pDst++ = (UCHAR) (val & 0xff);
 275                 val >>= 8;
 276         }
 277 }
 278
 279 /*
 280         ========================================================================
 281
 282         Routine Description:
 283                 Set the MIC Key.
 284
 285         Arguments:
 286       pAd               Pointer to our adapter
 287       pMICKey           pointer to MIC Key
 288
 289         Return Value:
 290                 None
 291
 292         IRQL = DISPATCH_LEVEL
 293
 294         Note:
 295
 296         ========================================================================
 297 */
 298 VOID RTMPTkipSetMICKey(
 299         IN      PTKIP_KEY_INFO  pTkip,
 300         IN      PUCHAR                  pMICKey)
 301 {
 302         // Set the key
 303         pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
 304         pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
 305         // and reset the message
 306         pTkip->L = pTkip->K0;
 307         pTkip->R = pTkip->K1;
 308         pTkip->nBytesInM = 0;
 309         pTkip->M = 0;
 310 }
 311
 312 /*
 313         ========================================================================
 314
 315         Routine Description:
 316                 Calculate the MIC Value.
 317
 318         Arguments:
 319       pAd               Pointer to our adapter
 320       uChar                     Append this uChar
 321
 322         Return Value:
 323                 None
 324
 325         IRQL = DISPATCH_LEVEL
 326
 327         Note:
 328
 329         ========================================================================
 330 */
 331 VOID    RTMPTkipAppendByte(
 332         IN      PTKIP_KEY_INFO  pTkip,
 333         IN      UCHAR                   uChar)
 334 {
 335         // Append the byte to our word-sized buffer
 336         pTkip->M |= (uChar << (8* pTkip->nBytesInM));
 337         pTkip->nBytesInM++;
 338         // Process the word if it is full.
 339         if( pTkip->nBytesInM >= 4 )
 340         {
 341                 pTkip->L ^= pTkip->M;
 342                 pTkip->R ^= ROL32( pTkip->L, 17 );
 343                 pTkip->L += pTkip->R;
 344                 pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
 345                 pTkip->L += pTkip->R;
 346                 pTkip->R ^= ROL32( pTkip->L, 3 );
 347                 pTkip->L += pTkip->R;
 348                 pTkip->R ^= ROR32( pTkip->L, 2 );
 349                 pTkip->L += pTkip->R;
 350                 // Clear the buffer
 351                 pTkip->M = 0;
 352                 pTkip->nBytesInM = 0;
 353         }
 354 }
 355
 356 /*
 357         ========================================================================
 358
 359         Routine Description:
 360                 Calculate the MIC Value.
 361
 362         Arguments:
 363       pAd               Pointer to our adapter
 364       pSrc                      Pointer to source data for Calculate MIC Value
 365       Len                       Indicate the length of the source data
 366
 367         Return Value:
 368                 None
 369
 370         IRQL = DISPATCH_LEVEL
 371
 372         Note:
 373
 374         ========================================================================
 375 */
 376 VOID    RTMPTkipAppend(
 377         IN      PTKIP_KEY_INFO  pTkip,
 378         IN      PUCHAR                  pSrc,
 379         IN      UINT                    nBytes)
 380 {
 381         // This is simple
 382         while(nBytes > 0)
 383         {
 384                 RTMPTkipAppendByte(pTkip, *pSrc++);
 385                 nBytes--;
 386         }
 387 }
 388
 389 /*
 390         ========================================================================
 391
 392         Routine Description:
 393                 Get the MIC Value.
 394
 395         Arguments:
 396       pAd               Pointer to our adapter
 397
 398         Return Value:
 399                 None
 400
 401         IRQL = DISPATCH_LEVEL
 402
 403         Note:
 404                 the MIC Value is store in pAd->PrivateInfo.MIC
 405         ========================================================================
 406 */
 407 VOID    RTMPTkipGetMIC(
 408         IN      PTKIP_KEY_INFO  pTkip)
 409 {
 410         // Append the minimum padding
 411         RTMPTkipAppendByte(pTkip, 0x5a );
 412         RTMPTkipAppendByte(pTkip, 0 );
 413         RTMPTkipAppendByte(pTkip, 0 );
 414         RTMPTkipAppendByte(pTkip, 0 );
 415         RTMPTkipAppendByte(pTkip, 0 );
 416         // and then zeroes until the length is a multiple of 4
 417         while( pTkip->nBytesInM != 0 )
 418         {
 419                 RTMPTkipAppendByte(pTkip, 0 );
 420         }
 421         // The appendByte function has already computed the result.
 422         RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
 423         RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
 424 }
 425
 426 /*
 427         ========================================================================
 428
 429         Routine Description:
 430                 Init Tkip function.
 431
 432         Arguments:
 433       pAd               Pointer to our adapter
 434                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 435                 KeyId           TK Key ID
 436                 pTA                     Pointer to transmitter address
 437                 pMICKey         pointer to MIC Key
 438
 439         Return Value:
 440                 None
 441
 442         IRQL = DISPATCH_LEVEL
 443
 444         Note:
 445
 446         ========================================================================
 447 */
 448 VOID    RTMPInitTkipEngine(
 449         IN      PRTMP_ADAPTER   pAd,
 450         IN      PUCHAR                  pKey,
 451         IN      UCHAR                   KeyId,
 452         IN      PUCHAR                  pTA,
 453         IN      PUCHAR                  pMICKey,
 454         IN      PUCHAR                  pTSC,
 455         OUT     PULONG                  pIV16,
 456         OUT     PULONG                  pIV32)
 457 {
 458         TKIP_IV tkipIv;
 459
 460         // Prepare 8 bytes TKIP encapsulation for MPDU
 461         NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
 462         tkipIv.IV16.field.rc0 = *(pTSC + 1);
 463         tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
 464         tkipIv.IV16.field.rc2 = *pTSC;
 465         tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
 466         tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
 467         NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
 468
 469         *pIV16 = tkipIv.IV16.word;
 470         *pIV32 = tkipIv.IV32;
 471 }
 472
 473 /*
 474         ========================================================================
 475
 476         Routine Description:
 477                 Init MIC Value calculation function which include set MIC key &
 478                 calculate first 16 bytes (DA + SA + priority +  0)
 479
 480         Arguments:
 481       pAd               Pointer to our adapter
 482                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 483                 pDA                     Pointer to DA address
 484                 pSA                     Pointer to SA address
 485                 pMICKey         pointer to MIC Key
 486
 487         Return Value:
 488                 None
 489
 490         Note:
 491
 492         ========================================================================
 493 */
 494 VOID    RTMPInitMICEngine(
 495         IN      PRTMP_ADAPTER   pAd,
 496         IN      PUCHAR                  pKey,
 497         IN      PUCHAR                  pDA,
 498         IN      PUCHAR                  pSA,
 499         IN  UCHAR           UserPriority,
 500         IN      PUCHAR                  pMICKey)
 501 {
 502         ULONG Priority = UserPriority;
 503
 504         // Init MIC value calculation
 505         RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
 506         // DA
 507         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
 508         // SA
 509         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
 510         // Priority + 3 bytes of 0
 511         RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
 512 }
 513
 514 /*
 515         ========================================================================
 516
 517         Routine Description:
 518                 Compare MIC value of received MSDU
 519
 520         Arguments:
 521                 pAd     Pointer to our adapter
 522                 pSrc        Pointer to the received Plain text data
 523                 pDA                     Pointer to DA address
 524                 pSA                     Pointer to SA address
 525                 pMICKey         pointer to MIC Key
 526                 Len         the length of the received plain text data exclude MIC value
 527
 528         Return Value:
 529                 TRUE        MIC value matched
 530                 FALSE       MIC value mismatched
 531
 532         IRQL = DISPATCH_LEVEL
 533
 534         Note:
 535
 536         ========================================================================
 537 */
 538 BOOLEAN RTMPTkipCompareMICValue(
 539         IN      PRTMP_ADAPTER   pAd,
 540         IN      PUCHAR                  pSrc,
 541         IN      PUCHAR                  pDA,
 542         IN      PUCHAR                  pSA,
 543         IN      PUCHAR                  pMICKey,
 544         IN      UCHAR                   UserPriority,
 545         IN      UINT                    Len)
 546 {
 547         UCHAR   OldMic[8];
 548         ULONG   Priority = UserPriority;
 549
 550         // Init MIC value calculation
 551         RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
 552         // DA
 553         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
 554         // SA
 555         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
 556         // Priority + 3 bytes of 0
 557         RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
 558
 559         // Calculate MIC value from plain text data
 560         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
 561
 562         // Get MIC valude from received frame
 563         NdisMoveMemory(OldMic, pSrc + Len, 8);
 564
 565         // Get MIC value from decrypted plain data
 566         RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
 567
 568         // Move MIC value from MSDU, this steps should move to data path.
 569         // Since the MIC value might cross MPDUs.
 570         if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
 571         {
 572                 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
 573
 574
 575                 return (FALSE);
 576         }
 577         return (TRUE);
 578 }
 579
 580 /*
 581         ========================================================================
 582
 583         Routine Description:
 584                 Compare MIC value of received MSDU
 585
 586         Arguments:
 587                 pAd     Pointer to our adapter
 588                 pLLC            LLC header
 589                 pSrc        Pointer to the received Plain text data
 590                 pDA                     Pointer to DA address
 591                 pSA                     Pointer to SA address
 592                 pMICKey         pointer to MIC Key
 593                 Len         the length of the received plain text data exclude MIC value
 594
 595         Return Value:
 596                 TRUE        MIC value matched
 597                 FALSE       MIC value mismatched
 598
 599         IRQL = DISPATCH_LEVEL
 600
 601         Note:
 602
 603         ========================================================================
 604 */
 605 BOOLEAN RTMPTkipCompareMICValueWithLLC(
 606         IN      PRTMP_ADAPTER   pAd,
 607         IN      PUCHAR                  pLLC,
 608         IN      PUCHAR                  pSrc,
 609         IN      PUCHAR                  pDA,
 610         IN      PUCHAR                  pSA,
 611         IN      PUCHAR                  pMICKey,
 612         IN      UINT                    Len)
 613 {
 614         UCHAR   OldMic[8];
 615         ULONG   Priority = 0;
 616
 617         // Init MIC value calculation
 618         RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
 619         // DA
 620         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
 621         // SA
 622         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
 623         // Priority + 3 bytes of 0
 624         RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
 625
 626         // Start with LLC header
 627         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
 628
 629         // Calculate MIC value from plain text data
 630         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
 631
 632         // Get MIC valude from received frame
 633         NdisMoveMemory(OldMic, pSrc + Len, 8);
 634
 635         // Get MIC value from decrypted plain data
 636         RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
 637
 638         // Move MIC value from MSDU, this steps should move to data path.
 639         // Since the MIC value might cross MPDUs.
 640         if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
 641         {
 642                 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n"));  //MIC error.
 643
 644
 645                 return (FALSE);
 646         }
 647         return (TRUE);
 648 }
 649 /*
 650         ========================================================================
 651
 652         Routine Description:
 653                 Copy frame from waiting queue into relative ring buffer and set
 654         appropriate ASIC register to kick hardware transmit function
 655
 656         Arguments:
 657                 pAd             Pointer to our adapter
 658                 PNDIS_PACKET    Pointer to Ndis Packet for MIC calculation
 659                 pEncap                  Pointer to LLC encap data
 660                 LenEncap                Total encap length, might be 0 which indicates no encap
 661
 662         Return Value:
 663                 None
 664
 665         IRQL = DISPATCH_LEVEL
 666
 667         Note:
 668
 669         ========================================================================
 670 */
 671 VOID    RTMPCalculateMICValue(
 672         IN      PRTMP_ADAPTER   pAd,
 673         IN      PNDIS_PACKET    pPacket,
 674         IN      PUCHAR                  pEncap,
 675         IN      PCIPHER_KEY             pKey,
 676         IN      UCHAR                   apidx)
 677 {
 678         PACKET_INFO             PacketInfo;
 679         PUCHAR                  pSrcBufVA;
 680         UINT                    SrcBufLen;
 681         PUCHAR                  pSrc;
 682     UCHAR           UserPriority;
 683         UCHAR                   vlan_offset = 0;
 684
 685         RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
 686
 687         UserPriority = RTMP_GET_PACKET_UP(pPacket);
 688         pSrc = pSrcBufVA;
 689
 690         // determine if this is a vlan packet
 691         if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
 692                 vlan_offset = 4;
 693         {
 694                 RTMPInitMICEngine(
 695                         pAd,
 696                         pKey->Key,
 697                         pSrc,
 698                         pSrc + 6,
 699                         UserPriority,
 700                         pKey->TxMic);
 701         }
 702
 703
 704         if (pEncap != NULL)
 705         {
 706                 // LLC encapsulation
 707                 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
 708                 // Protocol Type
 709                 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
 710         }
 711         SrcBufLen -= (14 + vlan_offset);
 712         pSrc += (14 + vlan_offset);
 713         do
 714         {
 715                 if (SrcBufLen > 0)
 716                 {
 717                         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
 718                 }
 719
 720                 break;  // No need handle next packet
 721
 722         }       while (TRUE);           // End of copying payload
 723
 724         // Compute the final MIC Value
 725         RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
 726 }
 727
 728
 729 /************************************************************/
 730 /* tkip_sbox()                                                                                                                          */
 731 /* Returns a 16 bit value from a 64K entry table. The Table */
 732 /* is synthesized from two 256 entry byte wide tables.          */
 733 /************************************************************/
 734
 735 UINT tkip_sbox(UINT index)
 736 {
 737         UINT index_low;
 738         UINT index_high;
 739         UINT left, right;
 740
 741         index_low = (index % 256);
 742         index_high = ((index >> 8) % 256);
 743
 744         left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
 745         right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
 746
 747         return (left ^ right);
 748 }
 749
 750 UINT rotr1(UINT a)
 751 {
 752         unsigned int b;
 753
 754         if ((a & 0x01) == 0x01)
 755         {
 756                 b = (a >> 1) | 0x8000;
 757         }
 758         else
 759         {
 760                 b = (a >> 1) & 0x7fff;
 761         }
 762         b = b % 65536;
 763         return b;
 764 }
 765
 766 VOID RTMPTkipMixKey(
 767         UCHAR *key,
 768         UCHAR *ta,
 769         ULONG pnl, /* Least significant 16 bits of PN */
 770         ULONG pnh, /* Most significant 32 bits of PN */
 771         UCHAR *rc4key,
 772         UINT *p1k)
 773 {
 774
 775         UINT tsc0;
 776         UINT tsc1;
 777         UINT tsc2;
 778
 779         UINT ppk0;
 780         UINT ppk1;
 781         UINT ppk2;
 782         UINT ppk3;
 783         UINT ppk4;
 784         UINT ppk5;
 785
 786         INT i;
 787         INT j;
 788
 789         tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
 790         tsc1 = (unsigned int)(pnh % 65536);
 791         tsc2 = (unsigned int)(pnl % 65536); /* lsb */
 792
 793         /* Phase 1, step 1 */
 794         p1k[0] = tsc1;
 795         p1k[1] = tsc0;
 796         p1k[2] = (UINT)(ta[0] + (ta[1]*256));
 797         p1k[3] = (UINT)(ta[2] + (ta[3]*256));
 798         p1k[4] = (UINT)(ta[4] + (ta[5]*256));
 799
 800         /* Phase 1, step 2 */
 801         for (i=0; i<8; i++)
 802         {
 803                 j = 2*(i & 1);
 804                 p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
 805                 p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
 806                 p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
 807                 p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
 808                 p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
 809                 p1k[4] = (p1k[4] + i) % 65536;
 810         }
 811
 812         /* Phase 2, Step 1 */
 813         ppk0 = p1k[0];
 814         ppk1 = p1k[1];
 815         ppk2 = p1k[2];
 816         ppk3 = p1k[3];
 817         ppk4 = p1k[4];
 818         ppk5 = (p1k[4] + tsc2) % 65536;
 819
 820         /* Phase2, Step 2 */
 821         ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
 822         ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
 823         ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
 824         ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
 825         ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
 826         ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
 827
 828         ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
 829         ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
 830         ppk2 = ppk2 + rotr1(ppk1);
 831         ppk3 = ppk3 + rotr1(ppk2);
 832         ppk4 = ppk4 + rotr1(ppk3);
 833         ppk5 = ppk5 + rotr1(ppk4);
 834
 835         /* Phase 2, Step 3 */
 836     /* Phase 2, Step 3 */
 837
 838         tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
 839         tsc1 = (unsigned int)(pnh % 65536);
 840         tsc2 = (unsigned int)(pnl % 65536); /* lsb */
 841
 842         rc4key[0] = (tsc2 >> 8) % 256;
 843         rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
 844         rc4key[2] = tsc2 % 256;
 845         rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
 846
 847         rc4key[4] = ppk0 % 256;
 848         rc4key[5] = (ppk0 >> 8) % 256;
 849
 850         rc4key[6] = ppk1 % 256;
 851         rc4key[7] = (ppk1 >> 8) % 256;
 852
 853         rc4key[8] = ppk2 % 256;
 854         rc4key[9] = (ppk2 >> 8) % 256;
 855
 856         rc4key[10] = ppk3 % 256;
 857         rc4key[11] = (ppk3 >> 8) % 256;
 858
 859         rc4key[12] = ppk4 % 256;
 860         rc4key[13] = (ppk4 >> 8) % 256;
 861
 862         rc4key[14] = ppk5 % 256;
 863         rc4key[15] = (ppk5 >> 8) % 256;
 864 }
 865
 866
 867 /************************************************/
 868 /* construct_mic_header1()                      */
 869 /* Builds the first MIC header block from       */
 870 /* header fields.                               */
 871 /************************************************/
 872
 873 void construct_mic_header1(
 874         unsigned char *mic_header1,
 875         int header_length,
 876         unsigned char *mpdu)
 877 {
 878         mic_header1[0] = (unsigned char)((header_length - 2) / 256);
 879         mic_header1[1] = (unsigned char)((header_length - 2) % 256);
 880         mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
 881         mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
 882         mic_header1[4] = mpdu[4];       /* A1 */
 883         mic_header1[5] = mpdu[5];
 884         mic_header1[6] = mpdu[6];
 885         mic_header1[7] = mpdu[7];
 886         mic_header1[8] = mpdu[8];
 887         mic_header1[9] = mpdu[9];
 888         mic_header1[10] = mpdu[10];     /* A2 */
 889         mic_header1[11] = mpdu[11];
 890         mic_header1[12] = mpdu[12];
 891         mic_header1[13] = mpdu[13];
 892         mic_header1[14] = mpdu[14];
 893         mic_header1[15] = mpdu[15];
 894 }
 895
 896 /************************************************/
 897 /* construct_mic_header2()                      */
 898 /* Builds the last MIC header block from        */
 899 /* header fields.                               */
 900 /************************************************/
 901
 902 void construct_mic_header2(
 903         unsigned char *mic_header2,
 904         unsigned char *mpdu,
 905         int a4_exists,
 906         int qc_exists)
 907 {
 908         int i;
 909
 910         for (i = 0; i<16; i++) mic_header2[i]=0x00;
 911
 912         mic_header2[0] = mpdu[16];    /* A3 */
 913         mic_header2[1] = mpdu[17];
 914         mic_header2[2] = mpdu[18];
 915         mic_header2[3] = mpdu[19];
 916         mic_header2[4] = mpdu[20];
 917         mic_header2[5] = mpdu[21];
 918
 919         // In Sequence Control field, mute sequence numer bits (12-bit)
 920         mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
 921         mic_header2[7] = 0x00; /* mpdu[23]; */
 922
 923         if ((!qc_exists) & a4_exists)
 924         {
 925                 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
 926
 927         }
 928
 929         if (qc_exists && (!a4_exists))
 930         {
 931                 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
 932                 mic_header2[9] = mpdu[25] & 0x00;
 933         }
 934
 935         if (qc_exists && a4_exists)
 936         {
 937                 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
 938
 939                 mic_header2[14] = mpdu[30] & 0x0f;
 940                 mic_header2[15] = mpdu[31] & 0x00;
 941         }
 942 }
 943
 944
 945 /************************************************/
 946 /* construct_mic_iv()                           */
 947 /* Builds the MIC IV from header fields and PN  */
 948 /************************************************/
 949
 950 void construct_mic_iv(
 951         unsigned char *mic_iv,
 952         int qc_exists,
 953         int a4_exists,
 954         unsigned char *mpdu,
 955         unsigned int payload_length,
 956         unsigned char *pn_vector)
 957 {
 958         int i;
 959
 960         mic_iv[0] = 0x59;
 961         if (qc_exists && a4_exists)
 962                 mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
 963         if (qc_exists && !a4_exists)
 964                 mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
 965         if (!qc_exists)
 966                 mic_iv[1] = 0x00;
 967         for (i = 2; i < 8; i++)
 968                 mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
 969 #ifdef CONSISTENT_PN_ORDER
 970                 for (i = 8; i < 14; i++)
 971                         mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
 972 #else
 973                 for (i = 8; i < 14; i++)
 974                         mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
 975 #endif
 976         i = (payload_length / 256);
 977         i = (payload_length % 256);
 978         mic_iv[14] = (unsigned char) (payload_length / 256);
 979         mic_iv[15] = (unsigned char) (payload_length % 256);
 980
 981 }
 982
 983
 984
 985 /************************************/
 986 /* bitwise_xor()                    */
 987 /* A 128 bit, bitwise exclusive or  */
 988 /************************************/
 989
 990 void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
 991 {
 992         int i;
 993         for (i=0; i<16; i++)
 994         {
 995                 out[i] = ina[i] ^ inb[i];
 996         }
 997 }
 998
 999
1000 void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
1001 {
1002         int round;
1003         int i;
1004         unsigned char intermediatea[16];
1005         unsigned char intermediateb[16];
1006         unsigned char round_key[16];
1007
1008         for(i=0; i<16; i++) round_key[i] = key[i];
1009
1010         for (round = 0; round < 11; round++)
1011         {
1012                 if (round == 0)
1013                 {
1014                         xor_128(round_key, data, ciphertext);
1015                         next_key(round_key, round);
1016                 }
1017                 else if (round == 10)
1018                 {
1019                         byte_sub(ciphertext, intermediatea);
1020                         shift_row(intermediatea, intermediateb);
1021                         xor_128(intermediateb, round_key, ciphertext);
1022                 }
1023                 else    /* 1 - 9 */
1024                 {
1025                         byte_sub(ciphertext, intermediatea);
1026                         shift_row(intermediatea, intermediateb);
1027                         mix_column(&intermediateb[0], &intermediatea[0]);
1028                         mix_column(&intermediateb[4], &intermediatea[4]);
1029                         mix_column(&intermediateb[8], &intermediatea[8]);
1030                         mix_column(&intermediateb[12], &intermediatea[12]);
1031                         xor_128(intermediatea, round_key, ciphertext);
1032                         next_key(round_key, round);
1033                 }
1034         }
1035
1036 }
1037
1038 void construct_ctr_preload(
1039         unsigned char *ctr_preload,
1040         int a4_exists,
1041         int qc_exists,
1042         unsigned char *mpdu,
1043         unsigned char *pn_vector,
1044         int c)
1045 {
1046
1047         int i = 0;
1048         for (i=0; i<16; i++) ctr_preload[i] = 0x00;
1049         i = 0;
1050
1051         ctr_preload[0] = 0x01;                                  /* flag */
1052         if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
1053         if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
1054
1055         for (i = 2; i < 8; i++)
1056                 ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1057 #ifdef CONSISTENT_PN_ORDER
1058           for (i = 8; i < 14; i++)
1059                         ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
1060 #else
1061           for (i = 8; i < 14; i++)
1062                         ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
1063 #endif
1064         ctr_preload[14] =  (unsigned char) (c / 256); // Ctr
1065         ctr_preload[15] =  (unsigned char) (c % 256);
1066
1067 }
1068
1069
1070 //
1071 // TRUE: Success!
1072 // FALSE: Decrypt Error!
1073 //
1074 BOOLEAN RTMPSoftDecryptTKIP(
1075         IN PRTMP_ADAPTER pAd,
1076         IN PUCHAR       pData,
1077         IN ULONG        DataByteCnt,
1078         IN UCHAR    UserPriority,
1079         IN PCIPHER_KEY  pWpaKey)
1080 {
1081         UCHAR                   KeyID;
1082         UINT                    HeaderLen;
1083     UCHAR                       fc0;
1084         UCHAR                   fc1;
1085         USHORT                  fc;
1086         UINT                    frame_type;
1087         UINT                    frame_subtype;
1088     UINT                        from_ds;
1089     UINT                        to_ds;
1090         INT                             a4_exists;
1091         INT                             qc_exists;
1092         USHORT                  duration;
1093         USHORT                  seq_control;
1094         USHORT                  qos_control;
1095         UCHAR                   TA[MAC_ADDR_LEN];
1096         UCHAR                   DA[MAC_ADDR_LEN];
1097         UCHAR                   SA[MAC_ADDR_LEN];
1098         UCHAR                   RC4Key[16];
1099         UINT                    p1k[5]; //for mix_key;
1100         ULONG                   pnl;/* Least significant 16 bits of PN */
1101         ULONG                   pnh;/* Most significant 32 bits of PN */
1102         UINT                    num_blocks;
1103         UINT                    payload_remainder;
1104         ARCFOURCONTEXT  ArcFourContext;
1105         UINT                    crc32 = 0;
1106         UINT                    trailfcs = 0;
1107         UCHAR                   MIC[8];
1108         UCHAR                   TrailMIC[8];
1109
1110         fc0 = *pData;
1111         fc1 = *(pData + 1);
1112
1113         fc = *((PUSHORT)pData);
1114
1115         frame_type = ((fc0 >> 2) & 0x03);
1116         frame_subtype = ((fc0 >> 4) & 0x0f);
1117
1118     from_ds = (fc1 & 0x2) >> 1;
1119     to_ds = (fc1 & 0x1);
1120
1121     a4_exists = (from_ds & to_ds);
1122     qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
1123                   (frame_subtype == 0x09) ||   /* Likely to change.    */
1124                   (frame_subtype == 0x0a) ||
1125                   (frame_subtype == 0x0b)
1126                  );
1127
1128         HeaderLen = 24;
1129         if (a4_exists)
1130                 HeaderLen += 6;
1131
1132         KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1133         KeyID = KeyID >> 6;
1134
1135         if (pWpaKey[KeyID].KeyLen == 0)
1136         {
1137                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1138                 return FALSE;
1139         }
1140
1141         duration = *((PUSHORT)(pData+2));
1142
1143         seq_control = *((PUSHORT)(pData+22));
1144
1145         if (qc_exists)
1146         {
1147                 if (a4_exists)
1148                 {
1149                         qos_control = *((PUSHORT)(pData+30));
1150                 }
1151                 else
1152                 {
1153                         qos_control = *((PUSHORT)(pData+24));
1154                 }
1155         }
1156
1157         if (to_ds == 0 && from_ds == 1)
1158         {
1159                 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1160                 NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
1161                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
1162         }
1163         else if (to_ds == 0 && from_ds == 0 )
1164         {
1165                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1166                 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1167                 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1168         }
1169         else if (to_ds == 1 && from_ds == 0)
1170         {
1171                 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1172                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1173                 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1174         }
1175         else if (to_ds == 1 && from_ds == 1)
1176         {
1177                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1178                 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1179                 NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
1180         }
1181
1182         num_blocks = (DataByteCnt - 16) / 16;
1183         payload_remainder = (DataByteCnt - 16) % 16;
1184
1185         pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
1186         pnh = *((PULONG)(pData + HeaderLen + 4));
1187         pnh = cpu2le32(pnh);
1188         RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
1189
1190         ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
1191
1192         ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
1193         NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
1194         crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
1195         crc32 ^= 0xffffffff;             /* complement */
1196
1197     if(crc32 != cpu2le32(trailfcs))
1198         {
1199                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));       //ICV error.
1200
1201                 return (FALSE);
1202         }
1203
1204         NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
1205         RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
1206         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
1207         RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
1208         NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
1209
1210         if (!NdisEqualMemory(MIC, TrailMIC, 8))
1211         {
1212                 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));       //MIC error.
1213                 return (FALSE);
1214         }
1215
1216         return TRUE;
1217 }
1218
1219
1220
1221
1222 BOOLEAN RTMPSoftDecryptAES(
1223         IN PRTMP_ADAPTER pAd,
1224         IN PUCHAR       pData,
1225         IN ULONG        DataByteCnt,
1226         IN PCIPHER_KEY  pWpaKey)
1227 {
1228         UCHAR                   KeyID;
1229         UINT                    HeaderLen;
1230         UCHAR                   PN[6];
1231         UINT                    payload_len;
1232         UINT                    num_blocks;
1233         UINT                    payload_remainder;
1234         USHORT                  fc;
1235         UCHAR                   fc0;
1236         UCHAR                   fc1;
1237         UINT                    frame_type;
1238         UINT                    frame_subtype;
1239         UINT                    from_ds;
1240         UINT                    to_ds;
1241         INT                             a4_exists;
1242         INT                             qc_exists;
1243         UCHAR                   aes_out[16];
1244         int                     payload_index;
1245         UINT                    i;
1246         UCHAR                   ctr_preload[16];
1247         UCHAR                   chain_buffer[16];
1248         UCHAR                   padded_buffer[16];
1249         UCHAR                   mic_iv[16];
1250         UCHAR                   mic_header1[16];
1251         UCHAR                   mic_header2[16];
1252         UCHAR                   MIC[8];
1253         UCHAR                   TrailMIC[8];
1254
1255         fc0 = *pData;
1256         fc1 = *(pData + 1);
1257
1258         fc = *((PUSHORT)pData);
1259
1260         frame_type = ((fc0 >> 2) & 0x03);
1261         frame_subtype = ((fc0 >> 4) & 0x0f);
1262
1263         from_ds = (fc1 & 0x2) >> 1;
1264         to_ds = (fc1 & 0x1);
1265
1266         a4_exists = (from_ds & to_ds);
1267         qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
1268                                   (frame_subtype == 0x09) ||   /* Likely to change.    */
1269                                   (frame_subtype == 0x0a) ||
1270                                   (frame_subtype == 0x0b)
1271                                  );
1272
1273         HeaderLen = 24;
1274         if (a4_exists)
1275                 HeaderLen += 6;
1276
1277         KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1278         KeyID = KeyID >> 6;
1279
1280         if (pWpaKey[KeyID].KeyLen == 0)
1281         {
1282                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1283                 return FALSE;
1284         }
1285
1286         PN[0] = *(pData+ HeaderLen);
1287         PN[1] = *(pData+ HeaderLen + 1);
1288         PN[2] = *(pData+ HeaderLen + 4);
1289         PN[3] = *(pData+ HeaderLen + 5);
1290         PN[4] = *(pData+ HeaderLen + 6);
1291         PN[5] = *(pData+ HeaderLen + 7);
1292
1293         payload_len = DataByteCnt - HeaderLen - 8 - 8;  // 8 bytes for CCMP header , 8 bytes for MIC
1294         payload_remainder = (payload_len) % 16;
1295         num_blocks = (payload_len) / 16;
1296
1297
1298
1299         // Find start of payload
1300         payload_index = HeaderLen + 8; //IV+EIV
1301
1302         for (i=0; i< num_blocks; i++)
1303         {
1304                 construct_ctr_preload(ctr_preload,
1305                                                                 a4_exists,
1306                                                                 qc_exists,
1307                                                                 pData,
1308                                                                 PN,
1309                                                                 i+1 );
1310
1311                 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1312
1313                 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1314                 NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
1315                 payload_index += 16;
1316         }
1317
1318         //
1319         // If there is a short final block, then pad it
1320         // encrypt it and copy the unpadded part back
1321         //
1322         if (payload_remainder > 0)
1323         {
1324                 construct_ctr_preload(ctr_preload,
1325                                                                 a4_exists,
1326                                                                 qc_exists,
1327                                                                 pData,
1328                                                                 PN,
1329                                                                 num_blocks + 1);
1330
1331                 NdisZeroMemory(padded_buffer, 16);
1332                 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1333
1334                 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1335
1336                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1337                 NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
1338                 payload_index += payload_remainder;
1339         }
1340
1341         //
1342         // Descrypt the MIC
1343         //
1344         construct_ctr_preload(ctr_preload,
1345                                                         a4_exists,
1346                                                         qc_exists,
1347                                                         pData,
1348                                                         PN,
1349                                                         0);
1350         NdisZeroMemory(padded_buffer, 16);
1351         NdisMoveMemory(padded_buffer, pData + payload_index, 8);
1352
1353         aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1354
1355         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1356
1357         NdisMoveMemory(TrailMIC, chain_buffer, 8);
1358
1359         //
1360         // Calculate MIC
1361         //
1362
1363         //Force the protected frame bit on
1364         *(pData + 1) = *(pData + 1) | 0x40;
1365
1366         // Find start of payload
1367         // Because the CCMP header has been removed
1368         payload_index = HeaderLen;
1369
1370         construct_mic_iv(
1371                                         mic_iv,
1372                                         qc_exists,
1373                                         a4_exists,
1374                                         pData,
1375                                         payload_len,
1376                                         PN);
1377
1378         construct_mic_header1(
1379                                                 mic_header1,
1380                                                 HeaderLen,
1381                                                 pData);
1382
1383         construct_mic_header2(
1384                                                 mic_header2,
1385                                                 pData,
1386                                                 a4_exists,
1387                                                 qc_exists);
1388
1389         aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
1390         bitwise_xor(aes_out, mic_header1, chain_buffer);
1391         aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1392         bitwise_xor(aes_out, mic_header2, chain_buffer);
1393         aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1394
1395         // iterate through each 16 byte payload block
1396         for (i = 0; i < num_blocks; i++)
1397         {
1398                 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1399                 payload_index += 16;
1400                 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1401         }
1402
1403         // Add on the final payload block if it needs padding
1404         if (payload_remainder > 0)
1405         {
1406                 NdisZeroMemory(padded_buffer, 16);
1407                 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1408
1409                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1410                 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1411         }
1412
1413         // aes_out contains padded mic, discard most significant
1414         // 8 bytes to generate 64 bit MIC
1415         for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
1416
1417         if (!NdisEqualMemory(MIC, TrailMIC, 8))
1418         {
1419                 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));         //MIC error.
1420                 return FALSE;
1421         }
1422
1423         return TRUE;
1424 }
1425
1426 /****************************************/
1427 /* aes128k128d()                        */
1428 /* Performs a 128 bit AES encrypt with  */
1429 /* 128 bit data.                        */
1430 /****************************************/
1431 VOID xor_128(
1432         IN  PUCHAR  a,
1433         IN  PUCHAR  b,
1434         OUT PUCHAR  out)
1435 {
1436         INT i;
1437
1438         for (i=0;i<16; i++)
1439         {
1440                 out[i] = a[i] ^ b[i];
1441         }
1442 }
1443
1444 VOID next_key(
1445         IN  PUCHAR  key,
1446         IN  INT     round)
1447 {
1448         UCHAR       rcon;
1449         UCHAR       sbox_key[4];
1450         UCHAR       rcon_table[12] =
1451         {
1452                 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
1453                 0x1b, 0x36, 0x36, 0x36
1454         };
1455
1456         sbox_key[0] = RTMPCkipSbox(key[13]);
1457         sbox_key[1] = RTMPCkipSbox(key[14]);
1458         sbox_key[2] = RTMPCkipSbox(key[15]);
1459         sbox_key[3] = RTMPCkipSbox(key[12]);
1460
1461         rcon = rcon_table[round];
1462
1463         xor_32(&key[0], sbox_key, &key[0]);
1464         key[0] = key[0] ^ rcon;
1465
1466         xor_32(&key[4], &key[0], &key[4]);
1467         xor_32(&key[8], &key[4], &key[8]);
1468         xor_32(&key[12], &key[8], &key[12]);
1469 }
1470
1471 VOID xor_32(
1472         IN  PUCHAR  a,
1473         IN  PUCHAR  b,
1474         OUT PUCHAR  out)
1475 {
1476         INT i;
1477
1478         for (i=0;i<4; i++)
1479         {
1480                 out[i] = a[i] ^ b[i];
1481         }
1482 }
1483
1484 VOID byte_sub(
1485         IN  PUCHAR  in,
1486         OUT PUCHAR  out)
1487 {
1488         INT i;
1489
1490         for (i=0; i< 16; i++)
1491         {
1492                 out[i] = RTMPCkipSbox(in[i]);
1493         }
1494 }
1495
1496 UCHAR RTMPCkipSbox(
1497         IN  UCHAR   a)
1498 {
1499         return SboxTable[(int)a];
1500 }
1501
1502 VOID shift_row(
1503         IN  PUCHAR  in,
1504         OUT PUCHAR  out)
1505 {
1506         out[0] =  in[0];
1507         out[1] =  in[5];
1508         out[2] =  in[10];
1509         out[3] =  in[15];
1510         out[4] =  in[4];
1511         out[5] =  in[9];
1512         out[6] =  in[14];
1513         out[7] =  in[3];
1514         out[8] =  in[8];
1515         out[9] =  in[13];
1516         out[10] = in[2];
1517         out[11] = in[7];
1518         out[12] = in[12];
1519         out[13] = in[1];
1520         out[14] = in[6];
1521         out[15] = in[11];
1522 }
1523
1524 VOID mix_column(
1525         IN  PUCHAR  in,
1526         OUT PUCHAR  out)
1527 {
1528         INT         i;
1529         UCHAR       add1b[4];
1530         UCHAR       add1bf7[4];
1531         UCHAR       rotl[4];
1532         UCHAR       swap_halfs[4];
1533         UCHAR       andf7[4];
1534         UCHAR       rotr[4];
1535         UCHAR       temp[4];
1536         UCHAR       tempb[4];
1537
1538         for (i=0 ; i<4; i++)
1539         {
1540                 if ((in[i] & 0x80)== 0x80)
1541                         add1b[i] = 0x1b;
1542                 else
1543                         add1b[i] = 0x00;
1544         }
1545
1546         swap_halfs[0] = in[2];    /* Swap halfs */
1547         swap_halfs[1] = in[3];
1548         swap_halfs[2] = in[0];
1549         swap_halfs[3] = in[1];
1550
1551         rotl[0] = in[3];        /* Rotate left 8 bits */
1552         rotl[1] = in[0];
1553         rotl[2] = in[1];
1554         rotl[3] = in[2];
1555
1556         andf7[0] = in[0] & 0x7f;
1557         andf7[1] = in[1] & 0x7f;
1558         andf7[2] = in[2] & 0x7f;
1559         andf7[3] = in[3] & 0x7f;
1560
1561         for (i = 3; i>0; i--)    /* logical shift left 1 bit */
1562         {
1563                 andf7[i] = andf7[i] << 1;
1564                 if ((andf7[i-1] & 0x80) == 0x80)
1565                 {
1566                         andf7[i] = (andf7[i] | 0x01);
1567                 }
1568         }
1569         andf7[0] = andf7[0] << 1;
1570         andf7[0] = andf7[0] & 0xfe;
1571
1572         xor_32(add1b, andf7, add1bf7);
1573
1574         xor_32(in, add1bf7, rotr);
1575
1576         temp[0] = rotr[0];         /* Rotate right 8 bits */
1577         rotr[0] = rotr[1];
1578         rotr[1] = rotr[2];
1579         rotr[2] = rotr[3];
1580         rotr[3] = temp[0];
1581
1582         xor_32(add1bf7, rotr, temp);
1583         xor_32(swap_halfs, rotl,tempb);
1584         xor_32(temp, tempb, out);
1585 }
1586