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