wpa.c

来自「台湾RALink公司的 rt2570无线 802.11g 网卡的 驱动的源代码 」· C语言 代码 · 共 1,350 行 · 第 1/4 页

    pMsg1 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
    
    // Process message 1 from authenticator
    // Key must be Pairwise key, already verified at callee.
    // 1. Save Replay counter, it will use to verify message 3 and construct message 2
    NdisMoveMemory(pAdapter->PortCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);     

    // 2. Save ANonce
    NdisMoveMemory(pAdapter->PortCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
        
    // TSNonce <--- SNonce
    // Generate random SNonce
    GenRandom(pAdapter, pAdapter->PortCfg.SNonce);  

    // TPTK <--- Calc PTK(ANonce, TSNonce)
    WpaCountPTK(pAdapter->PortCfg.PskKey.Key,   
        pAdapter->PortCfg.ANonce,
        pAdapter->PortCfg.Bssid.Octet, 
        pAdapter->PortCfg.SNonce, 
        pAdapter->CurrentAddress,    
        PTK, 
        LEN_PTK);   

    // Save key to PTK entry
    NdisMoveMemory(pAdapter->PortCfg.PTK, PTK, LEN_PTK);
    
    // =====================================
    // 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 2 body
    NdisZeroMemory(&Packet, sizeof(Packet));
    Packet.Version = EAPOL_VER;
    Packet.Type    = EAPOLKey;
    //
    // Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
    //
    Packet.KeyDesc.Type = RSN_KEY_DESC;
    // 1. Key descriptor version and appropriate RSN IE
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
        Packet.KeyDesc.KeyDataLen[1] = CipherWpaPskAesLen;
        NdisMoveMemory(Packet.KeyDesc.KeyData, CipherWpaPskAes, CipherWpaPskAesLen);
    }
    else    // TKIP
    {
        Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
        Packet.KeyDesc.KeyDataLen[1] = CipherWpaPskTkipLen;
        NdisMoveMemory(Packet.KeyDesc.KeyData, CipherWpaPskTkip, CipherWpaPskTkipLen);
    }
    // Update packet length after decide Key data payload
    Packet.Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];

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

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

    // 4. Fill SNonce
    NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAdapter->PortCfg.SNonce, LEN_KEY_DESC_NONCE);

    // 5. Key Replay Count
    NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAdapter->PortCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);     
    
    // Send EAPOL(0, 1, 0, 0, 0, K, 0, TSNonce, 0, MIC(TPTK), 0)
    // Out buffer for transmitting 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);

	

    // 5. Prepare and Fill MIC value
    NdisZeroMemory(Mic, sizeof(Mic));
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];
            
        HMAC_SHA1(OutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
        NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {
        INT i;
#if 0
        DBGPRINT_RAW(RT_DEBUG_TRACE, " PMK = ");
        for (i = 0; i < 16; i++)
            DBGPRINT_RAW(RT_DEBUG_TRACE, "%2x-", pAdapter->PortCfg.PskKey.Key[i]);
        
#endif
        DBGPRINT_RAW(RT_DEBUG_INFO, "\n PTK = ");
        for (i = 0; i < 64; i++)
            DBGPRINT_RAW(RT_DEBUG_INFO, "%2x-", pAdapter->PortCfg.PTK[i]);
        DBGPRINT_RAW(RT_DEBUG_INFO, "\n FrameLen = %d\n", FrameLen);
        
        hmac_md5(PTK,  LEN_EAP_MICK, OutBuffer, FrameLen, Mic);
    }
    NdisMoveMemory(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 (!NdisEqualMemory((PUCHAR) &pMsg3->KeyDesc.KeyData[2], pTmp + 2, CipherWpaPskTkipLen - 2))
    {
        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
    NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
    NdisZeroMemory(pMsg3->KeyDesc.KeyMic, 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);
        NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {
        hmac_md5(pAdapter->PortCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Len[1] + 4, Mic);
    }
    
    if (!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
    {
        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 (RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAdapter->PortCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
        return;

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

    // 4. Double check ANonce
    if (!NdisEqualMemory(pAdapter->PortCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
        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
    NdisZeroMemory(&Packet, 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 
    NdisMoveMemory(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
    NdisZeroMemory(Mic, sizeof(Mic));
    if (pAdapter->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];
            
        HMAC_SHA1(OutBuffer, FrameLen, pAdapter->PortCfg.PTK, LEN_EAP_MICK, digest);
        NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {
        hmac_md5(pAdapter->PortCfg.PTK, LEN_EAP_MICK, OutBuffer, FrameLen, Mic);
    }
    NdisMoveMemory(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
    NdisZeroMemory(&PeerKey, sizeof(PeerKey));
    PeerKey.Length    = sizeof(PeerKey);
    PeerKey.KeyIndex  = 0xe0000000;           
    PeerKey.KeyLength = 16;
    NdisMoveMemory(PeerKey.BSSID, pAdapter->PortCfg.Bssid.Octet, 6);
    NdisMoveMemory(&PeerKey.KeyRSC, pMsg3->KeyDesc.KeyRsc, LEN_KEY_DESC_RSC);
    NdisMoveMemory(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: