wpa.c

华硕无线网卡 167G linux 驱动

    memcpy(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);

    FrameLen = 0;
    // Make  Transmitting frame

    MakeOutgoingFrame(OutBuffer, &FrameLen, sizeof(MACHDR), &Header_802_11,
        sizeof(EAPHEAD), EAPHEAD, 
        Packet.Len[1] + 4, &Packet,
        END_OF_ARGS);

	
    // Send using priority queue
    MiniportMMRequest(pAdapter, OutBuffer, FrameLen);
        
    DBGPRINT(RT_DEBUG_TRACE, "WpaPairMsg1Action <-----\n");
}

/*
    ========================================================================
    
    Routine Description:
        Process Pairwise key 4-way handshaking

    Arguments:
        pAdapter    Pointer to our adapter
        Elem        Message body
        
    Return Value:
        None
        
    Note:
        
    ========================================================================
*/
VOID    WpaPairMsg3Action(
    IN  PRT2570ADAPTER   pAdapter, 
    IN  MLME_QUEUE_ELEM *Elem) 
{
    PHEADER_802_11      pHeader;
    UCHAR               *OutBuffer = NULL;
    HEADER_802_11       Header_802_11;
    NDIS_STATUS         NStatus;
    UCHAR               AckRate = RATE_2;
    USHORT              AckDuration = 0;
    ULONG               FrameLen = 0;
    UCHAR               EAPHEAD[8] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00,0x88,0x8e};
    EAPOL_PACKET        Packet;
    PEAPOL_PACKET       pMsg3;
    PUCHAR              pTmp;
     UCHAR               Mic[16], OldMic[16];    
    NDIS_802_11_KEY     PeerKey;
    
       
    DBGPRINT(RT_DEBUG_ERROR, "WpaPairMsg3Action ----->\n");
    
    pHeader = (PHEADER_802_11) Elem->Msg;
    
    // Process message 3 frame.
    pMsg3 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];

    // 1. Verify RSN IE & cipher type match
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        if (pMsg3->KeyDesc.KeyInfo.KeyDescVer != 2)
            return;
        pTmp = (PUCHAR) &CipherWpaPskAes;
    }
    else    // TKIP
    {
        if (pMsg3->KeyDesc.KeyInfo.KeyDescVer != 1)
            return;
         pTmp = (PUCHAR) &CipherWpaPskTkip;
    }

    // Fix compatibility issue, when AP append nonsense data after auth mode with different size.
    // We should qualify this kind of RSN as acceptable
    if (memcmp((PUCHAR) &pMsg3->KeyDesc.KeyData[2], pTmp + 2, CipherWpaPskTkipLen - 2) != 0)
    {
        DBGPRINT(RT_DEBUG_ERROR, " RSN IE mismatched msg 3 of 4-way handshake!!!!!!!!!! \n");
        return;
    }
    else
        DBGPRINT(RT_DEBUG_TRACE, " RSN IE matched in msg 3 of 4-way handshake!!!!!!!!!! \n");
    
    // 2. Check MIC value
    // Save the MIC and replace with zero
    memcpy(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
    memset(pMsg3->KeyDesc.KeyMic, 0, LEN_KEY_DESC_MIC);
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];
            
        HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Len[1] + 4, pAdapter->PortCfg.PTK, LEN_EAP_MICK, digest);
        memcpy(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {
        hmac_md5(pAdapter->PortCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Len[1] + 4, Mic);
    }
    
    if (memcmp(OldMic, Mic, LEN_KEY_DESC_MIC) != 0)
    {
        DBGPRINT(RT_DEBUG_ERROR, " MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n");
        return;
    }
    else
        DBGPRINT(RT_DEBUG_TRACE, " MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n");

    // 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
    if (memcmp(pMsg3->KeyDesc.ReplayCounter, pAdapter->PortCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) <= 0)
        return;

    // Update new replay counter
    memcpy(pAdapter->PortCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

    // 4. Double check ANonce
    if (memcmp(pAdapter->PortCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE) != 0)
        return;
    
    // 5. Construct Message 4
    // =====================================
    // Use Priority Ring & MiniportMMRequest
    // =====================================
    pAdapter->Sequence = ((pAdapter->Sequence) + 1) & (MAX_SEQ_NUMBER);
    WpaMacHeaderInit(pAdapter, &Header_802_11, 0, &pAdapter->PortCfg.Bssid);

    // ACK size is 14 include CRC, and its rate is based on real time information
    AckRate = pAdapter->PortCfg.ExpectedACKRate[pAdapter->PortCfg.TxRate];
    AckDuration = RTUSBCalcDuration(pAdapter, AckRate, 14);
    Header_802_11.Controlhead.Duration = pAdapter->PortCfg.Dsifs + AckDuration;
    
    // Zero message 4 body
    memset(&Packet, 0, sizeof(Packet));
    Packet.Version = EAPOL_VER;
    Packet.Type    = EAPOLKey;
    Packet.Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;     // No data field
    
    //
    // Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
    //
    Packet.KeyDesc.Type = RSN_KEY_DESC;
    
    // Key descriptor version and appropriate RSN IE
    Packet.KeyDesc.KeyInfo.KeyDescVer = pMsg3->KeyDesc.KeyInfo.KeyDescVer;

    // Key Type PeerKey
    Packet.KeyDesc.KeyInfo.KeyType = 1;

    // KeyMic field presented
    Packet.KeyDesc.KeyInfo.KeyMic  = 1;

    // Key Replay count 
    memcpy(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

    // Out buffer for transmitting message 4        
    NStatus = MlmeAllocateMemory(pAdapter, (PVOID)&OutBuffer);  //Get an unused nonpaged memory
    if (NStatus != NDIS_STATUS_SUCCESS)
        return;                 

    // Prepare EAPOL frame for MIC calculation
    // Be careful, only EAPOL frame is counted for MIC calculation
    MakeOutgoingFrame(OutBuffer, &FrameLen,
        Packet.Len[1] + 4, &Packet,
        END_OF_ARGS);

    // Prepare and Fill MIC value
    memset(Mic, 0, sizeof(Mic));
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];
            
        HMAC_SHA1(OutBuffer, FrameLen, pAdapter->PortCfg.PTK, LEN_EAP_MICK, digest);
        memcpy(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {
        hmac_md5(pAdapter->PortCfg.PTK, LEN_EAP_MICK, OutBuffer, FrameLen, Mic);
    }
    memcpy(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);

    FrameLen = 0;
    
    // Make  Transmitting frame
    MakeOutgoingFrame(OutBuffer, &FrameLen, sizeof(MACHDR), &Header_802_11,
        sizeof(EAPHEAD), EAPHEAD, 
        Packet.Len[1] + 4, &Packet,
        END_OF_ARGS);


    // 7. Update PTK
    memset(&PeerKey, 0, sizeof(PeerKey));
    PeerKey.Length    = sizeof(PeerKey);
    PeerKey.KeyIndex  = 0xe0000000;           
    PeerKey.KeyLength = 16;
    memcpy(PeerKey.BSSID, pAdapter->PortCfg.Bssid.Octet, 6);
    memcpy(&PeerKey.KeyRSC, pMsg3->KeyDesc.KeyRsc, LEN_KEY_DESC_RSC);
    memcpy(PeerKey.KeyMaterial, &pAdapter->PortCfg.PTK[32], 32);
    // Call Add peer key function
    RTMPWPAAddKeyProc(pAdapter, &PeerKey);
	//RTUSBEnqueueCmdFromNdis(pAdapter, OID_802_11_ADD_KEY, TRUE, &PeerKey, sizeof(PeerKey));
    
    // 6. Send Message 4 to authenticator
    // Send using priority queue
    MiniportMMRequest(pAdapter, OutBuffer, FrameLen);
    DBGPRINT(RT_DEBUG_ERROR, "WpaPairMsg3Action <-----\n");
}


/*
    ========================================================================
    
    Routine Description:
        Process Group key 2-way handshaking

    Arguments:
        pAdapter    Pointer to our adapter
        Elem        Message body
        
    Return Value:
        None
        
    Note:
        
    ========================================================================
*/
VOID    WpaGroupMsg1Action(
    IN  PRT2570ADAPTER   pAdapter, 
    IN  MLME_QUEUE_ELEM *Elem) 
{
	UCHAR               *OutBuffer = NULL;
	HEADER_802_11       Header_802_11;
	NDIS_STATUS         NStatus;
	UCHAR               AckRate = RATE_2;
	USHORT              AckDuration = 0;
	ULONG               FrameLen = 0;
	UCHAR               EAPHEAD[8] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00,0x88,0x8e};
	EAPOL_PACKET        Packet;
	PEAPOL_PACKET       pGroup;
	UCHAR               Mic[16], OldMic[16];
	UCHAR               GTK[32], Key[32];
	NDIS_802_11_KEY     GroupKey;
	UCHAR MSG[MAX_LEN_OF_MLME_BUFFER];
    
	if((memcmp(&Elem->Msg[LENGTH_802_11 + 8], EAPHEAD, LENGTH_802_1_H) == 0))
	{
	DBGPRINT(RT_DEBUG_ERROR, "WpaGroupMsg1Action ----->MsgLen=%d\n",Elem->MsgLen);
		memcpy(MSG, Elem->Msg, LENGTH_802_11);
		memcpy(&MSG[LENGTH_802_11], &Elem->Msg[LENGTH_802_11+8], (Elem->MsgLen ));
	}
	else
	{
	DBGPRINT(RT_DEBUG_ERROR, "WpaGroupMsg1Action ----->2\n");
		memcpy(MSG, Elem->Msg, Elem->MsgLen);
	}
	// Process Group message 1 frame.
	pGroup = (PEAPOL_PACKET) &MSG[LENGTH_802_11 + LENGTH_802_1_H];
//	pGroup = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H + 8];//red

    // 1. Verify Replay counter
    //    Check Replay Counter, it has to be larger than last one. No need to be exact one larger
    if (memcmp(pGroup->KeyDesc.ReplayCounter, pAdapter->PortCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) <= 0)
	{
		DBGPRINT(RT_DEBUG_ERROR,"Different Replay Counter in Group 2 Handshake\n");
		return;
	}

	// Update new replay counter
	memcpy(pAdapter->PortCfg.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

    // 2. Verify MIC is valid
    // Save the MIC and replace with zero
		
    memset(OldMic, 0, LEN_KEY_DESC_MIC);
    memcpy(OldMic, pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
    memset(pGroup->KeyDesc.KeyMic, 0, LEN_KEY_DESC_MIC);
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];
            
        HMAC_SHA1((PUCHAR) pGroup, pGroup->Len[1] + 4, pAdapter->PortCfg.PTK, LEN_EAP_MICK, digest);
        memcpy(Mic, digest, LEN_KEY_DESC_MIC);
    }
	else
	{
		hmac_md5(pAdapter->PortCfg.PTK, LEN_EAP_MICK, (PUCHAR) pGroup, 131, Mic);
	}
   
    if (memcmp(OldMic, Mic, LEN_KEY_DESC_MIC) != 0)
    {
        DBGPRINT(RT_DEBUG_ERROR, " MIC Different in group msg 1 of 2-way handshake!!!!!!!!!! \n");
        return;
    }
    else
        DBGPRINT(RT_DEBUG_TEMP, " MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n");

    // 3. Decrypt GTK from Key Data
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        if (pGroup->KeyDesc.KeyInfo.KeyDescVer != 2)
            return;
        // Decrypt AES GTK
        AES_GTK_KEY_UNWRAP(&pAdapter->PortCfg.PTK[16], GTK, pGroup->KeyDesc.KeyData);       
    }
    else    // TKIP
    {
        INT i;
        
        if (pGroup->KeyDesc.KeyInfo.KeyDescVer != 1)
            return;
        // Decrypt TKIP GTK
        // Construct 32 bytes RC4 Key
        memcpy(Key, pGroup->KeyDesc.KeyIv, 16);
        memcpy(&Key[16], &pAdapter->PortCfg.PTK[16], 16);
        ARCFOUR_INIT(&pAdapter->PrivateInfo.WEPCONTEXT, Key, 32);
        //discard first 256 bytes
        for (i = 0; i < 256; i++)
            ARCFOUR_BYTE(&pAdapter->PrivateInfo.WEPCONTEXT);
        // Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
        ARCFOUR_DECRYPT(&pAdapter->PrivateInfo.WEPCONTEXT, GTK, pGroup->KeyDesc.KeyData, 32);       
    }
    
    // 4. Construct Group Message 2
    pAdapter->Sequence = ((pAdapter->Sequence) + 1) & (MAX_SEQ_NUMBER);
    WpaMacHeaderInit(pAdapter, &Header_802_11, 1, &pAdapter->PortCfg.Bssid);

    // ACK size is 14 include CRC, and its rate is based on real time information
    AckRate = pAdapter->PortCfg.ExpectedACKRate[pAdapter->PortCfg.TxRate];
    AckDuration = RTUSBCalcDuration(pAdapter, AckRate, 14);
    Header_802_11.Controlhead.Duration = pAdapter->PortCfg.Dsifs + AckDuration;
    
    // Zero Group message 1 body
    memset(&Packet, 0, sizeof(Packet));
    Packet.Version = EAPOL_VER;
    Packet.Type    = EAPOLKey;
    Packet.Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;     // No data field
    
    //
    // Group Message 2 as  EAPOL-Key(1,0,0,0,G,0,0,MIC,0)
    //
    Packet.KeyDesc.Type = RSN_KEY_DESC;
    
    // Key descriptor version and appropriate RSN IE
    Packet.KeyDesc.KeyInfo.KeyDescVer = pGroup->KeyDesc.KeyInfo.KeyDescVer;

    // Key Type Group key
    Packet.KeyDesc.KeyInfo.KeyType = 0;

    // KeyMic field presented
    Packet.KeyDesc.KeyInfo.KeyMic  = 1;

    // Secure bit is 1
    Packet.KeyDesc.KeyInfo.Secure  = 1;
    
    // Key Replay count 
    memcpy(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
    // Out buffer for transmitting group message 2      
    NStatus = MlmeAllocateMemory(pAdapter, (PVOID)&OutBuffer);  //Get an unused nonpaged memory
    if (NStatus != NDIS_STATUS_SUCCESS)
        return;                 

    // Prepare EAPOL frame for MIC calculation
    // Be careful, only EAPOL frame is counted for MIC calculation
    MakeOutgoingFrame(OutBuffer, &FrameLen,
        Packet.Len[1] + 4 , &Packet,
        END_OF_ARGS);

    // Prepare and Fill MIC value
    memset(Mic, 0, sizeof(Mic));
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];
            
        HMAC_SHA1(OutBuffer, FrameLen, pAdapter->PortCfg.PTK, LEN_EAP_MICK, digest);
        memcpy(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {            
        hmac_md5(pAdapter->PortCfg.PTK, LEN_EAP_MICK, OutBuffer, FrameLen, Mic);
    }
    memcpy(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);

    FrameLen = 0;   

    MakeOutgoingFrame(OutBuffer, &FrameLen, sizeof(MACHDR), &Header_802_11,
//    	 8,pTempIvEiv,
        sizeof(EAPHEAD), EAPHEAD, 
        Packet.Len[1] + 4, &Packet,
        END_OF_ARGS);	

    // 5. Copy frame to Tx ring and prepare for encryption
     WpaHardEncrypt(pAdapter, OutBuffer, FrameLen);

    // 6 Free allocated memory
    MlmeFreeMemory(pAdapter, OutBuffer);
    
    // 6. Update GTK
    memset(&GroupKey, 0, sizeof(GroupKey));
    GroupKey.Length    = sizeof(GroupKey);
    GroupKey.KeyIndex  = 0x80000000 | pGroup->KeyDesc.KeyInfo.KeyIndex;           
    GroupKey.KeyLength = 16;
    memcpy(GroupKey.BSSID, pAdapter->PortCfg.Bssid.Octet, 6);
    memcpy(GroupKey.KeyMaterial, GTK, 32);
    // Call Add peer key function
    RTMPWPAAddKeyProc(pAdapter, &GroupKey);
    
    DBGPRINT(RT_DEBUG_TRACE, "WpaGroupMsg1Action <-----\n");
}
/*
    ========================================================================
    
    Routine Description:
        Init WPA MAC header

    Arguments:
        pAdapter    Pointer to our adapter
        
    Return Value:
        None
        
    Note:
        
    ========================================================================
*/
VOID    WpaMacHeaderInit(
    IN      PRT2570ADAPTER   pAd, 
    IN OUT  PHEADER_802_11  Hdr, 
    IN      UCHAR           wep, 
    IN      PMACADDR        pAddr1) 
{
    memset(Hdr, 0, sizeof(HEADER_802_11));
    Hdr->Controlhead.Frame.Type = BTYPE_DATA;   
    Hdr->Controlhead.Frame.ToDs = 1;
    if (wep == 1)
        Hdr->Controlhead.Frame.Wep = 1;
    
     // Addr1: DA, Addr2: BSSID, Addr3: SA
    COPY_MAC_ADDR(&Hdr->Controlhead.Addr1, pAddr1);
    COPY_MAC_ADDR(&Hdr->Controlhead.Addr2, &pAd->CurrentAddress);
    COPY_MAC_ADDR(&Hdr->Addr3, &pAd->PortCfg.Bssid);
    Hdr->Sequence = pAd->Sequence;      
}

/*
    ========================================================================

    Routine Description: