dls.c

ralink 2870 usb无线网卡 最新驱动

	for (i = MAX_NUM_OF_INIT_DLS_ENTRY; i < MAX_NUM_OF_DLS_ENTRY; i++)
	{
		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
		{
			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
		}
	}
}

/*
    ==========================================================================
    Description:
        
	IRQL = DISPATCH_LEVEL

    ==========================================================================
 */
VOID PeerDlsTearDownAction(
    IN PRTMP_ADAPTER pAd, 
    IN MLME_QUEUE_ELEM *Elem) 
{
	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
	USHORT			ReasonCode;
	UINT			i;
	BOOLEAN			TimerCancelled;

	if (!pAd->CommonCfg.bDLSCapable)
		return;

	if (!INFRA_ON(pAd))
		return;
	
	if (!PeerDlsTearDownSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &ReasonCode)) 
		return;

	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsTearDownAction() from %02x:%02x:%02x:%02x:%02x:%02x with ReasonCode=%d\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], ReasonCode));

	// clear local dls table entry
	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
	{
		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
		{
			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
		}
	}

	// clear peer dls table entry
	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
	{
		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
		{
			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
		}
	}
}

/*
    ==========================================================================
    Description:
        
	IRQL = DISPATCH_LEVEL

    ==========================================================================
 */
VOID RTMPCheckDLSTimeOut(
	IN PRTMP_ADAPTER	pAd)
{
	ULONG				i;
	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
	USHORT				reason = REASON_QOS_UNSPECIFY;
	
	if (! pAd->CommonCfg.bDLSCapable)
		return;

	if (! INFRA_ON(pAd))
		return;

	// If timeout value is equaled to zero, it means always not be timeout.

	// update local dls table entry
	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
	{
		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
		{
			pAd->StaCfg.DLSEntry[i].CountDownTimer --;

			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
			{
				reason = REASON_QOS_REQUEST_TIMEOUT;
				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
			}
		}
	}

	// update peer dls table entry
	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
	{
		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
		{
			pAd->StaCfg.DLSEntry[i].CountDownTimer --;

			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
			{
				reason = REASON_QOS_REQUEST_TIMEOUT;
				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
			}
		}
	}
}

/*
    ==========================================================================
    Description:
        
	IRQL = DISPATCH_LEVEL

    ==========================================================================
 */
BOOLEAN RTMPRcvFrameDLSCheck(
	IN PRTMP_ADAPTER	pAd,
	IN PHEADER_802_11	pHeader,
	IN ULONG			Len,
	IN PRT28XX_RXD_STRUC	pRxD)
{
	ULONG			i;
	BOOLEAN			bFindEntry = FALSE;
	BOOLEAN			bSTAKeyFrame = FALSE;
	PEAPOL_PACKET	pEap;
	PUCHAR			pProto, pAddr = NULL;
	PUCHAR			pSTAKey = NULL;
	UCHAR			ZeroReplay[LEN_KEY_DESC_REPLAY];
	UCHAR			Mic[16], OldMic[16];
	UCHAR			digest[80];
	UCHAR			DlsPTK[80];
	UCHAR			temp[64];
	BOOLEAN			TimerCancelled;
	CIPHER_KEY		PairwiseKey;

	
	if (! pAd->CommonCfg.bDLSCapable)
		return bSTAKeyFrame;

	if (! INFRA_ON(pAd))
		return bSTAKeyFrame;

	if (Len < LENGTH_802_11 + 6 + 2) /* LENGTH_802_11 + LLC + EAPOL protocol type */
		return bSTAKeyFrame;

	pProto	= (PUCHAR)pHeader + LENGTH_802_11;

	if ((pHeader->FC.SubType & 0x08))
		pProto	+= 2;	/* QOS Control field */ 

     /* Skip 4-bytes for HTC */
     if (pHeader->FC.Order && (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)))
     {    
          pProto += 4;  
     }

	/* L2PAD bit on will pad 2 bytes at LLC */
	if (pRxD->L2PAD)
	{
		pProto += 2;	
	}

	pProto	+= 6;	/* 0xAA 0xAA 0xAA 0x00 0x00 0x00 */

	if ((!(pHeader->FC.SubType & 0x08)) && (!RTMPEqualMemory(EAPOL, pProto, 2)))
		return bSTAKeyFrame;

	pAddr	= pHeader->Addr2;

	if (RTMPEqualMemory(EAPOL, pProto, 2) && (pAd->StaCfg.AuthMode >=  Ndis802_11AuthModeWPA))
	{
		pEap = (PEAPOL_PACKET) (pProto + 2);

		DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff Len=%ld, DataLen=%d, KeyMic=%d, Install=%d, KeyAck=%d, Secure=%d, EKD_DL=%d, Error=%d, Request=%d\n", Len,
			                                                             (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16),
			                                                             pEap->KeyDesc.KeyInfo.KeyMic,
			                                                             pEap->KeyDesc.KeyInfo.Install,
			                                                             pEap->KeyDesc.KeyInfo.KeyAck,
			                                                             pEap->KeyDesc.KeyInfo.Secure,
			                                                             pEap->KeyDesc.KeyInfo.EKD_DL,
			                                                             pEap->KeyDesc.KeyInfo.Error,
			                                                             pEap->KeyDesc.KeyInfo.Request));

		if ((Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16)) && pEap->KeyDesc.KeyInfo.KeyMic
			&& pEap->KeyDesc.KeyInfo.Install && pEap->KeyDesc.KeyInfo.KeyAck && pEap->KeyDesc.KeyInfo.Secure
			&& pEap->KeyDesc.KeyInfo.EKD_DL && !pEap->KeyDesc.KeyInfo.Error && !pEap->KeyDesc.KeyInfo.Request)
		{
			// First validate replay counter, only accept message with larger replay counter
			// Let equal pass, some AP start with all zero replay counter
			NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
			if ((RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
				(RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
				return bSTAKeyFrame;

			//RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter (%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));

			// put these code segment to get the replay counter
			if (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED)
				return bSTAKeyFrame;

			// Check MIC value
			// Save the MIC and replace with zero
			// use proprietary PTK
			NdisZeroMemory(temp, 64);
			NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
			WpaDerivePTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);

			NdisMoveMemory(OldMic, pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
			NdisZeroMemory(pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
			{
				// AES
				HMAC_SHA1(DlsPTK, LEN_EAP_MICK, (PUCHAR) pEap, pEap->Body_Len[1] + 4, digest, SHA1_DIGEST_SIZE);
				NdisMoveMemory(Mic,	digest,	LEN_KEY_DESC_MIC);
			}
			else
			{
				HMAC_MD5(DlsPTK, LEN_EAP_MICK, (PUCHAR) pEap, pEap->Body_Len[1] + 4, Mic, MD5_DIGEST_SIZE);
			}
			
			if (!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
			{
				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in Msg1 of STAKey handshake! \n"));
				return bSTAKeyFrame;
			}
			else
				DBGPRINT(RT_DEBUG_TRACE, ("MIC VALID in Msg1 of STAKey handshake! \n"));
			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0C)
				&& (pEap->KeyDesc.KeyData[4] == 0x43) && (pEap->KeyDesc.KeyData[5] == 0x02))
			{
				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)

				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%ld, KeyDataLen=%d\n",
					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));

				bSTAKeyFrame = TRUE;
			}
			
		}
		else if (Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE))
		{
			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter 2(%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
		}
	}

	// If timeout value is equaled to zero, it means always not be timeout.
	// update local dls table entry
	for (i= 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
	{
		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
		{
			if (bSTAKeyFrame)
			{
				PMAC_TABLE_ENTRY pEntry;

				// STAKey frame, add pairwise key table
				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
				
				PairwiseKey.KeyLen = LEN_TKIP_EK;
				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);

				//PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
				if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
					PairwiseKey.CipherAlg = CIPHER_TKIP;
				else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
					PairwiseKey.CipherAlg = CIPHER_AES;

				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
				// Add Pair-wise key to Asic
#ifdef RTMP_MAC_USB
				{
					RT_ADD_PAIRWISE_KEY_ENTRY KeyInfo;
					COPY_MAC_ADDR(KeyInfo.MacAddr,pAd->StaCfg.DLSEntry[i].MacAddr);
					KeyInfo.MacTabMatchWCID=pAd->StaCfg.DLSEntry[i].MacTabMatchWCID;
					NdisMoveMemory(&KeyInfo.CipherKey,  &PairwiseKey,sizeof(CIPHER_KEY));
					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_KEY_TABLE, &KeyInfo, sizeof(RT_ADD_PAIRWISE_KEY_ENTRY));
				}
				{
					PMAC_TABLE_ENTRY pDLSEntry;
					pDLSEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
					pDLSEntry->PairwiseKey.CipherAlg=PairwiseKey.CipherAlg;
					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_RX_WCID_TABLE, pDLSEntry, sizeof(MAC_TABLE_ENTRY));
				}
#endif // RTMP_MAC_USB //
				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Peer STA MAC Address STAKey) \n"));

				RTMPSendSTAKeyHandShake(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
				
				DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Initiator side)\n"));
			}
			else
			{
				// Data frame, update timeout value
				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
				{
					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
					//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
				}
			}

			bFindEntry = TRUE;
		}
	}

	// update peer dls table entry
	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
	{
		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
		{
			if (bSTAKeyFrame)
			{
				PMAC_TABLE_ENTRY pEntry = NULL;

				// STAKey frame, add pairwise key table, and send STAkey Msg-2
				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);

				PairwiseKey.KeyLen = LEN_TKIP_EK;
				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);

				//PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
				if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
					PairwiseKey.CipherAlg = CIPHER_TKIP;
				else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
					PairwiseKey.CipherAlg = CIPHER_AES;

				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast