Pileus Git - ~andy/linux/blob - drivers/staging/rt2870/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 //      tkipIv.IV32 = *(PULONG)(pTSC + 2);
 468         NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
 469
 470         *pIV16 = tkipIv.IV16.word;
 471         *pIV32 = tkipIv.IV32;
 472 }
 473
 474 /*
 475         ========================================================================
 476
 477         Routine Description:
 478                 Init MIC Value calculation function which include set MIC key &
 479                 calculate first 16 bytes (DA + SA + priority +  0)
 480
 481         Arguments:
 482       pAd               Pointer to our adapter
 483                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 484                 pDA                     Pointer to DA address
 485                 pSA                     Pointer to SA address
 486                 pMICKey         pointer to MIC Key
 487
 488         Return Value:
 489                 None
 490
 491         Note:
 492
 493         ========================================================================
 494 */
 495 VOID    RTMPInitMICEngine(
 496         IN      PRTMP_ADAPTER   pAd,
 497         IN      PUCHAR                  pKey,
 498         IN      PUCHAR                  pDA,
 499         IN      PUCHAR                  pSA,
 500         IN  UCHAR           UserPriority,
 501         IN      PUCHAR                  pMICKey)
 502 {
 503         ULONG Priority = UserPriority;
 504
 505         // Init MIC value calculation
 506         RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
 507         // DA
 508         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
 509         // SA
 510         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
 511         // Priority + 3 bytes of 0
 512         RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
 513 }
 514
 515 /*
 516         ========================================================================
 517
 518         Routine Description:
 519                 Compare MIC value of received MSDU
 520
 521         Arguments:
 522                 pAd     Pointer to our adapter
 523                 pSrc        Pointer to the received Plain text data
 524                 pDA                     Pointer to DA address
 525                 pSA                     Pointer to SA address
 526                 pMICKey         pointer to MIC Key
 527                 Len         the length of the received plain text data exclude MIC value
 528
 529         Return Value:
 530                 TRUE        MIC value matched
 531                 FALSE       MIC value mismatched
 532
 533         IRQL = DISPATCH_LEVEL
 534
 535         Note:
 536
 537         ========================================================================
 538 */
 539 BOOLEAN RTMPTkipCompareMICValue(
 540         IN      PRTMP_ADAPTER   pAd,
 541         IN      PUCHAR                  pSrc,
 542         IN      PUCHAR                  pDA,
 543         IN      PUCHAR                  pSA,
 544         IN      PUCHAR                  pMICKey,
 545         IN      UCHAR                   UserPriority,
 546         IN      UINT                    Len)
 547 {
 548         UCHAR   OldMic[8];
 549         ULONG   Priority = UserPriority;
 550
 551         // Init MIC value calculation
 552         RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
 553         // DA
 554         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
 555         // SA
 556         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
 557         // Priority + 3 bytes of 0
 558         RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
 559
 560         // Calculate MIC value from plain text data
 561         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
 562
 563         // Get MIC valude from received frame
 564         NdisMoveMemory(OldMic, pSrc + Len, 8);
 565
 566         // Get MIC value from decrypted plain data
 567         RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
 568
 569         // Move MIC value from MSDU, this steps should move to data path.
 570         // Since the MIC value might cross MPDUs.
 571         if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
 572         {
 573                 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
 574
 575
 576                 return (FALSE);
 577         }
 578         return (TRUE);
 579 }
 580
 581 /*
 582         ========================================================================
 583
 584         Routine Description:
 585                 Compare MIC value of received MSDU
 586
 587         Arguments:
 588                 pAd     Pointer to our adapter
 589                 pLLC            LLC header
 590                 pSrc        Pointer to the received Plain text data
 591                 pDA                     Pointer to DA address
 592                 pSA                     Pointer to SA address
 593                 pMICKey         pointer to MIC Key
 594                 Len         the length of the received plain text data exclude MIC value
 595
 596         Return Value:
 597                 TRUE        MIC value matched
 598                 FALSE       MIC value mismatched
 599
 600         IRQL = DISPATCH_LEVEL
 601
 602         Note:
 603
 604         ========================================================================
 605 */
 606 BOOLEAN RTMPTkipCompareMICValueWithLLC(
 607         IN      PRTMP_ADAPTER   pAd,
 608         IN      PUCHAR                  pLLC,
 609         IN      PUCHAR                  pSrc,
 610         IN      PUCHAR                  pDA,
 611         IN      PUCHAR                  pSA,
 612         IN      PUCHAR                  pMICKey,
 613         IN      UINT                    Len)
 614 {
 615         UCHAR   OldMic[8];
 616         ULONG   Priority = 0;
 617
 618         // Init MIC value calculation
 619         RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
 620         // DA
 621         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
 622         // SA
 623         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
 624         // Priority + 3 bytes of 0
 625         RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
 626
 627         // Start with LLC header
 628         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
 629
 630         // Calculate MIC value from plain text data
 631         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
 632
 633         // Get MIC valude from received frame
 634         NdisMoveMemory(OldMic, pSrc + Len, 8);
 635
 636         // Get MIC value from decrypted plain data
 637         RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
 638
 639         // Move MIC value from MSDU, this steps should move to data path.
 640         // Since the MIC value might cross MPDUs.
 641         if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
 642         {
 643                 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n"));  //MIC error.
 644
 645
 646                 return (FALSE);
 647         }
 648         return (TRUE);
 649 }
 650 /*
 651         ========================================================================
 652
 653         Routine Description:
 654                 Copy frame from waiting queue into relative ring buffer and set
 655         appropriate ASIC register to kick hardware transmit function
 656
 657         Arguments:
 658                 pAd             Pointer to our adapter
 659                 PNDIS_PACKET    Pointer to Ndis Packet for MIC calculation
 660                 pEncap                  Pointer to LLC encap data
 661                 LenEncap                Total encap length, might be 0 which indicates no encap
 662
 663         Return Value:
 664                 None
 665
 666         IRQL = DISPATCH_LEVEL
 667
 668         Note:
 669
 670         ========================================================================
 671 */
 672 VOID    RTMPCalculateMICValue(
 673         IN      PRTMP_ADAPTER   pAd,
 674         IN      PNDIS_PACKET    pPacket,
 675         IN      PUCHAR                  pEncap,
 676         IN      PCIPHER_KEY             pKey,
 677         IN      UCHAR                   apidx)
 678 {
 679         PACKET_INFO             PacketInfo;
 680         PUCHAR                  pSrcBufVA;
 681         UINT                    SrcBufLen;
 682         PUCHAR                  pSrc;
 683     UCHAR           UserPriority;
 684         UCHAR                   vlan_offset = 0;
 685
 686         RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
 687
 688         UserPriority = RTMP_GET_PACKET_UP(pPacket);
 689         pSrc = pSrcBufVA;
 690
 691         // determine if this is a vlan packet
 692         if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
 693                 vlan_offset = 4;
 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                 //RTMPReportMicError(pAd, &pWpaKey[KeyID]);     // marked by AlbertY @ 20060630
1215                 return (FALSE);
1216         }
1217
1218         //DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
1219         return TRUE;
1220 }
1221
1222
1223
1224
1225 BOOLEAN RTMPSoftDecryptAES(
1226         IN PRTMP_ADAPTER pAd,
1227         IN PUCHAR       pData,
1228         IN ULONG        DataByteCnt,
1229         IN PCIPHER_KEY  pWpaKey)
1230 {
1231         UCHAR                   KeyID;
1232         UINT                    HeaderLen;
1233         UCHAR                   PN[6];
1234         UINT                    payload_len;
1235         UINT                    num_blocks;
1236         UINT                    payload_remainder;
1237         USHORT                  fc;
1238         UCHAR                   fc0;
1239         UCHAR                   fc1;
1240         UINT                    frame_type;
1241         UINT                    frame_subtype;
1242         UINT                    from_ds;
1243         UINT                    to_ds;
1244         INT                             a4_exists;
1245         INT                             qc_exists;
1246         UCHAR                   aes_out[16];
1247         int                     payload_index;
1248         UINT                    i;
1249         UCHAR                   ctr_preload[16];
1250         UCHAR                   chain_buffer[16];
1251         UCHAR                   padded_buffer[16];
1252         UCHAR                   mic_iv[16];
1253         UCHAR                   mic_header1[16];
1254         UCHAR                   mic_header2[16];
1255         UCHAR                   MIC[8];
1256         UCHAR                   TrailMIC[8];
1257
1258         fc0 = *pData;
1259         fc1 = *(pData + 1);
1260
1261         fc = *((PUSHORT)pData);
1262
1263         frame_type = ((fc0 >> 2) & 0x03);
1264         frame_subtype = ((fc0 >> 4) & 0x0f);
1265
1266         from_ds = (fc1 & 0x2) >> 1;
1267         to_ds = (fc1 & 0x1);
1268
1269         a4_exists = (from_ds & to_ds);
1270         qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
1271                                   (frame_subtype == 0x09) ||   /* Likely to change.    */
1272                                   (frame_subtype == 0x0a) ||
1273                                   (frame_subtype == 0x0b)
1274                                  );
1275
1276         HeaderLen = 24;
1277         if (a4_exists)
1278                 HeaderLen += 6;
1279
1280         KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
1281         KeyID = KeyID >> 6;
1282
1283         if (pWpaKey[KeyID].KeyLen == 0)
1284         {
1285                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
1286                 return FALSE;
1287         }
1288
1289         PN[0] = *(pData+ HeaderLen);
1290         PN[1] = *(pData+ HeaderLen + 1);
1291         PN[2] = *(pData+ HeaderLen + 4);
1292         PN[3] = *(pData+ HeaderLen + 5);
1293         PN[4] = *(pData+ HeaderLen + 6);
1294         PN[5] = *(pData+ HeaderLen + 7);
1295
1296         payload_len = DataByteCnt - HeaderLen - 8 - 8;  // 8 bytes for CCMP header , 8 bytes for MIC
1297         payload_remainder = (payload_len) % 16;
1298         num_blocks = (payload_len) / 16;
1299
1300
1301
1302         // Find start of payload
1303         payload_index = HeaderLen + 8; //IV+EIV
1304
1305         for (i=0; i< num_blocks; i++)
1306         {
1307                 construct_ctr_preload(ctr_preload,
1308                                                                 a4_exists,
1309                                                                 qc_exists,
1310                                                                 pData,
1311                                                                 PN,
1312                                                                 i+1 );
1313
1314                 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1315
1316                 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1317                 NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
1318                 payload_index += 16;
1319         }
1320
1321         //
1322         // If there is a short final block, then pad it
1323         // encrypt it and copy the unpadded part back
1324         //
1325         if (payload_remainder > 0)
1326         {
1327                 construct_ctr_preload(ctr_preload,
1328                                                                 a4_exists,
1329                                                                 qc_exists,
1330                                                                 pData,
1331                                                                 PN,
1332                                                                 num_blocks + 1);
1333
1334                 NdisZeroMemory(padded_buffer, 16);
1335                 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1336
1337                 aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1338
1339                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1340                 NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
1341                 payload_index += payload_remainder;
1342         }
1343
1344         //
1345         // Descrypt the MIC
1346         //
1347         construct_ctr_preload(ctr_preload,
1348                                                         a4_exists,
1349                                                         qc_exists,
1350                                                         pData,
1351                                                         PN,
1352                                                         0);
1353         NdisZeroMemory(padded_buffer, 16);
1354         NdisMoveMemory(padded_buffer, pData + payload_index, 8);
1355
1356         aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
1357
1358         bitwise_xor(aes_out, padded_buffer, chain_buffer);
1359
1360         NdisMoveMemory(TrailMIC, chain_buffer, 8);
1361
1362         //
1363         // Calculate MIC
1364         //
1365
1366         //Force the protected frame bit on
1367         *(pData + 1) = *(pData + 1) | 0x40;
1368
1369         // Find start of payload
1370         // Because the CCMP header has been removed
1371         payload_index = HeaderLen;
1372
1373         construct_mic_iv(
1374                                         mic_iv,
1375                                         qc_exists,
1376                                         a4_exists,
1377                                         pData,
1378                                         payload_len,
1379                                         PN);
1380
1381         construct_mic_header1(
1382                                                 mic_header1,
1383                                                 HeaderLen,
1384                                                 pData);
1385
1386         construct_mic_header2(
1387                                                 mic_header2,
1388                                                 pData,
1389                                                 a4_exists,
1390                                                 qc_exists);
1391
1392         aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
1393         bitwise_xor(aes_out, mic_header1, chain_buffer);
1394         aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1395         bitwise_xor(aes_out, mic_header2, chain_buffer);
1396         aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1397
1398         // iterate through each 16 byte payload block
1399         for (i = 0; i < num_blocks; i++)
1400         {
1401                 bitwise_xor(aes_out, pData + payload_index, chain_buffer);
1402                 payload_index += 16;
1403                 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1404         }
1405
1406         // Add on the final payload block if it needs padding
1407         if (payload_remainder > 0)
1408         {
1409                 NdisZeroMemory(padded_buffer, 16);
1410                 NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
1411
1412                 bitwise_xor(aes_out, padded_buffer, chain_buffer);
1413                 aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
1414         }
1415
1416         // aes_out contains padded mic, discard most significant
1417         // 8 bytes to generate 64 bit MIC
1418         for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
1419
1420         if (!NdisEqualMemory(MIC, TrailMIC, 8))
1421         {
1422                 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));         //MIC error.
1423                 return FALSE;
1424         }
1425
1426         return TRUE;
1427 }
1428
1429 /****************************************/
1430 /* aes128k128d()                        */
1431 /* Performs a 128 bit AES encrypt with  */
1432 /* 128 bit data.                        */
1433 /****************************************/
1434 VOID xor_128(
1435         IN  PUCHAR  a,
1436         IN  PUCHAR  b,
1437         OUT PUCHAR  out)
1438 {
1439         INT i;
1440
1441         for (i=0;i<16; i++)
1442         {
1443                 out[i] = a[i] ^ b[i];
1444         }
1445 }
1446
1447 VOID next_key(
1448         IN  PUCHAR  key,
1449         IN  INT     round)
1450 {
1451         UCHAR       rcon;
1452         UCHAR       sbox_key[4];
1453         UCHAR       rcon_table[12] =
1454         {
1455                 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
1456                 0x1b, 0x36, 0x36, 0x36
1457         };
1458
1459         sbox_key[0] = RTMPCkipSbox(key[13]);
1460         sbox_key[1] = RTMPCkipSbox(key[14]);
1461         sbox_key[2] = RTMPCkipSbox(key[15]);
1462         sbox_key[3] = RTMPCkipSbox(key[12]);
1463
1464         rcon = rcon_table[round];
1465
1466         xor_32(&key[0], sbox_key, &key[0]);
1467         key[0] = key[0] ^ rcon;
1468
1469         xor_32(&key[4], &key[0], &key[4]);
1470         xor_32(&key[8], &key[4], &key[8]);
1471         xor_32(&key[12], &key[8], &key[12]);
1472 }
1473
1474 VOID xor_32(
1475         IN  PUCHAR  a,
1476         IN  PUCHAR  b,
1477         OUT PUCHAR  out)
1478 {
1479         INT i;
1480
1481         for (i=0;i<4; i++)
1482         {
1483                 out[i] = a[i] ^ b[i];
1484         }
1485 }
1486
1487 VOID byte_sub(
1488         IN  PUCHAR  in,
1489         OUT PUCHAR  out)
1490 {
1491         INT i;
1492
1493         for (i=0; i< 16; i++)
1494         {
1495                 out[i] = RTMPCkipSbox(in[i]);
1496         }
1497 }
1498
1499 UCHAR RTMPCkipSbox(
1500         IN  UCHAR   a)
1501 {
1502         return SboxTable[(int)a];
1503 }
1504
1505 VOID shift_row(
1506         IN  PUCHAR  in,
1507         OUT PUCHAR  out)
1508 {
1509         out[0] =  in[0];
1510         out[1] =  in[5];
1511         out[2] =  in[10];
1512         out[3] =  in[15];
1513         out[4] =  in[4];
1514         out[5] =  in[9];
1515         out[6] =  in[14];
1516         out[7] =  in[3];
1517         out[8] =  in[8];
1518         out[9] =  in[13];
1519         out[10] = in[2];
1520         out[11] = in[7];
1521         out[12] = in[12];
1522         out[13] = in[1];
1523         out[14] = in[6];
1524         out[15] = in[11];
1525 }
1526
1527 VOID mix_column(
1528         IN  PUCHAR  in,
1529         OUT PUCHAR  out)
1530 {
1531         INT         i;
1532         UCHAR       add1b[4];
1533         UCHAR       add1bf7[4];
1534         UCHAR       rotl[4];
1535         UCHAR       swap_halfs[4];
1536         UCHAR       andf7[4];
1537         UCHAR       rotr[4];
1538         UCHAR       temp[4];
1539         UCHAR       tempb[4];
1540
1541         for (i=0 ; i<4; i++)
1542         {
1543                 if ((in[i] & 0x80)== 0x80)
1544                         add1b[i] = 0x1b;
1545                 else
1546                         add1b[i] = 0x00;
1547         }
1548
1549         swap_halfs[0] = in[2];    /* Swap halfs */
1550         swap_halfs[1] = in[3];
1551         swap_halfs[2] = in[0];
1552         swap_halfs[3] = in[1];
1553
1554         rotl[0] = in[3];        /* Rotate left 8 bits */
1555         rotl[1] = in[0];
1556         rotl[2] = in[1];
1557         rotl[3] = in[2];
1558
1559         andf7[0] = in[0] & 0x7f;
1560         andf7[1] = in[1] & 0x7f;
1561         andf7[2] = in[2] & 0x7f;
1562         andf7[3] = in[3] & 0x7f;
1563
1564         for (i = 3; i>0; i--)    /* logical shift left 1 bit */
1565         {
1566                 andf7[i] = andf7[i] << 1;
1567                 if ((andf7[i-1] & 0x80) == 0x80)
1568                 {
1569                         andf7[i] = (andf7[i] | 0x01);
1570                 }
1571         }
1572         andf7[0] = andf7[0] << 1;
1573         andf7[0] = andf7[0] & 0xfe;
1574
1575         xor_32(add1b, andf7, add1bf7);
1576
1577         xor_32(in, add1bf7, rotr);
1578
1579         temp[0] = rotr[0];         /* Rotate right 8 bits */
1580         rotr[0] = rotr[1];
1581         rotr[1] = rotr[2];
1582         rotr[2] = rotr[3];
1583         rotr[3] = temp[0];
1584
1585         xor_32(add1bf7, rotr, temp);
1586         xor_32(swap_halfs, rotl,tempb);
1587         xor_32(temp, tempb, out);
1588 }
1589