Staging: add rt3090 wireless driver

This is the vendor driver for the Ralink RT3090 chipset.

It should be later cleaned and ported to use the existing rt2x00
infrastructure or just replaced by the proper version.

[ Unfortunately since it follows the same design/implementation like
  rt{286,287,307}0 drivers (already present in the staging tree)
  it is highly unlikely that it will see much love from the wireless
  development community.. ]

However since the development of the cleaner/proper version can take
significant time lets give distros (i.e. openSUSE seems to already
have the package with the original vendor driver) and users "something"
to use in the meantime.

I forward ported it to 2.6.31-rc1, ported to the Linux build system
and did some initial cleanups.  More fixes/cleanups to come later
(it seems that the driver can be made to share most of its code with
the other Ralink drivers already present in the staging tree).

Signed-off-by: Bartlomiej Zolnierkiewicz <bzolnier@gmail.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

1 files changed, 2661 insertions, 0 deletions

diff --git a/drivers/staging/rt3090/sta/rtmp_data.c b/drivers/staging/rt3090/sta/rtmp_data.c
new file mode 100644
index 0000000..5591364
--- /dev/null
+++ b/drivers/staging/rt3090/sta/rtmp_data.c
@@ -0,0 +1,2661 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_data.c
+
+	Abstract:
+	Data path subroutines
+
+	Revision History:
+	Who		When			What
+	--------	----------		----------------------------------------------
+*/
+
+#include "../rt_config.h"
+
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	UCHAR			*pTmpBuf;
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP)
+	{
+		// All EAPoL frames have to pass to upper layer (ex. WPA_SUPPLICANT daemon)
+		// TBD : process fragmented EAPol frames
+		{
+			// In 802.1x mode, if the received frame is EAP-SUCCESS packet, turn on the PortSecured variable
+			if ( pAd->StaCfg.IEEE8021X == TRUE &&
+				 (EAP_CODE_SUCCESS == WpaCheckEapCode(pAd, pRxBlk->pData, pRxBlk->DataSize, LENGTH_802_1_H)))
+			{
+				PUCHAR	Key;
+				UCHAR	CipherAlg;
+				int     idx = 0;
+
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("Receive EAP-SUCCESS Packet\n"));
+				//pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                if (pAd->StaCfg.IEEE8021x_required_keys == FALSE)
+                {
+                    idx = pAd->StaCfg.DesireSharedKeyId;
+                    CipherAlg = pAd->StaCfg.DesireSharedKey[idx].CipherAlg;
+					Key = pAd->StaCfg.DesireSharedKey[idx].Key;
+
+                    if (pAd->StaCfg.DesireSharedKey[idx].KeyLen > 0)
+				{
+#ifdef RTMP_MAC_PCI
+						MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[BSSID_WCID];
+
+						// Set key material and cipherAlg to Asic
+						AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+						// Assign group key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+
+						// Assign pairwise key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, pEntry);
+
+                        pAd->IndicateMediaState = NdisMediaStateConnected;
+                        pAd->ExtraInfo = GENERAL_LINK_UP;
+#endif // RTMP_MAC_PCI //
+						// For Preventing ShardKey Table is cleared by remove key procedure.
+					pAd->SharedKey[BSS0][idx].CipherAlg = CipherAlg;
+						pAd->SharedKey[BSS0][idx].KeyLen = pAd->StaCfg.DesireSharedKey[idx].KeyLen;
+						NdisMoveMemory(pAd->SharedKey[BSS0][idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].KeyLen);
+				}
+				}
+			}
+
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		// Special DATA frame that has to pass to MLME
+		//	 1. Cisco Aironet frames for CCX2. We need pass it to MLME for special process
+		//	 2. EAPOL handshaking frames when driver supplicant enabled, pass to MLME for special process
+		{
+			pTmpBuf = pRxBlk->pData - LENGTH_802_11;
+			NdisMoveMemory(pTmpBuf, pRxBlk->pHeader, LENGTH_802_11);
+			REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pTmpBuf, pRxBlk->DataSize + LENGTH_802_11, pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! report EAPOL/AIRONET DATA to MLME (len=%d) !!!\n", pRxBlk->DataSize));
+		}
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+
+VOID STARxDataFrameAnnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+
+	// non-EAP frame
+	if (!RTMPCheckWPAframe(pAd, pEntry, pRxBlk->pData, pRxBlk->DataSize, FromWhichBSSID))
+	{
+
+		{
+			// drop all non-EAP DATA frame before
+			// this client's Port-Access-Control is secured
+			if (pRxBlk->pHeader->FC.Wep)
+			{
+				// unsupported cipher suite
+				if (pAd->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+			else
+			{
+				// encryption in-use but receive a non-EAPOL clear text frame, drop it
+				if ((pAd->StaCfg.WepStatus != Ndis802_11EncryptionDisabled) &&
+					(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+		}
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_EAP);
+
+
+		if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_ARALINK))
+		{
+			// Normal legacy, AMPDU or AMSDU
+			CmmRxnonRalinkFrameIndicate(pAd, pRxBlk, FromWhichBSSID);
+
+		}
+		else
+		{
+			// ARALINK
+			CmmRxRalinkFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+#ifdef QOS_DLS_SUPPORT
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_DLS);
+#endif // QOS_DLS_SUPPORT //
+	}
+	else
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_EAP);
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+		{
+			Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// Determin the destination of the EAP frame
+			//  to WPA state machine or upper layer
+			STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+// For TKIP frame, calculate the MIC value
+BOOLEAN STACheckTkipMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	UCHAR			UserPriority = pRxBlk->UserPriority;
+	PCIPHER_KEY		pWpaKey;
+	UCHAR			*pDA, *pSA;
+
+	pWpaKey = &pAd->SharedKey[BSS0][pRxBlk->pRxWI->KeyIndex];
+
+	pDA = pHeader->Addr1;
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_INFRA))
+	{
+		pSA = pHeader->Addr3;
+	}
+	else
+	{
+		pSA = pHeader->Addr2;
+	}
+
+	if (RTMPTkipCompareMICValue(pAd,
+								pData,
+								pDA,
+								pSA,
+								pWpaKey->RxMic,
+								UserPriority,
+								DataSize) == FALSE)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error 2\n"));
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if (pAd->StaCfg.WpaSupplicantUP)
+		{
+			WpaSendMicFailureToWpaSupplicant(pAd, (pWpaKey->Type == PAIRWISEKEY) ? TRUE : FALSE);
+		}
+		else
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			RTMPReportMicError(pAd, pWpaKey);
+		}
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+
+//
+// All Rx routines use RX_BLK structure to hande rx events
+// It is very important to build pRxBlk attributes
+//  1. pHeader pointer to 802.11 Header
+//  2. pData pointer to payload including LLC (just skip Header)
+//  3. set payload size including LLC to DataSize
+//  4. set some flags with RX_BLK_SET_FLAG()
+//
+VOID STAHandleRxDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC				pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC						pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11					pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET					pRxPacket = pRxBlk->pRxPacket;
+	BOOLEAN							bFragment = FALSE;
+	MAC_TABLE_ENTRY				*pEntry = NULL;
+	UCHAR							FromWhichBSSID = BSS0;
+	UCHAR                           UserPriority = 0;
+
+	{
+		// before LINK UP, all DATA frames are rejected
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+#ifdef QOS_DLS_SUPPORT
+		//if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+		if (RTMPRcvFrameDLSCheck(pAd, pHeader, pRxWI->MPDUtotalByteCount, pRxD))
+		{
+			return;
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		// Drop not my BSS frames
+		if (pRxD->MyBss == 0)
+		{
+			{
+				// release packet
+				RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+
+		pAd->RalinkCounters.RxCountSinceLastNULL++;
+		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable && (pHeader->FC.SubType & 0x08))
+		{
+			UCHAR *pData;
+			DBGPRINT(RT_DEBUG_INFO,("bAPSDCapable\n"));
+
+			// Qos bit 4
+			pData = (PUCHAR)pHeader + LENGTH_802_11;
+			if ((*pData >> 4) & 0x01)
+			{
+				DBGPRINT(RT_DEBUG_INFO,("RxDone- Rcv EOSP frame, driver may fall into sleep\n"));
+				pAd->CommonCfg.bInServicePeriod = FALSE;
+
+				// Force driver to fall into sleep mode when rcv EOSP frame
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+				{
+					USHORT  TbttNumToNextWakeUp;
+					USHORT  NextDtim = pAd->StaCfg.DtimPeriod;
+					ULONG   Now;
+
+					NdisGetSystemUpTime(&Now);
+					NextDtim -= (USHORT)(Now - pAd->StaCfg.LastBeaconRxTime)/pAd->CommonCfg.BeaconPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					RTMP_SET_PSM_BIT(pAd, PWR_SAVE);
+					// if WMM-APSD is failed, try to disable following line
+					AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+				}
+			}
+
+			if ((pHeader->FC.MoreData) && (pAd->CommonCfg.bInServicePeriod))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("Sending another trigger frame when More Data bit is set to 1\n"));
+			}
+		}
+
+		// Drop NULL, CF-ACK(no data), CF-POLL(no data), and CF-ACK+CF-POLL(no data) data frame
+		if ((pHeader->FC.SubType & 0x04)) // bit 2 : no DATA
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+	    // Drop not my BSS frame (we can not only check the MyBss bit in RxD)
+#ifdef QOS_DLS_SUPPORT
+	    if (!pAd->CommonCfg.bDLSCapable)
+	    {
+#endif // QOS_DLS_SUPPORT //
+		if (INFRA_ON(pAd))
+		{
+			// Infrastructure mode, check address 2 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr2, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+		else	// Ad-Hoc mode or Not associated
+		{
+			// Ad-Hoc mode, check address 3 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr3, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+#ifdef QOS_DLS_SUPPORT
+	    }
+#endif // QOS_DLS_SUPPORT //
+
+		//
+		// find pEntry
+		//
+		if (pRxWI->WirelessCliID < MAX_LEN_OF_MAC_TABLE)
+		{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+
+		}
+		else
+		{
+			// 1. release packet if infra mode
+			// 2. new a pEntry if ad-hoc mode
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// infra or ad-hoc
+		if (INFRA_ON(pAd))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_INFRA);
+#ifdef QOS_DLS_SUPPORT
+			if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+				RX_BLK_SET_FLAG(pRxBlk, fRX_DLS);
+			else
+#endif // QOS_DLS_SUPPORT //
+			ASSERT(pRxWI->WirelessCliID == BSSID_WCID);
+		}
+
+		// check Atheros Client
+		if ((pEntry->bIAmBadAtheros == FALSE) &&  (pRxD->AMPDU == 1) && (pHeader->FC.Retry ))
+		{
+			pEntry->bIAmBadAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bCurrentAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bLastAtheros = TRUE;
+			if (!STA_AES_ON(pAd))
+			{
+				AsicUpdateProtect(pAd, 8, ALLN_SETPROTECT, TRUE, FALSE);
+			}
+		}
+	}
+
+	pRxBlk->pData = (UCHAR *)pHeader;
+
+	//
+	// update RxBlk->pData, DataSize
+	// 802.11 Header, QOS, HTC, Hw Padding
+	//
+
+	// 1. skip 802.11 HEADER
+	{
+		pRxBlk->pData += LENGTH_802_11;
+		pRxBlk->DataSize -= LENGTH_802_11;
+	}
+
+	// 2. QOS
+	if (pHeader->FC.SubType & 0x08)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_QOS);
+		UserPriority = *(pRxBlk->pData) & 0x0f;
+		// bit 7 in QoS Control field signals the HT A-MSDU format
+		if ((*pRxBlk->pData) & 0x80)
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_AMSDU);
+		}
+
+		// skip QOS contorl field
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -=2;
+	}
+	pRxBlk->UserPriority = UserPriority;
+
+	/* check if need to resend PS Poll when received packet with MoreData = 1 */
+	if ((pAd->StaCfg.Psm == PWR_SAVE) && (pHeader->FC.MoreData == 1))
+	{
+		if ((((UserPriority == 0) || (UserPriority == 3)) &&
+			pAd->CommonCfg.bAPSDAC_BE == 0) ||
+			(((UserPriority == 1) || (UserPriority == 2)) &&
+			pAd->CommonCfg.bAPSDAC_BK == 0) ||
+			(((UserPriority == 4) || (UserPriority == 5)) &&
+			pAd->CommonCfg.bAPSDAC_VI == 0) ||
+			(((UserPriority == 6) || (UserPriority == 7)) &&
+			pAd->CommonCfg.bAPSDAC_VO == 0))
+		{
+			/* non-UAPSD delivery-enabled AC */
+			RTMP_PS_POLL_ENQUEUE(pAd);
+		}
+	}
+
+	// 3. Order bit: A-Ralink or HTC+
+	if (pHeader->FC.Order)
+	{
+#ifdef AGGREGATION_SUPPORT
+		if ((pRxWI->PHYMODE <= MODE_OFDM) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_ARALINK);
+		}
+		else
+#endif // AGGREGATION_SUPPORT //
+		{
+#ifdef DOT11_N_SUPPORT
+			RX_BLK_SET_FLAG(pRxBlk, fRX_HTC);
+			// skip HTC contorl field
+			pRxBlk->pData += 4;
+			pRxBlk->DataSize -= 4;
+#endif // DOT11_N_SUPPORT //
+		}
+	}
+
+	// 4. skip HW padding
+	if (pRxD->L2PAD)
+	{
+		// just move pData pointer
+		// because DataSize excluding HW padding
+		RX_BLK_SET_FLAG(pRxBlk, fRX_PAD);
+		pRxBlk->pData += 2;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pRxD->BA)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_AMPDU);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+	//
+	// Case I  Process Broadcast & Multicast data frame
+	//
+	if (pRxD->Bcast || pRxD->Mcast)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastReceivedFrameCount);
+
+		// Drop Mcast/Bcast frame with fragment bit on
+		if (pHeader->FC.MoreFrag)
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// Filter out Bcast frame which AP relayed for us
+		if (pHeader->FC.FrDs && MAC_ADDR_EQUAL(pHeader->Addr3, pAd->CurrentAddress))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+	else if (pRxD->U2M)
+	{
+		pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+
+
+#ifdef QOS_DLS_SUPPORT
+        if (RX_BLK_TEST_FLAG(pRxBlk, fRX_DLS))
+		{
+			MAC_TABLE_ENTRY *pDlsEntry = NULL;
+
+			pDlsEntry = DlsEntryTableLookupByWcid(pAd, pRxWI->WirelessCliID, pHeader->Addr2, TRUE);
+										                        if(pDlsEntry)
+			Update_Rssi_Sample(pAd, &pDlsEntry->RssiSample, pRxWI);
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+		{
+			pEntry = MacTableLookup(pAd, pHeader->Addr2);
+			if (pEntry)
+				Update_Rssi_Sample(pAd, &pEntry->RssiSample, pRxWI);
+		}
+
+
+		Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+		pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+		pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+
+		pAd->RalinkCounters.OneSecRxOkDataCnt++;
+
+
+	if (!((pHeader->Frag == 0) && (pHeader->FC.MoreFrag == 0)))
+	{
+		// re-assemble the fragmented packets
+		// return complete frame (pRxPacket) or NULL
+		bFragment = TRUE;
+		pRxPacket = RTMPDeFragmentDataFrame(pAd, pRxBlk);
+	}
+
+	if (pRxPacket)
+	{
+		pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+
+		// process complete frame
+		if (bFragment && (pRxD->Decrypted) && (pEntry->WepStatus == Ndis802_11Encryption2Enabled))
+		{
+			// Minus MIC length
+			pRxBlk->DataSize -= 8;
+
+			// For TKIP frame, calculate the MIC value
+			if (STACheckTkipMICValue(pAd, pEntry, pRxBlk) == FALSE)
+			{
+				return;
+			}
+		}
+
+		STARxDataFrameAnnounce(pAd, pEntry, pRxBlk, FromWhichBSSID);
+			return;
+	}
+	else
+	{
+		// just return
+		// because RTMPDeFragmentDataFrame() will release rx packet,
+		// if packet is fragmented
+		return;
+	}
+	}
+
+	ASSERT(0);
+	// release packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+VOID STAHandleRxMgmtFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	do
+	{
+
+
+		/* check if need to resend PS Poll when received packet with MoreData = 1 */
+		if ((pAd->StaCfg.Psm == PWR_SAVE) && (pHeader->FC.MoreData == 1))
+		{
+			/* for UAPSD, all management frames will be VO priority */
+			if (pAd->CommonCfg.bAPSDAC_VO == 0)
+			{
+				/* non-UAPSD delivery-enabled AC */
+				RTMP_PS_POLL_ENQUEUE(pAd);
+			}
+		}
+
+		/* TODO: if MoreData == 0, station can go to sleep */
+
+
+		// We should collect RSSI not only U2M data but also my beacon
+		if ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2))
+			&& (pAd->RxAnt.EvaluatePeriod == 0))
+		{
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+			pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+			pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+		}
+
+#ifdef RT30xx
+#ifdef ANT_DIVERSITY_SUPPORT
+		// collect rssi information for antenna diversity
+		if ((pAd->NicConfig2.field.AntDiversity) &&
+			(pAd->CommonCfg.bRxAntDiversity != ANT_DIVERSITY_DISABLE))
+		{
+			if ((pRxD->U2M) || ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2))))
+			{
+					COLLECT_RX_ANTENNA_AVERAGE_RSSI(pAd, ConvertToRssi(pAd, (UCHAR)pRxWI->RSSI0, RSSI_0), 0); //Note: RSSI2 not used on RT73
+					pAd->StaCfg.NumOfAvgRssiSample ++;
+			}
+		}
+#endif // ANT_DIVERSITY_SUPPORT //
+#endif // RT30xx //
+
+		// First check the size, it MUST not exceed the mlme queue size
+		if (pRxWI->MPDUtotalByteCount > MGMT_DMA_BUFFER_SIZE)
+		{
+			DBGPRINT_ERR(("STAHandleRxMgmtFrame: frame too large, size = %d \n", pRxWI->MPDUtotalByteCount));
+			break;
+		}
+
+		REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pHeader, pRxWI->MPDUtotalByteCount,
+									pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+	} while (FALSE);
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+}
+
+VOID STAHandleRxControlFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+#ifdef DOT11_N_SUPPORT
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+#endif // DOT11_N_SUPPORT //
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	switch (pHeader->FC.SubType)
+	{
+		case SUBTYPE_BLOCK_ACK_REQ:
+#ifdef DOT11_N_SUPPORT
+			{
+				CntlEnqueueForRecv(pAd, pRxWI->WirelessCliID, (pRxWI->MPDUtotalByteCount), (PFRAME_BA_REQ)pHeader);
+			}
+			break;
+#endif // DOT11_N_SUPPORT //
+		case SUBTYPE_BLOCK_ACK:
+		case SUBTYPE_ACK:
+		default:
+			break;
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process RxDone interrupt, running in DPC level
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		This routine has to maintain Rx ring read pointer.
+		Need to consider QOS DATA format when converting to 802.3
+	========================================================================
+*/
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc)
+{
+	NDIS_STATUS			Status;
+	UINT32			RxProcessed, RxPending;
+	BOOLEAN			bReschedule = FALSE;
+	RT28XX_RXD_STRUC	*pRxD;
+	UCHAR			*pData;
+	PRXWI_STRUC		pRxWI;
+	PNDIS_PACKET	pRxPacket;
+	PHEADER_802_11	pHeader;
+	RX_BLK			RxCell;
+
+	RxProcessed = RxPending = 0;
+
+	// process whole rx ring
+	while (1)
+	{
+
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST) ||
+			!RTMP_TEST_FLAG(pAd,fRTMP_ADAPTER_START_UP))
+		{
+			break;
+		}
+
+#ifdef RTMP_MAC_PCI
+		if (RxProcessed++ > MAX_RX_PROCESS_CNT)
+		{
+			// need to reschedule rx handle
+			bReschedule = TRUE;
+			break;
+		}
+#endif // RTMP_MAC_PCI //
+
+		RxProcessed ++; // test
+
+		// 1. allocate a new data packet into rx ring to replace received packet
+		//    then processing the received packet
+		// 2. the callee must take charge of release of packet
+		// 3. As far as driver is concerned ,
+		//    the rx packet must
+		//      a. be indicated to upper layer or
+		//      b. be released if it is discarded
+		pRxPacket = GetPacketFromRxRing(pAd, &(RxCell.RxD), &bReschedule, &RxPending);
+		if (pRxPacket == NULL)
+		{
+			// no more packet to process
+			break;
+		}
+
+		// get rx ring descriptor
+		pRxD = &(RxCell.RxD);
+		// get rx data buffer
+		pData	= GET_OS_PKT_DATAPTR(pRxPacket);
+		pRxWI	= (PRXWI_STRUC) pData;
+		pHeader = (PHEADER_802_11) (pData+RXWI_SIZE) ;
+
+#ifdef RT_BIG_ENDIAN
+	    RTMPFrameEndianChange(pAd, (PUCHAR)pHeader, DIR_READ, TRUE);
+		RTMPWIEndianChange((PUCHAR)pRxWI, TYPE_RXWI);
+#endif
+
+		// build RxCell
+		RxCell.pRxWI = pRxWI;
+		RxCell.pHeader = pHeader;
+		RxCell.pRxPacket = pRxPacket;
+		RxCell.pData = (UCHAR *) pHeader;
+		RxCell.DataSize = pRxWI->MPDUtotalByteCount;
+		RxCell.Flags = 0;
+
+		// Increase Total receive byte counter after real data received no mater any error or not
+		pAd->RalinkCounters.ReceivedByteCount +=  pRxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.OneSecReceivedByteCount +=  pRxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.RxCount ++;
+
+		INC_COUNTER64(pAd->WlanCounters.ReceivedFragmentCount);
+
+		if (pRxWI->MPDUtotalByteCount < 14)
+			Status = NDIS_STATUS_FAILURE;
+
+        if (MONITOR_ON(pAd))
+		{
+            send_monitor_packets(pAd, &RxCell);
+			break;
+		}
+
+		/* STARxDoneInterruptHandle() is called in rtusb_bulk.c */
+#ifdef RALINK_ATE
+		if (ATE_ON(pAd))
+		{
+			pAd->ate.RxCntPerSec++;
+			ATESampleRssi(pAd, pRxWI);
+#ifdef RALINK_28xx_QA
+			if (pAd->ate.bQARxStart == TRUE)
+			{
+				/* (*pRxD) has been swapped in GetPacketFromRxRing() */
+				ATE_QA_Statistics(pAd, pRxWI, pRxD,	pHeader);
+			}
+#endif // RALINK_28xx_QA //
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+			continue;
+		}
+#endif // RALINK_ATE //
+
+		// Check for all RxD errors
+		Status = RTMPCheckRxError(pAd, pHeader, pRxWI, pRxD);
+
+		// Handle the received frame
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			switch (pHeader->FC.Type)
+			{
+				// CASE I, receive a DATA frame
+				case BTYPE_DATA:
+				{
+					// process DATA frame
+					STAHandleRxDataFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE II, receive a MGMT frame
+				case BTYPE_MGMT:
+				{
+					STAHandleRxMgmtFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE III. receive a CNTL frame
+				case BTYPE_CNTL:
+				{
+					STAHandleRxControlFrame(pAd, &RxCell);
+				}
+				break;
+				// discard other type
+				default:
+					RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+					break;
+			}
+		}
+		else
+		{
+			pAd->Counters8023.RxErrors++;
+			// discard this frame
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		}
+	}
+
+	return bReschedule;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		pAd	Pointer to our adapter
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	AsicForceWakeup(pAd, FALSE);
+}
+
+/*
+========================================================================
+Routine Description:
+    Early checking and OS-depened parsing for Tx packet send to our STA driver.
+
+Arguments:
+    NDIS_HANDLE		MiniportAdapterContext	Pointer refer to the device handle, i.e., the pAd.
+	PPNDIS_PACKET	ppPacketArray			The packet array need to do transmission.
+	UINT			NumberOfPackets			Number of packet in packet array.
+
+Return Value:
+	NONE
+
+Note:
+	This function do early checking and classification for send-out packet.
+	You only can put OS-depened & STA related code in here.
+========================================================================
+*/
+VOID STASendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets)
+{
+	UINT			Index;
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER) MiniportAdapterContext;
+	PNDIS_PACKET	pPacket;
+	BOOLEAN			allowToSend = FALSE;
+
+
+	for (Index = 0; Index < NumberOfPackets; Index++)
+	{
+		pPacket = ppPacketArray[Index];
+
+		do
+		{
+
+			if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+			{
+				// Drop send request since hardware is in reset state
+					break;
+			}
+			else if (!INFRA_ON(pAd) && !ADHOC_ON(pAd))
+			{
+				// Drop send request since there are no physical connection yet
+					break;
+			}
+			else
+			{
+				// Record that orignal packet source is from NDIS layer,so that
+				// later on driver knows how to release this NDIS PACKET
+#ifdef QOS_DLS_SUPPORT
+				MAC_TABLE_ENTRY *pEntry;
+				PUCHAR pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+				if (pEntry && (pEntry->ValidAsDls == TRUE))
+				{
+					RTMP_SET_PACKET_WCID(pPacket, pEntry->Aid);
+				}
+				else
+#endif // QOS_DLS_SUPPORT //
+				RTMP_SET_PACKET_WCID(pPacket, 0); // this field is useless when in STA mode
+				RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+				NDIS_SET_PACKET_STATUS(pPacket, NDIS_STATUS_PENDING);
+				pAd->RalinkCounters.PendingNdisPacketCount++;
+
+				allowToSend = TRUE;
+			}
+		} while(FALSE);
+
+		if (allowToSend == TRUE)
+			STASendPacket(pAd, pPacket);
+		else
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+
+	// Dequeue outgoing frames from TxSwQueue[] and process it
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+	This routine is used to do packet parsing and classification for Tx packet
+	to STA device, and it will en-queue packets to our TxSwQueue depends on AC
+	class.
+
+Arguments:
+	pAd		Pointer to our adapter
+	pPacket		Pointer to send packet
+
+Return Value:
+	NDIS_STATUS_SUCCESS			If succes to queue the packet into TxSwQueue.
+	NDIS_STATUS_FAILURE			If failed to do en-queue.
+
+Note:
+	You only can put OS-indepened & STA related code in here.
+========================================================================
+*/
+NDIS_STATUS STASendPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	UINT			AllowFragSize;
+	UCHAR			NumberOfFrag;
+	UCHAR			RTSRequired;
+	UCHAR			QueIdx, UserPriority;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+	unsigned int	IrqFlags;
+	UCHAR			FlgIsIP = 0;
+	UCHAR			Rate;
+
+	// Prepare packet information structure for buffer descriptor
+	// chained within a single NDIS packet.
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	if (pSrcBufVA == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> pSrcBufVA == NULL !!!SrcBufLen=%x\n",SrcBufLen));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+	if (SrcBufLen < 14)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> Ndis Packet buffer error !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+	// In HT rate adhoc mode, A-MPDU is often used. So need to lookup BA Table and MAC Entry.
+	// Note multicast packets in adhoc also use BSSID_WCID index.
+	{
+		if(INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			USHORT	tmpWcid;
+
+			tmpWcid = RTMP_GET_PACKET_WCID(pPacket);
+			if (VALID_WCID(tmpWcid) &&
+				(pAd->MacTab.Content[tmpWcid].ValidAsDls== TRUE))
+			{
+				pEntry = &pAd->MacTab.Content[tmpWcid];
+				Rate = pAd->MacTab.Content[tmpWcid].CurrTxRate;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			RTMP_SET_PACKET_WCID(pPacket, BSSID_WCID);
+			Rate = pAd->CommonCfg.TxRate;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			if (*pSrcBufVA & 0x01)
+			{
+				RTMP_SET_PACKET_WCID(pPacket, MCAST_WCID);
+				pEntry = &pAd->MacTab.Content[MCAST_WCID];
+			}
+			else
+			{
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+			}
+			Rate = pAd->CommonCfg.TxRate;
+		}
+	}
+
+	if (!pEntry)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket->Cannot find pEntry(%2x:%2x:%2x:%2x:%2x:%2x) in MacTab!\n", PRINT_MAC(pSrcBufVA)));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+	if (ADHOC_ON(pAd)
+		)
+	{
+		RTMP_SET_PACKET_WCID(pPacket, (UCHAR)pEntry->Aid);
+	}
+
+	//
+	// Check the Ethernet Frame type of this packet, and set the RTMP_SET_PACKET_SPECIFIC flags.
+	//		Here we set the PACKET_SPECIFIC flags(LLC, VLAN, DHCP/ARP, EAPOL).
+	RTMPCheckEtherType(pAd, pPacket);
+
+
+
+	//
+	// WPA 802.1x secured port control - drop all non-802.1x frame before port secured
+	//
+	if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+		  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+		  )
+		  && ((pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED) || (pAd->StaCfg.MicErrCnt >= 2))
+		  && (RTMP_GET_PACKET_EAPOL(pPacket)== FALSE)
+		  )
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("STASendPacket --> Drop packet before port secured !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return (NDIS_STATUS_FAILURE);
+	}
+
+
+	// STEP 1. Decide number of fragments required to deliver this MSDU.
+	//	   The estimation here is not very accurate because difficult to
+	//	   take encryption overhead into consideration here. The result
+	//	   "NumberOfFrag" is then just used to pre-check if enough free
+	//	   TXD are available to hold this MSDU.
+
+
+	if (*pSrcBufVA & 0x01)	// fragmentation not allowed on multicast & broadcast
+		NumberOfFrag = 1;
+	else if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+	else if (CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+#ifdef DOT11_N_SUPPORT
+	else if ((pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTMIX) || (pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTGREENFIELD))
+		NumberOfFrag = 1;	// MIMO RATE overwhelms fragmentation
+#endif // DOT11_N_SUPPORT //
+	else
+	{
+		// The calculated "NumberOfFrag" is a rough estimation because of various
+		// encryption/encapsulation overhead not taken into consideration. This number is just
+		// used to make sure enough free TXD are available before fragmentation takes place.
+		// In case the actual required number of fragments of an NDIS packet
+		// excceeds "NumberOfFrag"caculated here and not enough free TXD available, the
+		// last fragment (i.e. last MPDU) will be dropped in RTMPHardTransmit() due to out of
+		// resource, and the NDIS packet will be indicated NDIS_STATUS_FAILURE. This should
+		// rarely happen and the penalty is just like a TX RETRY fail. Affordable.
+
+		AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+		NumberOfFrag = ((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) / AllowFragSize) + 1;
+		// To get accurate number of fragmentation, Minus 1 if the size just match to allowable fragment size
+		if (((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) % AllowFragSize) == 0)
+		{
+			NumberOfFrag--;
+		}
+	}
+
+	// Save fragment number to Ndis packet reserved field
+	RTMP_SET_PACKET_FRAGMENTS(pPacket, NumberOfFrag);
+
+
+	// STEP 2. Check the requirement of RTS:
+	//	   If multiple fragment required, RTS is required only for the first fragment
+	//	   if the fragment size large than RTS threshold
+	//     For RT28xx, Let ASIC send RTS/CTS
+//	RTMP_SET_PACKET_RTS(pPacket, 0);
+	if (NumberOfFrag > 1)
+		RTSRequired = (pAd->CommonCfg.FragmentThreshold > pAd->CommonCfg.RtsThreshold) ? 1 : 0;
+	else
+		RTSRequired = (PacketInfo.TotalPacketLength > pAd->CommonCfg.RtsThreshold) ? 1 : 0;
+
+	// Save RTS requirement to Ndis packet reserved field
+	RTMP_SET_PACKET_RTS(pPacket, RTSRequired);
+	RTMP_SET_PACKET_TXRATE(pPacket, pAd->CommonCfg.TxRate);
+
+	//
+	// STEP 3. Traffic classification. outcome = <UserPriority, QueIdx>
+	//
+	UserPriority = 0;
+	QueIdx		 = QID_AC_BE;
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) &&
+		CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE))
+	{
+		USHORT Protocol;
+		UCHAR  LlcSnapLen = 0, Byte0, Byte1;
+		do
+		{
+			// get Ethernet protocol field
+			Protocol = (USHORT)((pSrcBufVA[12] << 8) + pSrcBufVA[13]);
+			if (Protocol <= 1500)
+			{
+				// get Ethernet protocol field from LLC/SNAP
+				if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + 6, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+					break;
+
+				Protocol = (USHORT)((Byte0 << 8) + Byte1);
+				LlcSnapLen = 8;
+			}
+
+			// always AC_BE for non-IP packet
+			if (Protocol != 0x0800)
+				break;
+
+			// get IP header
+			if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + LlcSnapLen, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+				break;
+
+			// return AC_BE if packet is not IPv4
+			if ((Byte0 & 0xf0) != 0x40)
+				break;
+
+			FlgIsIP = 1;
+			UserPriority = (Byte1 & 0xe0) >> 5;
+			QueIdx = MapUserPriorityToAccessCategory[UserPriority];
+
+			// TODO: have to check ACM bit. apply TSPEC if ACM is ON
+			// TODO: downgrade UP & QueIdx before passing ACM
+			/*
+				Under WMM ACM control, we dont need to check the bit;
+				Or when a TSPEC is built for VO but we will change to issue
+				BA session for BE here, so we will not use BA to send VO packets.
+			*/
+			if (pAd->CommonCfg.APEdcaParm.bACM[QueIdx])
+			{
+				UserPriority = 0;
+				QueIdx		 = QID_AC_BE;
+			}
+		} while (FALSE);
+	}
+
+	RTMP_SET_PACKET_UP(pPacket, UserPriority);
+
+
+
+	// Make sure SendTxWait queue resource won't be used by other threads
+	RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+	if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+#ifdef BLOCK_NET_IF
+		StopNetIfQueue(pAd, QueIdx, pPacket);
+#endif // BLOCK_NET_IF //
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+		InsertTailQueueAc(pAd, pEntry, &pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pPacket));
+	}
+	RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->CommonCfg.BACapability.field.AutoBA == TRUE)&&
+        IS_HT_STA(pEntry))
+	{
+	    //PMAC_TABLE_ENTRY pMacEntry = &pAd->MacTab.Content[BSSID_WCID];
+		if (((pEntry->TXBAbitmap & (1<<UserPriority)) == 0) &&
+            ((pEntry->BADeclineBitmap & (1<<UserPriority)) == 0) &&
+            (pEntry->PortSecured == WPA_802_1X_PORT_SECURED)
+			 // For IOT compatibility, if
+			 // 1. It is Ralink chip or
+			 // 2. It is OPEN or AES mode,
+			 // then BA session can be bulit.
+			 && ((pEntry->ValidAsCLI && pAd->MlmeAux.APRalinkIe != 0x0) ||
+				 (pEntry->WepStatus != Ndis802_11WEPEnabled && pEntry->WepStatus != Ndis802_11Encryption2Enabled))
+			)
+		{
+			BAOriSessionSetUp(pAd, pEntry, UserPriority, 0, 10, FALSE);
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->RalinkCounters.OneSecOsTxCount[QueIdx]++; // TODO: for debug only. to be removed
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		This subroutine will scan through releative ring descriptor to find
+		out avaliable free ring descriptor and compare with request size.
+
+	Arguments:
+		pAd Pointer to our adapter
+		QueIdx		Selected TX Ring
+
+	Return Value:
+		NDIS_STATUS_FAILURE	Not enough free descriptor
+		NDIS_STATUS_SUCCESS	Enough free descriptor
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#ifdef RTMP_MAC_PCI
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR			QueIdx,
+	IN		UCHAR			NumberRequired,
+	IN		PUCHAR			FreeNumberIs)
+{
+	ULONG		FreeNumber = 0;
+	NDIS_STATUS	Status = NDIS_STATUS_FAILURE;
+
+	switch (QueIdx)
+	{
+		case QID_AC_BK:
+		case QID_AC_BE:
+		case QID_AC_VI:
+		case QID_AC_VO:
+		/*case QID_HCCA:*/
+			if (pAd->TxRing[QueIdx].TxSwFreeIdx > pAd->TxRing[QueIdx].TxCpuIdx)
+				FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx - pAd->TxRing[QueIdx].TxCpuIdx - 1;
+			else
+				FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx + TX_RING_SIZE - pAd->TxRing[QueIdx].TxCpuIdx - 1;
+
+			if (FreeNumber >= NumberRequired)
+				Status = NDIS_STATUS_SUCCESS;
+			break;
+
+		case QID_MGMT:
+			if (pAd->MgmtRing.TxSwFreeIdx > pAd->MgmtRing.TxCpuIdx)
+				FreeNumber = pAd->MgmtRing.TxSwFreeIdx - pAd->MgmtRing.TxCpuIdx - 1;
+			else
+				FreeNumber = pAd->MgmtRing.TxSwFreeIdx + MGMT_RING_SIZE - pAd->MgmtRing.TxCpuIdx - 1;
+
+			if (FreeNumber >= NumberRequired)
+				Status = NDIS_STATUS_SUCCESS;
+			break;
+
+		default:
+			DBGPRINT(RT_DEBUG_ERROR,("RTMPFreeTXDRequest::Invalid QueIdx(=%d)\n", QueIdx));
+			break;
+	}
+	*FreeNumberIs = (UCHAR)FreeNumber;
+
+	return (Status);
+}
+#endif // RTMP_MAC_PCI //
+
+
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+VOID	RTMPSendNullFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			TxRate,
+	IN	BOOLEAN			bQosNull)
+{
+	UCHAR	NullFrame[48];
+	ULONG	Length;
+	PHEADER_802_11	pHeader_802_11;
+
+
+#ifdef RALINK_ATE
+	if(ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+    // WPA 802.1x secured port control
+    if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+			  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif
+        ) &&
+       (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	{
+		return;
+	}
+
+	NdisZeroMemory(NullFrame, 48);
+	Length = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (PHEADER_802_11) NullFrame;
+
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = SUBTYPE_NULL_FUNC;
+	pHeader_802_11->FC.ToDs = 1;
+	COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+	{
+		pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	}
+	else
+	{
+		pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE) ? 1: 0;
+	}
+	pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + RTMPCalcDuration(pAd, TxRate, 14);
+
+	pAd->Sequence++;
+	pHeader_802_11->Sequence = pAd->Sequence;
+
+	// Prepare QosNull function frame
+	if (bQosNull)
+	{
+		pHeader_802_11->FC.SubType = SUBTYPE_QOS_NULL;
+
+		// copy QOS control bytes
+		NullFrame[Length]	=  0;
+		NullFrame[Length+1] =  0;
+		Length += 2;// if pad with 2 bytes for alignment, APSD will fail
+	}
+
+	HAL_KickOutNullFrameTx(pAd, 0, NullFrame, Length);
+
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	RTMPSendRTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	unsigned int	NextMpduSize,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			RTSRate,
+	IN	USHORT			AckDuration,
+	IN	UCHAR			QueIdx,
+	IN	UCHAR			FrameGap)
+{
+}
+
+
+
+// --------------------------------------------------------
+//  FIND ENCRYPT KEY AND DECIDE CIPHER ALGORITHM
+//		Find the WPA key, either Group or Pairwise Key
+//		LEAP + TKIP also use WPA key.
+// --------------------------------------------------------
+// Decide WEP bit and cipher suite to be used. Same cipher suite should be used for whole fragment burst
+// In Cisco CCX 2.0 Leap Authentication
+//		   WepStatus is Ndis802_11Encryption1Enabled but the key will use PairwiseKey
+//		   Instead of the SharedKey, SharedKey Length may be Zero.
+VOID STAFindCipherAlgorithm(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	NDIS_802_11_ENCRYPTION_STATUS	Cipher;				// To indicate cipher used for this packet
+	UCHAR							CipherAlg = CIPHER_NONE;		// cipher alogrithm
+	UCHAR							KeyIdx = 0xff;
+	PUCHAR							pSrcBufVA;
+	PCIPHER_KEY						pKey = NULL;
+
+	pSrcBufVA = GET_OS_PKT_DATAPTR(pTxBlk->pPacket);
+
+	{
+	    // Select Cipher
+	    if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+	        Cipher = pAd->StaCfg.GroupCipher; // Cipher for Multicast or Broadcast
+	    else
+	        Cipher = pAd->StaCfg.PairCipher; // Cipher for Unicast
+
+		if (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		{
+			ASSERT(pAd->SharedKey[BSS0][0].CipherAlg <= CIPHER_CKIP128);
+
+			// 4-way handshaking frame must be clear
+			if (!(TX_BLK_TEST_FLAG(pTxBlk, fTX_bClearEAPFrame)) && (pAd->SharedKey[BSS0][0].CipherAlg) &&
+				(pAd->SharedKey[BSS0][0].KeyLen))
+			{
+				CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+				KeyIdx = 0;
+			}
+		}
+		else if (Cipher == Ndis802_11Encryption1Enabled)
+		{
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+		else if ((Cipher == Ndis802_11Encryption2Enabled) ||
+				 (Cipher == Ndis802_11Encryption3Enabled))
+		{
+			if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))) // multicast
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (pAd->SharedKey[BSS0][0].KeyLen)
+				KeyIdx = 0;
+			else
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+
+		if (KeyIdx == 0xff)
+			CipherAlg = CIPHER_NONE;
+		else if ((Cipher == Ndis802_11EncryptionDisabled) || (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 0))
+			CipherAlg = CIPHER_NONE;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    else if ( pAd->StaCfg.WpaSupplicantUP &&
+	             (Cipher == Ndis802_11Encryption1Enabled) &&
+	             (pAd->StaCfg.IEEE8021X == TRUE) &&
+	             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	        CipherAlg = CIPHER_NONE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+		else
+		{
+			//Header_802_11.FC.Wep = 1;
+			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+			pKey = &pAd->SharedKey[BSS0][KeyIdx];
+		}
+	}
+
+	pTxBlk->CipherAlg = CipherAlg;
+	pTxBlk->pKey = pKey;
+}
+
+
+VOID STABuildCommon802_11Header(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  TX_BLK          *pTxBlk)
+{
+
+	HEADER_802_11	*pHeader_802_11;
+#ifdef QOS_DLS_SUPPORT
+	BOOLEAN	bDLSFrame = FALSE;
+	INT	DlsEntryIndex = 0;
+#endif // QOS_DLS_SUPPORT //
+
+	//
+	// MAKE A COMMON 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (HEADER_802_11 *) &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+
+	NdisZeroMemory(pHeader_802_11, sizeof(HEADER_802_11));
+
+	pHeader_802_11->FC.FrDs = 0;
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = ((TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM)) ? SUBTYPE_QDATA : SUBTYPE_DATA);
+
+#ifdef QOS_DLS_SUPPORT
+	if (INFRA_ON(pAd))
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+	}
+#endif // QOS_DLS_SUPPORT //
+
+    if (pTxBlk->pMacEntry)
+	{
+		if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bForceNonQoS))
+		{
+			pHeader_802_11->Sequence = pTxBlk->pMacEntry->NonQosDataSeq;
+			pTxBlk->pMacEntry->NonQosDataSeq = (pTxBlk->pMacEntry->NonQosDataSeq+1) & MAXSEQ;
+		}
+		else
+		{
+			{
+	    pHeader_802_11->Sequence = pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority];
+	    pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority] = (pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+	}
+	}
+	}
+	else
+	{
+		pHeader_802_11->Sequence = pAd->Sequence;
+		pAd->Sequence = (pAd->Sequence+1) & MAXSEQ; // next sequence
+	}
+
+	pHeader_802_11->Frag = 0;
+
+	pHeader_802_11->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	{
+		if (INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			if (bDLSFrame)
+			{
+				COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+				COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+				COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+				pHeader_802_11->FC.ToDs = 0;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pTxBlk->pSrcBufHeader);
+			pHeader_802_11->FC.ToDs = 1;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+			pHeader_802_11->FC.ToDs = 0;
+		}
+	}
+
+	if (pTxBlk->CipherAlg != CIPHER_NONE)
+		pHeader_802_11->FC.Wep = 1;
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+	pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	else
+	pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID STABuildCache802_11Header(
+	IN RTMP_ADAPTER		*pAd,
+	IN TX_BLK			*pTxBlk,
+	IN UCHAR			*pHeader)
+{
+	MAC_TABLE_ENTRY	*pMacEntry;
+	PHEADER_802_11	pHeader80211;
+
+	pHeader80211 = (PHEADER_802_11)pHeader;
+	pMacEntry = pTxBlk->pMacEntry;
+
+	//
+	// Update the cached 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	// More Bit
+	pHeader80211->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	// Sequence
+	pHeader80211->Sequence = pMacEntry->TxSeq[pTxBlk->UserPriority];
+    pMacEntry->TxSeq[pTxBlk->UserPriority] = (pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+#ifdef QOS_DLS_SUPPORT
+		BOOLEAN	bDLSFrame = FALSE;
+		INT	DlsEntryIndex = 0;
+
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+#endif // QOS_DLS_SUPPORT //
+
+		// The addr3 of normal packet send from DS is Dest Mac address.
+#ifdef QOS_DLS_SUPPORT
+		if (bDLSFrame)
+		{
+			COPY_MAC_ADDR(pHeader80211->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+			pHeader80211->FC.ToDs = 0;
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+		else
+		COPY_MAC_ADDR(pHeader80211->Addr3, pTxBlk->pSrcBufHeader);
+	}
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+	pHeader80211->FC.PwrMgmt = PWR_SAVE;
+	else
+	pHeader80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+#endif // DOT11_N_SUPPORT //
+
+static inline PUCHAR STA_Build_ARalink_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+	PNDIS_PACKET	pNextPacket;
+	UINT32			nextBufLen;
+	PQUEUE_ENTRY	pQEntry;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// steal "order" bit to mark "aggregation"
+	pHeader_802_11->FC.Order = 1;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// padding at front of LLC header. LLC header should at 4-bytes aligment.
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PUCHAR)ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	// For RA Aggregation,
+	// put the 2nd MSDU length(extra 2-byte field) after QOS_CONTROL in little endian format
+	pQEntry = pTxBlk->TxPacketList.Head;
+	pNextPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	nextBufLen = GET_OS_PKT_LEN(pNextPacket);
+	if (RTMP_GET_PACKET_VLAN(pNextPacket))
+		nextBufLen -= LENGTH_802_1Q;
+
+	*pHeaderBufPtr = (UCHAR)nextBufLen & 0xff;
+	*(pHeaderBufPtr+1) = (UCHAR)(nextBufLen >> 8);
+
+	pHeaderBufPtr += 2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	return pHeaderBufPtr;
+
+}
+
+#ifdef DOT11_N_SUPPORT
+static inline PUCHAR STA_Build_AMSDU_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;//, pSaveBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	//
+	// build QOS Control bytes
+	//
+	*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+	//
+	// A-MSDU packet
+	//
+	*pHeaderBufPtr |= 0x80;
+
+	*(pHeaderBufPtr+1) = 0;
+	pHeaderBufPtr +=2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	//pSaveBufPtr = pHeaderBufPtr;
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	// @@@ MpduHeaderLen excluding padding @@@
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PUCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	return pHeaderBufPtr;
+
+}
+
+
+VOID STA_AMPDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	MAC_TABLE_ENTRY	*pMacEntry;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if ( RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		pMacEntry = pTxBlk->pMacEntry;
+		if (pMacEntry->isCached)
+		{
+			// NOTE: Please make sure the size of pMacEntry->CachedBuf[] is smaller than pTxBlk->HeaderBuf[]!!!!
+			NdisMoveMemory((PUCHAR)&pTxBlk->HeaderBuf[TXINFO_SIZE], (PUCHAR)&pMacEntry->CachedBuf[0], TXWI_SIZE + sizeof(HEADER_802_11));
+			pHeaderBufPtr = (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE]);
+			STABuildCache802_11Header(pAd, pTxBlk, pHeaderBufPtr);
+		}
+		else
+		{
+			STAFindCipherAlgorithm(pAd, pTxBlk);
+			STABuildCommon802_11Header(pAd, pTxBlk);
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+		}
+
+
+		pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+		// skip common header
+		pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+
+		//
+		// build HTC+
+		// HTC control filed following QoS field
+		//
+		if ((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pTxBlk->pMacEntry, fCLIENT_STATUS_RDG_CAPABLE))
+		{
+			if (pMacEntry->isCached == FALSE)
+			{
+				// mark HTC bit
+				pHeader_802_11->FC.Order = 1;
+
+				NdisZeroMemory(pHeaderBufPtr, 4);
+				*(pHeaderBufPtr+3) |= 0x80;
+			}
+			pHeaderBufPtr += 4;
+			pTxBlk->MpduHeaderLen += 4;
+		}
+
+		//pTxBlk->MpduHeaderLen = pHeaderBufPtr - pTxBlk->HeaderBuf - TXWI_SIZE - TXINFO_SIZE;
+		ASSERT(pTxBlk->MpduHeaderLen >= 24);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		//
+		// padding at front of LLC header
+		// LLC header should locate at 4-octets aligment
+		//
+		// @@@ MpduHeaderLen excluding padding @@@
+		//
+		pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+		pHeaderBufPtr = (PUCHAR) ROUND_UP(pHeaderBufPtr, 4);
+		pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+		{
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+
+		}
+
+		if (pMacEntry->isCached)
+		{
+            RTMPWriteTxWI_Cache(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+			NdisZeroMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), sizeof(pMacEntry->CachedBuf));
+			NdisMoveMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), (pHeaderBufPtr - (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE])));
+			pMacEntry->isCached = TRUE;
+		}
+
+		// calculate Transmitted AMPDU count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedMPDUsInAMPDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMPDUCount.QuadPart += pTxBlk->SrcBufLen;
+		}
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+		HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+		//
+		// Kick out Tx
+		//
+                if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
+		HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+	}
+
+}
+
+
+VOID STA_AMSDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			subFramePayloadLen = 0;	// AMSDU Subframe length without AMSDU-Header / Padding.
+	USHORT			totalMPDUSize=0;
+	UCHAR			*subFrameHeader;
+	UCHAR			padding = 0;
+	USHORT			FirstTx = 0, LastTxIdx = 0;
+	BOOLEAN			bVLANPkt;
+	int			frameNum = 0;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number > 1));
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{
+			pHeaderBufPtr = STA_Build_AMSDU_Frame_Header(pAd, pTxBlk);
+
+			// NOTE: TxWI->MPDUtotalByteCount will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			padding = ROUND_UP(LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen, 4) - (LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen);
+			NdisZeroMemory(pHeaderBufPtr, padding + LENGTH_AMSDU_SUBFRAMEHEAD);
+			pHeaderBufPtr += padding;
+			pTxBlk->MpduHeaderLen = padding;
+		}
+
+		//
+		// A-MSDU subframe
+		//   DA(6)+SA(6)+Length(2) + LLC/SNAP Encap
+		//
+		subFrameHeader = pHeaderBufPtr;
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		NdisMoveMemory(subFrameHeader, pTxBlk->pSrcBufHeader, 12);
+
+
+		pHeaderBufPtr += LENGTH_AMSDU_SUBFRAMEHEAD;
+		pTxBlk->MpduHeaderLen += LENGTH_AMSDU_SUBFRAMEHEAD;
+
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			subFramePayloadLen += LENGTH_802_1_H;
+		}
+
+		// update subFrame Length field
+		subFrameHeader[12] = (subFramePayloadLen & 0xFF00) >> 8;
+		subFrameHeader[13] = subFramePayloadLen & 0xFF;
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// calculate Transmitted AMSDU Count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedAMSDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMSDU.QuadPart += totalMPDUSize;
+		}
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+        if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID STA_Legacy_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	if (pTxBlk->TxFrameType == TX_MCAST_FRAME)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastTransmittedFrameCount);
+	}
+
+	if (RTMP_GET_PACKET_RTS(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bRtsRequired);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bRtsRequired);
+
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	if (pTxBlk->TxRate < pAd->CommonCfg.MinTxRate)
+		pTxBlk->TxRate = pAd->CommonCfg.MinTxRate;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*(pHeaderBufPtr) = ((pTxBlk->UserPriority & 0x0F) | (pAd->CommonCfg.AckPolicy[pTxBlk->QueIdx]<<5));
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// The remaining content of MPDU header should locate at 4-octets aligment
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PUCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	{
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		//
+		// if original Ethernet frame contains no LLC/SNAP,
+		// then an extra LLC/SNAP encap is required
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			UCHAR vlan_size;
+
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// skip vlan tag
+			vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+		}
+
+	}
+
+	//
+	// prepare for TXWI
+	// use Wcid as Key Index
+	//
+
+	RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+	//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+	HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	//
+	// Kick out Tx
+	//
+	if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+VOID STA_ARalink_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			totalMPDUSize=0;
+	USHORT			FirstTx, LastTxIdx;
+	int			frameNum = 0;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number== 2));
+
+
+	FirstTx = LastTxIdx = 0;  // Is it ok init they as 0?
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{	// For first frame, we need to create the 802.11 header + padding(optional) + RA-AGG-LEN + SNAP Header
+
+			pHeaderBufPtr = STA_Build_ARalink_Frame_Header(pAd, pTxBlk);
+
+			// It's ok write the TxWI here, because the TxWI->MPDUtotalByteCount
+			//	will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+		}
+		else
+		{	// For second aggregated frame, we need create the 802.3 header to headerBuf, because PCI will copy it to SDPtr0.
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			pTxBlk->MpduHeaderLen = 0;
+
+			// A-Ralink sub-sequent frame header is the same as 802.3 header.
+			//   DA(6)+SA(6)+FrameType(2)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader, 12);
+			pHeaderBufPtr += 12;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen = LENGTH_ARALINK_SUBFRAMEHEAD;
+		}
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.OneSecTxAggregationCount++;
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+}
+
+
+VOID STA_Fragment_Frame_Tx(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	UCHAR			fragNum = 0;
+	PACKET_INFO		PacketInfo;
+	USHORT			EncryptionOverhead = 0;
+	UINT32			FreeMpduSize, SrcRemainingBytes;
+	USHORT			AckDuration;
+	UINT			NextMpduSize;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+	HTTRANSMIT_SETTING	*pTransmit;
+
+
+	ASSERT(pTxBlk);
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag));
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+		pTxBlk->pPacket = duplicate_pkt_with_TKIP_MIC(pAd, pTxBlk->pPacket);
+		if (pTxBlk->pPacket == NULL)
+			return;
+		RTMP_QueryPacketInfo(pTxBlk->pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+	}
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *)pHeaderBufPtr;
+
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PUCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+
+
+	//
+	// Insert LLC-SNAP encapsulation - 8 octets
+	//
+	//
+	// if original Ethernet frame contains no LLC/SNAP,
+	// then an extra LLC/SNAP encap is required
+	//
+	EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+	if (pTxBlk->pExtraLlcSnapEncap)
+	{
+		UCHAR vlan_size;
+
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+		pHeaderBufPtr += 6;
+		// skip vlan tag
+		vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+		// get 2 octets (TypeofLen)
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+		pHeaderBufPtr += 2;
+		pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+	}
+
+
+	// If TKIP is used and fragmentation is required. Driver has to
+	//	append TKIP MIC at tail of the scatter buffer
+	//	MAC ASIC will only perform IV/EIV/ICV insertion but no TKIP MIC
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+		RTMPCalculateMICValue(pAd, pTxBlk->pPacket, pTxBlk->pExtraLlcSnapEncap, pTxBlk->pKey, 0);
+
+		// NOTE: DON'T refer the skb->len directly after following copy. Becasue the length is not adjust
+		//			to correct lenght, refer to pTxBlk->SrcBufLen for the packet length in following progress.
+		NdisMoveMemory(pTxBlk->pSrcBufData + pTxBlk->SrcBufLen, &pAd->PrivateInfo.Tx.MIC[0], 8);
+		//skb_put((RTPKT_TO_OSPKT(pTxBlk->pPacket))->tail, 8);
+		pTxBlk->SrcBufLen += 8;
+		pTxBlk->TotalFrameLen += 8;
+		pTxBlk->CipherAlg = CIPHER_TKIP_NO_MIC;
+	}
+
+	//
+	// calcuate the overhead bytes that encryption algorithm may add. This
+	// affects the calculate of "duration" field
+	//
+	if ((pTxBlk->CipherAlg == CIPHER_WEP64) || (pTxBlk->CipherAlg == CIPHER_WEP128))
+		EncryptionOverhead = 8; //WEP: IV[4] + ICV[4];
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP_NO_MIC)
+		EncryptionOverhead = 12;//TKIP: IV[4] + EIV[4] + ICV[4], MIC will be added to TotalPacketLength
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP)
+		EncryptionOverhead = 20;//TKIP: IV[4] + EIV[4] + ICV[4] + MIC[8]
+	else if (pTxBlk->CipherAlg == CIPHER_AES)
+		EncryptionOverhead = 16;	// AES: IV[4] + EIV[4] + MIC[8]
+	else
+		EncryptionOverhead = 0;
+
+	pTransmit = pTxBlk->pTransmit;
+	// Decide the TX rate
+	if (pTransmit->field.MODE == MODE_CCK)
+		pTxBlk->TxRate = pTransmit->field.MCS;
+	else if (pTransmit->field.MODE == MODE_OFDM)
+		pTxBlk->TxRate = pTransmit->field.MCS + RATE_FIRST_OFDM_RATE;
+	else
+		pTxBlk->TxRate = RATE_6_5;
+
+	// decide how much time an ACK/CTS frame will consume in the air
+	if (pTxBlk->TxRate <= RATE_LAST_OFDM_RATE)
+		AckDuration = RTMPCalcDuration(pAd, pAd->CommonCfg.ExpectedACKRate[pTxBlk->TxRate], 14);
+	else
+		AckDuration = RTMPCalcDuration(pAd, RATE_6_5, 14);
+
+	// Init the total payload length of this frame.
+	SrcRemainingBytes = pTxBlk->SrcBufLen;
+
+	pTxBlk->TotalFragNum = 0xff;
+
+	do {
+
+		FreeMpduSize = pAd->CommonCfg.FragmentThreshold - LENGTH_CRC;
+
+		FreeMpduSize -= pTxBlk->MpduHeaderLen;
+
+		if (SrcRemainingBytes <= FreeMpduSize)
+		{	// this is the last or only fragment
+
+			pTxBlk->SrcBufLen = SrcRemainingBytes;
+
+			pHeader_802_11->FC.MoreFrag = 0;
+			pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+			// Indicate the lower layer that this's the last fragment.
+			pTxBlk->TotalFragNum = fragNum;
+		}
+		else
+		{	// more fragment is required
+
+			pTxBlk->SrcBufLen = FreeMpduSize;
+
+			NextMpduSize = min(((UINT)SrcRemainingBytes - pTxBlk->SrcBufLen), ((UINT)pAd->CommonCfg.FragmentThreshold));
+			pHeader_802_11->FC.MoreFrag = 1;
+			pHeader_802_11->Duration = (3 * pAd->CommonCfg.Dsifs) + (2 * AckDuration) + RTMPCalcDuration(pAd, pTxBlk->TxRate, NextMpduSize + EncryptionOverhead);
+		}
+
+		if (fragNum == 0)
+			pTxBlk->FrameGap = IFS_HTTXOP;
+		else
+			pTxBlk->FrameGap = IFS_SIFS;
+
+		RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+		HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, &FreeNumber);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// Update the frame number, remaining size of the NDIS packet payload.
+
+		// space for 802.11 header.
+		if (fragNum == 0 && pTxBlk->pExtraLlcSnapEncap)
+			pTxBlk->MpduHeaderLen -= LENGTH_802_1_H;
+
+		fragNum++;
+		SrcRemainingBytes -= pTxBlk->SrcBufLen;
+		pTxBlk->pSrcBufData += pTxBlk->SrcBufLen;
+
+		pHeader_802_11->Frag++;	 // increase Frag #
+
+	}while(SrcRemainingBytes > 0);
+
+	//
+	// Kick out Tx
+	//
+	if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+#define RELEASE_FRAMES_OF_TXBLK(_pAd, _pTxBlk, _pQEntry, _Status)										\
+		while(_pTxBlk->TxPacketList.Head)														\
+		{																						\
+			_pQEntry = RemoveHeadQueue(&_pTxBlk->TxPacketList);									\
+			RELEASE_NDIS_PACKET(_pAd, QUEUE_ENTRY_TO_PACKET(_pQEntry), _Status);	\
+		}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd	Pointer to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx)
+{
+	NDIS_PACKET		*pPacket;
+	PQUEUE_ENTRY	pQEntry;
+
+	// ---------------------------------------------
+	// STEP 0. DO SANITY CHECK AND SOME EARLY PREPARATION.
+	// ---------------------------------------------
+	//
+	ASSERT(pTxBlk->TxPacketList.Number);
+	if (pTxBlk->TxPacketList.Head == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTxBlk->TotalFrameNum == %ld!\n", pTxBlk->TxPacketList.Number));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	pPacket = QUEUE_ENTRY_TO_PACKET(pTxBlk->TxPacketList.Head);
+
+
+	// ------------------------------------------------------------------
+	// STEP 1. WAKE UP PHY
+	//		outgoing frame always wakeup PHY to prevent frame lost and
+	//		turn off PSM bit to improve performance
+	// ------------------------------------------------------------------
+	// not to change PSM bit, just send this frame out?
+	if ((pAd->StaCfg.Psm == PWR_SAVE) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+	    DBGPRINT_RAW(RT_DEBUG_INFO, ("AsicForceWakeup At HardTx\n"));
+#ifdef RTMP_MAC_PCI
+		AsicForceWakeup(pAd, TRUE);
+#endif // RTMP_MAC_PCI //
+	}
+
+	// It should not change PSM bit, when APSD turn on.
+	if ((!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable) && (pAd->CommonCfg.bAPSDForcePowerSave == FALSE))
+		|| (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		|| (RTMP_GET_PACKET_WAI(pTxBlk->pPacket)))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+            (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeFast_PSP))
+			RTMP_SET_PSM_BIT(pAd, PWR_ACTIVE);
+	}
+
+	switch (pTxBlk->TxFrameType)
+	{
+#ifdef DOT11_N_SUPPORT
+		case TX_AMPDU_FRAME:
+				STA_AMPDU_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_AMSDU_FRAME:
+				STA_AMSDU_Frame_Tx(pAd, pTxBlk);
+			break;
+#endif // DOT11_N_SUPPORT //
+		case TX_LEGACY_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_MCAST_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_RALINK_FRAME:
+				STA_ARalink_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_FRAG_FRAME:
+				STA_Fragment_Frame_Tx(pAd, pTxBlk);
+			break;
+		default:
+			{
+				// It should not happened!
+				DBGPRINT(RT_DEBUG_ERROR, ("Send a pacekt was not classified!! It should not happen!\n"));
+				while(pTxBlk->TxPacketList.Number)
+				{
+					pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+					pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+					if (pPacket)
+						RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+			}
+			break;
+	}
+
+	return (NDIS_STATUS_SUCCESS);
+
+}
+
+ULONG  HashBytesPolynomial(UCHAR *value, unsigned int len)
+{
+   unsigned char *word = value;
+   unsigned int ret = 0;
+   unsigned int i;
+
+   for(i=0; i < len; i++)
+   {
+	  int mod = i % 32;
+	  ret ^=(unsigned int) (word[i]) << mod;
+	  ret ^=(unsigned int) (word[i]) >> (32 - mod);
+   }
+   return ret;
+}
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	if (TRUE
+		)
+	{
+		announce_802_3_packet(pAd, pPacket);
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+}