wpa.c

TP Link 321 Linux Driver

	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, pAd->PortCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
		return;

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

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

	// ACK size	is 14 include CRC, and its rate	is based on real time information
	AckRate = pAd->PortCfg.ExpectedACKRate[pAd->PortCfg.TxRate];
	AckDuration = RTMPCalcDuration(pAd, AckRate, 14);
	Header_802_11.Duration = pAd->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;
	
#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &pMsg3->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&pMsg3->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif
	
	// Key descriptor version and appropriate RSN IE
	Packet.KeyDesc.KeyInfo.KeyDescVer = pMsg3->KeyDesc.KeyInfo.KeyDescVer;

	// Update Key Length
	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
	
	// Key Type PeerKey
	Packet.KeyDesc.KeyInfo.KeyType = 1;

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

    // In Msg3,  KeyInfo.secure =0 if Group Key HS to come. 1 if no group key HS
    // Station sends Msg4  KeyInfo.secure should be the same as that in Msg.3
    Packet.KeyDesc.KeyInfo.Secure= pMsg3->KeyDesc.KeyInfo.Secure;

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

#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &Packet.KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&Packet.KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif

	// Out buffer for transmitting message 4
    NStatus = MlmeAllocateMemory(pAd, (PVOID *)&pOutBuffer);  //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(pOutBuffer,           &FrameLen,
		              Packet.Len[1] + 4,    &Packet,
		              END_OF_ARGS);

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

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

	
	// 7. Update PTK
    pPeerKey = kmalloc(MAX_LEN_OF_MLME_BUFFER, MEM_ALLOC_FLAG);  // allocate memory
    if (pPeerKey == NULL)
		return;
	
	NdisZeroMemory(pPeerKey, sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY);
	pPeerKey->Length    = sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY;
	pPeerKey->KeyIndex  = 0xe0000000;
	pPeerKey->KeyLength = pMsg3->KeyDesc.KeyLength[0] * 256 + pMsg3->KeyDesc.KeyLength[1];

	COPY_MAC_ADDR(pPeerKey->BSSID, pAd->PortCfg.Bssid);
	NdisMoveMemory(&pPeerKey->KeyRSC, pMsg3->KeyDesc.KeyRsc, LEN_KEY_DESC_RSC);
	NdisMoveMemory(pPeerKey->KeyMaterial, &pAd->PortCfg.PTK[32], LEN_EAP_KEY);
	// Call Add peer key function
	RTMPWPAAddKeyProc(pAd, pPeerKey);
	kfree(pPeerKey);

	// 6. Send Message 4 to authenticator
	// Send using priority queue
	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);


	DBGPRINT(RT_DEBUG_TRACE, "WpaPairMsg3Action <-----\n");
}

VOID    Wpa2PairMsg3Action(
    IN  PRTMP_ADAPTER   pAd, 
    IN  MLME_QUEUE_ELEM *Elem) 
{
    PHEADER_802_11      pHeader;
    PUCHAR              pOutBuffer = NULL;
    HEADER_802_11       Header_802_11;
    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;
    UCHAR               Mic[16], OldMic[16];    
    PNDIS_802_11_KEY    pPeerKey;
    UCHAR               *KEYDATA, Key[32];
    USHORT              NStatus;
    UCHAR               *mpool, *DescKeyData;

    DBGPRINT(RT_DEBUG_TRACE, "Wpa2PairMsg3Action ----->\n");
    
    mpool = kmalloc(1056, MEM_ALLOC_FLAG);  // allocate memory
    if (mpool == NULL)
        return;

    // KEYDATA Len = 512
    KEYDATA = (UCHAR *) ROUND_UP(mpool, 4);
    // DescKeyData Len = 512
    DescKeyData = (UCHAR *) ROUND_UP(KEYDATA+512, 4);
    
    pHeader = (PHEADER_802_11) Elem->Msg;
    
    // Process message 3 frame.
    pMsg3 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
    
    // Store KeyData, protect received data more than MAX_LEN_OF_RSNIE
    NdisZeroMemory(DescKeyData, 512);
    NdisMoveMemory( DescKeyData, 
                    &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE], 
                    pMsg3->KeyDesc.KeyDataLen[1]);

#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &pMsg3->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&pMsg3->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif    

    // 1. Verify RSN IE & cipher type match
    if (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled && (pMsg3->KeyDesc.KeyInfo.KeyDescVer != 2))
    {
        kfree(mpool);
        return;
    }
    else if (pAd->PortCfg.WepStatus == Ndis802_11Encryption2Enabled && (pMsg3->KeyDesc.KeyInfo.KeyDescVer != 1))
    {
        kfree(mpool);
        return;
    }

#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &pMsg3->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&pMsg3->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif

    // 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 (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];
            
        HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Len[1] + 4, pAd->PortCfg.PTK, LEN_EAP_MICK, digest);
        NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {
        hmac_md5(pAd->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");
        kfree(mpool);
        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, pAd->PortCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
    {
        kfree(mpool);
        return;
    }

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

    // 4. Double check ANonce
    if (!NdisEqualMemory(pAd->PortCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
    {
        kfree(mpool);
        return;
    }

    // Obtain GTK
    // 5. Decrypt GTK from Key Data
    DBGPRINT_RAW(RT_DEBUG_TRACE, "EKD = %d\n", pMsg3->KeyDesc.KeyInfo.EKD_DL);
    if (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
    
        // Decrypt AES GTK  
        AES_GTK_KEY_UNWRAP(&pAd->PortCfg.PTK[16], KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], DescKeyData);       

        ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], 0, FALSE);
    }
    else    // TKIP
    {
        INT i;
        // Decrypt TKIP GTK
        // Construct 32 bytes RC4 Key
        NdisMoveMemory(Key, pMsg3->KeyDesc.KeyIv, 16);
        NdisMoveMemory(&Key[16], &pAd->PortCfg.PTK[16], 16);
        ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
        //discard first 256 bytes
        for (i = 0; i < 256; i++)
            ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
        // Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
        ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, DescKeyData, pMsg3->KeyDesc.KeyDataLen[1]);       

        DBGPRINT_RAW(RT_DEBUG_TRACE, "KEYDATA = \n");
        for (i = 0; i < 100; i++)
        {
            DBGPRINT_RAW(RT_DEBUG_TRACE, "%2x ", KEYDATA[i]);
            if (i%16 == 15)
                DBGPRINT_RAW(RT_DEBUG_TRACE, "\n ");
        }
        DBGPRINT_RAW(RT_DEBUG_TRACE, "\n  \n");

        ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], 0, FALSE);

    }
    
    
    // 6. Construct Message 4
    // =====================================
    // Use Priority Ring & MiniportMMRequest
    // =====================================
    pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
    WpaMacHeaderInit(pAd, &Header_802_11, 0, pAd->PortCfg.Bssid);

    // ACK size is 14 include CRC, and its rate is based on real time information
    AckRate = pAd->PortCfg.ExpectedACKRate[pAd->PortCfg.TxRate];
    AckDuration = RTMPCalcDuration(pAd, AckRate, 14);
    Header_802_11.Duration = pAd->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;
    
#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &pMsg3->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&pMsg3->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif

    // Key descriptor version and appropriate RSN IE
    Packet.KeyDesc.KeyInfo.KeyDescVer = pMsg3->KeyDesc.KeyInfo.KeyDescVer;

    // Update Key Length
	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
	
    // Key Type PeerKey
    Packet.KeyDesc.KeyInfo.KeyType = 1;

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

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

#ifdef BIG_ENDIAN
   // recovery original byte order, before forward Elem to another routine	   
{
    USHORT	tmpKeyinfo;

    NdisMoveMemory(&tmpKeyinfo, &Packet.KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&Packet.KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif

    // Out buffer for transmitting message 4        
    NStatus = MlmeAllocateMemory(pAd, (PVOID *)&pOutBuffer);  //Get an unused nonpaged memory
    if (NStatus != NDIS_STATUS_SUCCESS)
    {
        kfree(mpool);
        return;                 
    }

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

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

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


    // 7. Update PTK
    pPeerKey = kmalloc(MAX_LEN_OF_MLME_BUFFER, MEM_ALLOC_FLAG);  // allocate memory
    if (pPeerKey == NULL)
    {
        kfree(mpool);
		return;
	}
		
    NdisZeroMemory(pPeerKey, sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY);
    pPeerKey->Length    = sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY;
    pPeerKey->KeyIndex  = 0xe0000000;           
    pPeerKey->KeyLength = pMsg3->KeyDesc.KeyLength[0] * 256 + pMsg3->KeyDesc.KeyLength[1];

    COPY_MAC_ADDR(pPeerKey->BSSID, pAd->PortCfg.Bssid);
    NdisMoveMemory(&pPeerKey->KeyRSC, pMsg3->KeyDesc.KeyRsc, LEN_KEY_DESC_RSC);
    NdisMoveMemory(pPeerKey->KeyMaterial, &pAd->PortCfg.PTK[32], 32);
    // Call Add peer key function
    RTMPWPAAddKeyProc(pAd, pPeerKey);
    kfree(pPeerKey);

    // 6. Send Message 4 to authenticator
    // Send using priority queue
    MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);

    kfree(mpool);