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                 else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
1388                         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
1389                 else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
1390                         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;
1391
1392         //hex_dump("Group Key :", pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, LEN_TKIP_EK);
1393         }
1394
1395         // Update group key information to ASIC Shared Key Table
1396         AsicAddSharedKeyEntry(pAd,
1397                                                   BSS0,
1398                                                   pAd->StaCfg.DefaultKeyId,
1399                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
1400                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
1401                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
1402                                                   pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
1403
1404         // Update ASIC WCID attribute table and IVEIV table
1405         RTMPAddWcidAttributeEntry(pAd,
1406                                                           BSS0,
1407                                                           pAd->StaCfg.DefaultKeyId,
1408                                                           pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
1409                                                           NULL);
1410
1411         // set 802.1x port control
1412         STA_PORT_SECURED(pAd);
1413
1414     // Indicate Connected for GUI
1415     pAd->IndicateMediaState = NdisMediaStateConnected;
1416
1417         // init header and Fill Packet
1418         MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
1419
1420         // Zero Group message 1 body
1421         NdisZeroMemory(&Packet, sizeof(Packet));
1422         Packet.ProVer   = EAPOL_VER;
1423         Packet.ProType  = EAPOLKey;
1424         Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;                // No data field
1425
1426         //
1427         // Group Message 2 as  EAPOL-Key(1,0,0,0,G,0,0,MIC,0)
1428         //
1429         if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
1430         {
1431                 Packet.KeyDesc.Type = WPA2_KEY_DESC;
1432         }
1433         else
1434         {
1435                 Packet.KeyDesc.Type = WPA1_KEY_DESC;
1436         }
1437
1438         // Key descriptor version and appropriate RSN IE
1439         Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
1440
1441         // Update Key Length
1442         Packet.KeyDesc.KeyLength[0] = pGroup->KeyDesc.KeyLength[0];
1443         Packet.KeyDesc.KeyLength[1] = pGroup->KeyDesc.KeyLength[1];
1444
1445         // Key Index as G-Msg 1
1446         if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
1447                 Packet.KeyDesc.KeyInfo.KeyIndex = peerKeyInfo.KeyIndex;
1448
1449         // Key Type Group key
1450         Packet.KeyDesc.KeyInfo.KeyType = GROUPKEY;
1451
1452         // KeyMic field presented
1453         Packet.KeyDesc.KeyInfo.KeyMic  = 1;
1454
1455         // Secure bit
1456         Packet.KeyDesc.KeyInfo.Secure  = 1;
1457
1458         // Convert to little-endian format.
1459         *((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
1460
1461         // Key Replay count
1462         NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
1463
1464         // Out buffer for transmitting group message 2
1465         MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
1466         if(pOutBuffer == NULL)
1467         {
1468                 MlmeFreeMemory(pAd, (PUCHAR)mpool);
1469                 return;
1470         }
1471
1472         // Prepare EAPOL frame for MIC calculation
1473         // Be careful, only EAPOL frame is counted for MIC calculation
1474         MakeOutgoingFrame(pOutBuffer,           &FrameLen,
1475                 Packet.Body_Len[1] + 4,    &Packet,
1476                 END_OF_ARGS);
1477
1478         // Prepare and Fill MIC value
1479         NdisZeroMemory(Mic, sizeof(Mic));
1480         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
1481         {
1482                 // AES
1483                 HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
1484                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
1485         }
1486         else
1487         {
1488                 hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
1489         }
1490         NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
1491
1492
1493         MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
1494                                                 LENGTH_802_3,                   &Header802_3,
1495                                                 Packet.Body_Len[1] + 4, &Packet,
1496                                                 END_OF_ARGS);
1497
1498
1499         // 5. Copy frame to Tx ring and prepare for encryption
1500         RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
1501
1502         // 6 Free allocated memory
1503         MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
1504         os_free_mem(pAd, (PUCHAR)mpool);
1505
1506         // send wireless event - for set key done WPA2
1507         if (pAd->CommonCfg.bWirelessEvent)
1508                 RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
1509
1510         DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action <-----\n"));
1511 }
1512
1513 /*
1514         ========================================================================
1515
1516         Routine Description:
1517                 Init WPA MAC header
1518
1519         Arguments:
1520                 pAd     Pointer to our adapter
1521
1522         Return Value:
1523                 None
1524
1525         Note:
1526
1527         ========================================================================
1528 */
1529 VOID    WpaMacHeaderInit(
1530         IN              PRTMP_ADAPTER   pAd,
1531         IN OUT  PHEADER_802_11  pHdr80211,
1532         IN              UCHAR                   wep,
1533         IN              PUCHAR              pAddr1)
1534 {
1535         NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
1536         pHdr80211->FC.Type      = BTYPE_DATA;
1537         pHdr80211->FC.ToDs      = 1;
1538         if (wep == 1)
1539                 pHdr80211->FC.Wep = 1;
1540
1541          //     Addr1: BSSID, Addr2: SA, Addr3: DA
1542         COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
1543         COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
1544         COPY_MAC_ADDR(pHdr80211->Addr3, pAd->CommonCfg.Bssid);
1545         pHdr80211->Sequence =   pAd->Sequence;
1546 }
1547
1548 /*
1549         ========================================================================
1550
1551         Routine Description:
1552                 Copy frame from waiting queue into relative ring buffer and set
1553         appropriate ASIC register to kick hardware encryption before really
1554         sent out to air.
1555
1556         Arguments:
1557                 pAd             Pointer to our adapter
1558                 PNDIS_PACKET    Pointer to outgoing Ndis frame
1559                 NumberOfFrag    Number of fragment required
1560
1561         Return Value:
1562                 None
1563
1564         Note:
1565
1566         ========================================================================
1567 */
1568 VOID    RTMPToWirelessSta(
1569         IN      PRTMP_ADAPTER   pAd,
1570         IN  PUCHAR          pHeader802_3,
1571     IN  UINT            HdrLen,
1572         IN  PUCHAR          pData,
1573     IN  UINT            DataLen,
1574     IN  BOOLEAN                 is4wayFrame)
1575
1576 {
1577         NDIS_STATUS     Status;
1578         PNDIS_PACKET    pPacket;
1579         UCHAR   Index;
1580
1581         do
1582         {
1583                 // 1. build a NDIS packet and call RTMPSendPacket();
1584                 //    be careful about how/when to release this internal allocated NDIS PACKET buffer
1585                 Status = RTMPAllocateNdisPacket(pAd, &pPacket, pHeader802_3, HdrLen, pData, DataLen);
1586                 if (Status != NDIS_STATUS_SUCCESS)
1587                         break;
1588
1589                 if (is4wayFrame)
1590                         RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 1);
1591                 else
1592                         RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 0);
1593
1594                 // 2. send out the packet
1595                 Status = STASendPacket(pAd, pPacket);
1596                 if(Status == NDIS_STATUS_SUCCESS)
1597                 {
1598                         // Dequeue one frame from TxSwQueue0..3 queue and process it
1599                         // There are three place calling dequeue for TX ring.
1600                         // 1. Here, right after queueing the frame.
1601                         // 2. At the end of TxRingTxDone service routine.
1602                         // 3. Upon NDIS call RTMPSendPackets
1603                         if((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
1604                                 (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)))
1605                         {
1606                                 for(Index = 0; Index < 5; Index ++)
1607                                         if(pAd->TxSwQueue[Index].Number > 0)
1608                                                 RTMPDeQueuePacket(pAd, FALSE, Index, MAX_TX_PROCESS);
1609                         }
1610                 }
1611         } while(FALSE);
1612
1613 }
1614
1615 /*
1616     ========================================================================
1617
1618     Routine Description:
1619     Check Sanity RSN IE form AP
1620
1621     Arguments:
1622
1623     Return Value:
1624
1625
1626     ========================================================================
1627 */
1628 BOOLEAN CheckRSNIE(
1629         IN  PRTMP_ADAPTER   pAd,
1630         IN  PUCHAR          pData,
1631         IN  UCHAR           DataLen,
1632         OUT     UCHAR                   *Offset)
1633 {
1634         PUCHAR              pVIE;
1635         UCHAR               len;
1636         PEID_STRUCT         pEid;
1637         BOOLEAN                         result = FALSE;
1638
1639         pVIE = pData;
1640         len      = DataLen;
1641         *Offset = 0;
1642
1643         while (len > sizeof(RSNIE2))
1644         {
1645                 pEid = (PEID_STRUCT) pVIE;
1646                 // WPA RSN IE
1647                 if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
1648                 {
1649                         if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) &&
1650                                 (NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
1651                                 (pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
1652                         {
1653                                         DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA/WPAPSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
1654                                         result = TRUE;
1655                         }
1656
1657                         *Offset += (pEid->Len + 2);
1658                 }
1659                 // WPA2 RSN IE
1660                 else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
1661                 {
1662                         if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) &&
1663                                 (NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
1664                                 (pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
1665                         {
1666                                         DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
1667                                         result = TRUE;
1668                         }
1669
1670                         *Offset += (pEid->Len + 2);
1671                 }
1672                 else
1673                 {
1674                         break;
1675                 }
1676
1677                 pVIE += (pEid->Len + 2);
1678                 len  -= (pEid->Len + 2);
1679         }
1680
1681         DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> skip_offset(%d) \n", *Offset));
1682
1683         return result;
1684
1685 }
1686
1687
1688 /*
1689     ========================================================================
1690
1691     Routine Description:
1692     Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
1693     GTK  is encaptulated in KDE format at  p.83 802.11i D10
1694
1695     Arguments:
1696
1697     Return Value:
1698
1699     Note:
1700         802.11i D10
1701
1702     ========================================================================
1703 */
1704 BOOLEAN ParseKeyData(
1705         IN  PRTMP_ADAPTER   pAd,
1706         IN  PUCHAR          pKeyData,
1707         IN  UCHAR           KeyDataLen,
1708         IN      UCHAR                   bPairewise)
1709 {
1710     PKDE_ENCAP          pKDE = NULL;
1711     PUCHAR              pMyKeyData = pKeyData;
1712     UCHAR               KeyDataLength = KeyDataLen;
1713     UCHAR               GTKLEN;
1714         UCHAR                           skip_offset;
1715
1716         // Verify The RSN IE contained in Pairewise-Msg 3 and skip it
1717         if (bPairewise)
1718     {
1719                 // Check RSN IE whether it is WPA2/WPA2PSK
1720                 if (!CheckRSNIE(pAd, pKeyData, KeyDataLen, &skip_offset))
1721                 {
1722                         DBGPRINT(RT_DEBUG_ERROR, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE mismatched \n"));
1723                         hex_dump("Get KEYDATA :", pKeyData, KeyDataLen);
1724                         return FALSE;
1725         }
1726         else
1727                 {
1728                         // skip RSN IE
1729                         pMyKeyData += skip_offset;
1730                         KeyDataLength -= skip_offset;
1731
1732                         //DBGPRINT(RT_DEBUG_TRACE, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
1733                 }
1734         }
1735
1736         DBGPRINT(RT_DEBUG_TRACE,("ParseKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
1737
1738         // Parse EKD format
1739         if (KeyDataLength >= 8)
1740     {
1741         pKDE = (PKDE_ENCAP) pMyKeyData;
1742     }
1743         else
1744     {
1745                 DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KeyDataLength is too short \n"));
1746         return FALSE;
1747     }
1748
1749
1750         // Sanity check - shared key index should not be 0
1751         if (pKDE->GTKEncap.Kid == 0)
1752     {
1753         DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index zero \n"));
1754         return FALSE;
1755     }
1756
1757         // Sanity check - KED length
1758         if (KeyDataLength < (pKDE->Len + 2))
1759     {
1760         DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
1761         return FALSE;
1762     }
1763
1764         // Get GTK length - refer to IEEE 802.11i-2004 p.82
1765         GTKLEN = pKDE->Len -6;
1766
1767         if (GTKLEN < MIN_LEN_OF_GTK)
1768         {
1769                 DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
1770         return FALSE;
1771         }
1772         else
1773                 DBGPRINT(RT_DEBUG_TRACE, ("GTK Key with KDE formet got index=%d, len=%d \n", pKDE->GTKEncap.Kid, GTKLEN));
1774
1775         // Update GTK
1776         // set key material, TxMic and RxMic for WPAPSK
1777         NdisMoveMemory(pAd->StaCfg.GTK, pKDE->GTKEncap.GTK, 32);
1778         pAd->StaCfg.DefaultKeyId = pKDE->GTKEncap.Kid;
1779
1780         // Update shared key table
1781         NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
1782         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
1783         NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKDE->GTKEncap.GTK, LEN_TKIP_EK);
1784         NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &pKDE->GTKEncap.GTK[16], LEN_TKIP_RXMICK);
1785         NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &pKDE->GTKEncap.GTK[24], LEN_TKIP_TXMICK);
1786
1787         // Update Shared Key CipherAlg
1788         pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
1789         if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
1790                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
1791         else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
1792                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
1793         else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
1794                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
1795         else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
1796                 pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;
1797
1798         return TRUE;
1799
1800 }
1801
1802 /*
1803         ========================================================================
1804
1805         Routine Description:
1806                 Cisco CCKM PRF function
1807
1808         Arguments:
1809                 key                             Cisco Base Transient Key (BTK)
1810                 key_len                 The key length of the BTK
1811                 data                    Ruquest Number(RN) + BSSID
1812                 data_len                The length of the data
1813                 output                  Store for PTK(Pairwise transient keys)
1814                 len                             The length of the output
1815         Return Value:
1816                 None
1817
1818         Note:
1819                 802.1i  Annex F.9
1820
1821         ========================================================================
1822 */
1823 VOID CCKMPRF(
1824         IN      UCHAR   *key,
1825         IN      INT             key_len,
1826         IN      UCHAR   *data,
1827         IN      INT             data_len,
1828         OUT     UCHAR   *output,
1829         IN      INT             len)
1830 {
1831         INT             i;
1832         UCHAR   input[1024];
1833         INT             currentindex = 0;
1834         INT             total_len;
1835
1836         NdisMoveMemory(input, data, data_len);
1837         total_len = data_len;
1838         input[total_len] = 0;
1839         total_len++;
1840         for     (i = 0; i <     (len + 19) / 20; i++)
1841         {
1842                 HMAC_SHA1(input, total_len,     key, key_len, &output[currentindex]);
1843                 currentindex += 20;
1844                 input[total_len - 1]++;
1845         }
1846 }
1847
1848 /*
1849         ========================================================================
1850
1851         Routine Description:
1852                 Process MIC error indication and record MIC error timer.
1853
1854         Arguments:
1855                 pAd     Pointer to our adapter
1856                 pWpaKey                 Pointer to the WPA key structure
1857
1858         Return Value:
1859                 None
1860
1861         IRQL = DISPATCH_LEVEL
1862
1863         Note:
1864
1865         ========================================================================
1866 */
1867 VOID    RTMPReportMicError(
1868         IN      PRTMP_ADAPTER   pAd,
1869         IN      PCIPHER_KEY     pWpaKey)
1870 {
1871         ULONG   Now;
1872     UCHAR   unicastKey = (pWpaKey->Type == PAIRWISE_KEY ? 1:0);
1873
1874         // Record Last MIC error time and count
1875         Now = jiffies;
1876         if (pAd->StaCfg.MicErrCnt == 0)
1877         {
1878                 pAd->StaCfg.MicErrCnt++;
1879                 pAd->StaCfg.LastMicErrorTime = Now;
1880         NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
1881         }
1882         else if (pAd->StaCfg.MicErrCnt == 1)
1883         {
1884                 if ((pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ)) < Now)
1885                 {
1886                         // Update Last MIC error time, this did not violate two MIC errors within 60 seconds
1887                         pAd->StaCfg.LastMicErrorTime = Now;
1888                 }
1889                 else
1890                 {
1891
1892                         if (pAd->CommonCfg.bWirelessEvent)
1893                                 RTMPSendWirelessEvent(pAd, IW_COUNTER_MEASURES_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
1894
1895                         pAd->StaCfg.LastMicErrorTime = Now;
1896                         // Violate MIC error counts, MIC countermeasures kicks in
1897                         pAd->StaCfg.MicErrCnt++;
1898                 }
1899         }
1900         else
1901         {
1902                 // MIC error count >= 2
1903                 // This should not happen
1904                 ;
1905         }
1906     MlmeEnqueue(pAd,
1907                                 MLME_CNTL_STATE_MACHINE,
1908                                 OID_802_11_MIC_FAILURE_REPORT_FRAME,
1909                                 1,
1910                                 &unicastKey);
1911
1912     if (pAd->StaCfg.MicErrCnt == 2)
1913     {
1914         RTMPSetTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, 100);
1915     }
1916 }
1917
1918
1919 #ifdef WPA_SUPPLICANT_SUPPORT
1920 #define LENGTH_EAP_H    4
1921 // If the received frame is EAP-Packet ,find out its EAP-Code (Request(0x01), Response(0x02), Success(0x03), Failure(0x04)).
1922 INT         WpaCheckEapCode(
1923         IN  PRTMP_ADAPTER               pAd,
1924         IN  PUCHAR                              pFrame,
1925         IN  USHORT                              FrameLen,
1926         IN  USHORT                              OffSet)
1927 {
1928
1929         PUCHAR  pData;
1930         INT     result = 0;
1931
1932         if( FrameLen < OffSet + LENGTH_EAPOL_H + LENGTH_EAP_H )
1933                 return result;
1934
1935         pData = pFrame + OffSet; // skip offset bytes
1936
1937         if(*(pData+1) == EAPPacket)     // 802.1x header - Packet Type
1938         {
1939                  result = *(pData+4);           // EAP header - Code
1940         }
1941
1942         return result;
1943 }
1944
1945 VOID    WpaSendMicFailureToWpaSupplicant(
1946     IN  PRTMP_ADAPTER    pAd,
1947     IN  BOOLEAN          bUnicast)
1948 {
1949     union iwreq_data    wrqu;
1950     char custom[IW_CUSTOM_MAX] = {0};
1951
1952     sprintf(custom, "MLME-MICHAELMICFAILURE.indication");
1953     if (bUnicast)
1954         sprintf(custom, "%s unicast", custom);
1955     wrqu.data.length = strlen(custom);
1956     wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
1957
1958     return;
1959 }
1960 #endif // WPA_SUPPLICANT_SUPPORT //
1961
1962 VOID    WpaMicFailureReportFrame(
1963         IN  PRTMP_ADAPTER   pAd,
1964         IN MLME_QUEUE_ELEM *Elem)
1965 {
1966         PUCHAR              pOutBuffer = NULL;
1967         UCHAR               Header802_3[14];
1968         ULONG               FrameLen = 0;
1969         EAPOL_PACKET        Packet;
1970         UCHAR               Mic[16];
1971     BOOLEAN             bUnicast;
1972
1973         DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame ----->\n"));
1974
1975     bUnicast = (Elem->Msg[0] == 1 ? TRUE:FALSE);
1976         pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
1977
1978         // init 802.3 header and Fill Packet
1979         MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
1980
1981         NdisZeroMemory(&Packet, sizeof(Packet));
1982         Packet.ProVer   = EAPOL_VER;
1983         Packet.ProType  = EAPOLKey;
1984
1985         Packet.KeyDesc.Type = WPA1_KEY_DESC;
1986
1987     // Request field presented
1988     Packet.KeyDesc.KeyInfo.Request = 1;
1989
1990         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
1991         {
1992                 Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
1993         }
1994         else      // TKIP
1995         {
1996                 Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
1997         }
1998
1999     Packet.KeyDesc.KeyInfo.KeyType = (bUnicast ? PAIRWISEKEY : GROUPKEY);
2000
2001         // KeyMic field presented
2002         Packet.KeyDesc.KeyInfo.KeyMic  = 1;
2003
2004     // Error field presented
2005         Packet.KeyDesc.KeyInfo.Error  = 1;
2006
2007         // Update packet length after decide Key data payload
2008         Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;
2009
2010         // Key Replay Count
2011         NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
2012     inc_byte_array(pAd->StaCfg.ReplayCounter, 8);
2013
2014         // Convert to little-endian format.
2015         *((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
2016
2017
2018         MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
2019         if(pOutBuffer == NULL)
2020         {
2021                 return;
2022         }
2023
2024         // Prepare EAPOL frame for MIC calculation
2025         // Be careful, only EAPOL frame is counted for MIC calculation
2026         MakeOutgoingFrame(pOutBuffer,               &FrameLen,
2027                               Packet.Body_Len[1] + 4,   &Packet,
2028                               END_OF_ARGS);
2029
2030         // Prepare and Fill MIC value
2031         NdisZeroMemory(Mic, sizeof(Mic));
2032         if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
2033         {       // AES
2034         UCHAR digest[20] = {0};
2035                 HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
2036                 NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
2037         }
2038         else
2039         {       // TKIP
2040                 hmac_md5(pAd->StaCfg.PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
2041         }
2042         NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
2043
2044     MakeOutgoingFrame(pOutBuffer,               &FrameLen,
2045                                 LENGTH_802_3,                           &Header802_3,
2046                                 Packet.Body_Len[1] + 4,     &Packet,
2047                                 END_OF_ARGS);
2048
2049         // opy frame to Tx ring and send MIC failure report frame to authenticator
2050         RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
2051
2052         MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
2053
2054         DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame <-----\n"));
2055 }
2056
2057 /** from wpa_supplicant
2058  * inc_byte_array - Increment arbitrary length byte array by one
2059  * @counter: Pointer to byte array
2060  * @len: Length of the counter in bytes
2061  *
2062  * This function increments the last byte of the counter by one and continues
2063  * rolling over to more significant bytes if the byte was incremented from
2064  * 0xff to 0x00.
2065  */
2066 void inc_byte_array(UCHAR *counter, int len)
2067 {
2068         int pos = len - 1;
2069         while (pos >= 0) {
2070                 counter[pos]++;
2071                 if (counter[pos] != 0)
2072                         break;
2073                 pos--;
2074         }
2075 }
2076
2077 VOID WpaDisassocApAndBlockAssoc(
2078     IN PVOID SystemSpecific1,
2079     IN PVOID FunctionContext,
2080     IN PVOID SystemSpecific2,
2081     IN PVOID SystemSpecific3)
2082 {
2083     RTMP_ADAPTER                *pAd = (PRTMP_ADAPTER)FunctionContext;
2084     MLME_DISASSOC_REQ_STRUCT    DisassocReq;
2085
2086         // disassoc from current AP first
2087         DBGPRINT(RT_DEBUG_TRACE, ("RTMPReportMicError - disassociate with current AP after sending second continuous EAPOL frame\n"));
2088         DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_MIC_FAILURE);
2089         MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
2090
2091         pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
2092         pAd->StaCfg.bBlockAssoc = TRUE;
2093 }
2094