wpa.c

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

    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], 32);
    // 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, "Wpa2PairMsg3Action <-----\n");

}

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

	Routine Description:
		Process Group key 2-way handshaking

	Arguments:
		pAd	Pointer	to our adapter
		Elem		Message body

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	WpaGroupMsg1Action(
	IN	PRTMP_ADAPTER	pAd,
	IN	MLME_QUEUE_ELEM	*Elem)
{
	PUCHAR				pOutBuffer = NULL;
	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];
	PNDIS_802_11_KEY	pGroupKey = NULL;
//    UCHAR               MSG[MAX_LEN_OF_MLME_BUFFER];
	UCHAR				*mpool, *MSG, *KEYDATA;
    UCHAR               Header802_3[14];
//    UCHAR               KEYDATA[512];
    USHORT              NStatus;


	mpool = kmalloc(2*MAX_LEN_OF_MLME_BUFFER, MEM_ALLOC_FLAG);  // allocate memory
    if (mpool == NULL)
        return;

	MSG = (UCHAR *) (((unsigned long)(mpool+3)) & ((unsigned long)~(0x03)));
	KEYDATA =(UCHAR *) (((unsigned long)(MSG+MAX_LEN_OF_MLME_BUFFER+3)) & ((unsigned long)~(0x03)));


    if(NdisEqualMemory(&Elem->Msg[LENGTH_802_11 + 8], EAPHEAD, LENGTH_802_1_H))
    {
        DBGPRINT(RT_DEBUG_TRACE, "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_TRACE, "WpaGroupMsg1Action ----->\n");
        memcpy(MSG, Elem->Msg, Elem->MsgLen);
    }

	// Process Group message 1 frame.
	pGroup = (PEAPOL_PACKET) &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, &pGroup->KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    memcpy(&pGroup->KeyDesc.KeyInfo, &tmpKeyinfo, sizeof(USHORT));
}
#endif

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

	// 1. Verify Replay counter
	//    Check Replay Counter, it has to be larger than last one. No need to be exact one larger
	if (RTMPCompareMemory(pGroup->KeyDesc.ReplayCounter, pAd->PortCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
	{
		if(mpool != NULL){
			kfree(mpool);
		}
		return;
	}

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

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

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

	// 2. Verify MIC is valid
	// Save the MIC and replace with zero
	memcpy(OldMic, pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
	memset(pGroup->KeyDesc.KeyMic, 0, LEN_KEY_DESC_MIC);

	if (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];

		HMAC_SHA1((PUCHAR) pGroup, pGroup->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) pGroup, pGroup->Len[1] + 4, Mic);
	}

	if (!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
	{
		DBGPRINT(RT_DEBUG_ERROR, " MIC Different in group msg 1 of 2-way handshake!!!!!!!!!! \n");
		if(mpool != NULL){
			kfree(mpool);
		}
		return;
	}
	else
		DBGPRINT(RT_DEBUG_TRACE, " MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n");

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

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

    // 3. Decrypt GTK from Key Data
    DBGPRINT(RT_DEBUG_TRACE, " Install = %d!!!!EKD_DL = %d!!!!!KeyIndex = %d! \n", pGroup->KeyDesc.KeyInfo.Install, pGroup->KeyDesc.KeyInfo.EKD_DL,
        pGroup->KeyDesc.KeyInfo.KeyIndex);


    pGroupKey = kmalloc(MAX_LEN_OF_MLME_BUFFER, MEM_ALLOC_FLAG);  // allocate memory
    if (pGroupKey == NULL)
    {

	    if(mpool != NULL){
		    kfree(mpool);
	    }
	    return;
    }

	if (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		INT i;
        // Decrypt AES GTK
        memcpy(KEYDATA, pGroup->KeyDesc.KeyData, 32);

        //if (pGroup->KeyDesc.KeyInfo.EKD_DL == 1)
            AES_GTK_KEY_UNWRAP(&pAd->PortCfg.PTK[16], KEYDATA,  pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyData);

		// Update GTK
        memset(pGroupKey, 0, sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY);
	    pGroupKey->Length    = sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY;
	    pGroupKey->KeyIndex  = 0x20000000 | pGroup->KeyDesc.KeyInfo.KeyIndex;
	    pGroupKey->KeyLength = pGroup->KeyDesc.KeyLength[0] * 256 + pGroup->KeyDesc.KeyLength[1];

	    COPY_MAC_ADDR(pGroupKey->BSSID, pAd->PortCfg.Bssid);
	    memcpy(pGroupKey->KeyMaterial, KEYDATA, 32);
	    memcpy(GTK, KEYDATA, 32);

        DBGPRINT(RT_DEBUG_TRACE, "AES GTK =");
        for (i = 0; i < 32; i++)
        {
            DBGPRINT_RAW(RT_DEBUG_TRACE, " %02x", pGroup->KeyDesc.KeyData[i]);
        }
        DBGPRINT_RAW(RT_DEBUG_TRACE, "\n");


        // Call Add peer key function
        RTMPWPAAddKeyProc(pAd, pGroupKey);
	}
	else	// TKIP
	{
		INT	i;

		// Decrypt TKIP GTK
		// Construct 32 bytes RC4 Key
		memcpy(Key, pGroup->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, GTK, pGroup->KeyDesc.KeyData, 32);

        DBGPRINT(RT_DEBUG_TRACE, "TKIP GTK =");
        for (i = 0; i < 32; i++)
        {
            DBGPRINT_RAW(RT_DEBUG_TRACE, " %2x", GTK[i]);
        }
        DBGPRINT_RAW(RT_DEBUG_TRACE, "\n");

        //RTMPWPAAddKeyProc(pAd, pGroupKey);
        //ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1]);

	}

	// 4. Construct Group Message 2
    pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
    WPAMake8023Hdr(pAd, pAd->PortCfg.Bssid, Header802_3);

	// Zero Group message 1 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

	//
	// 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;

	// Update Key Length and Key Index
	Packet.KeyDesc.KeyInfo.KeyIndex = pGroup->KeyDesc.KeyInfo.KeyIndex;
	Packet.KeyDesc.KeyLength[0] = pGroup->KeyDesc.KeyLength[0];
	Packet.KeyDesc.KeyLength[1] = pGroup->KeyDesc.KeyLength[1];

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

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

	// Secure bit
    if (pAd->PortCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
	    Packet.KeyDesc.KeyInfo.Secure  = 1;

	// Key Replay count
	memcpy(Packet.KeyDesc.ReplayCounter, pGroup->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 group message 2
    NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
    if (NStatus != NDIS_STATUS_SUCCESS)
    {
	    if(mpool != NULL){
		    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
	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
	{
		INT i;

		DBGPRINT(RT_DEBUG_INFO, "PTK = ");
		for (i = 0; i < 64; i++)
			DBGPRINT_RAW(RT_DEBUG_INFO, "%2x-", pAd->PortCfg.PTK[i]);
		DBGPRINT_RAW(RT_DEBUG_INFO, "\n");
		DBGPRINT(RT_DEBUG_INFO, "- FrameLen = %d\n", FrameLen);

		hmac_md5(pAd->PortCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	memcpy(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);


    FrameLen = 0;
    MakeOutgoingFrame(pOutBuffer,   &FrameLen,
                      LENGTH_802_3, &Header802_3,
//                    sizeof(EAPHEAD), EAPHEAD,
		              Packet.Len[1] + 4,    &Packet,
		              END_OF_ARGS);

	// 5. Copy frame to Tx ring and prepare for encryption
    RTMPToWirelessSta(pAd, pOutBuffer, FrameLen);

    // Free allocated memory
    MlmeFreeMemory(pAd, pOutBuffer);

	// 6. Update GTK
	memset(pGroupKey, 0, sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY);
	pGroupKey->Length    = sizeof(NDIS_802_11_KEY) + LEN_EAP_KEY;
	pGroupKey->KeyIndex  = 0x20000000 | pGroup->KeyDesc.KeyInfo.KeyIndex;
	pGroupKey->KeyLength = pGroup->KeyDesc.KeyLength[0] * 256 + pGroup->KeyDesc.KeyLength[1];

	COPY_MAC_ADDR(pGroupKey->BSSID, pAd->PortCfg.Bssid);
	memcpy(pGroupKey->KeyMaterial, GTK, LEN_EAP_KEY);
	// Call Add peer key function
	RTMPWPAAddKeyProc(pAd, pGroupKey);

	if(pGroupKey != NULL){
		kfree(pGroupKey);
	}
	if(mpool != NULL){
		kfree(mpool);
	}

	DBGPRINT(RT_DEBUG_TRACE, "WpaGroupMsg1Action <-----\n");
}
/*
	========================================================================

	Routine Description:
		Init WPA MAC header

	Arguments:
		pAd	Pointer	to our adapter

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	WpaMacHeaderInit(
	IN		PRTMP_ADAPTER	pAd,
	IN OUT	PHEADER_802_11	pHdr80211,
	IN		UCHAR			wep,
	IN		PUCHAR		    pAddr1)
{
	memset(pHdr80211, 0, sizeof(HEADER_802_11));
	pHdr80211->FC.Type	= BTYPE_DATA;
	pHdr80211->FC.ToDs	= 1;
	if (wep	== 1)
		pHdr80211->FC.Wep = 1;

	 //	Addr1: DA, Addr2: BSSID, Addr3:	SA
	COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
	COPY_MAC_ADDR(pHdr80211->Addr3, pAd->PortCfg.Bssid);
	pHdr80211->Sequence =	pAd->Sequence;
}

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

	Routine Description:
		SHA1 function

	Arguments:

	Return Value:

	Note:

	========================================================================
*/
VOID	HMAC_SHA1(
	IN	UCHAR	*text,
	IN	UINT	text_len,
	IN	UCHAR	*key,
	IN	UINT	key_len,
	IN	UCHAR	*digest)
{
	SHA_CTX	context;
	UCHAR	k_ipad[65]; /* inner padding - key XORd with ipad	*/
	UCHAR	k_opad[65]; /* outer padding - key XORd with opad	*/
	INT		i;