wpa.c

TP Link 321 Linux Driver

/*
	========================================================================
	
	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
	NdisMoveMemory(pAd->PortCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);		

	// 2. Save ANonce
	NdisMoveMemory(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
	NdisMoveMemory(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
	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 (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;
    NdisMoveMemory(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
	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->PortCfg.SNonce, LEN_KEY_DESC_NONCE);

	// 5. Key Replay Count
	NdisMoveMemory(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;

    NdisMoveMemory(&tmpKeyinfo, &Packet.KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&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
	NdisZeroMemory(Mic,	sizeof(Mic));
	if (pAd->PortCfg.WepStatus	== Ndis802_11Encryption3Enabled)
	{
		// AES
		UCHAR digest[80];
			
		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
		NdisMoveMemory(Mic,	digest,	LEN_KEY_DESC_MIC);
	}
	else
	{
		INT i;
		DBGPRINT_RAW(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 PTK = ");
		for (i = 0; i < 64; i++)
			DBGPRINT_RAW(RT_DEBUG_INFO, "%2x-", pAd->PortCfg.PTK[i]);
		DBGPRINT_RAW(RT_DEBUG_INFO, "\n FrameLen = %d\n", FrameLen);
		
	    hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
	}
	NdisMoveMemory(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, QID_AC_BE, 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
    NdisMoveMemory(pAd->PortCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);

    // 2. Save ANonce
    NdisMoveMemory(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
    NdisMoveMemory(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
    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 = WPA2_KEY_DESC;
    // 1. Key descriptor version and appropriate RSN IE
    NdisMoveMemory(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
    NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->PortCfg.SNonce, LEN_KEY_DESC_NONCE);

    // 5. Key Replay Count
    NdisMoveMemory(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;

    NdisMoveMemory(&tmpKeyinfo, &Packet.KeyDesc.KeyInfo, sizeof(USHORT));
    tmpKeyinfo = SWAP16(tmpKeyinfo);
    NdisMoveMemory(&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
    NdisZeroMemory(Mic, sizeof(Mic));
    if (pAd->PortCfg.WepStatus == Ndis802_11Encryption3Enabled)
    {
        // AES
        UCHAR digest[80];
            
        HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
        NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
    }
    else
    {
        hmac_md5(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);

    
    // Send using priority queue
    MiniportMMRequest(pAd, QID_AC_BE, 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;

    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))
	{
	    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;

    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);
	}