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