drivers/staging/rt2860/common/cmm_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         cmm_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 u32 Tkip_Sbox_Lower[256] = {
  46         0xA5, 0x84, 0x99, 0x8D, 0x0D, 0xBD, 0xB1, 0x54,
  47         0x50, 0x03, 0xA9, 0x7D, 0x19, 0x62, 0xE6, 0x9A,
  48         0x45, 0x9D, 0x40, 0x87, 0x15, 0xEB, 0xC9, 0x0B,
  49         0xEC, 0x67, 0xFD, 0xEA, 0xBF, 0xF7, 0x96, 0x5B,
  50         0xC2, 0x1C, 0xAE, 0x6A, 0x5A, 0x41, 0x02, 0x4F,
  51         0x5C, 0xF4, 0x34, 0x08, 0x93, 0x73, 0x53, 0x3F,
  52         0x0C, 0x52, 0x65, 0x5E, 0x28, 0xA1, 0x0F, 0xB5,
  53         0x09, 0x36, 0x9B, 0x3D, 0x26, 0x69, 0xCD, 0x9F,
  54         0x1B, 0x9E, 0x74, 0x2E, 0x2D, 0xB2, 0xEE, 0xFB,
  55         0xF6, 0x4D, 0x61, 0xCE, 0x7B, 0x3E, 0x71, 0x97,
  56         0xF5, 0x68, 0x00, 0x2C, 0x60, 0x1F, 0xC8, 0xED,
  57         0xBE, 0x46, 0xD9, 0x4B, 0xDE, 0xD4, 0xE8, 0x4A,
  58         0x6B, 0x2A, 0xE5, 0x16, 0xC5, 0xD7, 0x55, 0x94,
  59         0xCF, 0x10, 0x06, 0x81, 0xF0, 0x44, 0xBA, 0xE3,
  60         0xF3, 0xFE, 0xC0, 0x8A, 0xAD, 0xBC, 0x48, 0x04,
  61         0xDF, 0xC1, 0x75, 0x63, 0x30, 0x1A, 0x0E, 0x6D,
  62         0x4C, 0x14, 0x35, 0x2F, 0xE1, 0xA2, 0xCC, 0x39,
  63         0x57, 0xF2, 0x82, 0x47, 0xAC, 0xE7, 0x2B, 0x95,
  64         0xA0, 0x98, 0xD1, 0x7F, 0x66, 0x7E, 0xAB, 0x83,
  65         0xCA, 0x29, 0xD3, 0x3C, 0x79, 0xE2, 0x1D, 0x76,
  66         0x3B, 0x56, 0x4E, 0x1E, 0xDB, 0x0A, 0x6C, 0xE4,
  67         0x5D, 0x6E, 0xEF, 0xA6, 0xA8, 0xA4, 0x37, 0x8B,
  68         0x32, 0x43, 0x59, 0xB7, 0x8C, 0x64, 0xD2, 0xE0,
  69         0xB4, 0xFA, 0x07, 0x25, 0xAF, 0x8E, 0xE9, 0x18,
  70         0xD5, 0x88, 0x6F, 0x72, 0x24, 0xF1, 0xC7, 0x51,
  71         0x23, 0x7C, 0x9C, 0x21, 0xDD, 0xDC, 0x86, 0x85,
  72         0x90, 0x42, 0xC4, 0xAA, 0xD8, 0x05, 0x01, 0x12,
  73         0xA3, 0x5F, 0xF9, 0xD0, 0x91, 0x58, 0x27, 0xB9,
  74         0x38, 0x13, 0xB3, 0x33, 0xBB, 0x70, 0x89, 0xA7,
  75         0xB6, 0x22, 0x92, 0x20, 0x49, 0xFF, 0x78, 0x7A,
  76         0x8F, 0xF8, 0x80, 0x17, 0xDA, 0x31, 0xC6, 0xB8,
  77         0xC3, 0xB0, 0x77, 0x11, 0xCB, 0xFC, 0xD6, 0x3A
  78 };
  79
  80 u32 Tkip_Sbox_Upper[256] = {
  81         0xC6, 0xF8, 0xEE, 0xF6, 0xFF, 0xD6, 0xDE, 0x91,
  82         0x60, 0x02, 0xCE, 0x56, 0xE7, 0xB5, 0x4D, 0xEC,
  83         0x8F, 0x1F, 0x89, 0xFA, 0xEF, 0xB2, 0x8E, 0xFB,
  84         0x41, 0xB3, 0x5F, 0x45, 0x23, 0x53, 0xE4, 0x9B,
  85         0x75, 0xE1, 0x3D, 0x4C, 0x6C, 0x7E, 0xF5, 0x83,
  86         0x68, 0x51, 0xD1, 0xF9, 0xE2, 0xAB, 0x62, 0x2A,
  87         0x08, 0x95, 0x46, 0x9D, 0x30, 0x37, 0x0A, 0x2F,
  88         0x0E, 0x24, 0x1B, 0xDF, 0xCD, 0x4E, 0x7F, 0xEA,
  89         0x12, 0x1D, 0x58, 0x34, 0x36, 0xDC, 0xB4, 0x5B,
  90         0xA4, 0x76, 0xB7, 0x7D, 0x52, 0xDD, 0x5E, 0x13,
  91         0xA6, 0xB9, 0x00, 0xC1, 0x40, 0xE3, 0x79, 0xB6,
  92         0xD4, 0x8D, 0x67, 0x72, 0x94, 0x98, 0xB0, 0x85,
  93         0xBB, 0xC5, 0x4F, 0xED, 0x86, 0x9A, 0x66, 0x11,
  94         0x8A, 0xE9, 0x04, 0xFE, 0xA0, 0x78, 0x25, 0x4B,
  95         0xA2, 0x5D, 0x80, 0x05, 0x3F, 0x21, 0x70, 0xF1,
  96         0x63, 0x77, 0xAF, 0x42, 0x20, 0xE5, 0xFD, 0xBF,
  97         0x81, 0x18, 0x26, 0xC3, 0xBE, 0x35, 0x88, 0x2E,
  98         0x93, 0x55, 0xFC, 0x7A, 0xC8, 0xBA, 0x32, 0xE6,
  99         0xC0, 0x19, 0x9E, 0xA3, 0x44, 0x54, 0x3B, 0x0B,
 100         0x8C, 0xC7, 0x6B, 0x28, 0xA7, 0xBC, 0x16, 0xAD,
 101         0xDB, 0x64, 0x74, 0x14, 0x92, 0x0C, 0x48, 0xB8,
 102         0x9F, 0xBD, 0x43, 0xC4, 0x39, 0x31, 0xD3, 0xF2,
 103         0xD5, 0x8B, 0x6E, 0xDA, 0x01, 0xB1, 0x9C, 0x49,
 104         0xD8, 0xAC, 0xF3, 0xCF, 0xCA, 0xF4, 0x47, 0x10,
 105         0x6F, 0xF0, 0x4A, 0x5C, 0x38, 0x57, 0x73, 0x97,
 106         0xCB, 0xA1, 0xE8, 0x3E, 0x96, 0x61, 0x0D, 0x0F,
 107         0xE0, 0x7C, 0x71, 0xCC, 0x90, 0x06, 0xF7, 0x1C,
 108         0xC2, 0x6A, 0xAE, 0x69, 0x17, 0x99, 0x3A, 0x27,
 109         0xD9, 0xEB, 0x2B, 0x22, 0xD2, 0xA9, 0x07, 0x33,
 110         0x2D, 0x3C, 0x15, 0xC9, 0x87, 0xAA, 0x50, 0xA5,
 111         0x03, 0x59, 0x09, 0x1A, 0x65, 0xD7, 0x84, 0xD0,
 112         0x82, 0x29, 0x5A, 0x1E, 0x7B, 0xA8, 0x6D, 0x2C
 113 };
 114
 115 /* */
 116 /* Expanded IV for TKIP function. */
 117 /* */
 118 struct PACKED rt_tkip_iv {
 119         union PACKED {
 120                 struct PACKED {
 121                         u8 rc0;
 122                         u8 rc1;
 123                         u8 rc2;
 124
 125                         union PACKED {
 126                                 struct PACKED {
 127                                         u8 Rsvd:5;
 128                                         u8 ExtIV:1;
 129                                         u8 KeyID:2;
 130                                 } field;
 131                                 u8 Byte;
 132                         } CONTROL;
 133                 } field;
 134
 135                 unsigned long word;
 136         } IV16;
 137
 138         unsigned long IV32;
 139 };
 140
 141 /*
 142         ========================================================================
 143
 144         Routine Description:
 145                 Convert from u8[] to unsigned long in a portable way
 146
 147         Arguments:
 148       pMICKey           pointer to MIC Key
 149
 150         Return Value:
 151                 None
 152
 153         Note:
 154
 155         ========================================================================
 156 */
 157 unsigned long RTMPTkipGetUInt32(u8 *pMICKey)
 158 {
 159         unsigned long res = 0;
 160         int i;
 161
 162         for (i = 0; i < 4; i++) {
 163                 res |= (*pMICKey++) << (8 * i);
 164         }
 165
 166         return res;
 167 }
 168
 169 /*
 170         ========================================================================
 171
 172         Routine Description:
 173                 Convert from unsigned long to u8[] in a portable way
 174
 175         Arguments:
 176       pDst                      pointer to destination for convert unsigned long to u8[]
 177       val                       the value for convert
 178
 179         Return Value:
 180                 None
 181
 182         IRQL = DISPATCH_LEVEL
 183
 184         Note:
 185
 186         ========================================================================
 187 */
 188 void RTMPTkipPutUInt32(IN u8 *pDst, unsigned long val)
 189 {
 190         int i;
 191
 192         for (i = 0; i < 4; i++) {
 193                 *pDst++ = (u8)(val & 0xff);
 194                 val >>= 8;
 195         }
 196 }
 197
 198 /*
 199         ========================================================================
 200
 201         Routine Description:
 202                 Set the MIC Key.
 203
 204         Arguments:
 205       pAd               Pointer to our adapter
 206       pMICKey           pointer to MIC Key
 207
 208         Return Value:
 209                 None
 210
 211         IRQL = DISPATCH_LEVEL
 212
 213         Note:
 214
 215         ========================================================================
 216 */
 217 void RTMPTkipSetMICKey(struct rt_tkip_key_info *pTkip, u8 *pMICKey)
 218 {
 219         /* Set the key */
 220         pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
 221         pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
 222         /* and reset the message */
 223         pTkip->L = pTkip->K0;
 224         pTkip->R = pTkip->K1;
 225         pTkip->nBytesInM = 0;
 226         pTkip->M = 0;
 227 }
 228
 229 /*
 230         ========================================================================
 231
 232         Routine Description:
 233                 Calculate the MIC Value.
 234
 235         Arguments:
 236       pAd               Pointer to our adapter
 237       uChar                     Append this uChar
 238
 239         Return Value:
 240                 None
 241
 242         IRQL = DISPATCH_LEVEL
 243
 244         Note:
 245
 246         ========================================================================
 247 */
 248 void RTMPTkipAppendByte(struct rt_tkip_key_info *pTkip, u8 uChar)
 249 {
 250         /* Append the byte to our word-sized buffer */
 251         pTkip->M |= (uChar << (8 * pTkip->nBytesInM));
 252         pTkip->nBytesInM++;
 253         /* Process the word if it is full. */
 254         if (pTkip->nBytesInM >= 4) {
 255                 pTkip->L ^= pTkip->M;
 256                 pTkip->R ^= ROL32(pTkip->L, 17);
 257                 pTkip->L += pTkip->R;
 258                 pTkip->R ^=
 259                     ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->
 260                                                        L & 0x00ff00ff) << 8);
 261                 pTkip->L += pTkip->R;
 262                 pTkip->R ^= ROL32(pTkip->L, 3);
 263                 pTkip->L += pTkip->R;
 264                 pTkip->R ^= ROR32(pTkip->L, 2);
 265                 pTkip->L += pTkip->R;
 266                 /* Clear the buffer */
 267                 pTkip->M = 0;
 268                 pTkip->nBytesInM = 0;
 269         }
 270 }
 271
 272 /*
 273         ========================================================================
 274
 275         Routine Description:
 276                 Calculate the MIC Value.
 277
 278         Arguments:
 279       pAd               Pointer to our adapter
 280       pSrc                      Pointer to source data for Calculate MIC Value
 281       Len                       Indicate the length of the source data
 282
 283         Return Value:
 284                 None
 285
 286         IRQL = DISPATCH_LEVEL
 287
 288         Note:
 289
 290         ========================================================================
 291 */
 292 void RTMPTkipAppend(struct rt_tkip_key_info *pTkip, u8 *pSrc, u32 nBytes)
 293 {
 294         /* This is simple */
 295         while (nBytes > 0) {
 296                 RTMPTkipAppendByte(pTkip, *pSrc++);
 297                 nBytes--;
 298         }
 299 }
 300
 301 /*
 302         ========================================================================
 303
 304         Routine Description:
 305                 Get the MIC Value.
 306
 307         Arguments:
 308       pAd               Pointer to our adapter
 309
 310         Return Value:
 311                 None
 312
 313         IRQL = DISPATCH_LEVEL
 314
 315         Note:
 316                 the MIC Value is store in pAd->PrivateInfo.MIC
 317         ========================================================================
 318 */
 319 void RTMPTkipGetMIC(struct rt_tkip_key_info *pTkip)
 320 {
 321         /* Append the minimum padding */
 322         RTMPTkipAppendByte(pTkip, 0x5a);
 323         RTMPTkipAppendByte(pTkip, 0);
 324         RTMPTkipAppendByte(pTkip, 0);
 325         RTMPTkipAppendByte(pTkip, 0);
 326         RTMPTkipAppendByte(pTkip, 0);
 327         /* and then zeroes until the length is a multiple of 4 */
 328         while (pTkip->nBytesInM != 0) {
 329                 RTMPTkipAppendByte(pTkip, 0);
 330         }
 331         /* The appendByte function has already computed the result. */
 332         RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
 333         RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
 334 }
 335
 336 /*
 337         ========================================================================
 338
 339         Routine Description:
 340                 Init Tkip function.
 341
 342         Arguments:
 343       pAd               Pointer to our adapter
 344                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 345                 KeyId           TK Key ID
 346                 pTA                     Pointer to transmitter address
 347                 pMICKey         pointer to MIC Key
 348
 349         Return Value:
 350                 None
 351
 352         IRQL = DISPATCH_LEVEL
 353
 354         Note:
 355
 356         ========================================================================
 357 */
 358 void RTMPInitTkipEngine(struct rt_rtmp_adapter *pAd,
 359                         u8 *pKey,
 360                         u8 KeyId,
 361                         u8 *pTA,
 362                         u8 *pMICKey,
 363                         u8 *pTSC, unsigned long *pIV16, unsigned long *pIV32)
 364 {
 365         struct rt_tkip_iv tkipIv;
 366
 367         /* Prepare 8 bytes TKIP encapsulation for MPDU */
 368         NdisZeroMemory(&tkipIv, sizeof(struct rt_tkip_iv));
 369         tkipIv.IV16.field.rc0 = *(pTSC + 1);
 370         tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
 371         tkipIv.IV16.field.rc2 = *pTSC;
 372         tkipIv.IV16.field.CONTROL.field.ExtIV = 1;      /* 0: non-extended IV, 1: an extended IV */
 373         tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
 374 /*      tkipIv.IV32 = *(unsigned long *)(pTSC + 2); */
 375         NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);    /* Copy IV */
 376
 377         *pIV16 = tkipIv.IV16.word;
 378         *pIV32 = tkipIv.IV32;
 379 }
 380
 381 /*
 382         ========================================================================
 383
 384         Routine Description:
 385                 Init MIC Value calculation function which include set MIC key &
 386                 calculate first 16 bytes (DA + SA + priority +  0)
 387
 388         Arguments:
 389       pAd               Pointer to our adapter
 390                 pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
 391                 pDA                     Pointer to DA address
 392                 pSA                     Pointer to SA address
 393                 pMICKey         pointer to MIC Key
 394
 395         Return Value:
 396                 None
 397
 398         Note:
 399
 400         ========================================================================
 401 */
 402 void RTMPInitMICEngine(struct rt_rtmp_adapter *pAd,
 403                        u8 *pKey,
 404                        u8 *pDA,
 405                        u8 *pSA, u8 UserPriority, u8 *pMICKey)
 406 {
 407         unsigned long Priority = UserPriority;
 408
 409         /* Init MIC value calculation */
 410         RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
 411         /* DA */
 412         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
 413         /* SA */
 414         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
 415         /* Priority + 3 bytes of 0 */
 416         RTMPTkipAppend(&pAd->PrivateInfo.Tx, (u8 *)& Priority, 4);
 417 }
 418
 419 /*
 420         ========================================================================
 421
 422         Routine Description:
 423                 Compare MIC value of received MSDU
 424
 425         Arguments:
 426                 pAd     Pointer to our adapter
 427                 pSrc        Pointer to the received Plain text data
 428                 pDA                     Pointer to DA address
 429                 pSA                     Pointer to SA address
 430                 pMICKey         pointer to MIC Key
 431                 Len         the length of the received plain text data exclude MIC value
 432
 433         Return Value:
 434                 TRUE        MIC value matched
 435                 FALSE       MIC value mismatched
 436
 437         IRQL = DISPATCH_LEVEL
 438
 439         Note:
 440
 441         ========================================================================
 442 */
 443 BOOLEAN RTMPTkipCompareMICValue(struct rt_rtmp_adapter *pAd,
 444                                 u8 *pSrc,
 445                                 u8 *pDA,
 446                                 u8 *pSA,
 447                                 u8 *pMICKey,
 448                                 u8 UserPriority, u32 Len)
 449 {
 450         u8 OldMic[8];
 451         unsigned long Priority = UserPriority;
 452
 453         /* Init MIC value calculation */
 454         RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
 455         /* DA */
 456         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
 457         /* SA */
 458         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
 459         /* Priority + 3 bytes of 0 */
 460         RTMPTkipAppend(&pAd->PrivateInfo.Rx, (u8 *)& Priority, 4);
 461
 462         /* Calculate MIC value from plain text data */
 463         RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
 464
 465         /* Get MIC valude from received frame */
 466         NdisMoveMemory(OldMic, pSrc + Len, 8);
 467
 468         /* Get MIC value from decrypted plain data */
 469         RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
 470
 471         /* Move MIC value from MSDU, this steps should move to data path. */
 472         /* Since the MIC value might cross MPDUs. */
 473         if (!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8)) {
 474                 DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));        /*MIC error. */
 475
 476                 return (FALSE);
 477         }
 478         return (TRUE);
 479 }
 480
 481 /*
 482         ========================================================================
 483
 484         Routine Description:
 485                 Copy frame from waiting queue into relative ring buffer and set
 486         appropriate ASIC register to kick hardware transmit function
 487
 488         Arguments:
 489                 pAd             Pointer to our adapter
 490                 void *  Pointer to Ndis Packet for MIC calculation
 491                 pEncap                  Pointer to LLC encap data
 492                 LenEncap                Total encap length, might be 0 which indicates no encap
 493
 494         Return Value:
 495                 None
 496
 497         IRQL = DISPATCH_LEVEL
 498
 499         Note:
 500
 501         ========================================================================
 502 */
 503 void RTMPCalculateMICValue(struct rt_rtmp_adapter *pAd,
 504                            void *pPacket,
 505                            u8 *pEncap,
 506                            struct rt_cipher_key *pKey, u8 apidx)
 507 {
 508         struct rt_packet_info PacketInfo;
 509         u8 *pSrcBufVA;
 510         u32 SrcBufLen;
 511         u8 *pSrc;
 512         u8 UserPriority;
 513         u8 vlan_offset = 0;
 514
 515         RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
 516
 517         UserPriority = RTMP_GET_PACKET_UP(pPacket);
 518         pSrc = pSrcBufVA;
 519
 520         /* determine if this is a vlan packet */
 521         if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
 522                 vlan_offset = 4;
 523
 524         {
 525                 RTMPInitMICEngine(pAd,
 526                                   pKey->Key,
 527                                   pSrc, pSrc + 6, UserPriority, pKey->TxMic);
 528         }
 529
 530         if (pEncap != NULL) {
 531                 /* LLC encapsulation */
 532                 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
 533                 /* Protocol Type */
 534                 RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset,
 535                                2);
 536         }
 537         SrcBufLen -= (14 + vlan_offset);
 538         pSrc += (14 + vlan_offset);
 539         do {
 540                 if (SrcBufLen > 0) {
 541                         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
 542                 }
 543
 544                 break;          /* No need handle next packet */
 545
 546         } while (TRUE);         /* End of copying payload */
 547
 548         /* Compute the final MIC Value */
 549         RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
 550 }
 551
 552 /************************************************************/
 553 /* tkip_sbox()                                                                                                                          */
 554 /* Returns a 16 bit value from a 64K entry table. The Table */
 555 /* is synthesized from two 256 entry byte wide tables.          */
 556 /************************************************************/
 557
 558 u32 tkip_sbox(u32 index)
 559 {
 560         u32 index_low;
 561         u32 index_high;
 562         u32 left, right;
 563
 564         index_low = (index % 256);
 565         index_high = ((index >> 8) % 256);
 566
 567         left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
 568         right =
 569             Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
 570
 571         return (left ^ right);
 572 }
 573
 574 u32 rotr1(u32 a)
 575 {
 576         unsigned int b;
 577
 578         if ((a & 0x01) == 0x01) {
 579                 b = (a >> 1) | 0x8000;
 580         } else {
 581                 b = (a >> 1) & 0x7fff;
 582         }
 583         b = b % 65536;
 584         return b;
 585 }
 586
 587 void RTMPTkipMixKey(u8 * key, u8 * ta, unsigned long pnl,       /* Least significant 16 bits of PN */
 588                     unsigned long pnh,  /* Most significant 32 bits of PN */
 589                     u8 * rc4key, u32 * p1k)
 590 {
 591
 592         u32 tsc0;
 593         u32 tsc1;
 594         u32 tsc2;
 595
 596         u32 ppk0;
 597         u32 ppk1;
 598         u32 ppk2;
 599         u32 ppk3;
 600         u32 ppk4;
 601         u32 ppk5;
 602
 603         int i;
 604         int j;
 605
 606         tsc0 = (unsigned int)((pnh >> 16) % 65536);     /* msb */
 607         tsc1 = (unsigned int)(pnh % 65536);
 608         tsc2 = (unsigned int)(pnl % 65536);     /* lsb */
 609
 610         /* Phase 1, step 1 */
 611         p1k[0] = tsc1;
 612         p1k[1] = tsc0;
 613         p1k[2] = (u32)(ta[0] + (ta[1] * 256));
 614         p1k[3] = (u32)(ta[2] + (ta[3] * 256));
 615         p1k[4] = (u32)(ta[4] + (ta[5] * 256));
 616
 617         /* Phase 1, step 2 */
 618         for (i = 0; i < 8; i++) {
 619                 j = 2 * (i & 1);
 620                 p1k[0] =
 621                     (p1k[0] +
 622                      tkip_sbox((p1k[4] ^ ((256 * key[1 + j]) + key[j])) %
 623                                65536)) % 65536;
 624                 p1k[1] =
 625                     (p1k[1] +
 626                      tkip_sbox((p1k[0] ^ ((256 * key[5 + j]) + key[4 + j])) %
 627                                65536)) % 65536;
 628                 p1k[2] =
 629                     (p1k[2] +
 630                      tkip_sbox((p1k[1] ^ ((256 * key[9 + j]) + key[8 + j])) %
 631                                65536)) % 65536;
 632                 p1k[3] =
 633                     (p1k[3] +
 634                      tkip_sbox((p1k[2] ^ ((256 * key[13 + j]) + key[12 + j])) %
 635                                65536)) % 65536;
 636                 p1k[4] =
 637                     (p1k[4] +
 638                      tkip_sbox((p1k[3] ^ (((256 * key[1 + j]) + key[j]))) %
 639                                65536)) % 65536;
 640                 p1k[4] = (p1k[4] + i) % 65536;
 641         }
 642
 643         /* Phase 2, Step 1 */
 644         ppk0 = p1k[0];
 645         ppk1 = p1k[1];
 646         ppk2 = p1k[2];
 647         ppk3 = p1k[3];
 648         ppk4 = p1k[4];
 649         ppk5 = (p1k[4] + tsc2) % 65536;
 650
 651         /* Phase2, Step 2 */
 652         ppk0 = ppk0 + tkip_sbox((ppk5 ^ ((256 * key[1]) + key[0])) % 65536);
 653         ppk1 = ppk1 + tkip_sbox((ppk0 ^ ((256 * key[3]) + key[2])) % 65536);
 654         ppk2 = ppk2 + tkip_sbox((ppk1 ^ ((256 * key[5]) + key[4])) % 65536);
 655         ppk3 = ppk3 + tkip_sbox((ppk2 ^ ((256 * key[7]) + key[6])) % 65536);
 656         ppk4 = ppk4 + tkip_sbox((ppk3 ^ ((256 * key[9]) + key[8])) % 65536);
 657         ppk5 = ppk5 + tkip_sbox((ppk4 ^ ((256 * key[11]) + key[10])) % 65536);
 658
 659         ppk0 = ppk0 + rotr1(ppk5 ^ ((256 * key[13]) + key[12]));
 660         ppk1 = ppk1 + rotr1(ppk0 ^ ((256 * key[15]) + key[14]));
 661         ppk2 = ppk2 + rotr1(ppk1);
 662         ppk3 = ppk3 + rotr1(ppk2);
 663         ppk4 = ppk4 + rotr1(ppk3);
 664         ppk5 = ppk5 + rotr1(ppk4);
 665
 666         /* Phase 2, Step 3 */
 667         /* Phase 2, Step 3 */
 668
 669         tsc0 = (unsigned int)((pnh >> 16) % 65536);     /* msb */
 670         tsc1 = (unsigned int)(pnh % 65536);
 671         tsc2 = (unsigned int)(pnl % 65536);     /* lsb */
 672
 673         rc4key[0] = (tsc2 >> 8) % 256;
 674         rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
 675         rc4key[2] = tsc2 % 256;
 676         rc4key[3] = ((ppk5 ^ ((256 * key[1]) + key[0])) >> 1) % 256;
 677
 678         rc4key[4] = ppk0 % 256;
 679         rc4key[5] = (ppk0 >> 8) % 256;
 680
 681         rc4key[6] = ppk1 % 256;
 682         rc4key[7] = (ppk1 >> 8) % 256;
 683
 684         rc4key[8] = ppk2 % 256;
 685         rc4key[9] = (ppk2 >> 8) % 256;
 686
 687         rc4key[10] = ppk3 % 256;
 688         rc4key[11] = (ppk3 >> 8) % 256;
 689
 690         rc4key[12] = ppk4 % 256;
 691         rc4key[13] = (ppk4 >> 8) % 256;
 692
 693         rc4key[14] = ppk5 % 256;
 694         rc4key[15] = (ppk5 >> 8) % 256;
 695 }
 696
 697 /* */
 698 /* TRUE: Success! */
 699 /* FALSE: Decrypt Error! */
 700 /* */
 701 BOOLEAN RTMPSoftDecryptTKIP(struct rt_rtmp_adapter *pAd,
 702                             u8 *pData,
 703                             unsigned long DataByteCnt,
 704                             u8 UserPriority, struct rt_cipher_key *pWpaKey)
 705 {
 706         u8 KeyID;
 707         u32 HeaderLen;
 708         u8 fc0;
 709         u8 fc1;
 710         u16 fc;
 711         u32 frame_type;
 712         u32 frame_subtype;
 713         u32 from_ds;
 714         u32 to_ds;
 715         int a4_exists;
 716         int qc_exists;
 717         u16 duration;
 718         u16 seq_control;
 719         u16 qos_control;
 720         u8 TA[MAC_ADDR_LEN];
 721         u8 DA[MAC_ADDR_LEN];
 722         u8 SA[MAC_ADDR_LEN];
 723         u8 RC4Key[16];
 724         u32 p1k[5];             /*for mix_key; */
 725         unsigned long pnl;              /* Least significant 16 bits of PN */
 726         unsigned long pnh;              /* Most significant 32 bits of PN */
 727         u32 num_blocks;
 728         u32 payload_remainder;
 729         struct rt_arcfourcontext ArcFourContext;
 730         u32 crc32 = 0;
 731         u32 trailfcs = 0;
 732         u8 MIC[8];
 733         u8 TrailMIC[8];
 734
 735         fc0 = *pData;
 736         fc1 = *(pData + 1);
 737
 738         fc = *((u16 *)pData);
 739
 740         frame_type = ((fc0 >> 2) & 0x03);
 741         frame_subtype = ((fc0 >> 4) & 0x0f);
 742
 743         from_ds = (fc1 & 0x2) >> 1;
 744         to_ds = (fc1 & 0x1);
 745
 746         a4_exists = (from_ds & to_ds);
 747         qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
 748                      (frame_subtype == 0x09) || /* Likely to change.    */
 749                      (frame_subtype == 0x0a) || (frame_subtype == 0x0b)
 750             );
 751
 752         HeaderLen = 24;
 753         if (a4_exists)
 754                 HeaderLen += 6;
 755
 756         KeyID = *((u8 *)(pData + HeaderLen + 3));
 757         KeyID = KeyID >> 6;
 758
 759         if (pWpaKey[KeyID].KeyLen == 0) {
 760                 DBGPRINT(RT_DEBUG_TRACE,
 761                          ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n",
 762                           KeyID));
 763                 return FALSE;
 764         }
 765
 766         duration = *((u16 *)(pData + 2));
 767
 768         seq_control = *((u16 *)(pData + 22));
 769
 770         if (qc_exists) {
 771                 if (a4_exists) {
 772                         qos_control = *((u16 *)(pData + 30));
 773                 } else {
 774                         qos_control = *((u16 *)(pData + 24));
 775                 }
 776         }
 777
 778         if (to_ds == 0 && from_ds == 1) {
 779                 NdisMoveMemory(DA, pData + 4, MAC_ADDR_LEN);
 780                 NdisMoveMemory(SA, pData + 16, MAC_ADDR_LEN);
 781                 NdisMoveMemory(TA, pData + 10, MAC_ADDR_LEN);   /*BSSID */
 782         } else if (to_ds == 0 && from_ds == 0) {
 783                 NdisMoveMemory(TA, pData + 10, MAC_ADDR_LEN);
 784                 NdisMoveMemory(DA, pData + 4, MAC_ADDR_LEN);
 785                 NdisMoveMemory(SA, pData + 10, MAC_ADDR_LEN);
 786         } else if (to_ds == 1 && from_ds == 0) {
 787                 NdisMoveMemory(SA, pData + 10, MAC_ADDR_LEN);
 788                 NdisMoveMemory(TA, pData + 10, MAC_ADDR_LEN);
 789                 NdisMoveMemory(DA, pData + 16, MAC_ADDR_LEN);
 790         } else if (to_ds == 1 && from_ds == 1) {
 791                 NdisMoveMemory(TA, pData + 10, MAC_ADDR_LEN);
 792                 NdisMoveMemory(DA, pData + 16, MAC_ADDR_LEN);
 793                 NdisMoveMemory(SA, pData + 22, MAC_ADDR_LEN);
 794         }
 795
 796         num_blocks = (DataByteCnt - 16) / 16;
 797         payload_remainder = (DataByteCnt - 16) % 16;
 798
 799         pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
 800         pnh = *((unsigned long *)(pData + HeaderLen + 4));
 801         pnh = cpu2le32(pnh);
 802         RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
 803
 804         ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
 805
 806         ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen,
 807                         pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
 808         NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
 809         crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);      /*Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS). */
 810         crc32 ^= 0xffffffff;    /* complement */
 811
 812         if (crc32 != cpu2le32(trailfcs)) {
 813                 DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));      /*ICV error. */
 814
 815                 return (FALSE);
 816         }
 817
 818         NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
 819         RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority,
 820                           pWpaKey[KeyID].RxMic);
 821         RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen,
 822                        DataByteCnt - HeaderLen - 8 - 12);
 823         RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
 824         NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
 825
 826         if (!NdisEqualMemory(MIC, TrailMIC, 8)) {
 827                 DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));      /*MIC error. */
 828                 /*RTMPReportMicError(pAd, &pWpaKey[KeyID]);     // marked by AlbertY @ 20060630 */
 829                 return (FALSE);
 830         }
 831         /*DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!\n"); */
 832         return TRUE;
 833 }