wpa.c

r73模块的无线网卡在Linux下的驱动程序

{
    USHORT	tmpKeyinfo;

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

	// 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 (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];

		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Len[1] + 4, pAd->PortCfg.PTK, LEN_EAP_MICK, digest);
		memcpy(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");
		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) {
		DBGPRINT(RT_DEBUG_ERROR, "WPA - (%s) invalid replay ctr\n",
				__FUNCTION__);
		return;
	}
	// Update new replay counter
	memcpy(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)) {
		DBGPRINT(RT_DEBUG_ERROR, "WPA - (%s) Nonce mismatch\n",
				__FUNCTION__);
		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
	memset(&Packet, 0, sizeof(Packet));
	Packet.Version = pAd->EAPOLVer;
	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;

    memcpy(&tmpKeyinfo, &pMsg3->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    memcpy(&pMsg3->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
//	*(USHORT *)(&(pMsg3->KeyDesc.KeyInfo)) = SWAP16(*(USHORT *)(&(pMsg3->KeyDesc.KeyInfo)));
#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
	memcpy(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;

    memcpy(&tmpKeyinfo, &Packet.KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    memcpy(&Packet.KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
//    *(USHORT *)&Packet.KeyDesc.KeyInfo = SWAP16(*(USHORT *)&Packet.KeyDesc.KeyInfo);
#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
	memset(Mic, 0, sizeof(Mic));
	if (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];

		HMAC_SHA1(pOutBuffer, FrameLen, pAd->PortCfg.PTK, LEN_EAP_MICK, digest);
		memcpy(Mic,	digest,	LEN_KEY_DESC_MIC);
	}
	else
	{
		hmac_md5(pAd->PortCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	memcpy(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;

	memset(pPeerKey, 0, 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);
	memcpy(&pPeerKey->KeyRSC, pMsg3->KeyDesc.KeyRsc, LEN_KEY_DESC_RSC);
	memcpy(pPeerKey->KeyMaterial, &pAd->PortCfg.PTK[32], LEN_EAP_KEY);
	// Call Add peer key function
	RTMPWPAAddKeyProc(pAd, pPeerKey);
	if(pPeerKey != NULL){
		kfree(pPeerKey);
	}
	// 6. Send Message 4 to authenticator
	// Send using priority queue
	MiniportMMRequest(pAd, 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[512], Key[32];
    USHORT              NStatus;

    DBGPRINT(RT_DEBUG_TRACE, "Wpa2PairMsg3Action ----->\n");

    pHeader = (PHEADER_802_11) Elem->Msg;

    // Process message 3 frame.
    pMsg3 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];

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

    memcpy(&tmpKeyinfo, &pMsg3->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    memcpy(&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))
    {
        return;
    }
    else if (pAd->PortCfg.WepStatus == Ndis802_11Encryption2Enabled && (pMsg3->KeyDesc.KeyInfo.KeyDescVer != 1))
    {
        return;
    }

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

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

    // 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 (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];

        HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Len[1] + 4, pAd->PortCfg.PTK, LEN_EAP_MICK, digest);
        memcpy(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");
        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) {
		DBGPRINT(RT_DEBUG_ERROR, "WPA - (%s) invalid replay ctr\n",
				__FUNCTION__);
        return;
	}
    // Update new replay counter
    memcpy(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)) {
		DBGPRINT(RT_DEBUG_ERROR, "WPA - (%s) Nonce mismatch\n",
				__FUNCTION__);
        return;
	}
    // Obtain GTK
    // 5. Decrypt GTK from Key Data
    DBGPRINT(RT_DEBUG_INFO, "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],pMsg3->KeyDesc.KeyData);

        ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1]);
    }
    else    // TKIP
    {
        INT i;
        // Decrypt TKIP GTK
        // Construct 32 bytes RC4 Key
        memcpy(Key, pMsg3->KeyDesc.KeyIv, 16);
        memcpy(&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, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);

        DBGPRINT(RT_DEBUG_TRACE, "KEYDATA =\n");
        DBGPRINT(RT_DEBUG_TRACE, " ");
        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(RT_DEBUG_TRACE, " ");
			}
        }
        DBGPRINT_RAW(RT_DEBUG_TRACE, "\n");

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

    }


    // 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
    memset(&Packet, 0, sizeof(Packet));
    Packet.Version = pAd->EAPOLVer;
    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;

    memcpy(&tmpKeyinfo, &pMsg3->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    memcpy(&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
    memcpy(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;

    memcpy(&tmpKeyinfo, &Packet.KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    memcpy(&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
    memset(Mic, 0, sizeof(Mic));
    if (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];

        HMAC_SHA1(pOutBuffer, FrameLen, pAd->PortCfg.PTK, LEN_EAP_MICK, digest);
        memcpy(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {
        hmac_md5(pAd->PortCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
    }
    memcpy(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;

    memset(pPeerKey, 0, sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY);