drivers/staging/rt2860/sta/rtmp_data.c

   1 /*
   2  *************************************************************************
   3  * Ralink Tech Inc.
   4  * 5F., No.36, Taiyuan St., Jhubei City,
   5  * Hsinchu County 302,
   6  * Taiwan, R.O.C.
   7  *
   8  * (c) Copyright 2002-2007, Ralink Technology, Inc.
   9  *
  10  * This program is free software; you can redistribute it and/or modify  *
  11  * it under the terms of the GNU General Public License as published by  *
  12  * the Free Software Foundation; either version 2 of the License, or     *
  13  * (at your option) any later version.                                   *
  14  *                                                                       *
  15  * This program is distributed in the hope that it will be useful,       *
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  18  * GNU General Public License for more details.                          *
  19  *                                                                       *
  20  * You should have received a copy of the GNU General Public License     *
  21  * along with this program; if not, write to the                         *
  22  * Free Software Foundation, Inc.,                                       *
  23  * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  24  *                                                                       *
  25  *************************************************************************
  26
  27         Module Name:
  28         rtmp_data.c
  29
  30         Abstract:
  31         Data path subroutines
  32
  33         Revision History:
  34         Who             When                    What
  35         --------        ----------              ----------------------------------------------
  36         John                  Aug/17/04         major modification for RT2561/2661
  37         Jan Lee       Mar/17/06         major modification for RT2860 New Ring Design
  38 */
  39 #include "../rt_config.h"
  40
  41
  42
  43 VOID STARxEAPOLFrameIndicate(
  44         IN      PRTMP_ADAPTER   pAd,
  45         IN      MAC_TABLE_ENTRY *pEntry,
  46         IN      RX_BLK                  *pRxBlk,
  47         IN      UCHAR                   FromWhichBSSID)
  48 {
  49         PRT28XX_RXD_STRUC       pRxD = &(pRxBlk->RxD);
  50         PRXWI_STRUC             pRxWI = pRxBlk->pRxWI;
  51         UCHAR                   *pTmpBuf;
  52
  53         if (pAd->StaCfg.WpaSupplicantUP)
  54         {
  55                 // All EAPoL frames have to pass to upper layer (ex. WPA_SUPPLICANT daemon)
  56                 // TBD : process fragmented EAPol frames
  57                 {
  58                         // In 802.1x mode, if the received frame is EAP-SUCCESS packet, turn on the PortSecured variable
  59                         if ( pAd->StaCfg.IEEE8021X == TRUE &&
  60                                  (EAP_CODE_SUCCESS == WpaCheckEapCode(pAd, pRxBlk->pData, pRxBlk->DataSize, LENGTH_802_1_H)))
  61                         {
  62                                 PUCHAR  Key;
  63                                 UCHAR   CipherAlg;
  64                                 int     idx = 0;
  65
  66                                 DBGPRINT_RAW(RT_DEBUG_TRACE, ("Receive EAP-SUCCESS Packet\n"));
  67                                 STA_PORT_SECURED(pAd);
  68
  69                 if (pAd->StaCfg.IEEE8021x_required_keys == FALSE)
  70                 {
  71                     idx = pAd->StaCfg.DesireSharedKeyId;
  72                     CipherAlg = pAd->StaCfg.DesireSharedKey[idx].CipherAlg;
  73                                         Key = pAd->StaCfg.DesireSharedKey[idx].Key;
  74
  75                     if (pAd->StaCfg.DesireSharedKey[idx].KeyLen > 0)
  76                                 {
  77 #ifdef RT2860
  78                                                 MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[BSSID_WCID];
  79
  80                                                 // Set key material and cipherAlg to Asic
  81                                                 AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
  82
  83                                                 // Assign group key info
  84                                                 RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
  85
  86                                                 // Assign pairwise key info
  87                                                 RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, pEntry);
  88
  89                         pAd->IndicateMediaState = NdisMediaStateConnected;
  90                         pAd->ExtraInfo = GENERAL_LINK_UP;
  91 #endif
  92 #ifdef RT2870
  93                                                 union
  94                                                 {
  95                                                         char buf[sizeof(NDIS_802_11_WEP)+MAX_LEN_OF_KEY- 1];
  96                                                         NDIS_802_11_WEP keyinfo;
  97                                                 }  WepKey;
  98                                                 int len;
  99
 100
 101                                                 NdisZeroMemory(&WepKey, sizeof(WepKey));
 102                                                 len =pAd->StaCfg.DesireSharedKey[idx].KeyLen;
 103
 104                                                 NdisMoveMemory(WepKey.keyinfo.KeyMaterial,
 105                                                         pAd->StaCfg.DesireSharedKey[idx].Key,
 106                                                         pAd->StaCfg.DesireSharedKey[idx].KeyLen);
 107
 108                                                 WepKey.keyinfo.KeyIndex = 0x80000000 + idx;
 109                                                 WepKey.keyinfo.KeyLength = len;
 110                                                 pAd->SharedKey[BSS0][idx].KeyLen =(UCHAR) (len <= 5 ? 5 : 13);
 111
 112                                                 pAd->IndicateMediaState = NdisMediaStateConnected;
 113                                                 pAd->ExtraInfo = GENERAL_LINK_UP;
 114                                                 // need to enqueue cmd to thread
 115                                                 RTUSBEnqueueCmdFromNdis(pAd, OID_802_11_ADD_WEP, TRUE, &WepKey, sizeof(WepKey.keyinfo) + len - 1);
 116 #endif // RT2870 //
 117                                                 // For Preventing ShardKey Table is cleared by remove key procedure.
 118                                         pAd->SharedKey[BSS0][idx].CipherAlg = CipherAlg;
 119                                                 pAd->SharedKey[BSS0][idx].KeyLen = pAd->StaCfg.DesireSharedKey[idx].KeyLen;
 120                                                 NdisMoveMemory(pAd->SharedKey[BSS0][idx].Key,
 121                                                                            pAd->StaCfg.DesireSharedKey[idx].Key,
 122                                                                            pAd->StaCfg.DesireSharedKey[idx].KeyLen);
 123                                 }
 124                                 }
 125                         }
 126
 127                         Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
 128                         return;
 129                 }
 130         }
 131         else
 132         {
 133                 // Special DATA frame that has to pass to MLME
 134                 //       1. Cisco Aironet frames for CCX2. We need pass it to MLME for special process
 135                 //       2. EAPOL handshaking frames when driver supplicant enabled, pass to MLME for special process
 136                 {
 137                         pTmpBuf = pRxBlk->pData - LENGTH_802_11;
 138                         NdisMoveMemory(pTmpBuf, pRxBlk->pHeader, LENGTH_802_11);
 139                         REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pTmpBuf, pRxBlk->DataSize + LENGTH_802_11, pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
 140                         DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! report EAPOL/AIRONET DATA to MLME (len=%d) !!!\n", pRxBlk->DataSize));
 141                 }
 142         }
 143
 144         RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
 145         return;
 146
 147 }
 148
 149 VOID STARxDataFrameAnnounce(
 150         IN      PRTMP_ADAPTER   pAd,
 151         IN      MAC_TABLE_ENTRY *pEntry,
 152         IN      RX_BLK                  *pRxBlk,
 153         IN      UCHAR                   FromWhichBSSID)
 154 {
 155
 156         // non-EAP frame
 157         if (!RTMPCheckWPAframe(pAd, pEntry, pRxBlk->pData, pRxBlk->DataSize, FromWhichBSSID))
 158         {
 159                 {
 160                         // drop all non-EAP DATA frame before
 161                         // this client's Port-Access-Control is secured
 162                         if (pRxBlk->pHeader->FC.Wep)
 163                         {
 164                                 // unsupported cipher suite
 165                                 if (pAd->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
 166                                 {
 167                                         // release packet
 168                                         RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
 169                                         return;
 170                                 }
 171                         }
 172                         else
 173                         {
 174                                 // encryption in-use but receive a non-EAPOL clear text frame, drop it
 175                                 if ((pAd->StaCfg.WepStatus != Ndis802_11EncryptionDisabled) &&
 176                                         (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
 177                                 {
 178                                         // release packet
 179                                         RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
 180                                         return;
 181                                 }
 182                         }
 183                 }
 184                 RX_BLK_CLEAR_FLAG(pRxBlk, fRX_EAP);
 185                 if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_ARALINK))
 186                 {
 187                         // Normal legacy, AMPDU or AMSDU
 188                         CmmRxnonRalinkFrameIndicate(pAd, pRxBlk, FromWhichBSSID);
 189
 190                 }
 191                 else
 192                 {
 193                         // ARALINK
 194                         CmmRxRalinkFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
 195                 }
 196         }
 197         else
 198         {
 199                 RX_BLK_SET_FLAG(pRxBlk, fRX_EAP);
 200
 201                 if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
 202                 {
 203                         Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
 204                 }
 205                 else
 206                 {
 207                         // Determin the destination of the EAP frame
 208                         //  to WPA state machine or upper layer
 209                         STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
 210                 }
 211         }
 212 }
 213
 214
 215 // For TKIP frame, calculate the MIC value
 216 BOOLEAN STACheckTkipMICValue(
 217         IN      PRTMP_ADAPTER   pAd,
 218         IN      MAC_TABLE_ENTRY *pEntry,
 219         IN      RX_BLK                  *pRxBlk)
 220 {
 221         PHEADER_802_11  pHeader = pRxBlk->pHeader;
 222         UCHAR                   *pData = pRxBlk->pData;
 223         USHORT                  DataSize = pRxBlk->DataSize;
 224         UCHAR                   UserPriority = pRxBlk->UserPriority;
 225         PCIPHER_KEY             pWpaKey;
 226         UCHAR                   *pDA, *pSA;
 227
 228         pWpaKey = &pAd->SharedKey[BSS0][pRxBlk->pRxWI->KeyIndex];
 229
 230         pDA = pHeader->Addr1;
 231         if (RX_BLK_TEST_FLAG(pRxBlk, fRX_INFRA))
 232         {
 233                 pSA = pHeader->Addr3;
 234         }
 235         else
 236         {
 237                 pSA = pHeader->Addr2;
 238         }
 239
 240         if (RTMPTkipCompareMICValue(pAd,
 241                                                                 pData,
 242                                                                 pDA,
 243                                                                 pSA,
 244                                                                 pWpaKey->RxMic,
 245                                                                 UserPriority,
 246                                                                 DataSize) == FALSE)
 247         {
 248                 DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error 2\n"));
 249
 250                 if (pAd->StaCfg.WpaSupplicantUP)
 251                 {
 252                         WpaSendMicFailureToWpaSupplicant(pAd, (pWpaKey->Type == PAIRWISEKEY) ? TRUE : FALSE);
 253                 }
 254                 else
 255                 {
 256                         RTMPReportMicError(pAd, pWpaKey);
 257                 }
 258
 259                 // release packet
 260                 RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
 261                 return FALSE;
 262         }
 263
 264         return TRUE;
 265 }
 266
 267
 268 //
 269 // All Rx routines use RX_BLK structure to hande rx events
 270 // It is very important to build pRxBlk attributes
 271 //  1. pHeader pointer to 802.11 Header
 272 //  2. pData pointer to payload including LLC (just skip Header)
 273 //  3. set payload size including LLC to DataSize
 274 //  4. set some flags with RX_BLK_SET_FLAG()
 275 //
 276 VOID STAHandleRxDataFrame(
 277         IN      PRTMP_ADAPTER   pAd,
 278         IN      RX_BLK                  *pRxBlk)
 279 {
 280         PRT28XX_RXD_STRUC                               pRxD = &(pRxBlk->RxD);
 281         PRXWI_STRUC                                             pRxWI = pRxBlk->pRxWI;
 282         PHEADER_802_11                                  pHeader = pRxBlk->pHeader;
 283         PNDIS_PACKET                                    pRxPacket = pRxBlk->pRxPacket;
 284         BOOLEAN                                                 bFragment = FALSE;
 285         MAC_TABLE_ENTRY                         *pEntry = NULL;
 286         UCHAR                                                   FromWhichBSSID = BSS0;
 287         UCHAR                           UserPriority = 0;
 288
 289         {
 290                 // before LINK UP, all DATA frames are rejected
 291                 if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
 292                 {
 293                         // release packet
 294                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 295                         return;
 296                 }
 297
 298                 // Drop not my BSS frames
 299                 if (pRxD->MyBss == 0)
 300                 {
 301                         {
 302                                 // release packet
 303                                 RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 304                                 return;
 305                         }
 306                 }
 307
 308                 pAd->RalinkCounters.RxCountSinceLastNULL++;
 309                 if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable && (pHeader->FC.SubType & 0x08))
 310                 {
 311                         UCHAR *pData;
 312                         DBGPRINT(RT_DEBUG_TRACE,("bAPSDCapable\n"));
 313
 314                         // Qos bit 4
 315                         pData = (PUCHAR)pHeader + LENGTH_802_11;
 316                         if ((*pData >> 4) & 0x01)
 317                         {
 318                                 DBGPRINT(RT_DEBUG_TRACE,("RxDone- Rcv EOSP frame, driver may fall into sleep\n"));
 319                                 pAd->CommonCfg.bInServicePeriod = FALSE;
 320
 321                                 // Force driver to fall into sleep mode when rcv EOSP frame
 322                                 if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
 323                                 {
 324                                         USHORT  TbttNumToNextWakeUp;
 325                                         USHORT  NextDtim = pAd->StaCfg.DtimPeriod;
 326                                         ULONG   Now;
 327
 328                                         NdisGetSystemUpTime(&Now);
 329                                         NextDtim -= (USHORT)(Now - pAd->StaCfg.LastBeaconRxTime)/pAd->CommonCfg.BeaconPeriod;
 330
 331                                         TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
 332                                         if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
 333                                                 TbttNumToNextWakeUp = NextDtim;
 334
 335                                         MlmeSetPsmBit(pAd, PWR_SAVE);
 336                                         // if WMM-APSD is failed, try to disable following line
 337                                         AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
 338                                 }
 339                         }
 340
 341                         if ((pHeader->FC.MoreData) && (pAd->CommonCfg.bInServicePeriod))
 342                         {
 343                                 DBGPRINT(RT_DEBUG_TRACE,("Sending another trigger frame when More Data bit is set to 1\n"));
 344                         }
 345                 }
 346
 347                 // Drop NULL, CF-ACK(no data), CF-POLL(no data), and CF-ACK+CF-POLL(no data) data frame
 348                 if ((pHeader->FC.SubType & 0x04)) // bit 2 : no DATA
 349                 {
 350                         // release packet
 351                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 352                         return;
 353                 }
 354
 355             // Drop not my BSS frame (we can not only check the MyBss bit in RxD)
 356
 357                 if (INFRA_ON(pAd))
 358                 {
 359                         // Infrastructure mode, check address 2 for BSSID
 360                         if (!RTMPEqualMemory(&pHeader->Addr2, &pAd->CommonCfg.Bssid, 6))
 361                         {
 362                                 // Receive frame not my BSSID
 363                     // release packet
 364                     RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 365                                 return;
 366                         }
 367                 }
 368                 else    // Ad-Hoc mode or Not associated
 369                 {
 370                         // Ad-Hoc mode, check address 3 for BSSID
 371                         if (!RTMPEqualMemory(&pHeader->Addr3, &pAd->CommonCfg.Bssid, 6))
 372                         {
 373                                 // Receive frame not my BSSID
 374                     // release packet
 375                     RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 376                                 return;
 377                         }
 378                 }
 379
 380                 //
 381                 // find pEntry
 382                 //
 383                 if (pRxWI->WirelessCliID < MAX_LEN_OF_MAC_TABLE)
 384                 {
 385                         pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
 386                 }
 387                 else
 388                 {
 389                         // 1. release packet if infra mode
 390                         // 2. new a pEntry if ad-hoc mode
 391                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 392                         return;
 393                 }
 394
 395                 // infra or ad-hoc
 396                 if (INFRA_ON(pAd))
 397                 {
 398                         RX_BLK_SET_FLAG(pRxBlk, fRX_INFRA);
 399                         ASSERT(pRxWI->WirelessCliID == BSSID_WCID);
 400                 }
 401
 402                 // check Atheros Client
 403                 if ((pEntry->bIAmBadAtheros == FALSE) &&  (pRxD->AMPDU == 1) && (pHeader->FC.Retry ))
 404                 {
 405                         pEntry->bIAmBadAtheros = TRUE;
 406                         pAd->CommonCfg.IOTestParm.bCurrentAtheros = TRUE;
 407                         pAd->CommonCfg.IOTestParm.bLastAtheros = TRUE;
 408                         if (!STA_AES_ON(pAd))
 409                         {
 410                                 AsicUpdateProtect(pAd, 8, ALLN_SETPROTECT, TRUE, FALSE);
 411                         }
 412                 }
 413         }
 414
 415         pRxBlk->pData = (UCHAR *)pHeader;
 416
 417         //
 418         // update RxBlk->pData, DataSize
 419         // 802.11 Header, QOS, HTC, Hw Padding
 420         //
 421
 422         // 1. skip 802.11 HEADER
 423         {
 424                 pRxBlk->pData += LENGTH_802_11;
 425                 pRxBlk->DataSize -= LENGTH_802_11;
 426         }
 427
 428         // 2. QOS
 429         if (pHeader->FC.SubType & 0x08)
 430         {
 431                 RX_BLK_SET_FLAG(pRxBlk, fRX_QOS);
 432                 UserPriority = *(pRxBlk->pData) & 0x0f;
 433                 // bit 7 in QoS Control field signals the HT A-MSDU format
 434                 if ((*pRxBlk->pData) & 0x80)
 435                 {
 436                         RX_BLK_SET_FLAG(pRxBlk, fRX_AMSDU);
 437                 }
 438
 439                 // skip QOS contorl field
 440                 pRxBlk->pData += 2;
 441                 pRxBlk->DataSize -=2;
 442         }
 443         pRxBlk->UserPriority = UserPriority;
 444
 445         // 3. Order bit: A-Ralink or HTC+
 446         if (pHeader->FC.Order)
 447         {
 448 #ifdef AGGREGATION_SUPPORT
 449                 if ((pRxWI->PHYMODE <= MODE_OFDM) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)))
 450                 {
 451                         RX_BLK_SET_FLAG(pRxBlk, fRX_ARALINK);
 452                 }
 453                 else
 454 #endif
 455                 {
 456                         RX_BLK_SET_FLAG(pRxBlk, fRX_HTC);
 457                         // skip HTC contorl field
 458                         pRxBlk->pData += 4;
 459                         pRxBlk->DataSize -= 4;
 460                 }
 461         }
 462
 463         // 4. skip HW padding
 464         if (pRxD->L2PAD)
 465         {
 466                 // just move pData pointer
 467                 // because DataSize excluding HW padding
 468                 RX_BLK_SET_FLAG(pRxBlk, fRX_PAD);
 469                 pRxBlk->pData += 2;
 470         }
 471
 472         if (pRxD->BA)
 473         {
 474                 RX_BLK_SET_FLAG(pRxBlk, fRX_AMPDU);
 475         }
 476
 477         //
 478         // Case I  Process Broadcast & Multicast data frame
 479         //
 480         if (pRxD->Bcast || pRxD->Mcast)
 481         {
 482                 INC_COUNTER64(pAd->WlanCounters.MulticastReceivedFrameCount);
 483
 484                 // Drop Mcast/Bcast frame with fragment bit on
 485                 if (pHeader->FC.MoreFrag)
 486                 {
 487                         // release packet
 488                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 489                         return;
 490                 }
 491
 492                 // Filter out Bcast frame which AP relayed for us
 493                 if (pHeader->FC.FrDs && MAC_ADDR_EQUAL(pHeader->Addr3, pAd->CurrentAddress))
 494                 {
 495                         // release packet
 496                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 497                         return;
 498                 }
 499
 500                 Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
 501                 return;
 502         }
 503         else if (pRxD->U2M)
 504         {
 505                 pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
 506
 507                 if (ADHOC_ON(pAd))
 508                 {
 509                         pEntry = MacTableLookup(pAd, pHeader->Addr2);
 510                         if (pEntry)
 511                                 Update_Rssi_Sample(pAd, &pEntry->RssiSample, pRxWI);
 512                 }
 513
 514
 515                 Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
 516
 517                 pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
 518                 pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
 519
 520                 pAd->RalinkCounters.OneSecRxOkDataCnt++;
 521
 522
 523         if (!((pHeader->Frag == 0) && (pHeader->FC.MoreFrag == 0)))
 524         {
 525                 // re-assemble the fragmented packets
 526                 // return complete frame (pRxPacket) or NULL
 527                 bFragment = TRUE;
 528                 pRxPacket = RTMPDeFragmentDataFrame(pAd, pRxBlk);
 529         }
 530
 531         if (pRxPacket)
 532         {
 533                         pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
 534
 535                 // process complete frame
 536                 if (bFragment && (pRxD->Decrypted) && (pEntry->WepStatus == Ndis802_11Encryption2Enabled))
 537                 {
 538                                 // Minus MIC length
 539                                 pRxBlk->DataSize -= 8;
 540
 541                         // For TKIP frame, calculate the MIC value
 542                         if (STACheckTkipMICValue(pAd, pEntry, pRxBlk) == FALSE)
 543                         {
 544                                 return;
 545                         }
 546                 }
 547
 548                 STARxDataFrameAnnounce(pAd, pEntry, pRxBlk, FromWhichBSSID);
 549                         return;
 550         }
 551         else
 552         {
 553                 // just return
 554                 // because RTMPDeFragmentDataFrame() will release rx packet,
 555                 // if packet is fragmented
 556                 return;
 557         }
 558         }
 559
 560         ASSERT(0);
 561         // release packet
 562         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 563 }
 564
 565 VOID STAHandleRxMgmtFrame(
 566         IN      PRTMP_ADAPTER   pAd,
 567         IN      RX_BLK                  *pRxBlk)
 568 {
 569         PRT28XX_RXD_STRUC       pRxD = &(pRxBlk->RxD);
 570         PRXWI_STRUC             pRxWI = pRxBlk->pRxWI;
 571         PHEADER_802_11  pHeader = pRxBlk->pHeader;
 572         PNDIS_PACKET    pRxPacket = pRxBlk->pRxPacket;
 573
 574         do
 575         {
 576
 577                 // We should collect RSSI not only U2M data but also my beacon
 578                 if (
 579 #ifdef RT30xx
 580                     pAd->RxAnt.EvaluatePeriod == 0 &&
 581 #endif
 582                     pHeader->FC.SubType == SUBTYPE_BEACON &&
 583                     MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2)) {
 584                         Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
 585
 586                         pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
 587                         pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
 588                 }
 589
 590 #ifdef RT30xx
 591                 // collect rssi information for antenna diversity
 592                 if (pAd->NicConfig2.field.AntDiversity)
 593                 {
 594                         if ((pRxD->U2M) || ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2))))
 595                         {
 596                                         COLLECT_RX_ANTENNA_AVERAGE_RSSI(pAd, ConvertToRssi(pAd, (UCHAR)pRxWI->RSSI0, RSSI_0), 0); //Note: RSSI2 not used on RT73
 597                                         pAd->StaCfg.NumOfAvgRssiSample ++;
 598                         }
 599                 }
 600 #endif // RT30xx //
 601
 602                 // First check the size, it MUST not exceed the mlme queue size
 603                 if (pRxWI->MPDUtotalByteCount > MGMT_DMA_BUFFER_SIZE)
 604                 {
 605                         DBGPRINT_ERR(("STAHandleRxMgmtFrame: frame too large, size = %d \n", pRxWI->MPDUtotalByteCount));
 606                         break;
 607                 }
 608
 609                 REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pHeader, pRxWI->MPDUtotalByteCount,
 610                                                                         pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
 611         } while (FALSE);
 612
 613         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
 614 }
 615
 616 VOID STAHandleRxControlFrame(
 617         IN      PRTMP_ADAPTER   pAd,
 618         IN      RX_BLK                  *pRxBlk)
 619 {
 620         PRXWI_STRUC             pRxWI = pRxBlk->pRxWI;
 621         PHEADER_802_11  pHeader = pRxBlk->pHeader;
 622         PNDIS_PACKET    pRxPacket = pRxBlk->pRxPacket;
 623
 624         switch (pHeader->FC.SubType)
 625         {
 626                 case SUBTYPE_BLOCK_ACK_REQ:
 627                         {
 628                                 CntlEnqueueForRecv(pAd, pRxWI->WirelessCliID, (pRxWI->MPDUtotalByteCount), (PFRAME_BA_REQ)pHeader);
 629                         }
 630                         break;
 631                 case SUBTYPE_BLOCK_ACK:
 632                 case SUBTYPE_ACK:
 633                 default:
 634                         break;
 635         }
 636
 637         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 638 }
 639
 640
 641 /*
 642         ========================================================================
 643
 644         Routine Description:
 645                 Process RxDone interrupt, running in DPC level
 646
 647         Arguments:
 648                 pAd Pointer to our adapter
 649
 650         Return Value:
 651                 None
 652
 653         IRQL = DISPATCH_LEVEL
 654
 655         Note:
 656                 This routine has to maintain Rx ring read pointer.
 657                 Need to consider QOS DATA format when converting to 802.3
 658         ========================================================================
 659 */
 660 BOOLEAN STARxDoneInterruptHandle(
 661         IN      PRTMP_ADAPTER   pAd,
 662         IN      BOOLEAN                 argc)
 663 {
 664         NDIS_STATUS                     Status;
 665         UINT32                  RxProcessed, RxPending;
 666         BOOLEAN                 bReschedule = FALSE;
 667         RT28XX_RXD_STRUC        *pRxD;
 668         UCHAR                   *pData;
 669         PRXWI_STRUC             pRxWI;
 670         PNDIS_PACKET    pRxPacket;
 671         PHEADER_802_11  pHeader;
 672         RX_BLK                  RxCell;
 673
 674         RxProcessed = RxPending = 0;
 675
 676         // process whole rx ring
 677         while (1)
 678         {
 679
 680                 if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF |
 681                                                                 fRTMP_ADAPTER_RESET_IN_PROGRESS |
 682                                                                 fRTMP_ADAPTER_HALT_IN_PROGRESS |
 683                                                                 fRTMP_ADAPTER_NIC_NOT_EXIST) ||
 684                         !RTMP_TEST_FLAG(pAd,fRTMP_ADAPTER_START_UP))
 685                 {
 686                         break;
 687                 }
 688
 689 #ifdef RT2860
 690                 if (RxProcessed++ > MAX_RX_PROCESS_CNT)
 691                 {
 692                         // need to reschedule rx handle
 693                         bReschedule = TRUE;
 694                         break;
 695                 }
 696 #endif
 697
 698                 RxProcessed ++; // test
 699
 700                 // 1. allocate a new data packet into rx ring to replace received packet
 701                 //    then processing the received packet
 702                 // 2. the callee must take charge of release of packet
 703                 // 3. As far as driver is concerned ,
 704                 //    the rx packet must
 705                 //      a. be indicated to upper layer or
 706                 //      b. be released if it is discarded
 707                 pRxPacket = GetPacketFromRxRing(pAd, &(RxCell.RxD), &bReschedule, &RxPending);
 708                 if (pRxPacket == NULL)
 709                 {
 710                         // no more packet to process
 711                         break;
 712                 }
 713
 714                 // get rx ring descriptor
 715                 pRxD = &(RxCell.RxD);
 716                 // get rx data buffer
 717                 pData   = GET_OS_PKT_DATAPTR(pRxPacket);
 718                 pRxWI   = (PRXWI_STRUC) pData;
 719                 pHeader = (PHEADER_802_11) (pData+RXWI_SIZE) ;
 720
 721                 // build RxCell
 722                 RxCell.pRxWI = pRxWI;
 723                 RxCell.pHeader = pHeader;
 724                 RxCell.pRxPacket = pRxPacket;
 725                 RxCell.pData = (UCHAR *) pHeader;
 726                 RxCell.DataSize = pRxWI->MPDUtotalByteCount;
 727                 RxCell.Flags = 0;
 728
 729                 // Increase Total receive byte counter after real data received no mater any error or not
 730                 pAd->RalinkCounters.ReceivedByteCount +=  pRxWI->MPDUtotalByteCount;
 731                 pAd->RalinkCounters.RxCount ++;
 732
 733                 INC_COUNTER64(pAd->WlanCounters.ReceivedFragmentCount);
 734
 735                 if (pRxWI->MPDUtotalByteCount < 14)
 736                         Status = NDIS_STATUS_FAILURE;
 737
 738         if (MONITOR_ON(pAd))
 739                 {
 740             send_monitor_packets(pAd, &RxCell);
 741                         break;
 742                 }
 743                 /* RT2870 invokes STARxDoneInterruptHandle() in rtusb_bulk.c */
 744
 745                 // Check for all RxD errors
 746                 Status = RTMPCheckRxError(pAd, pHeader, pRxWI, pRxD);
 747
 748                 // Handle the received frame
 749                 if (Status == NDIS_STATUS_SUCCESS)
 750                 {
 751                         switch (pHeader->FC.Type)
 752                         {
 753                                 // CASE I, receive a DATA frame
 754                                 case BTYPE_DATA:
 755                                 {
 756                                         // process DATA frame
 757                                         STAHandleRxDataFrame(pAd, &RxCell);
 758                                 }
 759                                 break;
 760                                 // CASE II, receive a MGMT frame
 761                                 case BTYPE_MGMT:
 762                                 {
 763                                         STAHandleRxMgmtFrame(pAd, &RxCell);
 764                                 }
 765                                 break;
 766                                 // CASE III. receive a CNTL frame
 767                                 case BTYPE_CNTL:
 768                                 {
 769                                         STAHandleRxControlFrame(pAd, &RxCell);
 770                                 }
 771                                 break;
 772                                 // discard other type
 773                                 default:
 774                                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 775                                         break;
 776                         }
 777                 }
 778                 else
 779                 {
 780                         pAd->Counters8023.RxErrors++;
 781                         // discard this frame
 782                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 783                 }
 784         }
 785
 786 #ifdef RT2860
 787         // fRTMP_PS_GO_TO_SLEEP_NOW is set if receiving beacon.
 788         if (RTMP_TEST_PSFLAG(pAd, fRTMP_PS_GO_TO_SLEEP_NOW) && (INFRA_ON(pAd)))
 789         {
 790                 RTMP_CLEAR_PSFLAG(pAd, fRTMP_PS_GO_TO_SLEEP_NOW);
 791                 AsicSleepThenAutoWakeup(pAd, pAd->ThisTbttNumToNextWakeUp);
 792                 bReschedule = FALSE;
 793         }
 794 #endif
 795         return bReschedule;
 796 }
 797
 798 /*
 799         ========================================================================
 800
 801         Routine Description:
 802         Arguments:
 803                 pAd     Pointer to our adapter
 804
 805         IRQL = DISPATCH_LEVEL
 806
 807         ========================================================================
 808 */
 809 VOID    RTMPHandleTwakeupInterrupt(
 810         IN PRTMP_ADAPTER pAd)
 811 {
 812 #ifdef RT2860
 813         AsicForceWakeup(pAd, DOT11POWERSAVE);
 814 #endif
 815 #ifdef RT2870
 816         AsicForceWakeup(pAd, FALSE);
 817 #endif
 818 }
 819
 820 /*
 821 ========================================================================
 822 Routine Description:
 823     Early checking and OS-depened parsing for Tx packet send to our STA driver.
 824
 825 Arguments:
 826     NDIS_HANDLE         MiniportAdapterContext  Pointer refer to the device handle, i.e., the pAd.
 827         PPNDIS_PACKET   ppPacketArray                   The packet array need to do transmission.
 828         UINT                    NumberOfPackets                 Number of packet in packet array.
 829
 830 Return Value:
 831         NONE
 832
 833 Note:
 834         This function do early checking and classification for send-out packet.
 835         You only can put OS-depened & STA related code in here.
 836 ========================================================================
 837 */
 838 VOID STASendPackets(
 839         IN      NDIS_HANDLE             MiniportAdapterContext,
 840         IN      PPNDIS_PACKET   ppPacketArray,
 841         IN      UINT                    NumberOfPackets)
 842 {
 843         UINT                    Index;
 844         PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) MiniportAdapterContext;
 845         PNDIS_PACKET    pPacket;
 846         BOOLEAN                 allowToSend = FALSE;
 847
 848
 849         for (Index = 0; Index < NumberOfPackets; Index++)
 850         {
 851                 pPacket = ppPacketArray[Index];
 852
 853                 do
 854                 {
 855                         if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
 856                                 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
 857                                 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
 858                         {
 859                                 // Drop send request since hardware is in reset state
 860                                         break;
 861                         }
 862                         else if (!INFRA_ON(pAd) && !ADHOC_ON(pAd))
 863                         {
 864                                 // Drop send request since there are no physical connection yet
 865                                         break;
 866                         }
 867                         else
 868                         {
 869                                 // Record that orignal packet source is from NDIS layer,so that
 870                                 // later on driver knows how to release this NDIS PACKET
 871                                 RTMP_SET_PACKET_WCID(pPacket, 0); // this field is useless when in STA mode
 872                                 RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
 873                                 NDIS_SET_PACKET_STATUS(pPacket, NDIS_STATUS_PENDING);
 874                                 pAd->RalinkCounters.PendingNdisPacketCount++;
 875
 876                                 allowToSend = TRUE;
 877                         }
 878                 } while(FALSE);
 879
 880                 if (allowToSend == TRUE)
 881                         STASendPacket(pAd, pPacket);
 882                 else
 883                         RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
 884         }
 885
 886         // Dequeue outgoing frames from TxSwQueue[] and process it
 887         RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
 888
 889 }
 890
 891
 892 /*
 893 ========================================================================
 894 Routine Description:
 895         This routine is used to do packet parsing and classification for Tx packet
 896         to STA device, and it will en-queue packets to our TxSwQueue depends on AC
 897         class.
 898
 899 Arguments:
 900         pAd             Pointer to our adapter
 901         pPacket         Pointer to send packet
 902
 903 Return Value:
 904         NDIS_STATUS_SUCCESS                     If succes to queue the packet into TxSwQueue.
 905         NDIS_STATUS_FAILURE                     If failed to do en-queue.
 906
 907 Note:
 908         You only can put OS-indepened & STA related code in here.
 909 ========================================================================
 910 */
 911 NDIS_STATUS STASendPacket(
 912         IN      PRTMP_ADAPTER   pAd,
 913         IN      PNDIS_PACKET    pPacket)
 914 {
 915         PACKET_INFO     PacketInfo;
 916         PUCHAR                  pSrcBufVA;
 917         UINT                    SrcBufLen;
 918         UINT                    AllowFragSize;
 919         UCHAR                   NumberOfFrag;
 920         UCHAR                   QueIdx, UserPriority;
 921         MAC_TABLE_ENTRY *pEntry = NULL;
 922         unsigned int    IrqFlags;
 923         UCHAR                   FlgIsIP = 0;
 924         UCHAR                   Rate;
 925
 926         // Prepare packet information structure for buffer descriptor
 927         // chained within a single NDIS packet.
 928         RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
 929
 930         if (pSrcBufVA == NULL)
 931         {
 932                 DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> pSrcBufVA == NULL !!!SrcBufLen=%x\n",SrcBufLen));
 933                 // Resourece is low, system did not allocate virtual address
 934                 // return NDIS_STATUS_FAILURE directly to upper layer
 935                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
 936                 return NDIS_STATUS_FAILURE;
 937         }
 938
 939
 940         if (SrcBufLen < 14)
 941         {
 942                 DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> Ndis Packet buffer error !!!\n"));
 943                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
 944                 return (NDIS_STATUS_FAILURE);
 945         }
 946
 947         // In HT rate adhoc mode, A-MPDU is often used. So need to lookup BA Table and MAC Entry.
 948         // Note multicast packets in adhoc also use BSSID_WCID index.
 949         {
 950                 if(INFRA_ON(pAd))
 951                 {
 952                         {
 953                         pEntry = &pAd->MacTab.Content[BSSID_WCID];
 954                         RTMP_SET_PACKET_WCID(pPacket, BSSID_WCID);
 955                         Rate = pAd->CommonCfg.TxRate;
 956                 }
 957                 }
 958                 else if (ADHOC_ON(pAd))
 959                 {
 960                         if (*pSrcBufVA & 0x01)
 961                         {
 962                                 RTMP_SET_PACKET_WCID(pPacket, MCAST_WCID);
 963                                 pEntry = &pAd->MacTab.Content[MCAST_WCID];
 964                         }
 965                         else
 966                         {
 967                                 pEntry = MacTableLookup(pAd, pSrcBufVA);
 968                         }
 969                         Rate = pAd->CommonCfg.TxRate;
 970                 }
 971         }
 972
 973         if (!pEntry)
 974         {
 975                 DBGPRINT(RT_DEBUG_ERROR,("STASendPacket->Cannot find pEntry(%2x:%2x:%2x:%2x:%2x:%2x) in MacTab!\n", PRINT_MAC(pSrcBufVA)));
 976                 // Resourece is low, system did not allocate virtual address
 977                 // return NDIS_STATUS_FAILURE directly to upper layer
 978                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
 979                 return NDIS_STATUS_FAILURE;
 980         }
 981
 982         if (ADHOC_ON(pAd)
 983                 )
 984         {
 985                 RTMP_SET_PACKET_WCID(pPacket, (UCHAR)pEntry->Aid);
 986         }
 987
 988         //
 989         // Check the Ethernet Frame type of this packet, and set the RTMP_SET_PACKET_SPECIFIC flags.
 990         //              Here we set the PACKET_SPECIFIC flags(LLC, VLAN, DHCP/ARP, EAPOL).
 991         RTMPCheckEtherType(pAd, pPacket);
 992
 993
 994
 995         //
 996         // WPA 802.1x secured port control - drop all non-802.1x frame before port secured
 997         //
 998         if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
 999                  (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
1000                  (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
1001                  (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
1002                   || (pAd->StaCfg.IEEE8021X == TRUE)
1003                   )
1004                   && ((pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED) || (pAd->StaCfg.MicErrCnt >= 2))
1005                   && (RTMP_GET_PACKET_EAPOL(pPacket)== FALSE)
1006                   )
1007         {
1008                 DBGPRINT(RT_DEBUG_TRACE,("STASendPacket --> Drop packet before port secured !!!\n"));
1009                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
1010
1011                 return (NDIS_STATUS_FAILURE);
1012         }
1013
1014
1015         // STEP 1. Decide number of fragments required to deliver this MSDU.
1016         //         The estimation here is not very accurate because difficult to
1017         //         take encryption overhead into consideration here. The result
1018         //         "NumberOfFrag" is then just used to pre-check if enough free
1019         //         TXD are available to hold this MSDU.
1020
1021
1022         if (*pSrcBufVA & 0x01)  // fragmentation not allowed on multicast & broadcast
1023                 NumberOfFrag = 1;
1024         else if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED))
1025                 NumberOfFrag = 1;       // Aggregation overwhelms fragmentation
1026         else if (CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED))
1027                 NumberOfFrag = 1;       // Aggregation overwhelms fragmentation
1028         else if ((pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTMIX) || (pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTGREENFIELD))
1029                 NumberOfFrag = 1;       // MIMO RATE overwhelms fragmentation
1030         else
1031         {
1032                 // The calculated "NumberOfFrag" is a rough estimation because of various
1033                 // encryption/encapsulation overhead not taken into consideration. This number is just
1034                 // used to make sure enough free TXD are available before fragmentation takes place.
1035                 // In case the actual required number of fragments of an NDIS packet
1036                 // excceeds "NumberOfFrag"caculated here and not enough free TXD available, the
1037                 // last fragment (i.e. last MPDU) will be dropped in RTMPHardTransmit() due to out of
1038                 // resource, and the NDIS packet will be indicated NDIS_STATUS_FAILURE. This should
1039                 // rarely happen and the penalty is just like a TX RETRY fail. Affordable.
1040
1041                 AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
1042                 NumberOfFrag = ((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) / AllowFragSize) + 1;
1043                 // To get accurate number of fragmentation, Minus 1 if the size just match to allowable fragment size
1044                 if (((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) % AllowFragSize) == 0)
1045                 {
1046                         NumberOfFrag--;
1047                 }
1048         }
1049
1050         // Save fragment number to Ndis packet reserved field
1051         RTMP_SET_PACKET_FRAGMENTS(pPacket, NumberOfFrag);
1052
1053
1054         // STEP 2. Check the requirement of RTS:
1055         //         If multiple fragment required, RTS is required only for the first fragment
1056         //         if the fragment size large than RTS threshold
1057         //     For RT28xx, Let ASIC send RTS/CTS
1058         RTMP_SET_PACKET_RTS(pPacket, 0);
1059         RTMP_SET_PACKET_TXRATE(pPacket, pAd->CommonCfg.TxRate);
1060
1061         //
1062         // STEP 3. Traffic classification. outcome = <UserPriority, QueIdx>
1063         //
1064         UserPriority = 0;
1065         QueIdx           = QID_AC_BE;
1066 #ifdef RT2860
1067         if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED))
1068 #endif
1069 #ifdef RT2870
1070         if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) &&
1071                 CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE))
1072 #endif
1073         {
1074                 USHORT Protocol;
1075                 UCHAR  LlcSnapLen = 0, Byte0, Byte1;
1076                 do
1077                 {
1078                         // get Ethernet protocol field
1079                         Protocol = (USHORT)((pSrcBufVA[12] << 8) + pSrcBufVA[13]);
1080                         if (Protocol <= 1500)
1081                         {
1082                                 // get Ethernet protocol field from LLC/SNAP
1083                                 if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + 6, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
1084                                         break;
1085
1086                                 Protocol = (USHORT)((Byte0 << 8) + Byte1);
1087                                 LlcSnapLen = 8;
1088                         }
1089
1090                         // always AC_BE for non-IP packet
1091                         if (Protocol != 0x0800)
1092                                 break;
1093
1094                         // get IP header
1095                         if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + LlcSnapLen, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
1096                                 break;
1097
1098                         // return AC_BE if packet is not IPv4
1099                         if ((Byte0 & 0xf0) != 0x40)
1100                                 break;
1101
1102                         FlgIsIP = 1;
1103                         UserPriority = (Byte1 & 0xe0) >> 5;
1104                         QueIdx = MapUserPriorityToAccessCategory[UserPriority];
1105
1106                         // TODO: have to check ACM bit. apply TSPEC if ACM is ON
1107                         // TODO: downgrade UP & QueIdx before passing ACM
1108                         if (pAd->CommonCfg.APEdcaParm.bACM[QueIdx])
1109                         {
1110                                 UserPriority = 0;
1111                                 QueIdx           = QID_AC_BE;
1112                         }
1113                 } while (FALSE);
1114         }
1115
1116         RTMP_SET_PACKET_UP(pPacket, UserPriority);
1117
1118
1119
1120         // Make sure SendTxWait queue resource won't be used by other threads
1121         RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
1122         if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
1123         {
1124                 RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
1125                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
1126
1127                 return NDIS_STATUS_FAILURE;
1128         }
1129         else
1130         {
1131                 InsertTailQueue(&pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pPacket));
1132         }
1133         RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
1134
1135     if ((pAd->CommonCfg.BACapability.field.AutoBA == TRUE)&&
1136 #ifdef RT2860
1137         (pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE))
1138 #endif
1139 #ifdef RT2870
1140         IS_HT_STA(pEntry))
1141 #endif
1142         {
1143                 if (((pEntry->TXBAbitmap & (1<<UserPriority)) == 0) &&
1144             ((pEntry->BADeclineBitmap & (1<<UserPriority)) == 0) &&
1145             (pEntry->PortSecured == WPA_802_1X_PORT_SECURED)
1146                          // For IOT compatibility, if
1147                          // 1. It is Ralink chip or
1148                          // 2. It is OPEN or AES mode,
1149                          // then BA session can be bulit.
1150                          && ((pEntry->ValidAsCLI && pAd->MlmeAux.APRalinkIe != 0x0) ||
1151                                  (pEntry->WepStatus == Ndis802_11WEPDisabled || pEntry->WepStatus == Ndis802_11Encryption3Enabled))
1152                         )
1153                 {
1154                         BAOriSessionSetUp(pAd, pEntry, 0, 0, 10, FALSE);
1155                 }
1156         }
1157
1158         pAd->RalinkCounters.OneSecOsTxCount[QueIdx]++; // TODO: for debug only. to be removed
1159         return NDIS_STATUS_SUCCESS;
1160 }
1161
1162
1163 /*
1164         ========================================================================
1165
1166         Routine Description:
1167                 This subroutine will scan through releative ring descriptor to find
1168                 out avaliable free ring descriptor and compare with request size.
1169
1170         Arguments:
1171                 pAd Pointer to our adapter
1172                 QueIdx          Selected TX Ring
1173
1174         Return Value:
1175                 NDIS_STATUS_FAILURE     Not enough free descriptor
1176                 NDIS_STATUS_SUCCESS     Enough free descriptor
1177
1178         IRQL = PASSIVE_LEVEL
1179         IRQL = DISPATCH_LEVEL
1180
1181         Note:
1182
1183         ========================================================================
1184 */
1185
1186 #ifdef RT2870
1187 /*
1188         Actually, this function used to check if the TxHardware Queue still has frame need to send.
1189         If no frame need to send, go to sleep, else, still wake up.
1190 */
1191 #endif
1192 NDIS_STATUS RTMPFreeTXDRequest(
1193         IN              PRTMP_ADAPTER   pAd,
1194         IN              UCHAR                   QueIdx,
1195         IN              UCHAR                   NumberRequired,
1196         IN              PUCHAR                  FreeNumberIs)
1197 {
1198 #ifdef RT2860
1199         ULONG           FreeNumber = 0;
1200 #endif
1201         NDIS_STATUS     Status = NDIS_STATUS_FAILURE;
1202 #ifdef RT2870
1203         unsigned long   IrqFlags;
1204         HT_TX_CONTEXT   *pHTTXContext;
1205 #endif
1206
1207         switch (QueIdx)
1208         {
1209                 case QID_AC_BK:
1210                 case QID_AC_BE:
1211                 case QID_AC_VI:
1212                 case QID_AC_VO:
1213                 case QID_HCCA:
1214 #ifdef RT2860
1215                         if (pAd->TxRing[QueIdx].TxSwFreeIdx > pAd->TxRing[QueIdx].TxCpuIdx)
1216                                 FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx - pAd->TxRing[QueIdx].TxCpuIdx - 1;
1217                         else
1218                                 FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx + TX_RING_SIZE - pAd->TxRing[QueIdx].TxCpuIdx - 1;
1219
1220                         if (FreeNumber >= NumberRequired)
1221                                 Status = NDIS_STATUS_SUCCESS;
1222 #endif
1223 #ifdef RT2870
1224                         {
1225                                 pHTTXContext = &pAd->TxContext[QueIdx];
1226                                 RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
1227                                 if ((pHTTXContext->CurWritePosition != pHTTXContext->ENextBulkOutPosition) ||
1228                                         (pHTTXContext->IRPPending == TRUE))
1229                                 {
1230                                         Status = NDIS_STATUS_FAILURE;
1231                                 }
1232                                 else
1233                                 {
1234                                         Status = NDIS_STATUS_SUCCESS;
1235                                 }
1236                                 RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
1237                         }
1238 #endif
1239                         break;
1240
1241                 case QID_MGMT:
1242 #ifdef RT2860
1243                         if (pAd->MgmtRing.TxSwFreeIdx > pAd->MgmtRing.TxCpuIdx)
1244                                 FreeNumber = pAd->MgmtRing.TxSwFreeIdx - pAd->MgmtRing.TxCpuIdx - 1;
1245                         else
1246                                 FreeNumber = pAd->MgmtRing.TxSwFreeIdx + MGMT_RING_SIZE - pAd->MgmtRing.TxCpuIdx - 1;
1247
1248                         if (FreeNumber >= NumberRequired)
1249                                 Status = NDIS_STATUS_SUCCESS;
1250 #endif
1251 #ifdef RT2870
1252                         if (pAd->MgmtRing.TxSwFreeIdx != MGMT_RING_SIZE)
1253                                 Status = NDIS_STATUS_FAILURE;
1254                         else
1255                                 Status = NDIS_STATUS_SUCCESS;
1256 #endif
1257                         break;
1258
1259                 default:
1260                         DBGPRINT(RT_DEBUG_ERROR,("RTMPFreeTXDRequest::Invalid QueIdx(=%d)\n", QueIdx));
1261                         break;
1262         }
1263 #ifdef RT2860
1264         *FreeNumberIs = (UCHAR)FreeNumber;
1265 #endif
1266
1267         return (Status);
1268 }
1269
1270
1271
1272 VOID RTMPSendDisassociationFrame(
1273         IN      PRTMP_ADAPTER   pAd)
1274 {
1275 }
1276
1277 VOID    RTMPSendNullFrame(
1278         IN      PRTMP_ADAPTER   pAd,
1279         IN      UCHAR                   TxRate,
1280         IN      BOOLEAN                 bQosNull)
1281 {
1282         UCHAR   NullFrame[48];
1283         ULONG   Length;
1284         PHEADER_802_11  pHeader_802_11;
1285
1286     // WPA 802.1x secured port control
1287     if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
1288          (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
1289          (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
1290          (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
1291                           || (pAd->StaCfg.IEEE8021X == TRUE)
1292         ) &&
1293        (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
1294         {
1295                 return;
1296         }
1297
1298         NdisZeroMemory(NullFrame, 48);
1299         Length = sizeof(HEADER_802_11);
1300
1301         pHeader_802_11 = (PHEADER_802_11) NullFrame;
1302
1303         pHeader_802_11->FC.Type = BTYPE_DATA;
1304         pHeader_802_11->FC.SubType = SUBTYPE_NULL_FUNC;
1305         pHeader_802_11->FC.ToDs = 1;
1306         COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
1307         COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
1308         COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
1309
1310         if (pAd->CommonCfg.bAPSDForcePowerSave)
1311         {
1312                 pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
1313         }
1314         else
1315         {
1316                 pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE) ? 1: 0;
1317         }
1318         pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + RTMPCalcDuration(pAd, TxRate, 14);
1319
1320         pAd->Sequence++;
1321         pHeader_802_11->Sequence = pAd->Sequence;
1322
1323         // Prepare QosNull function frame
1324         if (bQosNull)
1325         {
1326                 pHeader_802_11->FC.SubType = SUBTYPE_QOS_NULL;
1327
1328                 // copy QOS control bytes
1329                 NullFrame[Length]       =  0;
1330                 NullFrame[Length+1] =  0;
1331                 Length += 2;// if pad with 2 bytes for alignment, APSD will fail
1332         }
1333
1334         HAL_KickOutNullFrameTx(pAd, 0, NullFrame, Length);
1335
1336 }
1337
1338 // IRQL = DISPATCH_LEVEL
1339 VOID    RTMPSendRTSFrame(
1340         IN      PRTMP_ADAPTER   pAd,
1341         IN      PUCHAR                  pDA,
1342         IN      unsigned int    NextMpduSize,
1343         IN      UCHAR                   TxRate,
1344         IN      UCHAR                   RTSRate,
1345         IN      USHORT                  AckDuration,
1346         IN      UCHAR                   QueIdx,
1347         IN      UCHAR                   FrameGap)
1348 {
1349 }
1350
1351
1352
1353 // --------------------------------------------------------
1354 //  FIND ENCRYPT KEY AND DECIDE CIPHER ALGORITHM
1355 //              Find the WPA key, either Group or Pairwise Key
1356 //              LEAP + TKIP also use WPA key.
1357 // --------------------------------------------------------
1358 // Decide WEP bit and cipher suite to be used. Same cipher suite should be used for whole fragment burst
1359 // In Cisco CCX 2.0 Leap Authentication
1360 //                 WepStatus is Ndis802_11Encryption1Enabled but the key will use PairwiseKey
1361 //                 Instead of the SharedKey, SharedKey Length may be Zero.
1362 VOID STAFindCipherAlgorithm(
1363         IN      PRTMP_ADAPTER   pAd,
1364         IN      TX_BLK                  *pTxBlk)
1365 {
1366         NDIS_802_11_ENCRYPTION_STATUS   Cipher;                         // To indicate cipher used for this packet
1367         UCHAR                                                   CipherAlg = CIPHER_NONE;                // cipher alogrithm
1368         UCHAR                                                   KeyIdx = 0xff;
1369         PUCHAR                                                  pSrcBufVA;
1370         PCIPHER_KEY                                             pKey = NULL;
1371
1372         pSrcBufVA = GET_OS_PKT_DATAPTR(pTxBlk->pPacket);
1373
1374         {
1375             // Select Cipher
1376             if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
1377                 Cipher = pAd->StaCfg.GroupCipher; // Cipher for Multicast or Broadcast
1378             else
1379                 Cipher = pAd->StaCfg.PairCipher; // Cipher for Unicast
1380
1381                 if (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
1382                 {
1383                         ASSERT(pAd->SharedKey[BSS0][0].CipherAlg <= CIPHER_CKIP128);
1384
1385                         // 4-way handshaking frame must be clear
1386                         if (!(TX_BLK_TEST_FLAG(pTxBlk, fTX_bClearEAPFrame)) && (pAd->SharedKey[BSS0][0].CipherAlg) &&
1387                                 (pAd->SharedKey[BSS0][0].KeyLen))
1388                         {
1389                                 CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
1390                                 KeyIdx = 0;
1391                         }
1392                 }
1393                 else if (Cipher == Ndis802_11Encryption1Enabled)
1394                 {
1395                                 KeyIdx = pAd->StaCfg.DefaultKeyId;
1396                 }
1397                 else if ((Cipher == Ndis802_11Encryption2Enabled) ||
1398                                  (Cipher == Ndis802_11Encryption3Enabled))
1399                 {
1400                         if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))) // multicast
1401                                 KeyIdx = pAd->StaCfg.DefaultKeyId;
1402                         else if (pAd->SharedKey[BSS0][0].KeyLen)
1403                                 KeyIdx = 0;
1404                         else
1405                                 KeyIdx = pAd->StaCfg.DefaultKeyId;
1406                 }
1407
1408                 if (KeyIdx == 0xff)
1409                         CipherAlg = CIPHER_NONE;
1410                 else if ((Cipher == Ndis802_11EncryptionDisabled) || (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 0))
1411                         CipherAlg = CIPHER_NONE;
1412             else if ( pAd->StaCfg.WpaSupplicantUP &&
1413                      (Cipher == Ndis802_11Encryption1Enabled) &&
1414                      (pAd->StaCfg.IEEE8021X == TRUE) &&
1415                      (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
1416                 CipherAlg = CIPHER_NONE;
1417                 else
1418                 {
1419                         //Header_802_11.FC.Wep = 1;
1420                         CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
1421                         pKey = &pAd->SharedKey[BSS0][KeyIdx];
1422                 }
1423         }
1424
1425         pTxBlk->CipherAlg = CipherAlg;
1426         pTxBlk->pKey = pKey;
1427 }
1428
1429
1430 VOID STABuildCommon802_11Header(
1431         IN  PRTMP_ADAPTER   pAd,
1432         IN  TX_BLK          *pTxBlk)
1433 {
1434         HEADER_802_11   *pHeader_802_11;
1435
1436         //
1437         // MAKE A COMMON 802.11 HEADER
1438         //
1439
1440         // normal wlan header size : 24 octets
1441         pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
1442
1443         pHeader_802_11 = (HEADER_802_11 *) &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1444
1445         NdisZeroMemory(pHeader_802_11, sizeof(HEADER_802_11));
1446
1447         pHeader_802_11->FC.FrDs = 0;
1448         pHeader_802_11->FC.Type = BTYPE_DATA;
1449         pHeader_802_11->FC.SubType = ((TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM)) ? SUBTYPE_QDATA : SUBTYPE_DATA);
1450
1451     if (pTxBlk->pMacEntry)
1452         {
1453                 if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bForceNonQoS))
1454                 {
1455                         pHeader_802_11->Sequence = pTxBlk->pMacEntry->NonQosDataSeq;
1456                         pTxBlk->pMacEntry->NonQosDataSeq = (pTxBlk->pMacEntry->NonQosDataSeq+1) & MAXSEQ;
1457                 }
1458                 else
1459                 {
1460                         {
1461             pHeader_802_11->Sequence = pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority];
1462             pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority] = (pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
1463         }
1464         }
1465         }
1466         else
1467         {
1468                 pHeader_802_11->Sequence = pAd->Sequence;
1469                 pAd->Sequence = (pAd->Sequence+1) & MAXSEQ; // next sequence
1470         }
1471
1472         pHeader_802_11->Frag = 0;
1473
1474         pHeader_802_11->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
1475
1476         {
1477                 if (INFRA_ON(pAd))
1478                 {
1479                         {
1480                         COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
1481                         COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
1482                         COPY_MAC_ADDR(pHeader_802_11->Addr3, pTxBlk->pSrcBufHeader);
1483                         pHeader_802_11->FC.ToDs = 1;
1484                 }
1485                 }
1486                 else if (ADHOC_ON(pAd))
1487                 {
1488                         COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
1489                         COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
1490                         COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
1491                         pHeader_802_11->FC.ToDs = 0;
1492                 }
1493         }
1494
1495         if (pTxBlk->CipherAlg != CIPHER_NONE)
1496                 pHeader_802_11->FC.Wep = 1;
1497
1498         // -----------------------------------------------------------------
1499         // STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
1500         // -----------------------------------------------------------------
1501         if (pAd->CommonCfg.bAPSDForcePowerSave)
1502         pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
1503         else
1504         pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
1505 }
1506
1507 VOID STABuildCache802_11Header(
1508         IN RTMP_ADAPTER         *pAd,
1509         IN TX_BLK                       *pTxBlk,
1510         IN UCHAR                        *pHeader)
1511 {
1512         MAC_TABLE_ENTRY *pMacEntry;
1513         PHEADER_802_11  pHeader80211;
1514
1515         pHeader80211 = (PHEADER_802_11)pHeader;
1516         pMacEntry = pTxBlk->pMacEntry;
1517
1518         //
1519         // Update the cached 802.11 HEADER
1520         //
1521
1522         // normal wlan header size : 24 octets
1523         pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
1524
1525         // More Bit
1526         pHeader80211->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
1527
1528         // Sequence
1529         pHeader80211->Sequence = pMacEntry->TxSeq[pTxBlk->UserPriority];
1530     pMacEntry->TxSeq[pTxBlk->UserPriority] = (pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
1531
1532         {
1533                 // The addr3 of normal packet send from DS is Dest Mac address.
1534                 if (ADHOC_ON(pAd))
1535                         COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
1536                 else
1537                         COPY_MAC_ADDR(pHeader80211->Addr3, pTxBlk->pSrcBufHeader);
1538         }
1539
1540         // -----------------------------------------------------------------
1541         // STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
1542         // -----------------------------------------------------------------
1543         if (pAd->CommonCfg.bAPSDForcePowerSave)
1544         pHeader80211->FC.PwrMgmt = PWR_SAVE;
1545         else
1546         pHeader80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
1547 }
1548
1549 static inline PUCHAR STA_Build_ARalink_Frame_Header(
1550         IN RTMP_ADAPTER *pAd,
1551         IN TX_BLK               *pTxBlk)
1552 {
1553         PUCHAR                  pHeaderBufPtr;
1554         HEADER_802_11   *pHeader_802_11;
1555         PNDIS_PACKET    pNextPacket;
1556         UINT32                  nextBufLen;
1557         PQUEUE_ENTRY    pQEntry;
1558
1559         STAFindCipherAlgorithm(pAd, pTxBlk);
1560         STABuildCommon802_11Header(pAd, pTxBlk);
1561
1562
1563         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1564         pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
1565
1566         // steal "order" bit to mark "aggregation"
1567         pHeader_802_11->FC.Order = 1;
1568
1569         // skip common header
1570         pHeaderBufPtr += pTxBlk->MpduHeaderLen;
1571
1572         if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
1573         {
1574                 //
1575                 // build QOS Control bytes
1576                 //
1577                 *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
1578
1579                 *(pHeaderBufPtr+1) = 0;
1580                 pHeaderBufPtr +=2;
1581                 pTxBlk->MpduHeaderLen += 2;
1582         }
1583
1584         // padding at front of LLC header. LLC header should at 4-bytes aligment.
1585         pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
1586         pHeaderBufPtr = (PCHAR)ROUND_UP(pHeaderBufPtr, 4);
1587         pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
1588
1589         // For RA Aggregation,
1590         // put the 2nd MSDU length(extra 2-byte field) after QOS_CONTROL in little endian format
1591         pQEntry = pTxBlk->TxPacketList.Head;
1592         pNextPacket = QUEUE_ENTRY_TO_PKT(pQEntry);
1593         nextBufLen = GET_OS_PKT_LEN(pNextPacket);
1594         if (RTMP_GET_PACKET_VLAN(pNextPacket))
1595                 nextBufLen -= LENGTH_802_1Q;
1596
1597         *pHeaderBufPtr = (UCHAR)nextBufLen & 0xff;
1598         *(pHeaderBufPtr+1) = (UCHAR)(nextBufLen >> 8);
1599
1600         pHeaderBufPtr += 2;
1601         pTxBlk->MpduHeaderLen += 2;
1602
1603         return pHeaderBufPtr;
1604
1605 }
1606
1607 static inline PUCHAR STA_Build_AMSDU_Frame_Header(
1608         IN RTMP_ADAPTER *pAd,
1609         IN TX_BLK               *pTxBlk)
1610 {
1611         PUCHAR                  pHeaderBufPtr;//, pSaveBufPtr;
1612         HEADER_802_11   *pHeader_802_11;
1613
1614
1615         STAFindCipherAlgorithm(pAd, pTxBlk);
1616         STABuildCommon802_11Header(pAd, pTxBlk);
1617
1618         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1619         pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
1620
1621         // skip common header
1622         pHeaderBufPtr += pTxBlk->MpduHeaderLen;
1623
1624         //
1625         // build QOS Control bytes
1626         //
1627         *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
1628
1629         //
1630         // A-MSDU packet
1631         //
1632         *pHeaderBufPtr |= 0x80;
1633
1634         *(pHeaderBufPtr+1) = 0;
1635         pHeaderBufPtr +=2;
1636         pTxBlk->MpduHeaderLen += 2;
1637
1638         //pSaveBufPtr = pHeaderBufPtr;
1639
1640         //
1641         // padding at front of LLC header
1642         // LLC header should locate at 4-octets aligment
1643         //
1644         // @@@ MpduHeaderLen excluding padding @@@
1645         //
1646         pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
1647         pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
1648         pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
1649
1650         return pHeaderBufPtr;
1651
1652 }
1653
1654
1655 VOID STA_AMPDU_Frame_Tx(
1656         IN      PRTMP_ADAPTER   pAd,
1657         IN      TX_BLK                  *pTxBlk)
1658 {
1659         HEADER_802_11   *pHeader_802_11;
1660         PUCHAR                  pHeaderBufPtr;
1661         USHORT                  FreeNumber;
1662         MAC_TABLE_ENTRY *pMacEntry;
1663         BOOLEAN                 bVLANPkt;
1664         PQUEUE_ENTRY    pQEntry;
1665
1666         ASSERT(pTxBlk);
1667
1668         while(pTxBlk->TxPacketList.Head)
1669         {
1670                 pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
1671                 pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
1672                 if ( RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
1673                 {
1674                         RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
1675                         continue;
1676                 }
1677
1678                 bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
1679
1680                 pMacEntry = pTxBlk->pMacEntry;
1681                 if (pMacEntry->isCached)
1682                 {
1683                         // NOTE: Please make sure the size of pMacEntry->CachedBuf[] is smaller than pTxBlk->HeaderBuf[]!!!!
1684                         NdisMoveMemory((PUCHAR)&pTxBlk->HeaderBuf[TXINFO_SIZE], (PUCHAR)&pMacEntry->CachedBuf[0], TXWI_SIZE + sizeof(HEADER_802_11));
1685                         pHeaderBufPtr = (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE]);
1686                         STABuildCache802_11Header(pAd, pTxBlk, pHeaderBufPtr);
1687                 }
1688                 else
1689                 {
1690                         STAFindCipherAlgorithm(pAd, pTxBlk);
1691                         STABuildCommon802_11Header(pAd, pTxBlk);
1692
1693                         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1694                 }
1695
1696
1697                 pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
1698
1699                 // skip common header
1700                 pHeaderBufPtr += pTxBlk->MpduHeaderLen;
1701
1702                 //
1703                 // build QOS Control bytes
1704                 //
1705                 *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
1706                 *(pHeaderBufPtr+1) = 0;
1707                 pHeaderBufPtr +=2;
1708                 pTxBlk->MpduHeaderLen += 2;
1709
1710                 //
1711                 // build HTC+
1712                 // HTC control filed following QoS field
1713                 //
1714                 if ((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pTxBlk->pMacEntry, fCLIENT_STATUS_RDG_CAPABLE))
1715                 {
1716                         if (pMacEntry->isCached == FALSE)
1717                         {
1718                                 // mark HTC bit
1719                                 pHeader_802_11->FC.Order = 1;
1720
1721                                 NdisZeroMemory(pHeaderBufPtr, 4);
1722                                 *(pHeaderBufPtr+3) |= 0x80;
1723                         }
1724                         pHeaderBufPtr += 4;
1725                         pTxBlk->MpduHeaderLen += 4;
1726                 }
1727
1728                 //pTxBlk->MpduHeaderLen = pHeaderBufPtr - pTxBlk->HeaderBuf - TXWI_SIZE - TXINFO_SIZE;
1729                 ASSERT(pTxBlk->MpduHeaderLen >= 24);
1730
1731                 // skip 802.3 header
1732                 pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
1733                 pTxBlk->SrcBufLen  -= LENGTH_802_3;
1734
1735                 // skip vlan tag
1736                 if (bVLANPkt)
1737                 {
1738                         pTxBlk->pSrcBufData     += LENGTH_802_1Q;
1739                         pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
1740                 }
1741
1742                 //
1743                 // padding at front of LLC header
1744                 // LLC header should locate at 4-octets aligment
1745                 //
1746                 // @@@ MpduHeaderLen excluding padding @@@
1747                 //
1748                 pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
1749                 pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
1750                 pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
1751
1752                 {
1753
1754                         //
1755                         // Insert LLC-SNAP encapsulation - 8 octets
1756                         //
1757                         EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
1758                         if (pTxBlk->pExtraLlcSnapEncap)
1759                         {
1760                                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
1761                                 pHeaderBufPtr += 6;
1762                                 // get 2 octets (TypeofLen)
1763                                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
1764                                 pHeaderBufPtr += 2;
1765                                 pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
1766                         }
1767
1768                 }
1769
1770                 if (pMacEntry->isCached)
1771                 {
1772             RTMPWriteTxWI_Cache(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
1773                 }
1774                 else
1775                 {
1776                         RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
1777
1778                         NdisZeroMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), sizeof(pMacEntry->CachedBuf));
1779                         NdisMoveMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), (pHeaderBufPtr - (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE])));
1780                         pMacEntry->isCached = TRUE;
1781                 }
1782
1783                 // calculate Transmitted AMPDU count and ByteCount
1784                 {
1785                         pAd->RalinkCounters.TransmittedMPDUsInAMPDUCount.u.LowPart ++;
1786                         pAd->RalinkCounters.TransmittedOctetsInAMPDUCount.QuadPart += pTxBlk->SrcBufLen;
1787                 }
1788
1789                 //FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
1790
1791                 HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
1792
1793                 //
1794                 // Kick out Tx
1795                 //
1796 #ifdef RT2860
1797                 if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
1798 #endif
1799                         HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
1800
1801                 pAd->RalinkCounters.KickTxCount++;
1802                 pAd->RalinkCounters.OneSecTxDoneCount++;
1803         }
1804
1805 }
1806
1807
1808 VOID STA_AMSDU_Frame_Tx(
1809         IN      PRTMP_ADAPTER   pAd,
1810         IN      TX_BLK                  *pTxBlk)
1811 {
1812         PUCHAR                  pHeaderBufPtr;
1813         USHORT                  FreeNumber;
1814         USHORT                  subFramePayloadLen = 0; // AMSDU Subframe length without AMSDU-Header / Padding.
1815         USHORT                  totalMPDUSize=0;
1816         UCHAR                   *subFrameHeader;
1817         UCHAR                   padding = 0;
1818         USHORT                  FirstTx = 0, LastTxIdx = 0;
1819         BOOLEAN                 bVLANPkt;
1820         int                     frameNum = 0;
1821         PQUEUE_ENTRY    pQEntry;
1822
1823
1824         ASSERT(pTxBlk);
1825
1826         ASSERT((pTxBlk->TxPacketList.Number > 1));
1827
1828         while(pTxBlk->TxPacketList.Head)
1829         {
1830                 pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
1831                 pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
1832                 if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
1833                 {
1834                         RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
1835                         continue;
1836                 }
1837
1838                 bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
1839
1840                 // skip 802.3 header
1841                 pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
1842                 pTxBlk->SrcBufLen  -= LENGTH_802_3;
1843
1844                 // skip vlan tag
1845                 if (bVLANPkt)
1846                 {
1847                         pTxBlk->pSrcBufData     += LENGTH_802_1Q;
1848                         pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
1849                 }
1850
1851                 if (frameNum == 0)
1852                 {
1853                         pHeaderBufPtr = STA_Build_AMSDU_Frame_Header(pAd, pTxBlk);
1854
1855                         // NOTE: TxWI->MPDUtotalByteCount will be updated after final frame was handled.
1856                         RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
1857                 }
1858                 else
1859                 {
1860                         pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
1861                         padding = ROUND_UP(LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen, 4) - (LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen);
1862                         NdisZeroMemory(pHeaderBufPtr, padding + LENGTH_AMSDU_SUBFRAMEHEAD);
1863                         pHeaderBufPtr += padding;
1864                         pTxBlk->MpduHeaderLen = padding;
1865                 }
1866
1867                 //
1868                 // A-MSDU subframe
1869                 //   DA(6)+SA(6)+Length(2) + LLC/SNAP Encap
1870                 //
1871                 subFrameHeader = pHeaderBufPtr;
1872                 subFramePayloadLen = pTxBlk->SrcBufLen;
1873
1874                 NdisMoveMemory(subFrameHeader, pTxBlk->pSrcBufHeader, 12);
1875
1876
1877                 pHeaderBufPtr += LENGTH_AMSDU_SUBFRAMEHEAD;
1878                 pTxBlk->MpduHeaderLen += LENGTH_AMSDU_SUBFRAMEHEAD;
1879
1880
1881                 //
1882                 // Insert LLC-SNAP encapsulation - 8 octets
1883                 //
1884                 EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
1885
1886                 subFramePayloadLen = pTxBlk->SrcBufLen;
1887
1888                 if (pTxBlk->pExtraLlcSnapEncap)
1889                 {
1890                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
1891                         pHeaderBufPtr += 6;
1892                         // get 2 octets (TypeofLen)
1893                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
1894                         pHeaderBufPtr += 2;
1895                         pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
1896                         subFramePayloadLen += LENGTH_802_1_H;
1897                 }
1898
1899                 // update subFrame Length field
1900                 subFrameHeader[12] = (subFramePayloadLen & 0xFF00) >> 8;
1901                 subFrameHeader[13] = subFramePayloadLen & 0xFF;
1902
1903                 totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
1904
1905                 if (frameNum ==0)
1906                         FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
1907                 else
1908                         LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
1909
1910                 frameNum++;
1911
1912                 pAd->RalinkCounters.KickTxCount++;
1913                 pAd->RalinkCounters.OneSecTxDoneCount++;
1914
1915                 // calculate Transmitted AMSDU Count and ByteCount
1916                 {
1917                         pAd->RalinkCounters.TransmittedAMSDUCount.u.LowPart ++;
1918                         pAd->RalinkCounters.TransmittedOctetsInAMSDU.QuadPart += totalMPDUSize;
1919                 }
1920
1921         }
1922
1923         HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
1924         HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
1925
1926         //
1927         // Kick out Tx
1928         //
1929 #ifdef RT2860
1930         if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
1931 #endif
1932                 HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
1933 }
1934
1935 VOID STA_Legacy_Frame_Tx(
1936         IN      PRTMP_ADAPTER   pAd,
1937         IN      TX_BLK                  *pTxBlk)
1938 {
1939         HEADER_802_11   *pHeader_802_11;
1940         PUCHAR                  pHeaderBufPtr;
1941         USHORT                  FreeNumber;
1942         BOOLEAN                 bVLANPkt;
1943         PQUEUE_ENTRY    pQEntry;
1944
1945         ASSERT(pTxBlk);
1946
1947
1948         pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
1949         pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
1950         if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
1951         {
1952                 RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
1953                 return;
1954         }
1955
1956         if (pTxBlk->TxFrameType == TX_MCAST_FRAME)
1957         {
1958                 INC_COUNTER64(pAd->WlanCounters.MulticastTransmittedFrameCount);
1959         }
1960
1961         if (RTMP_GET_PACKET_RTS(pTxBlk->pPacket))
1962                 TX_BLK_SET_FLAG(pTxBlk, fTX_bRtsRequired);
1963         else
1964                 TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bRtsRequired);
1965
1966         bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
1967
1968         if (pTxBlk->TxRate < pAd->CommonCfg.MinTxRate)
1969                 pTxBlk->TxRate = pAd->CommonCfg.MinTxRate;
1970
1971         STAFindCipherAlgorithm(pAd, pTxBlk);
1972         STABuildCommon802_11Header(pAd, pTxBlk);
1973
1974
1975         // skip 802.3 header
1976         pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
1977         pTxBlk->SrcBufLen  -= LENGTH_802_3;
1978
1979         // skip vlan tag
1980         if (bVLANPkt)
1981         {
1982                 pTxBlk->pSrcBufData     += LENGTH_802_1Q;
1983                 pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
1984         }
1985
1986         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1987         pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
1988
1989         // skip common header
1990         pHeaderBufPtr += pTxBlk->MpduHeaderLen;
1991
1992         if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
1993         {
1994                 //
1995                 // build QOS Control bytes
1996                 //
1997                 *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
1998                 *(pHeaderBufPtr+1) = 0;
1999                 pHeaderBufPtr +=2;
2000                 pTxBlk->MpduHeaderLen += 2;
2001         }
2002
2003         // The remaining content of MPDU header should locate at 4-octets aligment
2004         pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
2005         pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
2006         pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
2007
2008         {
2009
2010                 //
2011                 // Insert LLC-SNAP encapsulation - 8 octets
2012                 //
2013                 //
2014                 // if original Ethernet frame contains no LLC/SNAP,
2015                 // then an extra LLC/SNAP encap is required
2016                 //
2017                 EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
2018                 if (pTxBlk->pExtraLlcSnapEncap)
2019                 {
2020                         UCHAR vlan_size;
2021
2022                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
2023                         pHeaderBufPtr += 6;
2024                         // skip vlan tag
2025                         vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
2026                         // get 2 octets (TypeofLen)
2027                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
2028                         pHeaderBufPtr += 2;
2029                         pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
2030                 }
2031
2032         }
2033
2034         //
2035         // prepare for TXWI
2036         // use Wcid as Key Index
2037         //
2038
2039         RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
2040
2041         //FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
2042
2043         HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
2044
2045         pAd->RalinkCounters.KickTxCount++;
2046         pAd->RalinkCounters.OneSecTxDoneCount++;
2047
2048         //
2049         // Kick out Tx
2050         //
2051 #ifdef RT2860
2052         if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
2053 #endif
2054                 HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
2055 }
2056
2057
2058 VOID STA_ARalink_Frame_Tx(
2059         IN      PRTMP_ADAPTER   pAd,
2060         IN      TX_BLK                  *pTxBlk)
2061 {
2062         PUCHAR                  pHeaderBufPtr;
2063         USHORT                  FreeNumber;
2064         USHORT                  totalMPDUSize=0;
2065         USHORT                  FirstTx, LastTxIdx;
2066         int                     frameNum = 0;
2067         BOOLEAN                 bVLANPkt;
2068         PQUEUE_ENTRY    pQEntry;
2069
2070
2071         ASSERT(pTxBlk);
2072
2073         ASSERT((pTxBlk->TxPacketList.Number== 2));
2074
2075
2076         FirstTx = LastTxIdx = 0;  // Is it ok init they as 0?
2077         while(pTxBlk->TxPacketList.Head)
2078         {
2079                 pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
2080                 pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
2081
2082                 if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
2083                 {
2084                         RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
2085                         continue;
2086                 }
2087
2088                 bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
2089
2090                 // skip 802.3 header
2091                 pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
2092                 pTxBlk->SrcBufLen  -= LENGTH_802_3;
2093
2094                 // skip vlan tag
2095                 if (bVLANPkt)
2096                 {
2097                         pTxBlk->pSrcBufData     += LENGTH_802_1Q;
2098                         pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
2099                 }
2100
2101                 if (frameNum == 0)
2102                 {       // For first frame, we need to create the 802.11 header + padding(optional) + RA-AGG-LEN + SNAP Header
2103
2104                         pHeaderBufPtr = STA_Build_ARalink_Frame_Header(pAd, pTxBlk);
2105
2106                         // It's ok write the TxWI here, because the TxWI->MPDUtotalByteCount
2107                         //      will be updated after final frame was handled.
2108                         RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
2109
2110
2111                         //
2112                         // Insert LLC-SNAP encapsulation - 8 octets
2113                         //
2114                         EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
2115
2116                         if (pTxBlk->pExtraLlcSnapEncap)
2117                         {
2118                                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
2119                                 pHeaderBufPtr += 6;
2120                                 // get 2 octets (TypeofLen)
2121                                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
2122                                 pHeaderBufPtr += 2;
2123                                 pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
2124                         }
2125                 }
2126                 else
2127                 {       // For second aggregated frame, we need create the 802.3 header to headerBuf, because PCI will copy it to SDPtr0.
2128
2129                         pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
2130                         pTxBlk->MpduHeaderLen = 0;
2131
2132                         // A-Ralink sub-sequent frame header is the same as 802.3 header.
2133                         //   DA(6)+SA(6)+FrameType(2)
2134                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader, 12);
2135                         pHeaderBufPtr += 12;
2136                         // get 2 octets (TypeofLen)
2137                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
2138                         pHeaderBufPtr += 2;
2139                         pTxBlk->MpduHeaderLen = LENGTH_ARALINK_SUBFRAMEHEAD;
2140                 }
2141
2142                 totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
2143
2144                 //FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
2145                 if (frameNum ==0)
2146                         FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
2147                 else
2148                         LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
2149
2150                 frameNum++;
2151
2152                 pAd->RalinkCounters.OneSecTxAggregationCount++;
2153                 pAd->RalinkCounters.KickTxCount++;
2154                 pAd->RalinkCounters.OneSecTxDoneCount++;
2155
2156         }
2157
2158         HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
2159         HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
2160
2161         //
2162         // Kick out Tx
2163         //
2164 #ifdef RT2860
2165         if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
2166 #endif
2167                 HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
2168
2169 }
2170
2171
2172 VOID STA_Fragment_Frame_Tx(
2173         IN RTMP_ADAPTER *pAd,
2174         IN TX_BLK               *pTxBlk)
2175 {
2176         HEADER_802_11   *pHeader_802_11;
2177         PUCHAR                  pHeaderBufPtr;
2178         USHORT                  FreeNumber;
2179         UCHAR                   fragNum = 0;
2180         PACKET_INFO             PacketInfo;
2181         USHORT                  EncryptionOverhead = 0;
2182         UINT32                  FreeMpduSize, SrcRemainingBytes;
2183         USHORT                  AckDuration;
2184         UINT                    NextMpduSize;
2185         BOOLEAN                 bVLANPkt;
2186         PQUEUE_ENTRY    pQEntry;
2187
2188
2189         ASSERT(pTxBlk);
2190
2191         pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
2192         pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
2193         if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
2194         {
2195                 RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
2196                 return;
2197         }
2198
2199         ASSERT(TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag));
2200         bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
2201
2202         STAFindCipherAlgorithm(pAd, pTxBlk);
2203         STABuildCommon802_11Header(pAd, pTxBlk);
2204
2205         if (pTxBlk->CipherAlg == CIPHER_TKIP)
2206         {
2207                 pTxBlk->pPacket = duplicate_pkt_with_TKIP_MIC(pAd, pTxBlk->pPacket);
2208                 if (pTxBlk->pPacket == NULL)
2209                         return;
2210                 RTMP_QueryPacketInfo(pTxBlk->pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
2211         }
2212
2213         // skip 802.3 header
2214         pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
2215         pTxBlk->SrcBufLen  -= LENGTH_802_3;
2216
2217
2218         // skip vlan tag
2219         if (bVLANPkt)
2220         {
2221                 pTxBlk->pSrcBufData     += LENGTH_802_1Q;
2222                 pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
2223         }
2224
2225         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
2226         pHeader_802_11 = (HEADER_802_11 *)pHeaderBufPtr;
2227
2228
2229         // skip common header
2230         pHeaderBufPtr += pTxBlk->MpduHeaderLen;
2231
2232         if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
2233         {
2234                 //
2235                 // build QOS Control bytes
2236                 //
2237                 *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
2238
2239                 *(pHeaderBufPtr+1) = 0;
2240                 pHeaderBufPtr +=2;
2241                 pTxBlk->MpduHeaderLen += 2;
2242         }
2243
2244         //
2245         // padding at front of LLC header
2246         // LLC header should locate at 4-octets aligment
2247         //
2248         pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
2249         pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
2250         pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
2251
2252
2253
2254         //
2255         // Insert LLC-SNAP encapsulation - 8 octets
2256         //
2257         //
2258         // if original Ethernet frame contains no LLC/SNAP,
2259         // then an extra LLC/SNAP encap is required
2260         //
2261         EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
2262         if (pTxBlk->pExtraLlcSnapEncap)
2263         {
2264                 UCHAR vlan_size;
2265
2266                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
2267                 pHeaderBufPtr += 6;
2268                 // skip vlan tag
2269                 vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
2270                 // get 2 octets (TypeofLen)
2271                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
2272                 pHeaderBufPtr += 2;
2273                 pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
2274         }
2275
2276
2277         // If TKIP is used and fragmentation is required. Driver has to
2278         //      append TKIP MIC at tail of the scatter buffer
2279         //      MAC ASIC will only perform IV/EIV/ICV insertion but no TKIP MIC
2280         if (pTxBlk->CipherAlg == CIPHER_TKIP)
2281         {
2282
2283                 // NOTE: DON'T refer the skb->len directly after following copy. Becasue the length is not adjust
2284                 //                      to correct lenght, refer to pTxBlk->SrcBufLen for the packet length in following progress.
2285                 NdisMoveMemory(pTxBlk->pSrcBufData + pTxBlk->SrcBufLen, &pAd->PrivateInfo.Tx.MIC[0], 8);
2286                 //skb_put((RTPKT_TO_OSPKT(pTxBlk->pPacket))->tail, 8);
2287                 pTxBlk->SrcBufLen += 8;
2288                 pTxBlk->TotalFrameLen += 8;
2289                 pTxBlk->CipherAlg = CIPHER_TKIP_NO_MIC;
2290         }
2291
2292         //
2293         // calcuate the overhead bytes that encryption algorithm may add. This
2294         // affects the calculate of "duration" field
2295         //
2296         if ((pTxBlk->CipherAlg == CIPHER_WEP64) || (pTxBlk->CipherAlg == CIPHER_WEP128))
2297                 EncryptionOverhead = 8; //WEP: IV[4] + ICV[4];
2298         else if (pTxBlk->CipherAlg == CIPHER_TKIP_NO_MIC)
2299                 EncryptionOverhead = 12;//TKIP: IV[4] + EIV[4] + ICV[4], MIC will be added to TotalPacketLength
2300         else if (pTxBlk->CipherAlg == CIPHER_TKIP)
2301                 EncryptionOverhead = 20;//TKIP: IV[4] + EIV[4] + ICV[4] + MIC[8]
2302         else if (pTxBlk->CipherAlg == CIPHER_AES)
2303                 EncryptionOverhead = 16;        // AES: IV[4] + EIV[4] + MIC[8]
2304         else
2305                 EncryptionOverhead = 0;
2306
2307         // decide how much time an ACK/CTS frame will consume in the air
2308         AckDuration = RTMPCalcDuration(pAd, pAd->CommonCfg.ExpectedACKRate[pTxBlk->TxRate], 14);
2309
2310         // Init the total payload length of this frame.
2311         SrcRemainingBytes = pTxBlk->SrcBufLen;
2312
2313         pTxBlk->TotalFragNum = 0xff;
2314
2315         do {
2316
2317                 FreeMpduSize = pAd->CommonCfg.FragmentThreshold - LENGTH_CRC;
2318
2319                 FreeMpduSize -= pTxBlk->MpduHeaderLen;
2320
2321                 if (SrcRemainingBytes <= FreeMpduSize)
2322                 {       // this is the last or only fragment
2323
2324                         pTxBlk->SrcBufLen = SrcRemainingBytes;
2325
2326                         pHeader_802_11->FC.MoreFrag = 0;
2327                         pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + AckDuration;
2328
2329                         // Indicate the lower layer that this's the last fragment.
2330                         pTxBlk->TotalFragNum = fragNum;
2331                 }
2332                 else
2333                 {       // more fragment is required
2334
2335                         pTxBlk->SrcBufLen = FreeMpduSize;
2336
2337                         NextMpduSize = min(((UINT)SrcRemainingBytes - pTxBlk->SrcBufLen), ((UINT)pAd->CommonCfg.FragmentThreshold));
2338                         pHeader_802_11->FC.MoreFrag = 1;
2339                         pHeader_802_11->Duration = (3 * pAd->CommonCfg.Dsifs) + (2 * AckDuration) + RTMPCalcDuration(pAd, pTxBlk->TxRate, NextMpduSize + EncryptionOverhead);
2340                 }
2341
2342                 if (fragNum == 0)
2343                         pTxBlk->FrameGap = IFS_HTTXOP;
2344                 else
2345                         pTxBlk->FrameGap = IFS_SIFS;
2346
2347                 RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
2348
2349                 HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, &FreeNumber);
2350
2351                 pAd->RalinkCounters.KickTxCount++;
2352                 pAd->RalinkCounters.OneSecTxDoneCount++;
2353
2354                 // Update the frame number, remaining size of the NDIS packet payload.
2355
2356                 // space for 802.11 header.
2357                 if (fragNum == 0 && pTxBlk->pExtraLlcSnapEncap)
2358                         pTxBlk->MpduHeaderLen -= LENGTH_802_1_H;
2359
2360                 fragNum++;
2361                 SrcRemainingBytes -= pTxBlk->SrcBufLen;
2362                 pTxBlk->pSrcBufData += pTxBlk->SrcBufLen;
2363
2364                 pHeader_802_11->Frag++;  // increase Frag #
2365
2366         }while(SrcRemainingBytes > 0);
2367
2368         //
2369         // Kick out Tx
2370         //
2371         HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
2372 }
2373
2374
2375 #define RELEASE_FRAMES_OF_TXBLK(_pAd, _pTxBlk, _pQEntry, _Status)                                                                               \
2376                 while(_pTxBlk->TxPacketList.Head)                                                                                                               \
2377                 {                                                                                                                                                                               \
2378                         _pQEntry = RemoveHeadQueue(&_pTxBlk->TxPacketList);                                                                     \
2379                         RELEASE_NDIS_PACKET(_pAd, QUEUE_ENTRY_TO_PACKET(_pQEntry), _Status);    \
2380                 }
2381
2382
2383 /*
2384         ========================================================================
2385
2386         Routine Description:
2387                 Copy frame from waiting queue into relative ring buffer and set
2388         appropriate ASIC register to kick hardware encryption before really
2389         sent out to air.
2390
2391         Arguments:
2392                 pAd     Pointer to our adapter
2393                 PNDIS_PACKET    Pointer to outgoing Ndis frame
2394                 NumberOfFrag    Number of fragment required
2395
2396         Return Value:
2397                 None
2398
2399         IRQL = DISPATCH_LEVEL
2400
2401         Note:
2402
2403         ========================================================================
2404 */
2405 NDIS_STATUS STAHardTransmit(
2406         IN PRTMP_ADAPTER        pAd,
2407         IN TX_BLK                       *pTxBlk,
2408         IN      UCHAR                   QueIdx)
2409 {
2410         NDIS_PACKET             *pPacket;
2411         PQUEUE_ENTRY    pQEntry;
2412
2413         // ---------------------------------------------
2414         // STEP 0. DO SANITY CHECK AND SOME EARLY PREPARATION.
2415         // ---------------------------------------------
2416         //
2417         ASSERT(pTxBlk->TxPacketList.Number);
2418         if (pTxBlk->TxPacketList.Head == NULL)
2419         {
2420                 DBGPRINT(RT_DEBUG_ERROR, ("pTxBlk->TotalFrameNum == %ld!\n", pTxBlk->TxPacketList.Number));
2421                 return NDIS_STATUS_FAILURE;
2422         }
2423
2424         pPacket = QUEUE_ENTRY_TO_PACKET(pTxBlk->TxPacketList.Head);
2425
2426         // ------------------------------------------------------------------
2427         // STEP 1. WAKE UP PHY
2428         //              outgoing frame always wakeup PHY to prevent frame lost and
2429         //              turn off PSM bit to improve performance
2430         // ------------------------------------------------------------------
2431         // not to change PSM bit, just send this frame out?
2432         if ((pAd->StaCfg.Psm == PWR_SAVE) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
2433         {
2434             DBGPRINT_RAW(RT_DEBUG_TRACE, ("AsicForceWakeup At HardTx\n"));
2435 #ifdef RT2860
2436                 AsicForceWakeup(pAd, FROM_TX);
2437 #endif
2438 #ifdef RT2870
2439                 AsicForceWakeup(pAd, TRUE);
2440 #endif
2441         }
2442
2443         // It should not change PSM bit, when APSD turn on.
2444         if ((!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable) && (pAd->CommonCfg.bAPSDForcePowerSave == FALSE))
2445                 || (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
2446                 || (RTMP_GET_PACKET_WAI(pTxBlk->pPacket)))
2447         {
2448                 if ((pAd->StaCfg.Psm == PWR_SAVE) &&
2449             (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeFast_PSP))
2450                         MlmeSetPsmBit(pAd, PWR_ACTIVE);
2451         }
2452
2453         switch (pTxBlk->TxFrameType)
2454         {
2455                 case TX_AMPDU_FRAME:
2456                                 STA_AMPDU_Frame_Tx(pAd, pTxBlk);
2457                         break;
2458                 case TX_AMSDU_FRAME:
2459                                 STA_AMSDU_Frame_Tx(pAd, pTxBlk);
2460                         break;
2461                 case TX_LEGACY_FRAME:
2462                                 STA_Legacy_Frame_Tx(pAd, pTxBlk);
2463                         break;
2464                 case TX_MCAST_FRAME:
2465                                 STA_Legacy_Frame_Tx(pAd, pTxBlk);
2466                         break;
2467                 case TX_RALINK_FRAME:
2468                                 STA_ARalink_Frame_Tx(pAd, pTxBlk);
2469                         break;
2470                 case TX_FRAG_FRAME:
2471                                 STA_Fragment_Frame_Tx(pAd, pTxBlk);
2472                         break;
2473                 default:
2474                         {
2475                                 // It should not happened!
2476                                 DBGPRINT(RT_DEBUG_ERROR, ("Send a pacekt was not classified!! It should not happen!\n"));
2477                                 while(pTxBlk->TxPacketList.Number)
2478                                 {
2479                                         pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
2480                                         pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
2481                                         if (pPacket)
2482                                                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
2483                                 }
2484                         }
2485                         break;
2486         }
2487
2488         return (NDIS_STATUS_SUCCESS);
2489
2490 }
2491
2492 ULONG  HashBytesPolynomial(UCHAR *value, unsigned int len)
2493 {
2494    unsigned char *word = value;
2495    unsigned int ret = 0;
2496    unsigned int i;
2497
2498    for(i=0; i < len; i++)
2499    {
2500           int mod = i % 32;
2501           ret ^=(unsigned int) (word[i]) << mod;
2502           ret ^=(unsigned int) (word[i]) >> (32 - mod);
2503    }
2504    return ret;
2505 }
2506
2507 VOID Sta_Announce_or_Forward_802_3_Packet(
2508         IN      PRTMP_ADAPTER   pAd,
2509         IN      PNDIS_PACKET    pPacket,
2510         IN      UCHAR                   FromWhichBSSID)
2511 {
2512         if (TRUE
2513                 )
2514         {
2515                 announce_802_3_packet(pAd, pPacket);
2516         }
2517         else
2518         {
2519                 // release packet
2520                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
2521         }
2522 }
2523