git.oblomov.eu Git - linux-2.6/blob - drivers/staging/rt2860/sta/wpa.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         wpa.c
  29
  30         Abstract:
  31
  32         Revision History:
  33         Who                     When                    What
  34         --------        ----------              ----------------------------------------------
  35         Jan     Lee             03-07-22                Initial
  36         Paul Lin        03-11-28                Modify for supplicant
  37 */
  38 #include "../rt_config.h"
  39
  40 #define         WPARSNIE        0xdd
  41 #define         WPA2RSNIE       0x30
  42
  43 //extern UCHAR BIT8[];
  44 UCHAR   CipherWpaPskTkip[] = {
  45                 0xDD, 0x16,                             // RSN IE
  46                 0x00, 0x50, 0xf2, 0x01, // oui
  47                 0x01, 0x00,                             // Version
  48                 0x00, 0x50, 0xf2, 0x02, // Multicast
  49                 0x01, 0x00,                             // Number of unicast
  50                 0x00, 0x50, 0xf2, 0x02, // unicast
  51                 0x01, 0x00,                             // number of authentication method
  52                 0x00, 0x50, 0xf2, 0x02  // authentication
  53                 };
  54 UCHAR   CipherWpaPskTkipLen = (sizeof(CipherWpaPskTkip) / sizeof(UCHAR));
  55
  56 UCHAR   CipherWpaPskAes[] = {
  57                 0xDD, 0x16,                     // RSN IE
  58                 0x00, 0x50, 0xf2, 0x01, // oui
  59                 0x01, 0x00,                             // Version
  60                 0x00, 0x50, 0xf2, 0x04, // Multicast
  61                 0x01, 0x00,                             // Number of unicast
  62                 0x00, 0x50, 0xf2, 0x04, // unicast
  63                 0x01, 0x00,                             // number of authentication method
  64                 0x00, 0x50, 0xf2, 0x02  // authentication
  65                 };
  66 UCHAR   CipherWpaPskAesLen = (sizeof(CipherWpaPskAes) / sizeof(UCHAR));
  67
  68 UCHAR   CipherSuiteCiscoCCKM[] = {
  69                 0xDD, 0x16,                             // RSN IE
  70                 0x00, 0x50, 0xf2, 0x01, // oui
  71                 0x01, 0x00,                             // Version
  72                 0x00, 0x40, 0x96, 0x01, // Multicast
  73                 0x01, 0x00,                             // Number of uicast
  74                 0x00, 0x40, 0x96, 0x01, // unicast
  75                 0x01, 0x00,                             // number of authentication method
  76                 0x00, 0x40, 0x96, 0x00  // Authentication
  77                 };
  78 UCHAR   CipherSuiteCiscoCCKMLen = (sizeof(CipherSuiteCiscoCCKM) / sizeof(UCHAR));
  79
  80 UCHAR   CipherSuiteCiscoCCKM24[] = {
  81                 0xDD, 0x18,                             // RSN IE
  82                 0x00, 0x50, 0xf2, 0x01, // oui
  83                 0x01, 0x00,                             // Version
  84                 0x00, 0x40, 0x96, 0x01, // Multicast
  85                 0x01, 0x00,                             // Number of uicast
  86                 0x00, 0x40, 0x96, 0x01, // unicast
  87                 0x01, 0x00,                             // number of authentication method
  88                 0x00, 0x40, 0x96, 0x00,
  89                 0x28, 0x00// Authentication
  90                 };
  91
  92 UCHAR   CipherSuiteCiscoCCKM24Len = (sizeof(CipherSuiteCiscoCCKM24) / sizeof(UCHAR));
  93
  94 UCHAR   CipherSuiteCCXTkip[] = {
  95                 0xDD, 0x16,                             // RSN IE
  96                 0x00, 0x50, 0xf2, 0x01, // oui
  97                 0x01, 0x00,                             // Version
  98                 0x00, 0x50, 0xf2, 0x02, // Multicast
  99                 0x01, 0x00,                             // Number of unicast
 100                 0x00, 0x50, 0xf2, 0x02, // unicast
 101                 0x01, 0x00,                             // number of authentication method
 102                 0x00, 0x50, 0xf2, 0x01  // authentication
 103                 };
 104 UCHAR   CipherSuiteCCXTkipLen = (sizeof(CipherSuiteCCXTkip) / sizeof(UCHAR));
 105
 106 UCHAR   CCX_LLC_HDR[] = {0xAA, 0xAA, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
 107 UCHAR   LLC_NORMAL[] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
 108
 109 UCHAR   EAPOL_FRAME[] = {0x88, 0x8E};
 110
 111 BOOLEAN CheckRSNIE(
 112         IN  PRTMP_ADAPTER   pAd,
 113         IN  PUCHAR          pData,
 114         IN  UCHAR           DataLen,
 115         OUT     UCHAR                   *Offset);
 116
 117 void inc_byte_array(UCHAR *counter, int len);
 118
 119 /*
 120         ========================================================================
 121
 122         Routine Description:
 123                 Classify WPA EAP message type
 124
 125         Arguments:
 126                 EAPType         Value of EAP message type
 127                 MsgType         Internal Message definition for MLME state machine
 128
 129         Return Value:
 130                 TRUE            Found appropriate message type
 131                 FALSE           No appropriate message type
 132
 133         IRQL = DISPATCH_LEVEL
 134
 135         Note:
 136                 All these constants are defined in wpa.h
 137                 For supplicant, there is only EAPOL Key message avaliable
 138
 139         ========================================================================
 140 */
 141 BOOLEAN WpaMsgTypeSubst(
 142         IN      UCHAR   EAPType,
 143         OUT     INT             *MsgType)
 144 {
 145         switch (EAPType)
 146         {
 147                 case EAPPacket:
 148                         *MsgType = MT2_EAPPacket;
 149                         break;
 150                 case EAPOLStart:
 151                         *MsgType = MT2_EAPOLStart;
 152                         break;
 153                 case EAPOLLogoff:
 154                         *MsgType = MT2_EAPOLLogoff;
 155                         break;
 156                 case EAPOLKey:
 157                         *MsgType = MT2_EAPOLKey;
 158                         break;
 159                 case EAPOLASFAlert:
 160                         *MsgType = MT2_EAPOLASFAlert;
 161                         break;
 162                 default:
 163                         return FALSE;
 164         }
 165         return TRUE;
 166 }
 167
 168 /*
 169         ==========================================================================
 170         Description:
 171                 association     state machine init,     including state transition and timer init
 172         Parameters:
 173                 S -     pointer to the association state machine
 174         ==========================================================================
 175  */
 176 VOID WpaPskStateMachineInit(
 177         IN      PRTMP_ADAPTER   pAd,
 178         IN      STATE_MACHINE *S,
 179         OUT     STATE_MACHINE_FUNC Trans[])
 180 {
 181         StateMachineInit(S,     Trans, MAX_WPA_PSK_STATE, MAX_WPA_PSK_MSG, (STATE_MACHINE_FUNC)Drop, WPA_PSK_IDLE, WPA_MACHINE_BASE);
 182         StateMachineSetAction(S, WPA_PSK_IDLE, MT2_EAPOLKey, (STATE_MACHINE_FUNC)WpaEAPOLKeyAction);
 183 }
 184
 185 /*
 186         ==========================================================================
 187         Description:
 188                 This is state machine function.
 189                 When receiving EAPOL packets which is  for 802.1x key management.
 190                 Use     both in WPA, and WPAPSK case.
 191                 In this function, further dispatch to different functions according     to the received packet.  3 categories are :
 192                   1.  normal 4-way pairwisekey and 2-way groupkey handshake
 193                   2.  MIC error (Countermeasures attack)  report packet from STA.
 194                   3.  Request for pairwise/group key update     from STA
 195         Return:
 196         ==========================================================================
 197 */
 198 VOID WpaEAPOLKeyAction(
 199         IN      PRTMP_ADAPTER   pAd,
 200         IN      MLME_QUEUE_ELEM *Elem)
 201
 202 {
 203         INT             MsgType = EAPOL_MSG_INVALID;
 204         PKEY_DESCRIPTER pKeyDesc;
 205         PHEADER_802_11  pHeader; //red
 206         UCHAR           ZeroReplay[LEN_KEY_DESC_REPLAY];
 207         UCHAR EapolVr;
 208         KEY_INFO                peerKeyInfo;
 209
 210         DBGPRINT(RT_DEBUG_TRACE, ("-----> WpaEAPOLKeyAction\n"));
 211
 212         // Get 802.11 header first
 213         pHeader = (PHEADER_802_11) Elem->Msg;
 214
 215         // Get EAPoL-Key Descriptor
 216         pKeyDesc = (PKEY_DESCRIPTER) &Elem->Msg[(LENGTH_802_11 + LENGTH_802_1_H + LENGTH_EAPOL_H)];
 217
 218         NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
 219         NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pKeyDesc->KeyInfo, sizeof(KEY_INFO));
 220
 221         *((USHORT *)&peerKeyInfo) = cpu2le16(*((USHORT *)&peerKeyInfo));
 222
 223
 224         // 1. Check EAPOL frame version and type
 225         EapolVr = (UCHAR) Elem->Msg[LENGTH_802_11+LENGTH_802_1_H];
 226
 227     if (((EapolVr != EAPOL_VER) && (EapolVr != EAPOL_VER2)) || ((pKeyDesc->Type != WPA1_KEY_DESC) && (pKeyDesc->Type != WPA2_KEY_DESC)))
 228         {
 229         DBGPRINT(RT_DEBUG_ERROR, ("Key descripter does not match with WPA rule\n"));
 230                         return;
 231         }
 232
 233         // First validate replay counter, only accept message with larger replay counter
 234         // Let equal pass, some AP start with all zero replay counter
 235         NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
 236
 237         if((RTMPCompareMemory(pKeyDesc->ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
 238                 (RTMPCompareMemory(pKeyDesc->ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
 239         {
 240                 DBGPRINT(RT_DEBUG_ERROR, ("   ReplayCounter not match   \n"));
 241                 return;
 242         }
 243
 244         // Process WPA2PSK frame
 245         if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
 246         {
 247                 if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
 248                         (peerKeyInfo.EKD_DL == 0) &&
 249                         (peerKeyInfo.KeyAck == 1) &&
 250                         (peerKeyInfo.KeyMic == 0) &&
 251                         (peerKeyInfo.Secure == 0) &&
 252                         (peerKeyInfo.Error == 0) &&
 253                         (peerKeyInfo.Request == 0))
 254                 {
 255                         MsgType = EAPOL_PAIR_MSG_1;
 256                         DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
 257                 } else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
 258                         (peerKeyInfo.EKD_DL  == 1) &&
 259                         (peerKeyInfo.KeyAck == 1) &&
 260                         (peerKeyInfo.KeyMic == 1) &&
 261                         (peerKeyInfo.Secure == 1) &&
 262                         (peerKeyInfo.Error == 0) &&
 263                         (peerKeyInfo.Request == 0))
 264                 {
 265                         MsgType = EAPOL_PAIR_MSG_3;
 266                         DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
 267                 } else if((peerKeyInfo.KeyType == GROUPKEY) &&
 268                         (peerKeyInfo.EKD_DL == 1) &&
 269                         (peerKeyInfo.KeyAck == 1) &&
 270                         (peerKeyInfo.KeyMic == 1) &&
 271                         (peerKeyInfo.Secure == 1) &&
 272                         (peerKeyInfo.Error == 0) &&
 273                         (peerKeyInfo.Request == 0))
 274                 {
 275                         MsgType = EAPOL_GROUP_MSG_1;
 276                         DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
 277                 }
 278
 279                 // We will assume link is up (assoc suceess and port not secured).
 280                 // All state has to be able to process message from previous state
 281                 switch(pAd->StaCfg.WpaState)
 282                 {
 283                 case SS_START:
 284                         if(MsgType == EAPOL_PAIR_MSG_1)
 285                         {
 286                                 Wpa2PairMsg1Action(pAd, Elem);
 287                                 pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
 288                         }
 289                         break;
 290
 291                 case SS_WAIT_MSG_3:
 292                         if(MsgType == EAPOL_PAIR_MSG_1)
 293                         {
 294                                 Wpa2PairMsg1Action(pAd, Elem);
 295                                 pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
 296                         }
 297                         else if(MsgType == EAPOL_PAIR_MSG_3)
 298                         {
 299                                 Wpa2PairMsg3Action(pAd, Elem);
 300                                 pAd->StaCfg.WpaState = SS_WAIT_GROUP;
 301                         }
 302                         break;
 303
 304                 case SS_WAIT_GROUP:             // When doing group key exchange
 305                 case SS_FINISH:                 // This happened when update group key
 306                         if(MsgType == EAPOL_PAIR_MSG_1)
 307                         {
 308                             // Reset port secured variable
 309                                 pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
 310                                 Wpa2PairMsg1Action(pAd, Elem);
 311                                 pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
 312                         }
 313                         else if(MsgType == EAPOL_PAIR_MSG_3)
 314                         {
 315                             // Reset port secured variable
 316                                 pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
 317                                 Wpa2PairMsg3Action(pAd, Elem);
 318                                 pAd->StaCfg.WpaState = SS_WAIT_GROUP;
 319                         }
 320                         else if(MsgType == EAPOL_GROUP_MSG_1)
 321                         {
 322                                 WpaGroupMsg1Action(pAd, Elem);
 323                                 pAd->StaCfg.WpaState = SS_FINISH;
 324                         }
 325                         break;
 326
 327                 default:
 328                         break;
 329                 }
 330         }
 331         // Process WPAPSK Frame
 332         // Classify message Type, either pairwise message 1, 3, or group message 1 for supplicant
 333         else if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
 334         {
 335                 if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
 336                         (peerKeyInfo.KeyIndex == 0) &&
 337                         (peerKeyInfo.KeyAck == 1) &&
 338                         (peerKeyInfo.KeyMic == 0) &&
 339                         (peerKeyInfo.Secure == 0) &&
 340                         (peerKeyInfo.Error == 0) &&
 341                         (peerKeyInfo.Request == 0))
 342                 {
 343                         MsgType = EAPOL_PAIR_MSG_1;
 344                         DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
 345                 }
 346                 else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
 347                         (peerKeyInfo.KeyIndex == 0) &&
 348                         (peerKeyInfo.KeyAck == 1) &&
 349                         (peerKeyInfo.KeyMic == 1) &&
 350                         (peerKeyInfo.Secure == 0) &&
 351                         (peerKeyInfo.Error == 0) &&
 352                         (peerKeyInfo.Request == 0))
 353                 {
 354                         MsgType = EAPOL_PAIR_MSG_3;
 355                         DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
 356                 }
 357                 else if((peerKeyInfo.KeyType == GROUPKEY) &&
 358                         (peerKeyInfo.KeyIndex != 0) &&
 359                         (peerKeyInfo.KeyAck == 1) &&
 360                         (peerKeyInfo.KeyMic == 1) &&
 361                         (peerKeyInfo.Secure == 1) &&
 362                         (peerKeyInfo.Error == 0) &&
 363                         (peerKeyInfo.Request == 0))
 364                 {
 365                         MsgType = EAPOL_GROUP_MSG_1;
 366                         DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
 367                 }
 368
 369                 // We will assume link is up (assoc suceess and port not secured).
 370                 // All state has to be able to process message from previous state
 371                 switch(pAd->StaCfg.WpaState)
 372                 {
 373                 case SS_START:
 374                         if(MsgType == EAPOL_PAIR_MSG_1)
 375                         {
 376                                 WpaPairMsg1Action(pAd, Elem);
 377                                 pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
 378                         }
 379                         break;
 380
 381                 case SS_WAIT_MSG_3:
 382                         if(MsgType == EAPOL_PAIR_MSG_1)
 383                         {
 384                                 WpaPairMsg1Action(pAd, Elem);
 385                                 pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
 386                         }
 387                         else if(MsgType == EAPOL_PAIR_MSG_3)
 388                         {
 389                                 WpaPairMsg3Action(pAd, Elem);
 390                                 pAd->StaCfg.WpaState = SS_WAIT_GROUP;
 391                         }
 392                         break;
 393
 394                 case SS_WAIT_GROUP:             // When doing group key exchange
 395                 case SS_FINISH:                 // This happened when update group key
 396                         if(MsgType == EAPOL_PAIR_MSG_1)
 397                         {
 398                                 WpaPairMsg1Action(pAd, Elem);
 399                                 pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
 400                                 // Reset port secured variable
 401                                 pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
 402                         }
 403                         else if(MsgType == EAPOL_PAIR_MSG_3)
 404                         {
 405                                 WpaPairMsg3Action(pAd, Elem);
 406                                 pAd->StaCfg.WpaState = SS_WAIT_GROUP;
 407                                 // Reset port secured variable
 408                                 pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
 409                         }
 410                         else if(MsgType == EAPOL_GROUP_MSG_1)
 411                         {
 412                                 WpaGroupMsg1Action(pAd, Elem);
 413                                 pAd->StaCfg.WpaState = SS_FINISH;
 414                         }
 415                         break;
 416
 417                 default:
 418                         break;
 419                 }
 420         }
 421
 422         DBGPRINT(RT_DEBUG_TRACE, ("<----- WpaEAPOLKeyAction\n"));
 423 }
 424
 425 /*
 426         ========================================================================
 427
 428         Routine Description:
 429                 Process Pairwise key 4-way handshaking
 430
 431         Arguments:
 432                 pAd     Pointer to our adapter
 433                 Elem            Message body
 434
 435         Return Value:
 436                 None
 437
 438         Note:
 439
 440         ========================================================================
 441 */
 442 VOID    WpaPairMsg1Action(
 443         IN  PRTMP_ADAPTER   pAd,
 444         IN  MLME_QUEUE_ELEM *Elem)
 445 {
 446         PHEADER_802_11      pHeader;
 447         UCHAR                           *mpool, *PTK, *digest;
 448         PUCHAR              pOutBuffer = NULL;
 449         UCHAR               Header802_3[14];
 450         ULONG               FrameLen = 0;
 451         PEAPOL_PACKET       pMsg1;
 452         EAPOL_PACKET        Packet;
 453         UCHAR               Mic[16];
 454
 455         DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action ----->\n"));
 456
 457         // allocate memory pool
 458         os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
 459
 460         if (mpool == NULL)
 461                 return;
 462
 463         // PTK Len = 80.
 464         PTK = (UCHAR *) ROUND_UP(mpool, 4);
 465         // digest Len = 80.
 466         digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
 467
 468         pHeader = (PHEADER_802_11) Elem->Msg;
 469
 470         // Process message 1 from authenticator
 471         pMsg1 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
 472
 473         // 1. Save Replay counter, it will use to verify message 3 and construct message 2
 474         NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
 475
 476         // 2. Save ANonce
 477         NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
 478
 479         // Generate random SNonce
 480         GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
 481
 482         // Calc PTK(ANonce, SNonce)
 483         WpaCountPTK(pAd,
 484                 pAd->StaCfg.PMK,
 485                 pAd->StaCfg.ANonce,
 486                 pAd->CommonCfg.Bssid,
 487                 pAd->StaCfg.SNonce,
 488                 pAd->CurrentAddress,
 489                 PTK,
 490                 LEN_PTK);
 491
 492         // Save key to PTK entry
 493         NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
 494
 495         // init 802.3 header and Fill Packet
 496         MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
 497
 498         // Zero Message 2 body
 499         NdisZeroMemory(&Packet, sizeof(Packet));
 500         Packet.ProVer   = EAPOL_VER;
 501         Packet.ProType  = EAPOLKey;
 502         //
 503         // Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
 504         //
 505         Packet.KeyDesc.Type = WPA1_KEY_DESC;
 506         // 1. Key descriptor version and appropriate RSN IE
 507         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
 508         {
 509                 Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
 510         }
 511         else      // TKIP
 512         {
 513                 Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
 514         }
 515
 516         // fill in Data Material and its length
 517         Packet.KeyDesc.KeyData[0] = IE_WPA;
 518         Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
 519         Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
 520         NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
 521
 522         // Update packet length after decide Key data payload
 523         Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
 524
 525         // Update Key length
 526         Packet.KeyDesc.KeyLength[0] = pMsg1->KeyDesc.KeyLength[0];
 527         Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
 528         // 2. Key Type PeerKey
 529         Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
 530
 531         // 3. KeyMic field presented
 532         Packet.KeyDesc.KeyInfo.KeyMic  = 1;
 533
 534         //Convert to little-endian format.
 535         *((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
 536
 537
 538         // 4. Fill SNonce
 539         NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
 540
 541         // 5. Key Replay Count
 542         NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
 543
 544         // Send EAPOL(0, 1, 0, 0, 0, P, 0, SNonce, MIC, RSN_IE)
 545         // Out buffer for transmitting message 2
 546         MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
 547         if(pOutBuffer == NULL)
 548         {
 549                 os_free_mem(pAd, mpool);
 550                 return;
 551         }
 552         // Prepare EAPOL frame for MIC calculation
 553         // Be careful, only EAPOL frame is counted for MIC calculation
 554         MakeOutgoingFrame(pOutBuffer,           &FrameLen,
 555                 Packet.Body_Len[1] + 4,    &Packet,
 556                 END_OF_ARGS);
 557
 558         // 6. Prepare and Fill MIC value
 559         NdisZeroMemory(Mic, sizeof(Mic));
 560         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
 561         {       // AES
 562
 563                 HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
 564                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
 565         }
 566         else
 567         {       // TKIP
 568                 hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
 569         }
 570         NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
 571
 572         //hex_dump("MIC", Mic, LEN_KEY_DESC_MIC);
 573
 574                 MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
 575                                                 LENGTH_802_3,                           &Header802_3,
 576                                                 Packet.Body_Len[1] + 4,    &Packet,
 577                                                 END_OF_ARGS);
 578
 579
 580         // 5. Copy frame to Tx ring and send Msg 2 to authenticator
 581         RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
 582
 583         MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
 584         os_free_mem(pAd, (PUCHAR)mpool);
 585
 586         DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action <-----\n"));
 587 }
 588
 589 VOID Wpa2PairMsg1Action(
 590         IN  PRTMP_ADAPTER   pAd,
 591         IN  MLME_QUEUE_ELEM *Elem)
 592 {
 593         PHEADER_802_11      pHeader;
 594         UCHAR                           *mpool, *PTK, *digest;
 595         PUCHAR              pOutBuffer = NULL;
 596         UCHAR               Header802_3[14];
 597         ULONG               FrameLen = 0;
 598         PEAPOL_PACKET       pMsg1;
 599         EAPOL_PACKET        Packet;
 600         UCHAR               Mic[16];
 601
 602         DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action ----->\n"));
 603
 604         // allocate memory pool
 605         os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
 606
 607         if (mpool == NULL)
 608                 return;
 609
 610         // PTK Len = 80.
 611         PTK = (UCHAR *) ROUND_UP(mpool, 4);
 612         // digest Len = 80.
 613         digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
 614
 615         pHeader = (PHEADER_802_11) Elem->Msg;
 616
 617         // Process message 1 from authenticator
 618                 pMsg1 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
 619
 620         // 1. Save Replay counter, it will use to verify message 3 and construct message 2
 621         NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
 622
 623         // 2. Save ANonce
 624         NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
 625
 626         // Generate random SNonce
 627         GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
 628
 629         if(pMsg1->KeyDesc.KeyDataLen[1] > 0 )
 630         {
 631                 // cached PMKID
 632         }
 633
 634         // Calc PTK(ANonce, SNonce)
 635         WpaCountPTK(pAd,
 636                 pAd->StaCfg.PMK,
 637                 pAd->StaCfg.ANonce,
 638                 pAd->CommonCfg.Bssid,
 639                 pAd->StaCfg.SNonce,
 640                 pAd->CurrentAddress,
 641                 PTK,
 642                 LEN_PTK);
 643
 644         // Save key to PTK entry
 645         NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
 646
 647         // init 802.3 header and Fill Packet
 648         MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
 649
 650         // Zero message 2 body
 651         NdisZeroMemory(&Packet, sizeof(Packet));
 652         Packet.ProVer   = EAPOL_VER;
 653         Packet.ProType  = EAPOLKey;
 654         //
 655         // Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
 656         //
 657         Packet.KeyDesc.Type = WPA2_KEY_DESC;
 658
 659         // 1. Key descriptor version and appropriate RSN IE
 660         if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
 661         {
 662                 Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
 663         }
 664         else      // TKIP
 665         {
 666                 Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
 667         }
 668
 669         // fill in Data Material and its length
 670         Packet.KeyDesc.KeyData[0] = IE_WPA2;
 671         Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
 672         Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
 673         NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
 674
 675         // Update packet length after decide Key data payload
 676         Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
 677
 678         // 2. Key Type PeerKey
 679         Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
 680
 681         // 3. KeyMic field presented
 682         Packet.KeyDesc.KeyInfo.KeyMic  = 1;
 683
 684         // Update Key Length
 685         Packet.KeyDesc.KeyLength[0] = 0;
 686         Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
 687
 688         // 4. Fill SNonce
 689         NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
 690
 691         // 5. Key Replay Count
 692         NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
 693
 694         // Convert to little-endian format.
 695         *((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
 696
 697         // Send EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
 698         // Out buffer for transmitting message 2
 699         MlmeAllocateMemory(pAd,  (PUCHAR *)&pOutBuffer);  // allocate memory
 700         if(pOutBuffer == NULL)
 701         {
 702                 os_free_mem(pAd, mpool);
 703                 return;
 704         }
 705
 706         // Prepare EAPOL frame for MIC calculation
 707         // Be careful, only EAPOL frame is counted for MIC calculation
 708         MakeOutgoingFrame(pOutBuffer,        &FrameLen,
 709                 Packet.Body_Len[1] + 4, &Packet,
 710                 END_OF_ARGS);
 711
 712         // 6. Prepare and Fill MIC value
 713         NdisZeroMemory(Mic, sizeof(Mic));
 714         if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
 715         {
 716                 // AES
 717                 HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
 718                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
 719         }
 720         else
 721         {
 722                 hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
 723         }
 724         NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
 725
 726
 727         // Make  Transmitting frame
 728         MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
 729                                                 LENGTH_802_3,                   &Header802_3,
 730                                                 Packet.Body_Len[1] + 4, &Packet,
 731                                                 END_OF_ARGS);
 732
 733
 734         // 5. Copy frame to Tx ring
 735         RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
 736
 737         MlmeFreeMemory(pAd, pOutBuffer);
 738         os_free_mem(pAd, mpool);
 739
 740         DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action <-----\n"));
 741
 742 }
 743
 744 /*
 745         ========================================================================
 746
 747         Routine Description:
 748                 Process Pairwise key 4-way handshaking
 749
 750         Arguments:
 751                 pAd     Pointer to our adapter
 752                 Elem            Message body
 753
 754         Return Value:
 755                 None
 756
 757         Note:
 758
 759         ========================================================================
 760 */
 761 VOID    WpaPairMsg3Action(
 762         IN      PRTMP_ADAPTER   pAd,
 763         IN      MLME_QUEUE_ELEM *Elem)
 764
 765 {
 766         PHEADER_802_11      pHeader;
 767         PUCHAR                  pOutBuffer = NULL;
 768         UCHAR               Header802_3[14];
 769         ULONG                   FrameLen = 0;
 770         EAPOL_PACKET        Packet;
 771         PEAPOL_PACKET       pMsg3;
 772         UCHAR                   Mic[16], OldMic[16];
 773         MAC_TABLE_ENTRY         *pEntry = NULL;
 774         UCHAR                           skip_offset;
 775         KEY_INFO                        peerKeyInfo;
 776
 777         DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action ----->\n"));
 778
 779         // Record 802.11 header & the received EAPOL packet Msg3
 780         pHeader = (PHEADER_802_11) Elem->Msg;
 781         pMsg3 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
 782
 783         NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
 784         NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
 785
 786         *((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
 787
 788
 789         // 1. Verify cipher type match
 790         if (pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
 791         {
 792                 return;
 793         }
 794         else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
 795         {
 796                 return;
 797         }
 798
 799         // Verify RSN IE
 800         //if (!RTMPEqualMemory(pMsg3->KeyDesc.KeyData, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len))
 801         if (!CheckRSNIE(pAd, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1], &skip_offset))
 802         {
 803                 DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in Msg 3 of WPA1 4-way handshake!! \n"));
 804                 hex_dump("The original RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
 805                 hex_dump("The received RSN_IE", pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
 806                 return;
 807         }
 808         else
 809                 DBGPRINT(RT_DEBUG_TRACE, ("RSN_IE VALID in Msg 3 of WPA1 4-way handshake!! \n"));
 810
 811
 812         // 2. Check MIC value
 813         // Save the MIC and replace with zero
 814         NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
 815         NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
 816         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
 817         {
 818                 // AES
 819                 UCHAR digest[80];
 820
 821                 HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
 822                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
 823         }
 824         else    // TKIP
 825         {
 826                 hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
 827         }
 828
 829         if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
 830         {
 831                 DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
 832                 return;
 833         }
 834         else
 835                 DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
 836
 837         // 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
 838         if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
 839                 return;
 840
 841         // Update new replay counter
 842         NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
 843
 844         // 4. Double check ANonce
 845         if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
 846                 return;
 847
 848         // init 802.3 header and Fill Packet
 849         MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
 850
 851         // Zero Message 4 body
 852         NdisZeroMemory(&Packet, sizeof(Packet));
 853         Packet.ProVer   = EAPOL_VER;
 854         Packet.ProType  = EAPOLKey;
 855         Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;                // No data field
 856
 857         //
 858         // Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
 859         //
 860         Packet.KeyDesc.Type = WPA1_KEY_DESC;
 861
 862         // Key descriptor version and appropriate RSN IE
 863         Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
 864
 865         // Update Key Length
 866         Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
 867         Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
 868
 869         // Key Type PeerKey
 870         Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
 871
 872         // KeyMic field presented
 873         Packet.KeyDesc.KeyInfo.KeyMic  = 1;
 874
 875         // In Msg3,  KeyInfo.secure =0 if Group Key HS to come. 1 if no group key HS
 876         // Station sends Msg4  KeyInfo.secure should be the same as that in Msg.3
 877         Packet.KeyDesc.KeyInfo.Secure= peerKeyInfo.Secure;
 878
 879         // Convert to little-endian format.
 880         *((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
 881
 882         // Key Replay count
 883         NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
 884
 885         // Out buffer for transmitting message 4
 886         MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
 887         if(pOutBuffer == NULL)
 888                 return;
 889
 890         // Prepare EAPOL frame for MIC calculation
 891         // Be careful, only EAPOL frame is counted for MIC calculation
 892         MakeOutgoingFrame(pOutBuffer,           &FrameLen,
 893                 Packet.Body_Len[1] + 4,    &Packet,
 894                 END_OF_ARGS);
 895
 896         // Prepare and Fill MIC value
 897         NdisZeroMemory(Mic, sizeof(Mic));
 898         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
 899         {
 900                 // AES
 901                 UCHAR digest[80];
 902
 903                 HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
 904                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
 905         }
 906         else
 907         {
 908                 hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
 909         }
 910         NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
 911
 912         // Update PTK
 913         // Prepare pair-wise key information into shared key table
 914         NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
 915         pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
 916     NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
 917         NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
 918         NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
 919
 920         // Decide its ChiperAlg
 921         if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
 922                 pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
 923         else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
 924                 pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
 925         else
 926                 pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
 927
 928         // Update these related information to MAC_TABLE_ENTRY
 929         pEntry = &pAd->MacTab.Content[BSSID_WCID];
 930         NdisMoveMemory(pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
 931         NdisMoveMemory(pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
 932         NdisMoveMemory(pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
 933         pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
 934
 935         // Update pairwise key information to ASIC Shared Key Table
 936         AsicAddSharedKeyEntry(pAd,
 937                                                   BSS0,
 938                                                   0,
 939                                                   pAd->SharedKey[BSS0][0].CipherAlg,
 940                                                   pAd->SharedKey[BSS0][0].Key,
 941                                                   pAd->SharedKey[BSS0][0].TxMic,
 942                                                   pAd->SharedKey[BSS0][0].RxMic);
 943
 944         // Update ASIC WCID attribute table and IVEIV table
 945         RTMPAddWcidAttributeEntry(pAd,
 946                                                           BSS0,
 947                                                           0,
 948                                                           pAd->SharedKey[BSS0][0].CipherAlg,
 949                                                           pEntry);
 950
 951         // Make transmitting frame
 952         MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
 953                                                 LENGTH_802_3,                   &Header802_3,
 954                                                 Packet.Body_Len[1] + 4, &Packet,
 955                                                 END_OF_ARGS);
 956
 957
 958         // Copy frame to Tx ring and Send Message 4 to authenticator
 959         RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
 960
 961         MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
 962
 963         DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action <-----\n"));
 964 }
 965
 966 VOID    Wpa2PairMsg3Action(
 967         IN  PRTMP_ADAPTER   pAd,
 968         IN  MLME_QUEUE_ELEM *Elem)
 969
 970 {
 971         PHEADER_802_11      pHeader;
 972         PUCHAR              pOutBuffer = NULL;
 973         UCHAR               Header802_3[14];
 974         ULONG               FrameLen = 0;
 975         EAPOL_PACKET        Packet;
 976         PEAPOL_PACKET       pMsg3;
 977         UCHAR               Mic[16], OldMic[16];
 978         UCHAR               *mpool, *KEYDATA, *digest;
 979         UCHAR               Key[32];
 980         MAC_TABLE_ENTRY         *pEntry = NULL;
 981         KEY_INFO                        peerKeyInfo;
 982
 983         // allocate memory
 984         os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
 985
 986         if(mpool == NULL)
 987                 return;
 988
 989         // KEYDATA Len = 512.
 990         KEYDATA = (UCHAR *) ROUND_UP(mpool, 4);
 991         // digest Len = 80.
 992         digest = (UCHAR *) ROUND_UP(KEYDATA + 512, 4);
 993
 994         DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg3Action ----->\n"));
 995
 996         pHeader = (PHEADER_802_11) Elem->Msg;
 997
 998         // Process message 3 frame.
 999         pMsg3 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
1000
1001         NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
1002         NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
1003
1004         *((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
1005
1006         // 1. Verify cipher type match
1007         if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer!= 2))
1008         {
1009                 os_free_mem(pAd, (PUCHAR)mpool);
1010                 return;
1011         }
1012         else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
1013         {
1014                 os_free_mem(pAd, (PUCHAR)mpool);
1015                 return;
1016         }
1017
1018         // 2. Check MIC value
1019         // Save the MIC and replace with zero
1020         NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
1021         NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
1022         if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
1023         {
1024                 // AES
1025                 HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
1026                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
1027         }
1028         else
1029         {
1030                 hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
1031         }
1032
1033         if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
1034         {
1035                 DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
1036                 os_free_mem(pAd, (PUCHAR)mpool);
1037                 return;
1038         }
1039         else
1040                 DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
1041
1042         // 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
1043         if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
1044         {
1045                 os_free_mem(pAd, (PUCHAR)mpool);
1046                 return;
1047         }
1048
1049         // Update new replay counter
1050         NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
1051
1052         // 4. Double check ANonce
1053         if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
1054         {
1055                 os_free_mem(pAd, (PUCHAR)mpool);
1056                 return;
1057         }
1058
1059         // Obtain GTK
1060         // 5. Decrypt GTK from Key Data
1061         DBGPRINT_RAW(RT_DEBUG_TRACE, ("EKD = %d\n", peerKeyInfo.EKD_DL));
1062         if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
1063         {
1064                 // Decrypt AES GTK
1065                 AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA, pMsg3->KeyDesc.KeyDataLen[1],pMsg3->KeyDesc.KeyData);
1066         }
1067         else      // TKIP
1068         {
1069                 INT i;
1070                 // Decrypt TKIP GTK
1071                 // Construct 32 bytes RC4 Key
1072                 NdisMoveMemory(Key, pMsg3->KeyDesc.KeyIv, 16);
1073                 NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
1074                 ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
1075                 //discard first 256 bytes
1076                 for(i = 0; i < 256; i++)
1077                         ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
1078                 // Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
1079                 ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
1080         }
1081
1082         if (!ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], 1))
1083         {
1084                 os_free_mem(pAd, (PUCHAR)mpool);
1085                 return;
1086         }
1087
1088         // Update GTK to ASIC
1089         // Update group key information to ASIC Shared Key Table
1090         AsicAddSharedKeyEntry(pAd,
1091                                                   BSS0,
1092                                                   pAd->StaCfg.DefaultKeyId,
1093                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
1094                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
1095                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
1096                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
1097
1098         // Update ASIC WCID attribute table and IVEIV table
1099         RTMPAddWcidAttributeEntry(pAd,
1100                                                           BSS0,
1101                                                           pAd->StaCfg.DefaultKeyId,
1102                                                           pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
1103                                                           NULL);
1104
1105         // init 802.3 header and Fill Packet
1106         MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
1107
1108         // Zero message 4 body
1109         NdisZeroMemory(&Packet, sizeof(Packet));
1110         Packet.ProVer   = EAPOL_VER;
1111         Packet.ProType  = EAPOLKey;
1112         Packet.Body_Len[1]      = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;            // No data field
1113
1114         //
1115         // Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
1116         //
1117         Packet.KeyDesc.Type = WPA2_KEY_DESC;
1118
1119         // Key descriptor version and appropriate RSN IE
1120         Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
1121
1122         // Update Key Length
1123         Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
1124         Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
1125
1126         // Key Type PeerKey
1127         Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
1128
1129         // KeyMic field presented
1130         Packet.KeyDesc.KeyInfo.KeyMic  = 1;
1131         Packet.KeyDesc.KeyInfo.Secure = 1;
1132
1133         // Convert to little-endian format.
1134         *((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
1135
1136         // Key Replay count
1137         NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
1138
1139         // Out buffer for transmitting message 4
1140         MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
1141         if(pOutBuffer == NULL)
1142         {
1143                 os_free_mem(pAd, (PUCHAR)mpool);
1144                 return;
1145         }
1146
1147         // Prepare EAPOL frame for MIC calculation
1148         // Be careful, only EAPOL frame is counted for MIC calculation
1149         MakeOutgoingFrame(pOutBuffer,           &FrameLen,
1150                 Packet.Body_Len[1] + 4,    &Packet,
1151                 END_OF_ARGS);
1152
1153         // Prepare and Fill MIC value
1154         NdisZeroMemory(Mic, sizeof(Mic));
1155         if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
1156         {
1157                 // AES
1158                 HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
1159                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
1160         }
1161         else
1162         {
1163                 hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
1164         }
1165         NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
1166
1167         // Update PTK
1168         // Prepare pair-wise key information into shared key table
1169         NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
1170         pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
1171     NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
1172         NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
1173         NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
1174
1175         // Decide its ChiperAlg
1176         if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
1177                 pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
1178         else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
1179                 pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
1180         else
1181                 pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
1182
1183         // Update these related information to MAC_TABLE_ENTRY
1184         pEntry = &pAd->MacTab.Content[BSSID_WCID];
1185         NdisMoveMemory(&pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
1186         NdisMoveMemory(&pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
1187         NdisMoveMemory(&pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
1188         pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
1189
1190         // Update pairwise key information to ASIC Shared Key Table
1191         AsicAddSharedKeyEntry(pAd,
1192                                                   BSS0,
1193                                                   0,
1194                                                   pAd->SharedKey[BSS0][0].CipherAlg,
1195                                                   pAd->SharedKey[BSS0][0].Key,
1196                                                   pAd->SharedKey[BSS0][0].TxMic,
1197                                                   pAd->SharedKey[BSS0][0].RxMic);
1198
1199         // Update ASIC WCID attribute table and IVEIV table
1200         RTMPAddWcidAttributeEntry(pAd,
1201                                                           BSS0,
1202                                                           0,
1203                                                           pAd->SharedKey[BSS0][0].CipherAlg,
1204                                                           pEntry);
1205
1206         // Make  Transmitting frame
1207         MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
1208                                                 LENGTH_802_3,                   &Header802_3,
1209                                                 Packet.Body_Len[1] + 4, &Packet,
1210                                                 END_OF_ARGS);
1211
1212
1213         // Copy frame to Tx ring and Send Message 4 to authenticator
1214         RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
1215
1216         // set 802.1x port control
1217         STA_PORT_SECURED(pAd);
1218
1219     // Indicate Connected for GUI
1220     pAd->IndicateMediaState = NdisMediaStateConnected;
1221
1222         MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
1223         os_free_mem(pAd, (PUCHAR)mpool);
1224
1225
1226         // send wireless event - for set key done WPA2
1227         if (pAd->CommonCfg.bWirelessEvent)
1228                 RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pEntry->Addr, BSS0, 0);
1229
1230         DBGPRINT(RT_DEBUG_ERROR, ("Wpa2PairMsg3Action <-----\n"));
1231
1232 }
1233
1234 /*
1235         ========================================================================
1236
1237         Routine Description:
1238                 Process Group key 2-way handshaking
1239
1240         Arguments:
1241                 pAd     Pointer to our adapter
1242                 Elem            Message body
1243
1244         Return Value:
1245                 None
1246
1247         Note:
1248
1249         ========================================================================
1250 */
1251 VOID    WpaGroupMsg1Action(
1252         IN      PRTMP_ADAPTER   pAd,
1253         IN      MLME_QUEUE_ELEM *Elem)
1254
1255 {
1256         PUCHAR              pOutBuffer = NULL;
1257         UCHAR               Header802_3[14];
1258         ULONG               FrameLen = 0;
1259         EAPOL_PACKET        Packet;
1260         PEAPOL_PACKET       pGroup;
1261         UCHAR               *mpool, *digest, *KEYDATA;
1262         UCHAR               Mic[16], OldMic[16];
1263         UCHAR               GTK[32], Key[32];
1264         KEY_INFO                        peerKeyInfo;
1265
1266         // allocate memory
1267         os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
1268
1269         if(mpool == NULL)
1270                 return;
1271
1272         // digest Len = 80.
1273         digest = (UCHAR *) ROUND_UP(mpool, 4);
1274         // KEYDATA Len = 512.
1275         KEYDATA = (UCHAR *) ROUND_UP(digest + 80, 4);
1276
1277         DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action ----->\n"));
1278
1279         // Process Group Message 1 frame. skip 802.11 header(24) & LLC_SNAP header(8)
1280         pGroup = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
1281
1282         NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
1283         NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pGroup->KeyDesc.KeyInfo, sizeof(KEY_INFO));
1284
1285         *((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
1286
1287         // 0. Check cipher type match
1288         if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
1289         {
1290                 os_free_mem(pAd, (PUCHAR)mpool);
1291                 return;
1292         }
1293         else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
1294         {
1295                 os_free_mem(pAd, (PUCHAR)mpool);
1296                 return;
1297         }
1298
1299         // 1. Verify Replay counter
1300         //    Check Replay Counter, it has to be larger than last one. No need to be exact one larger
1301         if(RTMPCompareMemory(pGroup->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
1302         {
1303                 os_free_mem(pAd, (PUCHAR)mpool);
1304                 return;
1305         }
1306
1307         // Update new replay counter
1308         NdisMoveMemory(pAd->StaCfg.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
1309
1310         // 2. Verify MIC is valid
1311         // Save the MIC and replace with zero
1312         NdisMoveMemory(OldMic, pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
1313         NdisZeroMemory(pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
1314
1315         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
1316         {       // AES
1317                 HMAC_SHA1((PUCHAR) pGroup, pGroup->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
1318                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
1319         }
1320         else
1321         {       // TKIP
1322                 hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pGroup, pGroup->Body_Len[1] + 4, Mic);
1323         }
1324
1325         if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
1326         {
1327                 DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
1328                 MlmeFreeMemory(pAd, (PUCHAR)mpool);
1329                 return;
1330         }
1331         else
1332                 DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
1333
1334
1335         // 3. Decrypt GTK from Key Data
1336         if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
1337         {
1338                 // Decrypt AES GTK
1339                 AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA,  pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyData);
1340         }
1341         else    // TKIP
1342         {
1343                 INT i;
1344
1345                 // Decrypt TKIP GTK
1346                 // Construct 32 bytes RC4 Key
1347                 NdisMoveMemory(Key, pGroup->KeyDesc.KeyIv, 16);
1348                 NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
1349                 ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
1350                 //discard first 256 bytes
1351                 for(i = 0; i < 256; i++)
1352                         ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
1353                 // Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
1354                 ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pGroup->KeyDesc.KeyData, pGroup->KeyDesc.KeyDataLen[1]);
1355         }
1356
1357         // Process decrypted key data material
1358         // Parse keyData to handle KDE format for WPA2PSK
1359         if (peerKeyInfo.EKD_DL)
1360         {
1361                 if (!ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1], 0))
1362                 {
1363                         os_free_mem(pAd, (PUCHAR)mpool);
1364                         return;
1365                 }
1366         }
1367         else    // WPAPSK
1368         {
1369                 // set key material, TxMic and RxMic for WPAPSK
1370                 NdisMoveMemory(GTK, KEYDATA, 32);
1371                 NdisMoveMemory(pAd->StaCfg.GTK, GTK, 32);
1372                 pAd->StaCfg.DefaultKeyId = peerKeyInfo.KeyIndex;
1373
1374                 // Prepare pair-wise key information into shared key table
1375                 NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
1376                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
1377                 NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, GTK, LEN_TKIP_EK);
1378                 NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &GTK[16], LEN_TKIP_RXMICK);
1379                 NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &GTK[24], LEN_TKIP_TXMICK);
1380
1381                 // Update Shared Key CipherAlg
1382                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
1383                 if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
1384                         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
1385                 else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
1386                         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
1387 #ifndef RT30xx
1388                 else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
1389                         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
1390                 else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
1391                         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;
1392 #endif
1393
1394         //hex_dump("Group Key :", pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, LEN_TKIP_EK);
1395         }
1396
1397         // Update group key information to ASIC Shared Key Table
1398         AsicAddSharedKeyEntry(pAd,
1399                                                   BSS0,
1400                                                   pAd->StaCfg.DefaultKeyId,
1401                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
1402                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
1403                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
1404                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
1405
1406         // Update ASIC WCID attribute table and IVEIV table
1407         RTMPAddWcidAttributeEntry(pAd,
1408                                                           BSS0,
1409                                                           pAd->StaCfg.DefaultKeyId,
1410                                                           pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
1411                                                           NULL);
1412
1413         // set 802.1x port control
1414         STA_PORT_SECURED(pAd);
1415
1416     // Indicate Connected for GUI
1417     pAd->IndicateMediaState = NdisMediaStateConnected;
1418
1419         // init header and Fill Packet
1420         MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
1421
1422         // Zero Group message 1 body
1423         NdisZeroMemory(&Packet, sizeof(Packet));
1424         Packet.ProVer   = EAPOL_VER;
1425         Packet.ProType  = EAPOLKey;
1426         Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;                // No data field
1427
1428         //
1429         // Group Message 2 as  EAPOL-Key(1,0,0,0,G,0,0,MIC,0)
1430         //
1431         if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
1432         {
1433                 Packet.KeyDesc.Type = WPA2_KEY_DESC;
1434         }
1435         else
1436         {
1437                 Packet.KeyDesc.Type = WPA1_KEY_DESC;
1438         }
1439
1440         // Key descriptor version and appropriate RSN IE
1441         Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
1442
1443         // Update Key Length
1444         Packet.KeyDesc.KeyLength[0] = pGroup->KeyDesc.KeyLength[0];
1445         Packet.KeyDesc.KeyLength[1] = pGroup->KeyDesc.KeyLength[1];
1446
1447         // Key Index as G-Msg 1
1448         if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
1449                 Packet.KeyDesc.KeyInfo.KeyIndex = peerKeyInfo.KeyIndex;
1450
1451         // Key Type Group key
1452         Packet.KeyDesc.KeyInfo.KeyType = GROUPKEY;
1453
1454         // KeyMic field presented
1455         Packet.KeyDesc.KeyInfo.KeyMic  = 1;
1456
1457         // Secure bit
1458         Packet.KeyDesc.KeyInfo.Secure  = 1;
1459
1460         // Convert to little-endian format.
1461         *((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
1462
1463         // Key Replay count
1464         NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
1465
1466         // Out buffer for transmitting group message 2
1467         MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
1468         if(pOutBuffer == NULL)
1469         {
1470                 MlmeFreeMemory(pAd, (PUCHAR)mpool);
1471                 return;
1472         }
1473
1474         // Prepare EAPOL frame for MIC calculation
1475         // Be careful, only EAPOL frame is counted for MIC calculation
1476         MakeOutgoingFrame(pOutBuffer,           &FrameLen,
1477                 Packet.Body_Len[1] + 4,    &Packet,
1478                 END_OF_ARGS);
1479
1480         // Prepare and Fill MIC value
1481         NdisZeroMemory(Mic, sizeof(Mic));
1482         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
1483         {
1484                 // AES
1485                 HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
1486                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
1487         }
1488         else
1489         {
1490                 hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
1491         }
1492         NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
1493
1494
1495         MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
1496                                                 LENGTH_802_3,                   &Header802_3,
1497                                                 Packet.Body_Len[1] + 4, &Packet,
1498                                                 END_OF_ARGS);
1499
1500
1501         // 5. Copy frame to Tx ring and prepare for encryption
1502         RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
1503
1504         // 6 Free allocated memory
1505         MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
1506         os_free_mem(pAd, (PUCHAR)mpool);
1507
1508         // send wireless event - for set key done WPA2
1509         if (pAd->CommonCfg.bWirelessEvent)
1510                 RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
1511
1512         DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action <-----\n"));
1513 }
1514
1515 /*
1516         ========================================================================
1517
1518         Routine Description:
1519                 Init WPA MAC header
1520
1521         Arguments:
1522                 pAd     Pointer to our adapter
1523
1524         Return Value:
1525                 None
1526
1527         Note:
1528
1529         ========================================================================
1530 */
1531 VOID    WpaMacHeaderInit(
1532         IN              PRTMP_ADAPTER   pAd,
1533         IN OUT  PHEADER_802_11  pHdr80211,
1534         IN              UCHAR                   wep,
1535         IN              PUCHAR              pAddr1)
1536 {
1537         NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
1538         pHdr80211->FC.Type      = BTYPE_DATA;
1539         pHdr80211->FC.ToDs      = 1;
1540         if (wep == 1)
1541                 pHdr80211->FC.Wep = 1;
1542
1543          //     Addr1: BSSID, Addr2: SA, Addr3: DA
1544         COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
1545         COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
1546         COPY_MAC_ADDR(pHdr80211->Addr3, pAd->CommonCfg.Bssid);
1547         pHdr80211->Sequence =   pAd->Sequence;
1548 }
1549
1550 /*
1551         ========================================================================
1552
1553         Routine Description:
1554                 Copy frame from waiting queue into relative ring buffer and set
1555         appropriate ASIC register to kick hardware encryption before really
1556         sent out to air.
1557
1558         Arguments:
1559                 pAd             Pointer to our adapter
1560                 PNDIS_PACKET    Pointer to outgoing Ndis frame
1561                 NumberOfFrag    Number of fragment required
1562
1563         Return Value:
1564                 None
1565
1566         Note:
1567
1568         ========================================================================
1569 */
1570 VOID    RTMPToWirelessSta(
1571         IN      PRTMP_ADAPTER   pAd,
1572         IN  PUCHAR          pHeader802_3,
1573     IN  UINT            HdrLen,
1574         IN  PUCHAR          pData,
1575     IN  UINT            DataLen,
1576     IN  BOOLEAN                 is4wayFrame)
1577
1578 {
1579         NDIS_STATUS     Status;
1580         PNDIS_PACKET    pPacket;
1581         UCHAR   Index;
1582
1583         do
1584         {
1585                 // 1. build a NDIS packet and call RTMPSendPacket();
1586                 //    be careful about how/when to release this internal allocated NDIS PACKET buffer
1587                 Status = RTMPAllocateNdisPacket(pAd, &pPacket, pHeader802_3, HdrLen, pData, DataLen);
1588                 if (Status != NDIS_STATUS_SUCCESS)
1589                         break;
1590
1591                 if (is4wayFrame)
1592                         RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 1);
1593                 else
1594                         RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 0);
1595
1596                 // 2. send out the packet
1597                 Status = STASendPacket(pAd, pPacket);
1598                 if(Status == NDIS_STATUS_SUCCESS)
1599                 {
1600                         // Dequeue one frame from TxSwQueue0..3 queue and process it
1601                         // There are three place calling dequeue for TX ring.
1602                         // 1. Here, right after queueing the frame.
1603                         // 2. At the end of TxRingTxDone service routine.
1604                         // 3. Upon NDIS call RTMPSendPackets
1605                         if((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
1606                                 (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)))
1607                         {
1608                                 for(Index = 0; Index < 5; Index ++)
1609                                         if(pAd->TxSwQueue[Index].Number > 0)
1610                                                 RTMPDeQueuePacket(pAd, FALSE, Index, MAX_TX_PROCESS);
1611                         }
1612                 }
1613         } while(FALSE);
1614
1615 }
1616
1617 /*
1618     ========================================================================
1619
1620     Routine Description:
1621     Check Sanity RSN IE form AP
1622
1623     Arguments:
1624
1625     Return Value:
1626
1627
1628     ========================================================================
1629 */
1630 BOOLEAN CheckRSNIE(
1631         IN  PRTMP_ADAPTER   pAd,
1632         IN  PUCHAR          pData,
1633         IN  UCHAR           DataLen,
1634         OUT     UCHAR                   *Offset)
1635 {
1636         PUCHAR              pVIE;
1637         UCHAR               len;
1638         PEID_STRUCT         pEid;
1639         BOOLEAN                         result = FALSE;
1640
1641         pVIE = pData;
1642         len      = DataLen;
1643         *Offset = 0;
1644
1645         while (len > sizeof(RSNIE2))
1646         {
1647                 pEid = (PEID_STRUCT) pVIE;
1648                 // WPA RSN IE
1649                 if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
1650                 {
1651                         if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) &&
1652                                 (NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
1653                                 (pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
1654                         {
1655                                         DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA/WPAPSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
1656                                         result = TRUE;
1657                         }
1658
1659                         *Offset += (pEid->Len + 2);
1660                 }
1661                 // WPA2 RSN IE
1662                 else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
1663                 {
1664                         if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) &&
1665                                 (NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
1666                                 (pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
1667                         {
1668                                         DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
1669                                         result = TRUE;
1670                         }
1671
1672                         *Offset += (pEid->Len + 2);
1673                 }
1674                 else
1675                 {
1676                         break;
1677                 }
1678
1679                 pVIE += (pEid->Len + 2);
1680                 len  -= (pEid->Len + 2);
1681         }
1682
1683         DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> skip_offset(%d) \n", *Offset));
1684
1685         return result;
1686
1687 }
1688
1689
1690 /*
1691     ========================================================================
1692
1693     Routine Description:
1694     Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
1695     GTK  is encaptulated in KDE format at  p.83 802.11i D10
1696
1697     Arguments:
1698
1699     Return Value:
1700
1701     Note:
1702         802.11i D10
1703
1704     ========================================================================
1705 */
1706 BOOLEAN ParseKeyData(
1707         IN  PRTMP_ADAPTER   pAd,
1708         IN  PUCHAR          pKeyData,
1709         IN  UCHAR           KeyDataLen,
1710         IN      UCHAR                   bPairewise)
1711 {
1712     PKDE_ENCAP          pKDE = NULL;
1713     PUCHAR              pMyKeyData = pKeyData;
1714     UCHAR               KeyDataLength = KeyDataLen;
1715     UCHAR               GTKLEN;
1716         UCHAR                           skip_offset;
1717
1718         // Verify The RSN IE contained in Pairewise-Msg 3 and skip it
1719         if (bPairewise)
1720     {
1721                 // Check RSN IE whether it is WPA2/WPA2PSK
1722                 if (!CheckRSNIE(pAd, pKeyData, KeyDataLen, &skip_offset))
1723                 {
1724                         DBGPRINT(RT_DEBUG_ERROR, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE mismatched \n"));
1725                         hex_dump("Get KEYDATA :", pKeyData, KeyDataLen);
1726                         return FALSE;
1727         }
1728         else
1729                 {
1730                         // skip RSN IE
1731                         pMyKeyData += skip_offset;
1732                         KeyDataLength -= skip_offset;
1733
1734                         //DBGPRINT(RT_DEBUG_TRACE, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
1735                 }
1736         }
1737
1738         DBGPRINT(RT_DEBUG_TRACE,("ParseKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
1739
1740         // Parse EKD format
1741         if (KeyDataLength >= 8)
1742     {
1743         pKDE = (PKDE_ENCAP) pMyKeyData;
1744     }
1745         else
1746     {
1747                 DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KeyDataLength is too short \n"));
1748         return FALSE;
1749     }
1750
1751
1752         // Sanity check - shared key index should not be 0
1753         if (pKDE->GTKEncap.Kid == 0)
1754     {
1755         DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index zero \n"));
1756         return FALSE;
1757     }
1758
1759         // Sanity check - KED length
1760         if (KeyDataLength < (pKDE->Len + 2))
1761     {
1762         DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
1763         return FALSE;
1764     }
1765
1766         // Get GTK length - refer to IEEE 802.11i-2004 p.82
1767         GTKLEN = pKDE->Len -6;
1768
1769 #ifdef RT30xx
1770         if (GTKLEN < LEN_AES_KEY)
1771 #endif
1772 #ifndef RT30xx
1773         if (GTKLEN < MIN_LEN_OF_GTK)
1774 #endif
1775         {
1776                 DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
1777         return FALSE;
1778         }
1779         else
1780                 DBGPRINT(RT_DEBUG_TRACE, ("GTK Key with KDE formet got index=%d, len=%d \n", pKDE->GTKEncap.Kid, GTKLEN));
1781
1782         // Update GTK
1783         // set key material, TxMic and RxMic for WPAPSK
1784         NdisMoveMemory(pAd->StaCfg.GTK, pKDE->GTKEncap.GTK, 32);
1785         pAd->StaCfg.DefaultKeyId = pKDE->GTKEncap.Kid;
1786
1787         // Update shared key table
1788         NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
1789         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
1790         NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKDE->GTKEncap.GTK, LEN_TKIP_EK);
1791         NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &pKDE->GTKEncap.GTK[16], LEN_TKIP_RXMICK);
1792         NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &pKDE->GTKEncap.GTK[24], LEN_TKIP_TXMICK);
1793
1794         // Update Shared Key CipherAlg
1795         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
1796         if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
1797                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
1798         else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
1799                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
1800 #ifndef RT30xx
1801         else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
1802                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
1803         else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
1804                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;
1805 #endif
1806
1807         return TRUE;
1808
1809 }
1810
1811 /*
1812         ========================================================================
1813
1814         Routine Description:
1815                 Cisco CCKM PRF function
1816
1817         Arguments:
1818                 key                             Cisco Base Transient Key (BTK)
1819                 key_len                 The key length of the BTK
1820                 data                    Ruquest Number(RN) + BSSID
1821                 data_len                The length of the data
1822                 output                  Store for PTK(Pairwise transient keys)
1823                 len                             The length of the output
1824         Return Value:
1825                 None
1826
1827         Note:
1828                 802.1i  Annex F.9
1829
1830         ========================================================================
1831 */
1832 VOID CCKMPRF(
1833         IN      UCHAR   *key,
1834         IN      INT             key_len,
1835         IN      UCHAR   *data,
1836         IN      INT             data_len,
1837         OUT     UCHAR   *output,
1838         IN      INT             len)
1839 {
1840         INT             i;
1841         UCHAR   input[1024];
1842         INT             currentindex = 0;
1843         INT             total_len;
1844
1845         NdisMoveMemory(input, data, data_len);
1846         total_len = data_len;
1847         input[total_len] = 0;
1848         total_len++;
1849         for     (i = 0; i <     (len + 19) / 20; i++)
1850         {
1851                 HMAC_SHA1(input, total_len,     key, key_len, &output[currentindex]);
1852                 currentindex += 20;
1853                 input[total_len - 1]++;
1854         }
1855 }
1856
1857 /*
1858         ========================================================================
1859
1860         Routine Description:
1861                 Process MIC error indication and record MIC error timer.
1862
1863         Arguments:
1864                 pAd     Pointer to our adapter
1865                 pWpaKey                 Pointer to the WPA key structure
1866
1867         Return Value:
1868                 None
1869
1870         IRQL = DISPATCH_LEVEL
1871
1872         Note:
1873
1874         ========================================================================
1875 */
1876 VOID    RTMPReportMicError(
1877         IN      PRTMP_ADAPTER   pAd,
1878         IN      PCIPHER_KEY     pWpaKey)
1879 {
1880         ULONG   Now;
1881     UCHAR   unicastKey = (pWpaKey->Type == PAIRWISE_KEY ? 1:0);
1882
1883         // Record Last MIC error time and count
1884         Now = jiffies;
1885         if (pAd->StaCfg.MicErrCnt == 0)
1886         {
1887                 pAd->StaCfg.MicErrCnt++;
1888                 pAd->StaCfg.LastMicErrorTime = Now;
1889         NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
1890         }
1891         else if (pAd->StaCfg.MicErrCnt == 1)
1892         {
1893                 if ((pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ)) < Now)
1894                 {
1895                         // Update Last MIC error time, this did not violate two MIC errors within 60 seconds
1896                         pAd->StaCfg.LastMicErrorTime = Now;
1897                 }
1898                 else
1899                 {
1900
1901                         if (pAd->CommonCfg.bWirelessEvent)
1902                                 RTMPSendWirelessEvent(pAd, IW_COUNTER_MEASURES_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
1903
1904                         pAd->StaCfg.LastMicErrorTime = Now;
1905                         // Violate MIC error counts, MIC countermeasures kicks in
1906                         pAd->StaCfg.MicErrCnt++;
1907                 }
1908         }
1909         else
1910         {
1911                 // MIC error count >= 2
1912                 // This should not happen
1913                 ;
1914         }
1915     MlmeEnqueue(pAd,
1916                                 MLME_CNTL_STATE_MACHINE,
1917                                 OID_802_11_MIC_FAILURE_REPORT_FRAME,
1918                                 1,
1919                                 &unicastKey);
1920
1921     if (pAd->StaCfg.MicErrCnt == 2)
1922     {
1923         RTMPSetTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, 100);
1924     }
1925 }
1926
1927 #define LENGTH_EAP_H    4
1928 // If the received frame is EAP-Packet ,find out its EAP-Code (Request(0x01), Response(0x02), Success(0x03), Failure(0x04)).
1929 INT         WpaCheckEapCode(
1930         IN  PRTMP_ADAPTER               pAd,
1931         IN  PUCHAR                              pFrame,
1932         IN  USHORT                              FrameLen,
1933         IN  USHORT                              OffSet)
1934 {
1935
1936         PUCHAR  pData;
1937         INT     result = 0;
1938
1939         if( FrameLen < OffSet + LENGTH_EAPOL_H + LENGTH_EAP_H )
1940                 return result;
1941
1942         pData = pFrame + OffSet; // skip offset bytes
1943
1944         if(*(pData+1) == EAPPacket)     // 802.1x header - Packet Type
1945         {
1946                  result = *(pData+4);           // EAP header - Code
1947         }
1948
1949         return result;
1950 }
1951
1952 VOID    WpaSendMicFailureToWpaSupplicant(
1953     IN  PRTMP_ADAPTER    pAd,
1954     IN  BOOLEAN          bUnicast)
1955 {
1956     union iwreq_data    wrqu;
1957     char custom[IW_CUSTOM_MAX] = {0};
1958
1959     sprintf(custom, "MLME-MICHAELMICFAILURE.indication");
1960     if (bUnicast)
1961         sprintf(custom, "%s unicast", custom);
1962     wrqu.data.length = strlen(custom);
1963     wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
1964
1965     return;
1966 }
1967
1968 VOID    WpaMicFailureReportFrame(
1969         IN  PRTMP_ADAPTER   pAd,
1970         IN MLME_QUEUE_ELEM *Elem)
1971 {
1972         PUCHAR              pOutBuffer = NULL;
1973         UCHAR               Header802_3[14];
1974         ULONG               FrameLen = 0;
1975         EAPOL_PACKET        Packet;
1976         UCHAR               Mic[16];
1977     BOOLEAN             bUnicast;
1978
1979         DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame ----->\n"));
1980
1981     bUnicast = (Elem->Msg[0] == 1 ? TRUE:FALSE);
1982         pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
1983
1984         // init 802.3 header and Fill Packet
1985         MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
1986
1987         NdisZeroMemory(&Packet, sizeof(Packet));
1988         Packet.ProVer   = EAPOL_VER;
1989         Packet.ProType  = EAPOLKey;
1990
1991         Packet.KeyDesc.Type = WPA1_KEY_DESC;
1992
1993     // Request field presented
1994     Packet.KeyDesc.KeyInfo.Request = 1;
1995
1996         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
1997         {
1998                 Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
1999         }
2000         else      // TKIP
2001         {
2002                 Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
2003         }
2004
2005     Packet.KeyDesc.KeyInfo.KeyType = (bUnicast ? PAIRWISEKEY : GROUPKEY);
2006
2007         // KeyMic field presented
2008         Packet.KeyDesc.KeyInfo.KeyMic  = 1;
2009
2010     // Error field presented
2011         Packet.KeyDesc.KeyInfo.Error  = 1;
2012
2013         // Update packet length after decide Key data payload
2014         Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;
2015
2016         // Key Replay Count
2017         NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
2018     inc_byte_array(pAd->StaCfg.ReplayCounter, 8);
2019
2020         // Convert to little-endian format.
2021         *((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
2022
2023
2024         MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
2025         if(pOutBuffer == NULL)
2026         {
2027                 return;
2028         }
2029
2030         // Prepare EAPOL frame for MIC calculation
2031         // Be careful, only EAPOL frame is counted for MIC calculation
2032         MakeOutgoingFrame(pOutBuffer,               &FrameLen,
2033                               Packet.Body_Len[1] + 4,   &Packet,
2034                               END_OF_ARGS);
2035
2036         // Prepare and Fill MIC value
2037         NdisZeroMemory(Mic, sizeof(Mic));
2038         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
2039         {       // AES
2040         UCHAR digest[20] = {0};
2041                 HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
2042                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
2043         }
2044         else
2045         {       // TKIP
2046                 hmac_md5(pAd->StaCfg.PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
2047         }
2048         NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
2049
2050     MakeOutgoingFrame(pOutBuffer,               &FrameLen,
2051                                 LENGTH_802_3,                           &Header802_3,
2052                                 Packet.Body_Len[1] + 4,     &Packet,
2053                                 END_OF_ARGS);
2054
2055         // opy frame to Tx ring and send MIC failure report frame to authenticator
2056         RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
2057
2058         MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
2059
2060         DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame <-----\n"));
2061 }
2062
2063 /** from wpa_supplicant
2064  * inc_byte_array - Increment arbitrary length byte array by one
2065  * @counter: Pointer to byte array
2066  * @len: Length of the counter in bytes
2067  *
2068  * This function increments the last byte of the counter by one and continues
2069  * rolling over to more significant bytes if the byte was incremented from
2070  * 0xff to 0x00.
2071  */
2072 void inc_byte_array(UCHAR *counter, int len)
2073 {
2074         int pos = len - 1;
2075         while (pos >= 0) {
2076                 counter[pos]++;
2077                 if (counter[pos] != 0)
2078                         break;
2079                 pos--;
2080         }
2081 }
2082
2083 VOID WpaDisassocApAndBlockAssoc(
2084     IN PVOID SystemSpecific1,
2085     IN PVOID FunctionContext,
2086     IN PVOID SystemSpecific2,
2087     IN PVOID SystemSpecific3)
2088 {
2089     RTMP_ADAPTER                *pAd = (PRTMP_ADAPTER)FunctionContext;
2090     MLME_DISASSOC_REQ_STRUCT    DisassocReq;
2091
2092         // disassoc from current AP first
2093         DBGPRINT(RT_DEBUG_TRACE, ("RTMPReportMicError - disassociate with current AP after sending second continuous EAPOL frame\n"));
2094         DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_MIC_FAILURE);
2095         MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
2096
2097         pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
2098         pAd->StaCfg.bBlockAssoc = TRUE;
2099 }
2100