wpa.c

来自「r73模块的无线网卡在Linux下的驱动程序」· C语言 代码 · 共 2,118 行 · 第 1/5 页

				    WpaGroupMsg1Action(pAd, Elem);
				    pAd->PortCfg.WpaState = SS_FINISH;
			    }
			    break;

		    default:
			    break;
	    }
    }

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


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

	Routine Description:
		Process Pairwise key 4-way handshaking

	Arguments:
		pAd	Pointer	to our adapter
		Elem		Message body

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	WpaPairMsg1Action(
	IN	PRTMP_ADAPTER	pAd,
	IN	MLME_QUEUE_ELEM	*Elem)
{
	PHEADER_802_11		pHeader;
	UCHAR				PTK[80];
	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};
	PEAPOL_PACKET		pMsg1;
	EAPOL_PACKET		Packet;
	UCHAR				Mic[16];
	USHORT              NStatus;

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

	pHeader	= (PHEADER_802_11) Elem->Msg;

	// Save Data Length to pDesc for receiving packet, then put in outgoing frame	Data Len fields.
	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
	memcpy(pAd->PortCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

	// 2. Save ANonce
	memcpy(pAd->PortCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);

	// TSNonce <--- SNonce
	// Generate random SNonce
	GenRandom(pAd,	pAd->PortCfg.SNonce);

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

	// Save key to PTK entry
	memcpy(pAd->PortCfg.PTK, PTK, LEN_PTK);

	// =====================================
	// 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 2 body
	memset(&Packet, 0, sizeof(Packet));
	Packet.Version = pAd->EAPOLVer;
	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 (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
	}
	else	// TKIP
	{
        Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
	}
    Packet.KeyDesc.KeyDataLen[1] = pAd->PortCfg.RSN_IELen;
    memcpy(Packet.KeyDesc.KeyData, pAd->PortCfg.RSN_IE, pAd->PortCfg.RSN_IELen);

	// Update packet length after decide Key data payload
	Packet.Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];

	// Update Key length
	Packet.KeyDesc.KeyLength[0] = pMsg1->KeyDesc.KeyLength[0];
	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];

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

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

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

	// 5. Key Replay Count
	memcpy(Packet.KeyDesc.ReplayCounter, pAd->PortCfg.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

	// Send EAPOL(0, 1, 0, 0, 0, K, 0, TSNonce, 0, MIC(TPTK), 0)
	// Out buffer for transmitting message 2
    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);

	// 5. Prepare and Fill MIC value
	memset(Mic, 0, sizeof(Mic));
	if (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];

		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
		memcpy(Mic,	digest,	LEN_KEY_DESC_MIC);
	}
	else
	{
		INT i;
		DBGPRINT(RT_DEBUG_INFO, " PMK = ");
		for (i = 0; i < 16; i++)
			DBGPRINT_RAW(RT_DEBUG_INFO, "%2x-", pAd->PortCfg.PskKey.Key[i]);
		DBGPRINT_RAW(RT_DEBUG_INFO, "\n");

		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(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	memcpy(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);

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

	// Send using priority queue
	MiniportMMRequest(pAd, pOutBuffer, FrameLen);


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

VOID    Wpa2PairMsg1Action(
    IN  PRTMP_ADAPTER   pAd,
    IN  MLME_QUEUE_ELEM *Elem)
{
    PHEADER_802_11      pHeader;
    UCHAR               PTK[80];
    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};
    PEAPOL_PACKET       pMsg1;
    EAPOL_PACKET        Packet;
    UCHAR               Mic[16];
    USHORT              NStatus;

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

    pHeader = (PHEADER_802_11) Elem->Msg;

    // Save Data Length to pDesc for receiving packet, then put in outgoing frame   Data Len fields.
    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
    memcpy(pAd->PortCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

    // 2. Save ANonce
    memcpy(pAd->PortCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);

    // TSNonce <--- SNonce
    // Generate random SNonce
    GenRandom(pAd, pAd->PortCfg.SNonce);

    if (pMsg1->KeyDesc.KeyDataLen[1] > 0 )
    {
        // cached PMKID
    }

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

    // Save key to PTK entry
    memcpy(pAd->PortCfg.PTK, PTK, LEN_PTK);

    // =====================================
    // 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 2 body
    memset(&Packet, 0, sizeof(Packet));
    Packet.Version = pAd->EAPOLVer;
    Packet.Type    = EAPOLKey;
    //
    // Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
    //
    Packet.KeyDesc.Type = WPA2_KEY_DESC;
    // 1. Key descriptor version and appropriate RSN IE
    memcpy(Packet.KeyDesc.KeyData, pAd->PortCfg.RSN_IE, pAd->PortCfg.RSN_IELen);
    Packet.KeyDesc.KeyDataLen[1] = pAd->PortCfg.RSN_IELen;


    if (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
    }
    else    // TKIP
    {
        Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
    }
    // 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;

    // Update Key Length
    Packet.KeyDesc.KeyLength[0] = 0;
    Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];

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

    // 5. Key Replay Count
    memcpy(Packet.KeyDesc.ReplayCounter, pAd->PortCfg.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

    // Send EAPOL(0, 1, 0, 0, 0, K, 0, TSNonce, 0, MIC(TPTK), 0)
    // Out buffer for transmitting message 2
    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);


    // 5. Prepare and Fill MIC value
    memset(Mic, 0, sizeof(Mic));
    if (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];

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


    // Send using priority queue
    MiniportMMRequest(pAd, pOutBuffer, FrameLen);


    DBGPRINT(RT_DEBUG_TRACE, "Wpa2PairMsg1Action <-----\n");

}

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

	Routine Description:
		Process Pairwise key 4-way handshaking

	Arguments:
		pAd	Pointer	to our adapter
		Elem		Message body

	Return Value:
		None

	Note:

	========================================================================
*/
VOID	WpaPairMsg3Action(
	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;
	USHORT              NStatus;

	DBGPRINT(RT_DEBUG_TRACE, "WpaPairMsg3Action ----->\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));
}
//	*(USHORT *)(&(pMsg3->KeyDesc.KeyInfo)) = SWAP16(*(USHORT *)(&(pMsg3->KeyDesc.KeyInfo)));
#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