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