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