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 #ifdef RT_BIG_ENDIAN
 203                                         UCHAR   KeyID:2;
 204                                         UCHAR   ExtIV:1;
 205                                         UCHAR   Rsvd:5;
 206 #else
 207                                         UCHAR   Rsvd:5;
 208                                         UCHAR   ExtIV:1;
 209                                         UCHAR   KeyID:2;
 210 #endif
 211                                 }       field;
 212                                 UCHAR           Byte;
 213                         }       CONTROL;
 214                 }       field;
 215
 216                 ULONG   word;
 217         }       IV16;
 218
 219         ULONG   IV32;
 220 }       TKIP_IV, *PTKIP_IV;
 221
 222
 223 /*
 224         ========================================================================
 225
 226         Routine Description:
 227                 Convert from UCHAR[] to ULONG in a portable way
 228
 229         Arguments:
 230       pMICKey           pointer to MIC Key
 231
 232         Return Value:
 233                 None
 234
 235         Note:
 236
 237         ========================================================================
 238 */
 239 ULONG   RTMPTkipGetUInt32(
 240         IN      PUCHAR  pMICKey)
 241 {
 242         ULONG   res = 0;
 243         INT             i;
 244
 245         for (i = 0; i < 4; i++)
 246         {
 247                 res |= (*pMICKey++) << (8 * i);
 248         }
 249
 250         return res;
 251 }
 252
 253 /*
 254         ========================================================================
 255
 256         Routine Description:
 257                 Convert from ULONG to UCHAR[] in a portable way
 258
 259         Arguments:
 260       pDst                      pointer to destination for convert ULONG to UCHAR[]
 261       val                       the value for convert
 262
 263         Return Value:
 264                 None
 265
 266         IRQL = DISPATCH_LEVEL
 267
 268         Note:
 269
 270         ========================================================================
 271 */
 272 VOID    RTMPTkipPutUInt32(
 273         IN OUT  PUCHAR          pDst,
 274         IN              ULONG           val)
 275 {
 276         INT i;
 277
 278         for(i = 0; i < 4; i++)
 279         {
 280                 *pDst++ = (UCHAR) (val & 0xff);
 281                 val >>= 8;
 282         }
 283 }
 284
 285 /*
 286         ========================================================================
 287
 288         Routine Description:
 289                 Set the MIC Key.
 290
 291         Arguments:
 292       pAd               Pointer to our adapter
 293       pMICKey           pointer to MIC Key
 294
 295         Return Value:
 296                 None
 297
 298         IRQL = DISPATCH_LEVEL
 299
 300         Note:
 301
 302         ========================================================================
 303 */
 304 VOID RTMPTkipSetMICKey(
 305         IN      PTKIP_KEY_INFO  pTkip,
 306         IN      PUCHAR                  pMICKey)
 307 {
 308         // Set the key
 309         pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
 310         pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
 311         // and reset the message
 312         pTkip->L = pTkip->K0;
 313         pTkip->R = pTkip->K1;
 314         pTkip->nBytesInM = 0;
 315         pTkip->M = 0;
 316 }
 317
 318 /*
 319         ========================================================================
 320
 321         Routine Description:
 322                 Calculate the MIC Value.
 323
 324         Arguments:
 325       pAd               Pointer to our adapter
 326       uChar                     Append this uChar
 327
 328         Return Value:
 329                 None
 330
 331         IRQL = DISPATCH_LEVEL
 332
 333         Note:
 334
 335         ========================================================================
 336 */
 337 VOID    RTMPTkipAppendByte(
 338         IN      PTKIP_KEY_INFO  pTkip,
 339         IN      UCHAR                   uChar)
 340 {
 341         // Append the byte to our word-sized buffer
 342         pTkip->M |= (uChar << (8* pTkip->nBytesInM));
 343         pTkip->nBytesInM++;
 344         // Process the word if it is full.
 345         if( pTkip->nBytesInM >= 4 )
 346         {
 347                 pTkip->L ^= pTkip->M;
 348                 pTkip->R ^= ROL32( pTkip->L, 17 );
 349                 pTkip->L += pTkip->R;
 350                 pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
 351                 pTkip->L += pTkip->R;
 352                 pTkip->R ^= ROL32( pTkip->L, 3 );
 353                 pTkip->L += pTkip->R;
 354                 pTkip->R ^= ROR32( pTkip->L, 2 );
 355                 pTkip->L += pTkip->R;
 356                 // Clear the buffer
 357                 pTkip->M = 0;
 358                 pTkip->nBytesInM = 0;
 359         }
 360 }
 361
 362 /*
 363         ========================================================================
 364
 365         Routine Description:
 366                 Calculate the MIC Value.
 367
 368         Arguments:
 369       pAd               Pointer to our adapter
 370       pSrc                      Pointer to source data for Calculate MIC Value
 371       Len                       Indicate the length of the source data
 372
 373         Return Value:
 374                 None
 375
 376         IRQL = DISPATCH_LEVEL
 377
 378         Note:
 379
 380         ========================================================================
 381 */
 382 VOID    RTMPTkipAppend(
 383         IN      PTKIP_KEY_INFO  pTkip,
 384         IN      PUCHAR                  pSrc,
 385         IN      UINT                    nBytes)
 386 {
 387         // This is simple
 388         while(nBytes > 0)
 389         {
 390                 RTMPTkipAppendByte(pTkip, *pSrc++);
 391                 nBytes--;
 392         }
 393 }
 394
 395 /*
 396         ========================================================================
 397
 398         Routine Description:
 399                 Get the MIC Value.
 400
 401         Arguments:
 402       pAd               Pointer to our adapter
 403
 404         Return Value:
 405                 None
 406
 407         IRQL = DISPATCH_LEVEL
 408
 409         Note:
 410                 the MIC Value is store in pAd->PrivateInfo.MIC
 411         ========================================================================
 412 */
 413 VOID    RTMPTkipGetMIC(
 414         IN      PTKIP_KEY_INFO  pTkip)
 415 {
 416         // Append the minimum padding
 417         RTMPTkipAppendByte(pTkip, 0x5a );
 418         RTMPTkipAppendByte(pTkip, 0 );
 419         RTMPTkipAppendByte(pTkip, 0 );
 420         RTMPTkipAppendByte(pTkip, 0 );
 421         RTMPTkipAppendByte(pTkip, 0 );
 422         // and then zeroes until the length is a multiple of 4
 423         while( pTkip->nBytesInM != 0 )
 424         {
 425                 RTMPTkipAppendByte(pTkip, 0 );
 426         }
 427         // The appendByte function has already computed the result.
 428         RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
 429         RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
 430 }
 431
 432 /*
 433         ========================================================================
 434
 435         Routine Description:
 436                 Init Tkip function.
 437
 438         Arguments:
 439       pAd               Pointer to our adapter
 440                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 441                 KeyId           TK Key ID
 442                 pTA                     Pointer to transmitter address
 443                 pMICKey         pointer to MIC Key
 444
 445         Return Value:
 446                 None
 447
 448         IRQL = DISPATCH_LEVEL
 449
 450         Note:
 451
 452         ========================================================================
 453 */
 454 VOID    RTMPInitTkipEngine(
 455         IN      PRTMP_ADAPTER   pAd,
 456         IN      PUCHAR                  pKey,
 457         IN      UCHAR                   KeyId,
 458         IN      PUCHAR                  pTA,
 459         IN      PUCHAR                  pMICKey,
 460         IN      PUCHAR                  pTSC,
 461         OUT     PULONG                  pIV16,
 462         OUT     PULONG                  pIV32)
 463 {
 464         TKIP_IV tkipIv;
 465
 466         // Prepare 8 bytes TKIP encapsulation for MPDU
 467         NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
 468         tkipIv.IV16.field.rc0 = *(pTSC + 1);
 469         tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
 470         tkipIv.IV16.field.rc2 = *pTSC;
 471         tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
 472         tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
 473 //      tkipIv.IV32 = *(PULONG)(pTSC + 2);
 474         NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
 475
 476         *pIV16 = tkipIv.IV16.word;
 477         *pIV32 = tkipIv.IV32;
 478 }
 479
 480 /*
 481         ========================================================================
 482
 483         Routine Description:
 484                 Init MIC Value calculation function which include set MIC key &
 485                 calculate first 16 bytes (DA + SA + priority +  0)
 486
 487         Arguments:
 488       pAd               Pointer to our adapter
 489                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 490                 pDA                     Pointer to DA address
 491                 pSA                     Pointer to SA address
 492                 pMICKey         pointer to MIC Key
 493
 494         Return Value:
 495                 None
 496
 497         Note:
 498
 499         ========================================================================
 500 */
 501 VOID    RTMPInitMICEngine(
 502         IN      PRTMP_ADAPTER   pAd,
 503         IN      PUCHAR                  pKey,
 504         IN      PUCHAR                  pDA,
 505         IN      PUCHAR                  pSA,
 506         IN  UCHAR           UserPriority,
 507         IN      PUCHAR                  pMICKey)
 508 {
 509         ULONG Priority = UserPriority;
 510
 511         // Init MIC value calculation
 512         RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
 513         // DA
 514         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
 515         // SA
 516         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
 517         // Priority + 3 bytes of 0
 518         RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
 519 }
 520
 521 /*
 522         ========================================================================
 523
 524         Routine Description:
 525                 Compare MIC value of received MSDU
 526
 527         Arguments:
 528                 pAd     Pointer to our adapter
 529                 pSrc        Pointer to the received Plain text data
 530                 pDA                     Pointer to DA address
 531                 pSA                     Pointer to SA address
 532                 pMICKey         pointer to MIC Key
 533                 Len         the length of the received plain text data exclude MIC value
 534
 535         Return Value:
 536                 TRUE        MIC value matched
 537                 FALSE       MIC value mismatched
 538
 539         IRQL = DISPATCH_LEVEL
 540
 541         Note:
 542
 543         ========================================================================
 544 */
 545 BOOLEAN RTMPTkipCompareMICValue(
 546         IN      PRTMP_ADAPTER   pAd,
 547         IN      PUCHAR                  pSrc,
 548         IN      PUCHAR                  pDA,
 549         IN      PUCHAR                  pSA,
 550         IN      PUCHAR                  pMICKey,
 551         IN      UCHAR                   UserPriority,
 552         IN      UINT                    Len)
 553 {
 554         UCHAR   OldMic[8];
 555         ULONG   Priority = UserPriority;
 556
 557         // Init MIC value calculation
 558         RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
 559         // DA
 560         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
 561         // SA
 562         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
 563         // Priority + 3 bytes of 0
 564         RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
 565
 566         // Calculate MIC value from plain text data
 567         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
 568
 569         // Get MIC valude from received frame
 570         NdisMoveMemory(OldMic, pSrc + Len, 8);
 571
 572         // Get MIC value from decrypted plain data
 573         RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
 574
 575         // Move MIC value from MSDU, this steps should move to data path.
 576         // Since the MIC value might cross MPDUs.
 577         if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
 578         {
 579                 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
 580
 581
 582                 return (FALSE);
 583         }
 584         return (TRUE);
 585 }
 586
 587 /*
 588         ========================================================================
 589
 590         Routine Description:
 591                 Compare MIC value of received MSDU
 592
 593         Arguments:
 594                 pAd     Pointer to our adapter
 595                 pLLC            LLC header
 596                 pSrc        Pointer to the received Plain text data
 597                 pDA                     Pointer to DA address
 598                 pSA                     Pointer to SA address
 599                 pMICKey         pointer to MIC Key
 600                 Len         the length of the received plain text data exclude MIC value
 601
 602         Return Value:
 603                 TRUE        MIC value matched
 604                 FALSE       MIC value mismatched
 605
 606         IRQL = DISPATCH_LEVEL
 607
 608         Note:
 609
 610         ========================================================================
 611 */
 612 BOOLEAN RTMPTkipCompareMICValueWithLLC(
 613         IN      PRTMP_ADAPTER   pAd,
 614         IN      PUCHAR                  pLLC,
 615         IN      PUCHAR                  pSrc,
 616         IN      PUCHAR                  pDA,
 617         IN      PUCHAR                  pSA,
 618         IN      PUCHAR                  pMICKey,
 619         IN      UINT                    Len)
 620 {
 621         UCHAR   OldMic[8];
 622         ULONG   Priority = 0;
 623
 624         // Init MIC value calculation
 625         RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
 626         // DA
 627         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
 628         // SA
 629         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
 630         // Priority + 3 bytes of 0
 631         RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
 632
 633         // Start with LLC header
 634         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
 635
 636         // Calculate MIC value from plain text data
 637         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
 638
 639         // Get MIC valude from received frame
 640         NdisMoveMemory(OldMic, pSrc + Len, 8);
 641
 642         // Get MIC value from decrypted plain data
 643         RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
 644
 645         // Move MIC value from MSDU, this steps should move to data path.
 646         // Since the MIC value might cross MPDUs.
 647         if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
 648         {
 649                 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n"));  //MIC error.
 650
 651
 652                 return (FALSE);
 653         }
 654         return (TRUE);
 655 }
 656 /*
 657         ========================================================================
 658
 659         Routine Description:
 660                 Copy frame from waiting queue into relative ring buffer and set
 661         appropriate ASIC register to kick hardware transmit function
 662
 663         Arguments:
 664                 pAd             Pointer to our adapter
 665                 PNDIS_PACKET    Pointer to Ndis Packet for MIC calculation
 666                 pEncap                  Pointer to LLC encap data
 667                 LenEncap                Total encap length, might be 0 which indicates no encap
 668
 669         Return Value:
 670                 None
 671
 672         IRQL = DISPATCH_LEVEL
 673
 674         Note:
 675
 676         ========================================================================
 677 */
 678 VOID    RTMPCalculateMICValue(
 679         IN      PRTMP_ADAPTER   pAd,
 680         IN      PNDIS_PACKET    pPacket,
 681         IN      PUCHAR                  pEncap,
 682         IN      PCIPHER_KEY             pKey,
 683         IN      UCHAR                   apidx)
 684 {
 685         PACKET_INFO             PacketInfo;
 686         PUCHAR                  pSrcBufVA;
 687         UINT                    SrcBufLen;
 688         PUCHAR                  pSrc;
 689     UCHAR           UserPriority;
 690         UCHAR                   vlan_offset = 0;
 691
 692         RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
 693
 694         UserPriority = RTMP_GET_PACKET_UP(pPacket);
 695         pSrc = pSrcBufVA;
 696
 697         // determine if this is a vlan packet
 698         if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
 699                 vlan_offset = 4;
 700
 701 #ifdef CONFIG_STA_SUPPORT
 702 #endif // CONFIG_STA_SUPPORT //
 703         {
 704                 RTMPInitMICEngine(
 705                         pAd,
 706                         pKey->Key,
 707                         pSrc,
 708                         pSrc + 6,
 709                         UserPriority,
 710                         pKey->TxMic);
 711         }
 712
 713
 714         if (pEncap != NULL)
 715         {
 716                 // LLC encapsulation
 717                 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
 718                 // Protocol Type
 719                 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
 720         }
 721         SrcBufLen -= (14 + vlan_offset);
 722         pSrc += (14 + vlan_offset);
 723         do
 724         {
 725                 if (SrcBufLen > 0)
 726                 {
 727                         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
 728                 }
 729
 730                 break;  // No need handle next packet
 731
 732         }       while (TRUE);           // End of copying payload
 733
 734         // Compute the final MIC Value
 735         RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
 736 }
 737
 738
 739 /************************************************************/
 740 /* tkip_sbox()                                                                                                                          */
 741 /* Returns a 16 bit value from a 64K entry table. The Table */
 742 /* is synthesized from two 256 entry byte wide tables.          */
 743 /************************************************************/
 744
 745 UINT tkip_sbox(UINT index)
 746 {
 747         UINT index_low;
 748         UINT index_high;
 749         UINT left, right;
 750
 751         index_low = (index % 256);
 752         index_high = ((index >> 8) % 256);
 753
 754         left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
 755         right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
 756
 757         return (left ^ right);
 758 }
 759
 760 UINT rotr1(UINT a)
 761 {
 762         unsigned int b;
 763
 764         if ((a & 0x01) == 0x01)
 765         {
 766                 b = (a >> 1) | 0x8000;
 767         }
 768         else
 769         {
 770                 b = (a >> 1) & 0x7fff;
 771         }
 772         b = b % 65536;
 773         return b;
 774 }
 775
 776 VOID RTMPTkipMixKey(
 777         UCHAR *key,
 778         UCHAR *ta,
 779         ULONG pnl, /* Least significant 16 bits of PN */
 780         ULONG pnh, /* Most significant 32 bits of PN */
 781         UCHAR *rc4key,
 782         UINT *p1k)
 783 {
 784
 785         UINT tsc0;
 786         UINT tsc1;
 787         UINT tsc2;
 788
 789         UINT ppk0;
 790         UINT ppk1;
 791         UINT ppk2;
 792         UINT ppk3;
 793         UINT ppk4;
 794         UINT ppk5;
 795
 796         INT i;
 797         INT j;
 798
 799         tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
 800         tsc1 = (unsigned int)(pnh % 65536);
 801         tsc2 = (unsigned int)(pnl % 65536); /* lsb */
 802
 803         /* Phase 1, step 1 */
 804         p1k[0] = tsc1;
 805         p1k[1] = tsc0;
 806         p1k[2] = (UINT)(ta[0] + (ta[1]*256));
 807         p1k[3] = (UINT)(ta[2] + (ta[3]*256));
 808         p1k[4] = (UINT)(ta[4] + (ta[5]*256));
 809
 810         /* Phase 1, step 2 */
 811         for (i=0; i<8; i++)
 812         {
 813                 j = 2*(i & 1);
 814                 p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
 815                 p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
 816                 p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
 817                 p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
 818                 p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
 819                 p1k[4] = (p1k[4] + i) % 65536;
 820         }
 821
 822         /* Phase 2, Step 1 */
 823         ppk0 = p1k[0];
 824         ppk1 = p1k[1];
 825         ppk2 = p1k[2];
 826         ppk3 = p1k[3];
 827         ppk4 = p1k[4];
 828         ppk5 = (p1k[4] + tsc2) % 65536;
 829
 830         /* Phase2, Step 2 */
 831         ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
 832         ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
 833         ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
 834         ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
 835         ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
 836         ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
 837
 838         ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
 839         ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
 840         ppk2 = ppk2 + rotr1(ppk1);
 841         ppk3 = ppk3 + rotr1(ppk2);
 842         ppk4 = ppk4 + rotr1(ppk3);
 843         ppk5 = ppk5 + rotr1(ppk4);
 844
 845         /* Phase 2, Step 3 */
 846     /* Phase 2, Step 3 */
 847
 848         tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
 849         tsc1 = (unsigned int)(pnh % 65536);
 850         tsc2 = (unsigned int)(pnl % 65536); /* lsb */
 851
 852         rc4key[0] = (tsc2 >> 8) % 256;
 853         rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
 854         rc4key[2] = tsc2 % 256;
 855         rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
 856
 857         rc4key[4] = ppk0 % 256;
 858         rc4key[5] = (ppk0 >> 8) % 256;
 859
 860         rc4key[6] = ppk1 % 256;
 861         rc4key[7] = (ppk1 >> 8) % 256;
 862
 863         rc4key[8] = ppk2 % 256;
 864         rc4key[9] = (ppk2 >> 8) % 256;
 865
 866         rc4key[10] = ppk3 % 256;
 867         rc4key[11] = (ppk3 >> 8) % 256;
 868
 869         rc4key[12] = ppk4 % 256;
 870         rc4key[13] = (ppk4 >> 8) % 256;
 871
 872         rc4key[14] = ppk5 % 256;
 873         rc4key[15] = (ppk5 >> 8) % 256;
 874 }
 875
 876
 877 /************************************************/
 878 /* construct_mic_header1()                      */
 879 /* Builds the first MIC header block from       */
 880 /* header fields.                               */
 881 /************************************************/
 882
 883 void construct_mic_header1(
 884         unsigned char *mic_header1,
 885         int header_length,
 886         unsigned char *mpdu)
 887 {
 888         mic_header1[0] = (unsigned char)((header_length - 2) / 256);
 889         mic_header1[1] = (unsigned char)((header_length - 2) % 256);
 890         mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
 891         mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
 892         mic_header1[4] = mpdu[4];       /* A1 */
 893         mic_header1[5] = mpdu[5];
 894         mic_header1[6] = mpdu[6];
 895         mic_header1[7] = mpdu[7];
 896         mic_header1[8] = mpdu[8];
 897         mic_header1[9] = mpdu[9];
 898         mic_header1[10] = mpdu[10];     /* A2 */
 899         mic_header1[11] = mpdu[11];
 900         mic_header1[12] = mpdu[12];
 901         mic_header1[13] = mpdu[13];
 902         mic_header1[14] = mpdu[14];
 903         mic_header1[15] = mpdu[15];
 904 }
 905
 906 /************************************************/
 907 /* construct_mic_header2()                      */
 908 /* Builds the last MIC header block from        */
 909 /* header fields.                               */
 910 /************************************************/
 911
 912 void construct_mic_header2(
 913         unsigned char *mic_header2,
 914         unsigned char *mpdu,
 915         int a4_exists,
 916         int qc_exists)
 917 {
 918         int i;
 919
 920         for (i = 0; i<16; i++) mic_header2[i]=0x00;
 921
 922         mic_header2[0] = mpdu[16];    /* A3 */
 923         mic_header2[1] = mpdu[17];
 924         mic_header2[2] = mpdu[18];
 925         mic_header2[3] = mpdu[19];
 926         mic_header2[4] = mpdu[20];
 927         mic_header2[5] = mpdu[21];
 928
 929         // In Sequence Control field, mute sequence numer bits (12-bit)
 930         mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
 931         mic_header2[7] = 0x00; /* mpdu[23]; */
 932
 933         if ((!qc_exists) & a4_exists)
 934         {
 935                 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
 936
 937         }
 938
 939         if (qc_exists && (!a4_exists))
 940         {
 941                 mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
 942                 mic_header2[9] = mpdu[25] & 0x00;
 943         }
 944
 945         if (qc_exists && a4_exists)
 946         {
 947                 for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
 948
 949                 mic_header2[14] = mpdu[30] & 0x0f;
 950                 mic_header2[15] = mpdu[31] & 0x00;
 951         }
 952 }
 953
 954
 955 /************************************************/
 956 /* construct_mic_iv()                           */
 957 /* Builds the MIC IV from header fields and PN  */
 958 /************************************************/
 959
 960 void construct_mic_iv(
 961         unsigned char *mic_iv,
 962         int qc_exists,
 963         int a4_exists,
 964         unsigned char *mpdu,
 965         unsigned int payload_length,
 966         unsigned char *pn_vector)
 967 {
 968         int i;
 969
 970         mic_iv[0] = 0x59;
 971         if (qc_exists && a4_exists)
 972                 mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
 973         if (qc_exists && !a4_exists)
 974                 mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
 975         if (!qc_exists)
 976                 mic_iv[1] = 0x00;
 977         for (i = 2; i < 8; i++)
 978                 mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
 979 #ifdef CONSISTENT_PN_ORDER
 980                 for (i = 8; i < 14; i++)
 981                         mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
 982 #else
 983                 for (i = 8; i < 14; i++)
 984                         mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
 985 #endif
 986         i = (payload_length / 256);
 987         i = (payload_length % 256);
 988         mic_iv[14] = (unsigned char) (payload_length / 256);
 989         mic_iv[15] = (unsigned char) (payload_length % 256);
 990
 991 }
 992
 993
 994
 995 /************************************/
 996 /* bitwise_xor()                    */
 997 /* A 128 bit, bitwise exclusive or  */
 998 /************************************/
 999
1000 void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
1001 {
1002         int i;
1003         for (i=0; i<16; i++)
1004         {
1005                 out[i] = ina[i] ^ inb[i];
1006         }
1007 }
1008
1009
1010 void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
1011 {
1012         int round;
1013         int i;
1014         unsigned char intermediatea[16];
1015         unsigned char intermediateb[16];
1016         unsigned char round_key[16];
1017
1018         for(i=0; i<16; i++) round_key[i] = key[i];
1019
1020         for (round = 0; round < 11; round++)
1021         {
1022                 if (round == 0)
1023                 {
1024                         xor_128(round_key, data, ciphertext);
1025                         next_key(round_key, round);
1026                 }
1027                 else if (round == 10)
1028                 {
1029                         byte_sub(ciphertext, intermediatea);
1030                         shift_row(intermediatea, intermediateb);
1031                         xor_128(intermediateb, round_key, ciphertext);
1032                 }
1033                 else    /* 1 - 9 */
1034                 {
1035                         byte_sub(ciphertext, intermediatea);
1036                         shift_row(intermediatea, intermediateb);
1037                         mix_column(&intermediateb[0], &intermediatea[0]);
1038                         mix_column(&intermediateb[4], &intermediatea[4]);
1039                         mix_column(&intermediateb[8], &intermediatea[8]);
1040                         mix_column(&intermediateb[12], &intermediatea[12]);
1041                         xor_128(intermediatea, round_key, ciphertext);
1042                         next_key(round_key, round);
1043                 }
1044         }
1045
1046 }
1047
1048 void construct_ctr_preload(
1049         unsigned char *ctr_preload,
1050         int a4_exists,
1051         int qc_exists,
1052         unsigned char *mpdu,
1053         unsigned char *pn_vector,
1054         int c)
1055 {
1056
1057         int i = 0;
1058         for (i=0; i<16; i++) ctr_preload[i] = 0x00;
1059         i = 0;
1060
1061         ctr_preload[0] = 0x01;                                  /* flag */
1062         if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
1063         if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
1064
1065         for (i = 2; i < 8; i++)
1066                 ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
1067 #ifdef CONSISTENT_PN_ORDER
1068           for (i = 8; i < 14; i++)
1069                         ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
1070 #else
1071           for (i = 8; i < 14; i++)
1072                         ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
1073 #endif
1074         ctr_preload[14] =  (unsigned char) (c / 256); // Ctr
1075         ctr_preload[15] =  (unsigned char) (c % 256);
1076
1077 }
1078
1079
1080 //
1081 // TRUE: Success!
1082 // FALSE: Decrypt Error!
1083 //
1084 BOOLEAN RTMPSoftDecryptTKIP(
1085         IN PRTMP_ADAPTER pAd,
1086         IN PUCHAR       pData,
1087         IN ULONG        DataByteCnt,
1088         IN UCHAR    UserPriority,
1089         IN PCIPHER_KEY  pWpaKey)
1090 {
1091         UCHAR                   KeyID;
1092         UINT                    HeaderLen;
1093     UCHAR                       fc0;
1094         UCHAR                   fc1;
1095         USHORT                  fc;
1096         UINT                    frame_type;
1097         UINT                    frame_subtype;
1098     UINT                        from_ds;
1099     UINT                        to_ds;
1100         INT                             a4_exists;
1101         INT                             qc_exists;
1102         USHORT                  duration;
1103         USHORT                  seq_control;
1104         USHORT                  qos_control;
1105         UCHAR                   TA[MAC_ADDR_LEN];
1106         UCHAR                   DA[MAC_ADDR_LEN];
1107         UCHAR                   SA[MAC_ADDR_LEN];
1108         UCHAR                   RC4Key[16];
1109         UINT                    p1k[5]; //for mix_key;
1110         ULONG                   pnl;/* Least significant 16 bits of PN */
1111         ULONG                   pnh;/* Most significant 32 bits of PN */
1112         UINT                    num_blocks;
1113         UINT                    payload_remainder;
1114         ARCFOURCONTEXT  ArcFourContext;
1115         UINT                    crc32 = 0;
1116         UINT                    trailfcs = 0;
1117         UCHAR                   MIC[8];
1118         UCHAR                   TrailMIC[8];
1119
1120 #ifdef RT_BIG_ENDIAN
1121         RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1122 #endif
1123
1124         fc0 = *pData;
1125         fc1 = *(pData + 1);
1126
1127         fc = *((PUSHORT)pData);
1128
1129         frame_type = ((fc0 >> 2) & 0x03);
1130         frame_subtype = ((fc0 >> 4) & 0x0f);
1131
1132     from_ds = (fc1 & 0x2) >> 1;
1133     to_ds = (fc1 & 0x1);
1134
1135     a4_exists = (from_ds & to_ds);
1136     qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
1137                   (frame_subtype == 0x09) ||   /* Likely to change.    */
1138                   (frame_subtype == 0x0a) ||
1139                   (frame_subtype == 0x0b)
1140                  );
1141
1142         HeaderLen = 24;
1143         if (a4_exists)
1144                 HeaderLen += 6;
1145
1146         KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1147         KeyID = KeyID >> 6;
1148
1149         if (pWpaKey[KeyID].KeyLen == 0)
1150         {
1151                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1152                 return FALSE;
1153         }
1154
1155         duration = *((PUSHORT)(pData+2));
1156
1157         seq_control = *((PUSHORT)(pData+22));
1158
1159         if (qc_exists)
1160         {
1161                 if (a4_exists)
1162                 {
1163                         qos_control = *((PUSHORT)(pData+30));
1164                 }
1165                 else
1166                 {
1167                         qos_control = *((PUSHORT)(pData+24));
1168                 }
1169         }
1170
1171         if (to_ds == 0 && from_ds == 1)
1172         {
1173                 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1174                 NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
1175                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
1176         }
1177         else if (to_ds == 0 && from_ds == 0 )
1178         {
1179                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1180                 NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
1181                 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1182         }
1183         else if (to_ds == 1 && from_ds == 0)
1184         {
1185                 NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
1186                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1187                 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1188         }
1189         else if (to_ds == 1 && from_ds == 1)
1190         {
1191                 NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
1192                 NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
1193                 NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
1194         }
1195
1196         num_blocks = (DataByteCnt - 16) / 16;
1197         payload_remainder = (DataByteCnt - 16) % 16;
1198
1199         pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
1200         pnh = *((PULONG)(pData + HeaderLen + 4));
1201         pnh = cpu2le32(pnh);
1202         RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
1203
1204         ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
1205
1206         ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
1207         NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
1208         crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
1209         crc32 ^= 0xffffffff;             /* complement */
1210
1211     if(crc32 != cpu2le32(trailfcs))
1212         {
1213                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));       //ICV error.
1214
1215                 return (FALSE);
1216         }
1217
1218         NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
1219         RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
1220         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
1221         RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
1222         NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
1223
1224         if (!NdisEqualMemory(MIC, TrailMIC, 8))
1225         {
1226                 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));       //MIC error.
1227                 //RTMPReportMicError(pAd, &pWpaKey[KeyID]);     // marked by AlbertY @ 20060630
1228                 return (FALSE);
1229         }
1230
1231 #ifdef RT_BIG_ENDIAN
1232         RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1233 #endif
1234         //DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
1235         return TRUE;
1236 }
1237
1238
1239
1240
1241 BOOLEAN RTMPSoftDecryptAES(
1242         IN PRTMP_ADAPTER pAd,
1243         IN PUCHAR       pData,
1244         IN ULONG        DataByteCnt,
1245         IN PCIPHER_KEY  pWpaKey)
1246 {
1247         UCHAR                   KeyID;
1248         UINT                    HeaderLen;
1249         UCHAR                   PN[6];
1250         UINT                    payload_len;
1251         UINT                    num_blocks;
1252         UINT                    payload_remainder;
1253         USHORT                  fc;
1254         UCHAR                   fc0;
1255         UCHAR                   fc1;
1256         UINT                    frame_type;
1257         UINT                    frame_subtype;
1258         UINT                    from_ds;
1259         UINT                    to_ds;
1260         INT                             a4_exists;
1261         INT                             qc_exists;
1262         UCHAR                   aes_out[16];
1263         int                     payload_index;
1264         UINT                    i;
1265         UCHAR                   ctr_preload[16];
1266         UCHAR                   chain_buffer[16];
1267         UCHAR                   padded_buffer[16];
1268         UCHAR                   mic_iv[16];
1269         UCHAR                   mic_header1[16];
1270         UCHAR                   mic_header2[16];
1271         UCHAR                   MIC[8];
1272         UCHAR                   TrailMIC[8];
1273
1274 #ifdef RT_BIG_ENDIAN
1275         RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1276 #endif
1277
1278         fc0 = *pData;
1279         fc1 = *(pData + 1);
1280
1281         fc = *((PUSHORT)pData);
1282
1283         frame_type = ((fc0 >> 2) & 0x03);
1284         frame_subtype = ((fc0 >> 4) & 0x0f);
1285
1286         from_ds = (fc1 & 0x2) >> 1;
1287         to_ds = (fc1 & 0x1);
1288
1289         a4_exists = (from_ds & to_ds);
1290         qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
1291                                   (frame_subtype == 0x09) ||   /* Likely to change.    */
1292                                   (frame_subtype == 0x0a) ||
1293                                   (frame_subtype == 0x0b)
1294                                  );
1295
1296         HeaderLen = 24;
1297         if (a4_exists)
1298                 HeaderLen += 6;
1299
1300         KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1301         KeyID = KeyID >> 6;
1302
1303         if (pWpaKey[KeyID].KeyLen == 0)
1304         {
1305                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1306                 return FALSE;
1307         }
1308
1309         PN[0] = *(pData+ HeaderLen);
1310         PN[1] = *(pData+ HeaderLen + 1);
1311         PN[2] = *(pData+ HeaderLen + 4);
1312         PN[3] = *(pData+ HeaderLen + 5);
1313         PN[4] = *(pData+ HeaderLen + 6);
1314         PN[5] = *(pData+ HeaderLen + 7);
1315
1316         payload_len = DataByteCnt - HeaderLen - 8 - 8;  // 8 bytes for CCMP header , 8 bytes for MIC
1317         payload_remainder = (payload_len) % 16;
1318         num_blocks = (payload_len) / 16;
1319
1320
1321
1322         // Find start of payload
1323         payload_index = HeaderLen + 8; //IV+EIV
1324
1325         for (i=0; i< num_blocks; i++)
1326         {
1327                 construct_ctr_preload(ctr_preload,
1328                                                                 a4_exists,
1329                                                                 qc_exists,
1330                                                                 pData,
1331                                                                 PN,
1332                                                                 i+1 );
1333
1334                 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1335
1336                 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1337                 NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
1338                 payload_index += 16;
1339         }
1340
1341         //
1342         // If there is a short final block, then pad it
1343         // encrypt it and copy the unpadded part back
1344         //
1345         if (payload_remainder > 0)
1346         {
1347                 construct_ctr_preload(ctr_preload,
1348                                                                 a4_exists,
1349                                                                 qc_exists,
1350                                                                 pData,
1351                                                                 PN,
1352                                                                 num_blocks + 1);
1353
1354                 NdisZeroMemory(padded_buffer, 16);
1355                 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1356
1357                 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1358
1359                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1360                 NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
1361                 payload_index += payload_remainder;
1362         }
1363
1364         //
1365         // Descrypt the MIC
1366         //
1367         construct_ctr_preload(ctr_preload,
1368                                                         a4_exists,
1369                                                         qc_exists,
1370                                                         pData,
1371                                                         PN,
1372                                                         0);
1373         NdisZeroMemory(padded_buffer, 16);
1374         NdisMoveMemory(padded_buffer, pData + payload_index, 8);
1375
1376         aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1377
1378         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1379
1380         NdisMoveMemory(TrailMIC, chain_buffer, 8);
1381
1382         //
1383         // Calculate MIC
1384         //
1385
1386         //Force the protected frame bit on
1387         *(pData + 1) = *(pData + 1) | 0x40;
1388
1389         // Find start of payload
1390         // Because the CCMP header has been removed
1391         payload_index = HeaderLen;
1392
1393         construct_mic_iv(
1394                                         mic_iv,
1395                                         qc_exists,
1396                                         a4_exists,
1397                                         pData,
1398                                         payload_len,
1399                                         PN);
1400
1401         construct_mic_header1(
1402                                                 mic_header1,
1403                                                 HeaderLen,
1404                                                 pData);
1405
1406         construct_mic_header2(
1407                                                 mic_header2,
1408                                                 pData,
1409                                                 a4_exists,
1410                                                 qc_exists);
1411
1412         aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
1413         bitwise_xor(aes_out, mic_header1, chain_buffer);
1414         aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1415         bitwise_xor(aes_out, mic_header2, chain_buffer);
1416         aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1417
1418         // iterate through each 16 byte payload block
1419         for (i = 0; i < num_blocks; i++)
1420         {
1421                 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1422                 payload_index += 16;
1423                 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1424         }
1425
1426         // Add on the final payload block if it needs padding
1427         if (payload_remainder > 0)
1428         {
1429                 NdisZeroMemory(padded_buffer, 16);
1430                 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1431
1432                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1433                 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1434         }
1435
1436         // aes_out contains padded mic, discard most significant
1437         // 8 bytes to generate 64 bit MIC
1438         for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
1439
1440         if (!NdisEqualMemory(MIC, TrailMIC, 8))
1441         {
1442                 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));         //MIC error.
1443                 return FALSE;
1444         }
1445
1446 #ifdef RT_BIG_ENDIAN
1447         RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
1448 #endif
1449
1450         return TRUE;
1451 }
1452
1453 /****************************************/
1454 /* aes128k128d()                        */
1455 /* Performs a 128 bit AES encrypt with  */
1456 /* 128 bit data.                        */
1457 /****************************************/
1458 VOID xor_128(
1459         IN  PUCHAR  a,
1460         IN  PUCHAR  b,
1461         OUT PUCHAR  out)
1462 {
1463         INT i;
1464
1465         for (i=0;i<16; i++)
1466         {
1467                 out[i] = a[i] ^ b[i];
1468         }
1469 }
1470
1471 VOID next_key(
1472         IN  PUCHAR  key,
1473         IN  INT     round)
1474 {
1475         UCHAR       rcon;
1476         UCHAR       sbox_key[4];
1477         UCHAR       rcon_table[12] =
1478         {
1479                 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
1480                 0x1b, 0x36, 0x36, 0x36
1481         };
1482
1483         sbox_key[0] = RTMPCkipSbox(key[13]);
1484         sbox_key[1] = RTMPCkipSbox(key[14]);
1485         sbox_key[2] = RTMPCkipSbox(key[15]);
1486         sbox_key[3] = RTMPCkipSbox(key[12]);
1487
1488         rcon = rcon_table[round];
1489
1490         xor_32(&key[0], sbox_key, &key[0]);
1491         key[0] = key[0] ^ rcon;
1492
1493         xor_32(&key[4], &key[0], &key[4]);
1494         xor_32(&key[8], &key[4], &key[8]);
1495         xor_32(&key[12], &key[8], &key[12]);
1496 }
1497
1498 VOID xor_32(
1499         IN  PUCHAR  a,
1500         IN  PUCHAR  b,
1501         OUT PUCHAR  out)
1502 {
1503         INT i;
1504
1505         for (i=0;i<4; i++)
1506         {
1507                 out[i] = a[i] ^ b[i];
1508         }
1509 }
1510
1511 VOID byte_sub(
1512         IN  PUCHAR  in,
1513         OUT PUCHAR  out)
1514 {
1515         INT i;
1516
1517         for (i=0; i< 16; i++)
1518         {
1519                 out[i] = RTMPCkipSbox(in[i]);
1520         }
1521 }
1522
1523 UCHAR RTMPCkipSbox(
1524         IN  UCHAR   a)
1525 {
1526         return SboxTable[(int)a];
1527 }
1528
1529 VOID shift_row(
1530         IN  PUCHAR  in,
1531         OUT PUCHAR  out)
1532 {
1533         out[0] =  in[0];
1534         out[1] =  in[5];
1535         out[2] =  in[10];
1536         out[3] =  in[15];
1537         out[4] =  in[4];
1538         out[5] =  in[9];
1539         out[6] =  in[14];
1540         out[7] =  in[3];
1541         out[8] =  in[8];
1542         out[9] =  in[13];
1543         out[10] = in[2];
1544         out[11] = in[7];
1545         out[12] = in[12];
1546         out[13] = in[1];
1547         out[14] = in[6];
1548         out[15] = in[11];
1549 }
1550
1551 VOID mix_column(
1552         IN  PUCHAR  in,
1553         OUT PUCHAR  out)
1554 {
1555         INT         i;
1556         UCHAR       add1b[4];
1557         UCHAR       add1bf7[4];
1558         UCHAR       rotl[4];
1559         UCHAR       swap_halfs[4];
1560         UCHAR       andf7[4];
1561         UCHAR       rotr[4];
1562         UCHAR       temp[4];
1563         UCHAR       tempb[4];
1564
1565         for (i=0 ; i<4; i++)
1566         {
1567                 if ((in[i] & 0x80)== 0x80)
1568                         add1b[i] = 0x1b;
1569                 else
1570                         add1b[i] = 0x00;
1571         }
1572
1573         swap_halfs[0] = in[2];    /* Swap halfs */
1574         swap_halfs[1] = in[3];
1575         swap_halfs[2] = in[0];
1576         swap_halfs[3] = in[1];
1577
1578         rotl[0] = in[3];        /* Rotate left 8 bits */
1579         rotl[1] = in[0];
1580         rotl[2] = in[1];
1581         rotl[3] = in[2];
1582
1583         andf7[0] = in[0] & 0x7f;
1584         andf7[1] = in[1] & 0x7f;
1585         andf7[2] = in[2] & 0x7f;
1586         andf7[3] = in[3] & 0x7f;
1587
1588         for (i = 3; i>0; i--)    /* logical shift left 1 bit */
1589         {
1590                 andf7[i] = andf7[i] << 1;
1591                 if ((andf7[i-1] & 0x80) == 0x80)
1592                 {
1593                         andf7[i] = (andf7[i] | 0x01);
1594                 }
1595         }
1596         andf7[0] = andf7[0] << 1;
1597         andf7[0] = andf7[0] & 0xfe;
1598
1599         xor_32(add1b, andf7, add1bf7);
1600
1601         xor_32(in, add1bf7, rotr);
1602
1603         temp[0] = rotr[0];         /* Rotate right 8 bits */
1604         rotr[0] = rotr[1];
1605         rotr[1] = rotr[2];
1606         rotr[2] = rotr[3];
1607         rotr[3] = temp[0];
1608
1609         xor_32(add1bf7, rotr, temp);
1610         xor_32(swap_halfs, rotl,tempb);
1611         xor_32(temp, tempb, out);
1612 }
1613