git.oblomov.eu Git - linux-2.6/blob - 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 #ifdef RT30xx
 579                 if ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2))
 580                         && (pAd->RxAnt.EvaluatePeriod == 0))
 581 #endif
 582 #ifndef RT30xx
 583                 if ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2)))
 584 #endif
 585                 {
 586                         Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
 587
 588                         pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
 589                         pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
 590                 }
 591
 592 #ifdef RT30xx
 593                 // collect rssi information for antenna diversity
 594                 if (pAd->NicConfig2.field.AntDiversity)
 595                 {
 596                         if ((pRxD->U2M) || ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2))))
 597                         {
 598                                         COLLECT_RX_ANTENNA_AVERAGE_RSSI(pAd, ConvertToRssi(pAd, (UCHAR)pRxWI->RSSI0, RSSI_0), 0); //Note: RSSI2 not used on RT73
 599                                         pAd->StaCfg.NumOfAvgRssiSample ++;
 600                         }
 601                 }
 602 #endif // RT30xx //
 603
 604                 // First check the size, it MUST not exceed the mlme queue size
 605                 if (pRxWI->MPDUtotalByteCount > MGMT_DMA_BUFFER_SIZE)
 606                 {
 607                         DBGPRINT_ERR(("STAHandleRxMgmtFrame: frame too large, size = %d \n", pRxWI->MPDUtotalByteCount));
 608                         break;
 609                 }
 610
 611                 REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pHeader, pRxWI->MPDUtotalByteCount,
 612                                                                         pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
 613         } while (FALSE);
 614
 615         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
 616 }
 617
 618 VOID STAHandleRxControlFrame(
 619         IN      PRTMP_ADAPTER   pAd,
 620         IN      RX_BLK                  *pRxBlk)
 621 {
 622         PRXWI_STRUC             pRxWI = pRxBlk->pRxWI;
 623         PHEADER_802_11  pHeader = pRxBlk->pHeader;
 624         PNDIS_PACKET    pRxPacket = pRxBlk->pRxPacket;
 625
 626         switch (pHeader->FC.SubType)
 627         {
 628                 case SUBTYPE_BLOCK_ACK_REQ:
 629                         {
 630                                 CntlEnqueueForRecv(pAd, pRxWI->WirelessCliID, (pRxWI->MPDUtotalByteCount), (PFRAME_BA_REQ)pHeader);
 631                         }
 632                         break;
 633                 case SUBTYPE_BLOCK_ACK:
 634                 case SUBTYPE_ACK:
 635                 default:
 636                         break;
 637         }
 638
 639         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 640 }
 641
 642
 643 /*
 644         ========================================================================
 645
 646         Routine Description:
 647                 Process RxDone interrupt, running in DPC level
 648
 649         Arguments:
 650                 pAd Pointer to our adapter
 651
 652         Return Value:
 653                 None
 654
 655         IRQL = DISPATCH_LEVEL
 656
 657         Note:
 658                 This routine has to maintain Rx ring read pointer.
 659                 Need to consider QOS DATA format when converting to 802.3
 660         ========================================================================
 661 */
 662 BOOLEAN STARxDoneInterruptHandle(
 663         IN      PRTMP_ADAPTER   pAd,
 664         IN      BOOLEAN                 argc)
 665 {
 666         NDIS_STATUS                     Status;
 667         UINT32                  RxProcessed, RxPending;
 668         BOOLEAN                 bReschedule = FALSE;
 669         RT28XX_RXD_STRUC        *pRxD;
 670         UCHAR                   *pData;
 671         PRXWI_STRUC             pRxWI;
 672         PNDIS_PACKET    pRxPacket;
 673         PHEADER_802_11  pHeader;
 674         RX_BLK                  RxCell;
 675
 676         RxProcessed = RxPending = 0;
 677
 678         // process whole rx ring
 679         while (1)
 680         {
 681
 682                 if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF |
 683                                                                 fRTMP_ADAPTER_RESET_IN_PROGRESS |
 684                                                                 fRTMP_ADAPTER_HALT_IN_PROGRESS |
 685                                                                 fRTMP_ADAPTER_NIC_NOT_EXIST) ||
 686                         !RTMP_TEST_FLAG(pAd,fRTMP_ADAPTER_START_UP))
 687                 {
 688                         break;
 689                 }
 690
 691 #ifdef RT2860
 692                 if (RxProcessed++ > MAX_RX_PROCESS_CNT)
 693                 {
 694                         // need to reschedule rx handle
 695                         bReschedule = TRUE;
 696                         break;
 697                 }
 698 #endif
 699
 700                 RxProcessed ++; // test
 701
 702                 // 1. allocate a new data packet into rx ring to replace received packet
 703                 //    then processing the received packet
 704                 // 2. the callee must take charge of release of packet
 705                 // 3. As far as driver is concerned ,
 706                 //    the rx packet must
 707                 //      a. be indicated to upper layer or
 708                 //      b. be released if it is discarded
 709                 pRxPacket = GetPacketFromRxRing(pAd, &(RxCell.RxD), &bReschedule, &RxPending);
 710                 if (pRxPacket == NULL)
 711                 {
 712                         // no more packet to process
 713                         break;
 714                 }
 715
 716                 // get rx ring descriptor
 717                 pRxD = &(RxCell.RxD);
 718                 // get rx data buffer
 719                 pData   = GET_OS_PKT_DATAPTR(pRxPacket);
 720                 pRxWI   = (PRXWI_STRUC) pData;
 721                 pHeader = (PHEADER_802_11) (pData+RXWI_SIZE) ;
 722
 723                 // build RxCell
 724                 RxCell.pRxWI = pRxWI;
 725                 RxCell.pHeader = pHeader;
 726                 RxCell.pRxPacket = pRxPacket;
 727                 RxCell.pData = (UCHAR *) pHeader;
 728                 RxCell.DataSize = pRxWI->MPDUtotalByteCount;
 729                 RxCell.Flags = 0;
 730
 731                 // Increase Total receive byte counter after real data received no mater any error or not
 732                 pAd->RalinkCounters.ReceivedByteCount +=  pRxWI->MPDUtotalByteCount;
 733                 pAd->RalinkCounters.RxCount ++;
 734
 735                 INC_COUNTER64(pAd->WlanCounters.ReceivedFragmentCount);
 736
 737                 if (pRxWI->MPDUtotalByteCount < 14)
 738                         Status = NDIS_STATUS_FAILURE;
 739
 740         if (MONITOR_ON(pAd))
 741                 {
 742             send_monitor_packets(pAd, &RxCell);
 743                         break;
 744                 }
 745                 /* RT2870 invokes STARxDoneInterruptHandle() in rtusb_bulk.c */
 746
 747                 // Check for all RxD errors
 748                 Status = RTMPCheckRxError(pAd, pHeader, pRxWI, pRxD);
 749
 750                 // Handle the received frame
 751                 if (Status == NDIS_STATUS_SUCCESS)
 752                 {
 753                         switch (pHeader->FC.Type)
 754                         {
 755                                 // CASE I, receive a DATA frame
 756                                 case BTYPE_DATA:
 757                                 {
 758                                         // process DATA frame
 759                                         STAHandleRxDataFrame(pAd, &RxCell);
 760                                 }
 761                                 break;
 762                                 // CASE II, receive a MGMT frame
 763                                 case BTYPE_MGMT:
 764                                 {
 765                                         STAHandleRxMgmtFrame(pAd, &RxCell);
 766                                 }
 767                                 break;
 768                                 // CASE III. receive a CNTL frame
 769                                 case BTYPE_CNTL:
 770                                 {
 771                                         STAHandleRxControlFrame(pAd, &RxCell);
 772                                 }
 773                                 break;
 774                                 // discard other type
 775                                 default:
 776                                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 777                                         break;
 778                         }
 779                 }
 780                 else
 781                 {
 782                         pAd->Counters8023.RxErrors++;
 783                         // discard this frame
 784                         RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
 785                 }
 786         }
 787
 788 #ifdef RT2860
 789         // fRTMP_PS_GO_TO_SLEEP_NOW is set if receiving beacon.
 790         if (RTMP_TEST_PSFLAG(pAd, fRTMP_PS_GO_TO_SLEEP_NOW) && (INFRA_ON(pAd)))
 791         {
 792                 RTMP_CLEAR_PSFLAG(pAd, fRTMP_PS_GO_TO_SLEEP_NOW);
 793                 AsicSleepThenAutoWakeup(pAd, pAd->ThisTbttNumToNextWakeUp);
 794                 bReschedule = FALSE;
 795         }
 796 #endif
 797         return bReschedule;
 798 }
 799
 800 /*
 801         ========================================================================
 802
 803         Routine Description:
 804         Arguments:
 805                 pAd     Pointer to our adapter
 806
 807         IRQL = DISPATCH_LEVEL
 808
 809         ========================================================================
 810 */
 811 VOID    RTMPHandleTwakeupInterrupt(
 812         IN PRTMP_ADAPTER pAd)
 813 {
 814 #ifdef RT2860
 815         AsicForceWakeup(pAd, DOT11POWERSAVE);
 816 #endif
 817 #ifdef RT2870
 818         AsicForceWakeup(pAd, FALSE);
 819 #endif
 820 }
 821
 822 /*
 823 ========================================================================
 824 Routine Description:
 825     Early checking and OS-depened parsing for Tx packet send to our STA driver.
 826
 827 Arguments:
 828     NDIS_HANDLE         MiniportAdapterContext  Pointer refer to the device handle, i.e., the pAd.
 829         PPNDIS_PACKET   ppPacketArray                   The packet array need to do transmission.
 830         UINT                    NumberOfPackets                 Number of packet in packet array.
 831
 832 Return Value:
 833         NONE
 834
 835 Note:
 836         This function do early checking and classification for send-out packet.
 837         You only can put OS-depened & STA related code in here.
 838 ========================================================================
 839 */
 840 VOID STASendPackets(
 841         IN      NDIS_HANDLE             MiniportAdapterContext,
 842         IN      PPNDIS_PACKET   ppPacketArray,
 843         IN      UINT                    NumberOfPackets)
 844 {
 845         UINT                    Index;
 846         PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) MiniportAdapterContext;
 847         PNDIS_PACKET    pPacket;
 848         BOOLEAN                 allowToSend = FALSE;
 849
 850
 851         for (Index = 0; Index < NumberOfPackets; Index++)
 852         {
 853                 pPacket = ppPacketArray[Index];
 854
 855                 do
 856                 {
 857                         if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
 858                                 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
 859                                 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
 860                         {
 861                                 // Drop send request since hardware is in reset state
 862                                         break;
 863                         }
 864                         else if (!INFRA_ON(pAd) && !ADHOC_ON(pAd))
 865                         {
 866                                 // Drop send request since there are no physical connection yet
 867                                         break;
 868                         }
 869                         else
 870                         {
 871                                 // Record that orignal packet source is from NDIS layer,so that
 872                                 // later on driver knows how to release this NDIS PACKET
 873                                 RTMP_SET_PACKET_WCID(pPacket, 0); // this field is useless when in STA mode
 874                                 RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
 875                                 NDIS_SET_PACKET_STATUS(pPacket, NDIS_STATUS_PENDING);
 876                                 pAd->RalinkCounters.PendingNdisPacketCount++;
 877
 878                                 allowToSend = TRUE;
 879                         }
 880                 } while(FALSE);
 881
 882                 if (allowToSend == TRUE)
 883                         STASendPacket(pAd, pPacket);
 884                 else
 885                         RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
 886         }
 887
 888         // Dequeue outgoing frames from TxSwQueue[] and process it
 889         RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
 890
 891 }
 892
 893
 894 /*
 895 ========================================================================
 896 Routine Description:
 897         This routine is used to do packet parsing and classification for Tx packet
 898         to STA device, and it will en-queue packets to our TxSwQueue depends on AC
 899         class.
 900
 901 Arguments:
 902         pAd             Pointer to our adapter
 903         pPacket         Pointer to send packet
 904
 905 Return Value:
 906         NDIS_STATUS_SUCCESS                     If succes to queue the packet into TxSwQueue.
 907         NDIS_STATUS_FAILURE                     If failed to do en-queue.
 908
 909 Note:
 910         You only can put OS-indepened & STA related code in here.
 911 ========================================================================
 912 */
 913 NDIS_STATUS STASendPacket(
 914         IN      PRTMP_ADAPTER   pAd,
 915         IN      PNDIS_PACKET    pPacket)
 916 {
 917         PACKET_INFO     PacketInfo;
 918         PUCHAR                  pSrcBufVA;
 919         UINT                    SrcBufLen;
 920         UINT                    AllowFragSize;
 921         UCHAR                   NumberOfFrag;
 922         UCHAR                   QueIdx, UserPriority;
 923         MAC_TABLE_ENTRY *pEntry = NULL;
 924         unsigned int    IrqFlags;
 925         UCHAR                   FlgIsIP = 0;
 926         UCHAR                   Rate;
 927
 928         // Prepare packet information structure for buffer descriptor
 929         // chained within a single NDIS packet.
 930         RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
 931
 932         if (pSrcBufVA == NULL)
 933         {
 934                 DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> pSrcBufVA == NULL !!!SrcBufLen=%x\n",SrcBufLen));
 935                 // Resourece is low, system did not allocate virtual address
 936                 // return NDIS_STATUS_FAILURE directly to upper layer
 937                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
 938                 return NDIS_STATUS_FAILURE;
 939         }
 940
 941
 942         if (SrcBufLen < 14)
 943         {
 944                 DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> Ndis Packet buffer error !!!\n"));
 945                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
 946                 return (NDIS_STATUS_FAILURE);
 947         }
 948
 949         // In HT rate adhoc mode, A-MPDU is often used. So need to lookup BA Table and MAC Entry.
 950         // Note multicast packets in adhoc also use BSSID_WCID index.
 951         {
 952                 if(INFRA_ON(pAd))
 953                 {
 954                         {
 955                         pEntry = &pAd->MacTab.Content[BSSID_WCID];
 956                         RTMP_SET_PACKET_WCID(pPacket, BSSID_WCID);
 957                         Rate = pAd->CommonCfg.TxRate;
 958                 }
 959                 }
 960                 else if (ADHOC_ON(pAd))
 961                 {
 962                         if (*pSrcBufVA & 0x01)
 963                         {
 964                                 RTMP_SET_PACKET_WCID(pPacket, MCAST_WCID);
 965                                 pEntry = &pAd->MacTab.Content[MCAST_WCID];
 966                         }
 967                         else
 968                         {
 969                                 pEntry = MacTableLookup(pAd, pSrcBufVA);
 970                         }
 971                         Rate = pAd->CommonCfg.TxRate;
 972                 }
 973         }
 974
 975         if (!pEntry)
 976         {
 977                 DBGPRINT(RT_DEBUG_ERROR,("STASendPacket->Cannot find pEntry(%2x:%2x:%2x:%2x:%2x:%2x) in MacTab!\n", PRINT_MAC(pSrcBufVA)));
 978                 // Resourece is low, system did not allocate virtual address
 979                 // return NDIS_STATUS_FAILURE directly to upper layer
 980                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
 981                 return NDIS_STATUS_FAILURE;
 982         }
 983
 984         if (ADHOC_ON(pAd)
 985                 )
 986         {
 987                 RTMP_SET_PACKET_WCID(pPacket, (UCHAR)pEntry->Aid);
 988         }
 989
 990         //
 991         // Check the Ethernet Frame type of this packet, and set the RTMP_SET_PACKET_SPECIFIC flags.
 992         //              Here we set the PACKET_SPECIFIC flags(LLC, VLAN, DHCP/ARP, EAPOL).
 993         RTMPCheckEtherType(pAd, pPacket);
 994
 995
 996
 997         //
 998         // WPA 802.1x secured port control - drop all non-802.1x frame before port secured
 999         //
1000         if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
1001                  (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
1002                  (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
1003                  (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
1004                   || (pAd->StaCfg.IEEE8021X == TRUE)
1005                   )
1006                   && ((pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED) || (pAd->StaCfg.MicErrCnt >= 2))
1007                   && (RTMP_GET_PACKET_EAPOL(pPacket)== FALSE)
1008                   )
1009         {
1010                 DBGPRINT(RT_DEBUG_TRACE,("STASendPacket --> Drop packet before port secured !!!\n"));
1011                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
1012
1013                 return (NDIS_STATUS_FAILURE);
1014         }
1015
1016
1017         // STEP 1. Decide number of fragments required to deliver this MSDU.
1018         //         The estimation here is not very accurate because difficult to
1019         //         take encryption overhead into consideration here. The result
1020         //         "NumberOfFrag" is then just used to pre-check if enough free
1021         //         TXD are available to hold this MSDU.
1022
1023
1024         if (*pSrcBufVA & 0x01)  // fragmentation not allowed on multicast & broadcast
1025                 NumberOfFrag = 1;
1026         else if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED))
1027                 NumberOfFrag = 1;       // Aggregation overwhelms fragmentation
1028         else if (CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED))
1029                 NumberOfFrag = 1;       // Aggregation overwhelms fragmentation
1030         else if ((pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTMIX) || (pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTGREENFIELD))
1031                 NumberOfFrag = 1;       // MIMO RATE overwhelms fragmentation
1032         else
1033         {
1034                 // The calculated "NumberOfFrag" is a rough estimation because of various
1035                 // encryption/encapsulation overhead not taken into consideration. This number is just
1036                 // used to make sure enough free TXD are available before fragmentation takes place.
1037                 // In case the actual required number of fragments of an NDIS packet
1038                 // excceeds "NumberOfFrag"caculated here and not enough free TXD available, the
1039                 // last fragment (i.e. last MPDU) will be dropped in RTMPHardTransmit() due to out of
1040                 // resource, and the NDIS packet will be indicated NDIS_STATUS_FAILURE. This should
1041                 // rarely happen and the penalty is just like a TX RETRY fail. Affordable.
1042
1043                 AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
1044                 NumberOfFrag = ((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) / AllowFragSize) + 1;
1045                 // To get accurate number of fragmentation, Minus 1 if the size just match to allowable fragment size
1046                 if (((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) % AllowFragSize) == 0)
1047                 {
1048                         NumberOfFrag--;
1049                 }
1050         }
1051
1052         // Save fragment number to Ndis packet reserved field
1053         RTMP_SET_PACKET_FRAGMENTS(pPacket, NumberOfFrag);
1054
1055
1056         // STEP 2. Check the requirement of RTS:
1057         //         If multiple fragment required, RTS is required only for the first fragment
1058         //         if the fragment size large than RTS threshold
1059         //     For RT28xx, Let ASIC send RTS/CTS
1060         RTMP_SET_PACKET_RTS(pPacket, 0);
1061         RTMP_SET_PACKET_TXRATE(pPacket, pAd->CommonCfg.TxRate);
1062
1063         //
1064         // STEP 3. Traffic classification. outcome = <UserPriority, QueIdx>
1065         //
1066         UserPriority = 0;
1067         QueIdx           = QID_AC_BE;
1068 #ifdef RT2860
1069         if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED))
1070 #endif
1071 #ifdef RT2870
1072         if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) &&
1073                 CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE))
1074 #endif
1075         {
1076                 USHORT Protocol;
1077                 UCHAR  LlcSnapLen = 0, Byte0, Byte1;
1078                 do
1079                 {
1080                         // get Ethernet protocol field
1081                         Protocol = (USHORT)((pSrcBufVA[12] << 8) + pSrcBufVA[13]);
1082                         if (Protocol <= 1500)
1083                         {
1084                                 // get Ethernet protocol field from LLC/SNAP
1085                                 if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + 6, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
1086                                         break;
1087
1088                                 Protocol = (USHORT)((Byte0 << 8) + Byte1);
1089                                 LlcSnapLen = 8;
1090                         }
1091
1092                         // always AC_BE for non-IP packet
1093                         if (Protocol != 0x0800)
1094                                 break;
1095
1096                         // get IP header
1097                         if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + LlcSnapLen, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
1098                                 break;
1099
1100                         // return AC_BE if packet is not IPv4
1101                         if ((Byte0 & 0xf0) != 0x40)
1102                                 break;
1103
1104                         FlgIsIP = 1;
1105                         UserPriority = (Byte1 & 0xe0) >> 5;
1106                         QueIdx = MapUserPriorityToAccessCategory[UserPriority];
1107
1108                         // TODO: have to check ACM bit. apply TSPEC if ACM is ON
1109                         // TODO: downgrade UP & QueIdx before passing ACM
1110                         if (pAd->CommonCfg.APEdcaParm.bACM[QueIdx])
1111                         {
1112                                 UserPriority = 0;
1113                                 QueIdx           = QID_AC_BE;
1114                         }
1115                 } while (FALSE);
1116         }
1117
1118         RTMP_SET_PACKET_UP(pPacket, UserPriority);
1119
1120
1121
1122         // Make sure SendTxWait queue resource won't be used by other threads
1123         RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
1124         if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
1125         {
1126                 RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
1127                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
1128
1129                 return NDIS_STATUS_FAILURE;
1130         }
1131         else
1132         {
1133                 InsertTailQueue(&pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pPacket));
1134         }
1135         RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
1136
1137     if ((pAd->CommonCfg.BACapability.field.AutoBA == TRUE)&&
1138 #ifdef RT2860
1139         (pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE))
1140 #endif
1141 #ifdef RT2870
1142         IS_HT_STA(pEntry))
1143 #endif
1144         {
1145                 if (((pEntry->TXBAbitmap & (1<<UserPriority)) == 0) &&
1146             ((pEntry->BADeclineBitmap & (1<<UserPriority)) == 0) &&
1147             (pEntry->PortSecured == WPA_802_1X_PORT_SECURED)
1148                          // For IOT compatibility, if
1149                          // 1. It is Ralink chip or
1150                          // 2. It is OPEN or AES mode,
1151                          // then BA session can be bulit.
1152                          && ((pEntry->ValidAsCLI && pAd->MlmeAux.APRalinkIe != 0x0) ||
1153                                  (pEntry->WepStatus == Ndis802_11WEPDisabled || pEntry->WepStatus == Ndis802_11Encryption3Enabled))
1154                         )
1155                 {
1156                         BAOriSessionSetUp(pAd, pEntry, 0, 0, 10, FALSE);
1157                 }
1158         }
1159
1160         pAd->RalinkCounters.OneSecOsTxCount[QueIdx]++; // TODO: for debug only. to be removed
1161         return NDIS_STATUS_SUCCESS;
1162 }
1163
1164
1165 /*
1166         ========================================================================
1167
1168         Routine Description:
1169                 This subroutine will scan through releative ring descriptor to find
1170                 out avaliable free ring descriptor and compare with request size.
1171
1172         Arguments:
1173                 pAd Pointer to our adapter
1174                 QueIdx          Selected TX Ring
1175
1176         Return Value:
1177                 NDIS_STATUS_FAILURE     Not enough free descriptor
1178                 NDIS_STATUS_SUCCESS     Enough free descriptor
1179
1180         IRQL = PASSIVE_LEVEL
1181         IRQL = DISPATCH_LEVEL
1182
1183         Note:
1184
1185         ========================================================================
1186 */
1187
1188 #ifdef RT2870
1189 /*
1190         Actually, this function used to check if the TxHardware Queue still has frame need to send.
1191         If no frame need to send, go to sleep, else, still wake up.
1192 */
1193 #endif
1194 NDIS_STATUS RTMPFreeTXDRequest(
1195         IN              PRTMP_ADAPTER   pAd,
1196         IN              UCHAR                   QueIdx,
1197         IN              UCHAR                   NumberRequired,
1198         IN              PUCHAR                  FreeNumberIs)
1199 {
1200 #ifdef RT2860
1201         ULONG           FreeNumber = 0;
1202 #endif
1203         NDIS_STATUS     Status = NDIS_STATUS_FAILURE;
1204 #ifdef RT2870
1205         unsigned long   IrqFlags;
1206         HT_TX_CONTEXT   *pHTTXContext;
1207 #endif
1208
1209         switch (QueIdx)
1210         {
1211                 case QID_AC_BK:
1212                 case QID_AC_BE:
1213                 case QID_AC_VI:
1214                 case QID_AC_VO:
1215                 case QID_HCCA:
1216 #ifdef RT2860
1217                         if (pAd->TxRing[QueIdx].TxSwFreeIdx > pAd->TxRing[QueIdx].TxCpuIdx)
1218                                 FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx - pAd->TxRing[QueIdx].TxCpuIdx - 1;
1219                         else
1220                                 FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx + TX_RING_SIZE - pAd->TxRing[QueIdx].TxCpuIdx - 1;
1221
1222                         if (FreeNumber >= NumberRequired)
1223                                 Status = NDIS_STATUS_SUCCESS;
1224 #endif
1225 #ifdef RT2870
1226                         {
1227                                 pHTTXContext = &pAd->TxContext[QueIdx];
1228                                 RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
1229                                 if ((pHTTXContext->CurWritePosition != pHTTXContext->ENextBulkOutPosition) ||
1230                                         (pHTTXContext->IRPPending == TRUE))
1231                                 {
1232                                         Status = NDIS_STATUS_FAILURE;
1233                                 }
1234                                 else
1235                                 {
1236                                         Status = NDIS_STATUS_SUCCESS;
1237                                 }
1238                                 RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
1239                         }
1240 #endif
1241                         break;
1242
1243                 case QID_MGMT:
1244 #ifdef RT2860
1245                         if (pAd->MgmtRing.TxSwFreeIdx > pAd->MgmtRing.TxCpuIdx)
1246                                 FreeNumber = pAd->MgmtRing.TxSwFreeIdx - pAd->MgmtRing.TxCpuIdx - 1;
1247                         else
1248                                 FreeNumber = pAd->MgmtRing.TxSwFreeIdx + MGMT_RING_SIZE - pAd->MgmtRing.TxCpuIdx - 1;
1249
1250                         if (FreeNumber >= NumberRequired)
1251                                 Status = NDIS_STATUS_SUCCESS;
1252 #endif
1253 #ifdef RT2870
1254                         if (pAd->MgmtRing.TxSwFreeIdx != MGMT_RING_SIZE)
1255                                 Status = NDIS_STATUS_FAILURE;
1256                         else
1257                                 Status = NDIS_STATUS_SUCCESS;
1258 #endif
1259                         break;
1260
1261                 default:
1262                         DBGPRINT(RT_DEBUG_ERROR,("RTMPFreeTXDRequest::Invalid QueIdx(=%d)\n", QueIdx));
1263                         break;
1264         }
1265 #ifdef RT2860
1266         *FreeNumberIs = (UCHAR)FreeNumber;
1267 #endif
1268
1269         return (Status);
1270 }
1271
1272
1273
1274 VOID RTMPSendDisassociationFrame(
1275         IN      PRTMP_ADAPTER   pAd)
1276 {
1277 }
1278
1279 VOID    RTMPSendNullFrame(
1280         IN      PRTMP_ADAPTER   pAd,
1281         IN      UCHAR                   TxRate,
1282         IN      BOOLEAN                 bQosNull)
1283 {
1284         UCHAR   NullFrame[48];
1285         ULONG   Length;
1286         PHEADER_802_11  pHeader_802_11;
1287
1288     // WPA 802.1x secured port control
1289     if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
1290          (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
1291          (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
1292          (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
1293                           || (pAd->StaCfg.IEEE8021X == TRUE)
1294         ) &&
1295        (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
1296         {
1297                 return;
1298         }
1299
1300         NdisZeroMemory(NullFrame, 48);
1301         Length = sizeof(HEADER_802_11);
1302
1303         pHeader_802_11 = (PHEADER_802_11) NullFrame;
1304
1305         pHeader_802_11->FC.Type = BTYPE_DATA;
1306         pHeader_802_11->FC.SubType = SUBTYPE_NULL_FUNC;
1307         pHeader_802_11->FC.ToDs = 1;
1308         COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
1309         COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
1310         COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
1311
1312         if (pAd->CommonCfg.bAPSDForcePowerSave)
1313         {
1314                 pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
1315         }
1316         else
1317         {
1318                 pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE) ? 1: 0;
1319         }
1320         pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + RTMPCalcDuration(pAd, TxRate, 14);
1321
1322         pAd->Sequence++;
1323         pHeader_802_11->Sequence = pAd->Sequence;
1324
1325         // Prepare QosNull function frame
1326         if (bQosNull)
1327         {
1328                 pHeader_802_11->FC.SubType = SUBTYPE_QOS_NULL;
1329
1330                 // copy QOS control bytes
1331                 NullFrame[Length]       =  0;
1332                 NullFrame[Length+1] =  0;
1333                 Length += 2;// if pad with 2 bytes for alignment, APSD will fail
1334         }
1335
1336         HAL_KickOutNullFrameTx(pAd, 0, NullFrame, Length);
1337
1338 }
1339
1340 // IRQL = DISPATCH_LEVEL
1341 VOID    RTMPSendRTSFrame(
1342         IN      PRTMP_ADAPTER   pAd,
1343         IN      PUCHAR                  pDA,
1344         IN      unsigned int    NextMpduSize,
1345         IN      UCHAR                   TxRate,
1346         IN      UCHAR                   RTSRate,
1347         IN      USHORT                  AckDuration,
1348         IN      UCHAR                   QueIdx,
1349         IN      UCHAR                   FrameGap)
1350 {
1351 }
1352
1353
1354
1355 // --------------------------------------------------------
1356 //  FIND ENCRYPT KEY AND DECIDE CIPHER ALGORITHM
1357 //              Find the WPA key, either Group or Pairwise Key
1358 //              LEAP + TKIP also use WPA key.
1359 // --------------------------------------------------------
1360 // Decide WEP bit and cipher suite to be used. Same cipher suite should be used for whole fragment burst
1361 // In Cisco CCX 2.0 Leap Authentication
1362 //                 WepStatus is Ndis802_11Encryption1Enabled but the key will use PairwiseKey
1363 //                 Instead of the SharedKey, SharedKey Length may be Zero.
1364 VOID STAFindCipherAlgorithm(
1365         IN      PRTMP_ADAPTER   pAd,
1366         IN      TX_BLK                  *pTxBlk)
1367 {
1368         NDIS_802_11_ENCRYPTION_STATUS   Cipher;                         // To indicate cipher used for this packet
1369         UCHAR                                                   CipherAlg = CIPHER_NONE;                // cipher alogrithm
1370         UCHAR                                                   KeyIdx = 0xff;
1371         PUCHAR                                                  pSrcBufVA;
1372         PCIPHER_KEY                                             pKey = NULL;
1373
1374         pSrcBufVA = GET_OS_PKT_DATAPTR(pTxBlk->pPacket);
1375
1376         {
1377             // Select Cipher
1378             if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
1379                 Cipher = pAd->StaCfg.GroupCipher; // Cipher for Multicast or Broadcast
1380             else
1381                 Cipher = pAd->StaCfg.PairCipher; // Cipher for Unicast
1382
1383                 if (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
1384                 {
1385                         ASSERT(pAd->SharedKey[BSS0][0].CipherAlg <= CIPHER_CKIP128);
1386
1387                         // 4-way handshaking frame must be clear
1388                         if (!(TX_BLK_TEST_FLAG(pTxBlk, fTX_bClearEAPFrame)) && (pAd->SharedKey[BSS0][0].CipherAlg) &&
1389                                 (pAd->SharedKey[BSS0][0].KeyLen))
1390                         {
1391                                 CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
1392                                 KeyIdx = 0;
1393                         }
1394                 }
1395                 else if (Cipher == Ndis802_11Encryption1Enabled)
1396                 {
1397                                 KeyIdx = pAd->StaCfg.DefaultKeyId;
1398                 }
1399                 else if ((Cipher == Ndis802_11Encryption2Enabled) ||
1400                                  (Cipher == Ndis802_11Encryption3Enabled))
1401                 {
1402                         if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))) // multicast
1403                                 KeyIdx = pAd->StaCfg.DefaultKeyId;
1404                         else if (pAd->SharedKey[BSS0][0].KeyLen)
1405                                 KeyIdx = 0;
1406                         else
1407                                 KeyIdx = pAd->StaCfg.DefaultKeyId;
1408                 }
1409
1410                 if (KeyIdx == 0xff)
1411                         CipherAlg = CIPHER_NONE;
1412                 else if ((Cipher == Ndis802_11EncryptionDisabled) || (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 0))
1413                         CipherAlg = CIPHER_NONE;
1414             else if ( pAd->StaCfg.WpaSupplicantUP &&
1415                      (Cipher == Ndis802_11Encryption1Enabled) &&
1416                      (pAd->StaCfg.IEEE8021X == TRUE) &&
1417                      (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
1418                 CipherAlg = CIPHER_NONE;
1419                 else
1420                 {
1421                         //Header_802_11.FC.Wep = 1;
1422                         CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
1423                         pKey = &pAd->SharedKey[BSS0][KeyIdx];
1424                 }
1425         }
1426
1427         pTxBlk->CipherAlg = CipherAlg;
1428         pTxBlk->pKey = pKey;
1429 }
1430
1431
1432 VOID STABuildCommon802_11Header(
1433         IN  PRTMP_ADAPTER   pAd,
1434         IN  TX_BLK          *pTxBlk)
1435 {
1436         HEADER_802_11   *pHeader_802_11;
1437
1438         //
1439         // MAKE A COMMON 802.11 HEADER
1440         //
1441
1442         // normal wlan header size : 24 octets
1443         pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
1444
1445         pHeader_802_11 = (HEADER_802_11 *) &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1446
1447         NdisZeroMemory(pHeader_802_11, sizeof(HEADER_802_11));
1448
1449         pHeader_802_11->FC.FrDs = 0;
1450         pHeader_802_11->FC.Type = BTYPE_DATA;
1451         pHeader_802_11->FC.SubType = ((TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM)) ? SUBTYPE_QDATA : SUBTYPE_DATA);
1452
1453     if (pTxBlk->pMacEntry)
1454         {
1455                 if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bForceNonQoS))
1456                 {
1457                         pHeader_802_11->Sequence = pTxBlk->pMacEntry->NonQosDataSeq;
1458                         pTxBlk->pMacEntry->NonQosDataSeq = (pTxBlk->pMacEntry->NonQosDataSeq+1) & MAXSEQ;
1459                 }
1460                 else
1461                 {
1462                         {
1463             pHeader_802_11->Sequence = pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority];
1464             pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority] = (pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
1465         }
1466         }
1467         }
1468         else
1469         {
1470                 pHeader_802_11->Sequence = pAd->Sequence;
1471                 pAd->Sequence = (pAd->Sequence+1) & MAXSEQ; // next sequence
1472         }
1473
1474         pHeader_802_11->Frag = 0;
1475
1476         pHeader_802_11->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
1477
1478         {
1479                 if (INFRA_ON(pAd))
1480                 {
1481                         {
1482                         COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
1483                         COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
1484                         COPY_MAC_ADDR(pHeader_802_11->Addr3, pTxBlk->pSrcBufHeader);
1485                         pHeader_802_11->FC.ToDs = 1;
1486                 }
1487                 }
1488                 else if (ADHOC_ON(pAd))
1489                 {
1490                         COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
1491                         COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
1492                         COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
1493                         pHeader_802_11->FC.ToDs = 0;
1494                 }
1495         }
1496
1497         if (pTxBlk->CipherAlg != CIPHER_NONE)
1498                 pHeader_802_11->FC.Wep = 1;
1499
1500         // -----------------------------------------------------------------
1501         // STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
1502         // -----------------------------------------------------------------
1503         if (pAd->CommonCfg.bAPSDForcePowerSave)
1504         pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
1505         else
1506         pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
1507 }
1508
1509 VOID STABuildCache802_11Header(
1510         IN RTMP_ADAPTER         *pAd,
1511         IN TX_BLK                       *pTxBlk,
1512         IN UCHAR                        *pHeader)
1513 {
1514         MAC_TABLE_ENTRY *pMacEntry;
1515         PHEADER_802_11  pHeader80211;
1516
1517         pHeader80211 = (PHEADER_802_11)pHeader;
1518         pMacEntry = pTxBlk->pMacEntry;
1519
1520         //
1521         // Update the cached 802.11 HEADER
1522         //
1523
1524         // normal wlan header size : 24 octets
1525         pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
1526
1527         // More Bit
1528         pHeader80211->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
1529
1530         // Sequence
1531         pHeader80211->Sequence = pMacEntry->TxSeq[pTxBlk->UserPriority];
1532     pMacEntry->TxSeq[pTxBlk->UserPriority] = (pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
1533
1534         {
1535                 // The addr3 of normal packet send from DS is Dest Mac address.
1536                 if (ADHOC_ON(pAd))
1537                         COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
1538                 else
1539                         COPY_MAC_ADDR(pHeader80211->Addr3, pTxBlk->pSrcBufHeader);
1540         }
1541
1542         // -----------------------------------------------------------------
1543         // STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
1544         // -----------------------------------------------------------------
1545         if (pAd->CommonCfg.bAPSDForcePowerSave)
1546         pHeader80211->FC.PwrMgmt = PWR_SAVE;
1547         else
1548         pHeader80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
1549 }
1550
1551 static inline PUCHAR STA_Build_ARalink_Frame_Header(
1552         IN RTMP_ADAPTER *pAd,
1553         IN TX_BLK               *pTxBlk)
1554 {
1555         PUCHAR                  pHeaderBufPtr;
1556         HEADER_802_11   *pHeader_802_11;
1557         PNDIS_PACKET    pNextPacket;
1558         UINT32                  nextBufLen;
1559         PQUEUE_ENTRY    pQEntry;
1560
1561         STAFindCipherAlgorithm(pAd, pTxBlk);
1562         STABuildCommon802_11Header(pAd, pTxBlk);
1563
1564
1565         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1566         pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
1567
1568         // steal "order" bit to mark "aggregation"
1569         pHeader_802_11->FC.Order = 1;
1570
1571         // skip common header
1572         pHeaderBufPtr += pTxBlk->MpduHeaderLen;
1573
1574         if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
1575         {
1576                 //
1577                 // build QOS Control bytes
1578                 //
1579                 *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
1580
1581                 *(pHeaderBufPtr+1) = 0;
1582                 pHeaderBufPtr +=2;
1583                 pTxBlk->MpduHeaderLen += 2;
1584         }
1585
1586         // padding at front of LLC header. LLC header should at 4-bytes aligment.
1587         pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
1588         pHeaderBufPtr = (PCHAR)ROUND_UP(pHeaderBufPtr, 4);
1589         pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
1590
1591         // For RA Aggregation,
1592         // put the 2nd MSDU length(extra 2-byte field) after QOS_CONTROL in little endian format
1593         pQEntry = pTxBlk->TxPacketList.Head;
1594         pNextPacket = QUEUE_ENTRY_TO_PKT(pQEntry);
1595         nextBufLen = GET_OS_PKT_LEN(pNextPacket);
1596         if (RTMP_GET_PACKET_VLAN(pNextPacket))
1597                 nextBufLen -= LENGTH_802_1Q;
1598
1599         *pHeaderBufPtr = (UCHAR)nextBufLen & 0xff;
1600         *(pHeaderBufPtr+1) = (UCHAR)(nextBufLen >> 8);
1601
1602         pHeaderBufPtr += 2;
1603         pTxBlk->MpduHeaderLen += 2;
1604
1605         return pHeaderBufPtr;
1606
1607 }
1608
1609 static inline PUCHAR STA_Build_AMSDU_Frame_Header(
1610         IN RTMP_ADAPTER *pAd,
1611         IN TX_BLK               *pTxBlk)
1612 {
1613         PUCHAR                  pHeaderBufPtr;//, pSaveBufPtr;
1614         HEADER_802_11   *pHeader_802_11;
1615
1616
1617         STAFindCipherAlgorithm(pAd, pTxBlk);
1618         STABuildCommon802_11Header(pAd, pTxBlk);
1619
1620         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1621         pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
1622
1623         // skip common header
1624         pHeaderBufPtr += pTxBlk->MpduHeaderLen;
1625
1626         //
1627         // build QOS Control bytes
1628         //
1629         *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
1630
1631         //
1632         // A-MSDU packet
1633         //
1634         *pHeaderBufPtr |= 0x80;
1635
1636         *(pHeaderBufPtr+1) = 0;
1637         pHeaderBufPtr +=2;
1638         pTxBlk->MpduHeaderLen += 2;
1639
1640         //pSaveBufPtr = pHeaderBufPtr;
1641
1642         //
1643         // padding at front of LLC header
1644         // LLC header should locate at 4-octets aligment
1645         //
1646         // @@@ MpduHeaderLen excluding padding @@@
1647         //
1648         pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
1649         pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
1650         pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
1651
1652         return pHeaderBufPtr;
1653
1654 }
1655
1656
1657 VOID STA_AMPDU_Frame_Tx(
1658         IN      PRTMP_ADAPTER   pAd,
1659         IN      TX_BLK                  *pTxBlk)
1660 {
1661         HEADER_802_11   *pHeader_802_11;
1662         PUCHAR                  pHeaderBufPtr;
1663         USHORT                  FreeNumber;
1664         MAC_TABLE_ENTRY *pMacEntry;
1665         BOOLEAN                 bVLANPkt;
1666         PQUEUE_ENTRY    pQEntry;
1667
1668         ASSERT(pTxBlk);
1669
1670         while(pTxBlk->TxPacketList.Head)
1671         {
1672                 pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
1673                 pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
1674                 if ( RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
1675                 {
1676                         RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
1677                         continue;
1678                 }
1679
1680                 bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
1681
1682                 pMacEntry = pTxBlk->pMacEntry;
1683                 if (pMacEntry->isCached)
1684                 {
1685                         // NOTE: Please make sure the size of pMacEntry->CachedBuf[] is smaller than pTxBlk->HeaderBuf[]!!!!
1686                         NdisMoveMemory((PUCHAR)&pTxBlk->HeaderBuf[TXINFO_SIZE], (PUCHAR)&pMacEntry->CachedBuf[0], TXWI_SIZE + sizeof(HEADER_802_11));
1687                         pHeaderBufPtr = (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE]);
1688                         STABuildCache802_11Header(pAd, pTxBlk, pHeaderBufPtr);
1689                 }
1690                 else
1691                 {
1692                         STAFindCipherAlgorithm(pAd, pTxBlk);
1693                         STABuildCommon802_11Header(pAd, pTxBlk);
1694
1695                         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1696                 }
1697
1698
1699                 pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
1700
1701                 // skip common header
1702                 pHeaderBufPtr += pTxBlk->MpduHeaderLen;
1703
1704                 //
1705                 // build QOS Control bytes
1706                 //
1707                 *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
1708                 *(pHeaderBufPtr+1) = 0;
1709                 pHeaderBufPtr +=2;
1710                 pTxBlk->MpduHeaderLen += 2;
1711
1712                 //
1713                 // build HTC+
1714                 // HTC control filed following QoS field
1715                 //
1716                 if ((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pTxBlk->pMacEntry, fCLIENT_STATUS_RDG_CAPABLE))
1717                 {
1718                         if (pMacEntry->isCached == FALSE)
1719                         {
1720                                 // mark HTC bit
1721                                 pHeader_802_11->FC.Order = 1;
1722
1723                                 NdisZeroMemory(pHeaderBufPtr, 4);
1724                                 *(pHeaderBufPtr+3) |= 0x80;
1725                         }
1726                         pHeaderBufPtr += 4;
1727                         pTxBlk->MpduHeaderLen += 4;
1728                 }
1729
1730                 //pTxBlk->MpduHeaderLen = pHeaderBufPtr - pTxBlk->HeaderBuf - TXWI_SIZE - TXINFO_SIZE;
1731                 ASSERT(pTxBlk->MpduHeaderLen >= 24);
1732
1733                 // skip 802.3 header
1734                 pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
1735                 pTxBlk->SrcBufLen  -= LENGTH_802_3;
1736
1737                 // skip vlan tag
1738                 if (bVLANPkt)
1739                 {
1740                         pTxBlk->pSrcBufData     += LENGTH_802_1Q;
1741                         pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
1742                 }
1743
1744                 //
1745                 // padding at front of LLC header
1746                 // LLC header should locate at 4-octets aligment
1747                 //
1748                 // @@@ MpduHeaderLen excluding padding @@@
1749                 //
1750                 pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
1751                 pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
1752                 pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
1753
1754                 {
1755
1756                         //
1757                         // Insert LLC-SNAP encapsulation - 8 octets
1758                         //
1759                         EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
1760                         if (pTxBlk->pExtraLlcSnapEncap)
1761                         {
1762                                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
1763                                 pHeaderBufPtr += 6;
1764                                 // get 2 octets (TypeofLen)
1765                                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
1766                                 pHeaderBufPtr += 2;
1767                                 pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
1768                         }
1769
1770                 }
1771
1772                 if (pMacEntry->isCached)
1773                 {
1774             RTMPWriteTxWI_Cache(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
1775                 }
1776                 else
1777                 {
1778                         RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
1779
1780                         NdisZeroMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), sizeof(pMacEntry->CachedBuf));
1781                         NdisMoveMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), (pHeaderBufPtr - (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE])));
1782                         pMacEntry->isCached = TRUE;
1783                 }
1784
1785                 // calculate Transmitted AMPDU count and ByteCount
1786                 {
1787                         pAd->RalinkCounters.TransmittedMPDUsInAMPDUCount.u.LowPart ++;
1788                         pAd->RalinkCounters.TransmittedOctetsInAMPDUCount.QuadPart += pTxBlk->SrcBufLen;
1789                 }
1790
1791                 //FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
1792
1793                 HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
1794
1795                 //
1796                 // Kick out Tx
1797                 //
1798 #ifdef RT2860
1799                 if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
1800 #endif
1801                         HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
1802
1803                 pAd->RalinkCounters.KickTxCount++;
1804                 pAd->RalinkCounters.OneSecTxDoneCount++;
1805         }
1806
1807 }
1808
1809
1810 VOID STA_AMSDU_Frame_Tx(
1811         IN      PRTMP_ADAPTER   pAd,
1812         IN      TX_BLK                  *pTxBlk)
1813 {
1814         PUCHAR                  pHeaderBufPtr;
1815         USHORT                  FreeNumber;
1816         USHORT                  subFramePayloadLen = 0; // AMSDU Subframe length without AMSDU-Header / Padding.
1817         USHORT                  totalMPDUSize=0;
1818         UCHAR                   *subFrameHeader;
1819         UCHAR                   padding = 0;
1820         USHORT                  FirstTx = 0, LastTxIdx = 0;
1821         BOOLEAN                 bVLANPkt;
1822         int                     frameNum = 0;
1823         PQUEUE_ENTRY    pQEntry;
1824
1825
1826         ASSERT(pTxBlk);
1827
1828         ASSERT((pTxBlk->TxPacketList.Number > 1));
1829
1830         while(pTxBlk->TxPacketList.Head)
1831         {
1832                 pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
1833                 pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
1834                 if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
1835                 {
1836                         RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
1837                         continue;
1838                 }
1839
1840                 bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
1841
1842                 // skip 802.3 header
1843                 pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
1844                 pTxBlk->SrcBufLen  -= LENGTH_802_3;
1845
1846                 // skip vlan tag
1847                 if (bVLANPkt)
1848                 {
1849                         pTxBlk->pSrcBufData     += LENGTH_802_1Q;
1850                         pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
1851                 }
1852
1853                 if (frameNum == 0)
1854                 {
1855                         pHeaderBufPtr = STA_Build_AMSDU_Frame_Header(pAd, pTxBlk);
1856
1857                         // NOTE: TxWI->MPDUtotalByteCount will be updated after final frame was handled.
1858                         RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
1859                 }
1860                 else
1861                 {
1862                         pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
1863                         padding = ROUND_UP(LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen, 4) - (LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen);
1864                         NdisZeroMemory(pHeaderBufPtr, padding + LENGTH_AMSDU_SUBFRAMEHEAD);
1865                         pHeaderBufPtr += padding;
1866                         pTxBlk->MpduHeaderLen = padding;
1867                 }
1868
1869                 //
1870                 // A-MSDU subframe
1871                 //   DA(6)+SA(6)+Length(2) + LLC/SNAP Encap
1872                 //
1873                 subFrameHeader = pHeaderBufPtr;
1874                 subFramePayloadLen = pTxBlk->SrcBufLen;
1875
1876                 NdisMoveMemory(subFrameHeader, pTxBlk->pSrcBufHeader, 12);
1877
1878
1879                 pHeaderBufPtr += LENGTH_AMSDU_SUBFRAMEHEAD;
1880                 pTxBlk->MpduHeaderLen += LENGTH_AMSDU_SUBFRAMEHEAD;
1881
1882
1883                 //
1884                 // Insert LLC-SNAP encapsulation - 8 octets
1885                 //
1886                 EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
1887
1888                 subFramePayloadLen = pTxBlk->SrcBufLen;
1889
1890                 if (pTxBlk->pExtraLlcSnapEncap)
1891                 {
1892                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
1893                         pHeaderBufPtr += 6;
1894                         // get 2 octets (TypeofLen)
1895                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
1896                         pHeaderBufPtr += 2;
1897                         pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
1898                         subFramePayloadLen += LENGTH_802_1_H;
1899                 }
1900
1901                 // update subFrame Length field
1902                 subFrameHeader[12] = (subFramePayloadLen & 0xFF00) >> 8;
1903                 subFrameHeader[13] = subFramePayloadLen & 0xFF;
1904
1905                 totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
1906
1907                 if (frameNum ==0)
1908                         FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
1909                 else
1910                         LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
1911
1912                 frameNum++;
1913
1914                 pAd->RalinkCounters.KickTxCount++;
1915                 pAd->RalinkCounters.OneSecTxDoneCount++;
1916
1917                 // calculate Transmitted AMSDU Count and ByteCount
1918                 {
1919                         pAd->RalinkCounters.TransmittedAMSDUCount.u.LowPart ++;
1920                         pAd->RalinkCounters.TransmittedOctetsInAMSDU.QuadPart += totalMPDUSize;
1921                 }
1922
1923         }
1924
1925         HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
1926         HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
1927
1928         //
1929         // Kick out Tx
1930         //
1931 #ifdef RT2860
1932         if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
1933 #endif
1934                 HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
1935 }
1936
1937 VOID STA_Legacy_Frame_Tx(
1938         IN      PRTMP_ADAPTER   pAd,
1939         IN      TX_BLK                  *pTxBlk)
1940 {
1941         HEADER_802_11   *pHeader_802_11;
1942         PUCHAR                  pHeaderBufPtr;
1943         USHORT                  FreeNumber;
1944         BOOLEAN                 bVLANPkt;
1945         PQUEUE_ENTRY    pQEntry;
1946
1947         ASSERT(pTxBlk);
1948
1949
1950         pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
1951         pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
1952         if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
1953         {
1954                 RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
1955                 return;
1956         }
1957
1958         if (pTxBlk->TxFrameType == TX_MCAST_FRAME)
1959         {
1960                 INC_COUNTER64(pAd->WlanCounters.MulticastTransmittedFrameCount);
1961         }
1962
1963         if (RTMP_GET_PACKET_RTS(pTxBlk->pPacket))
1964                 TX_BLK_SET_FLAG(pTxBlk, fTX_bRtsRequired);
1965         else
1966                 TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bRtsRequired);
1967
1968         bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
1969
1970         if (pTxBlk->TxRate < pAd->CommonCfg.MinTxRate)
1971                 pTxBlk->TxRate = pAd->CommonCfg.MinTxRate;
1972
1973         STAFindCipherAlgorithm(pAd, pTxBlk);
1974         STABuildCommon802_11Header(pAd, pTxBlk);
1975
1976
1977         // skip 802.3 header
1978         pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
1979         pTxBlk->SrcBufLen  -= LENGTH_802_3;
1980
1981         // skip vlan tag
1982         if (bVLANPkt)
1983         {
1984                 pTxBlk->pSrcBufData     += LENGTH_802_1Q;
1985                 pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
1986         }
1987
1988         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
1989         pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
1990
1991         // skip common header
1992         pHeaderBufPtr += pTxBlk->MpduHeaderLen;
1993
1994         if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
1995         {
1996                 //
1997                 // build QOS Control bytes
1998                 //
1999                 *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
2000                 *(pHeaderBufPtr+1) = 0;
2001                 pHeaderBufPtr +=2;
2002                 pTxBlk->MpduHeaderLen += 2;
2003         }
2004
2005         // The remaining content of MPDU header should locate at 4-octets aligment
2006         pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
2007         pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
2008         pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
2009
2010         {
2011
2012                 //
2013                 // Insert LLC-SNAP encapsulation - 8 octets
2014                 //
2015                 //
2016                 // if original Ethernet frame contains no LLC/SNAP,
2017                 // then an extra LLC/SNAP encap is required
2018                 //
2019                 EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
2020                 if (pTxBlk->pExtraLlcSnapEncap)
2021                 {
2022                         UCHAR vlan_size;
2023
2024                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
2025                         pHeaderBufPtr += 6;
2026                         // skip vlan tag
2027                         vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
2028                         // get 2 octets (TypeofLen)
2029                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
2030                         pHeaderBufPtr += 2;
2031                         pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
2032                 }
2033
2034         }
2035
2036         //
2037         // prepare for TXWI
2038         // use Wcid as Key Index
2039         //
2040
2041         RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
2042
2043         //FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
2044
2045         HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
2046
2047         pAd->RalinkCounters.KickTxCount++;
2048         pAd->RalinkCounters.OneSecTxDoneCount++;
2049
2050         //
2051         // Kick out Tx
2052         //
2053 #ifdef RT2860
2054         if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
2055 #endif
2056                 HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
2057 }
2058
2059
2060 VOID STA_ARalink_Frame_Tx(
2061         IN      PRTMP_ADAPTER   pAd,
2062         IN      TX_BLK                  *pTxBlk)
2063 {
2064         PUCHAR                  pHeaderBufPtr;
2065         USHORT                  FreeNumber;
2066         USHORT                  totalMPDUSize=0;
2067         USHORT                  FirstTx, LastTxIdx;
2068         int                     frameNum = 0;
2069         BOOLEAN                 bVLANPkt;
2070         PQUEUE_ENTRY    pQEntry;
2071
2072
2073         ASSERT(pTxBlk);
2074
2075         ASSERT((pTxBlk->TxPacketList.Number== 2));
2076
2077
2078         FirstTx = LastTxIdx = 0;  // Is it ok init they as 0?
2079         while(pTxBlk->TxPacketList.Head)
2080         {
2081                 pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
2082                 pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
2083
2084                 if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
2085                 {
2086                         RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
2087                         continue;
2088                 }
2089
2090                 bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
2091
2092                 // skip 802.3 header
2093                 pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
2094                 pTxBlk->SrcBufLen  -= LENGTH_802_3;
2095
2096                 // skip vlan tag
2097                 if (bVLANPkt)
2098                 {
2099                         pTxBlk->pSrcBufData     += LENGTH_802_1Q;
2100                         pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
2101                 }
2102
2103                 if (frameNum == 0)
2104                 {       // For first frame, we need to create the 802.11 header + padding(optional) + RA-AGG-LEN + SNAP Header
2105
2106                         pHeaderBufPtr = STA_Build_ARalink_Frame_Header(pAd, pTxBlk);
2107
2108                         // It's ok write the TxWI here, because the TxWI->MPDUtotalByteCount
2109                         //      will be updated after final frame was handled.
2110                         RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
2111
2112
2113                         //
2114                         // Insert LLC-SNAP encapsulation - 8 octets
2115                         //
2116                         EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
2117
2118                         if (pTxBlk->pExtraLlcSnapEncap)
2119                         {
2120                                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
2121                                 pHeaderBufPtr += 6;
2122                                 // get 2 octets (TypeofLen)
2123                                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
2124                                 pHeaderBufPtr += 2;
2125                                 pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
2126                         }
2127                 }
2128                 else
2129                 {       // For second aggregated frame, we need create the 802.3 header to headerBuf, because PCI will copy it to SDPtr0.
2130
2131                         pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
2132                         pTxBlk->MpduHeaderLen = 0;
2133
2134                         // A-Ralink sub-sequent frame header is the same as 802.3 header.
2135                         //   DA(6)+SA(6)+FrameType(2)
2136                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader, 12);
2137                         pHeaderBufPtr += 12;
2138                         // get 2 octets (TypeofLen)
2139                         NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
2140                         pHeaderBufPtr += 2;
2141                         pTxBlk->MpduHeaderLen = LENGTH_ARALINK_SUBFRAMEHEAD;
2142                 }
2143
2144                 totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
2145
2146                 //FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
2147                 if (frameNum ==0)
2148                         FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
2149                 else
2150                         LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
2151
2152                 frameNum++;
2153
2154                 pAd->RalinkCounters.OneSecTxAggregationCount++;
2155                 pAd->RalinkCounters.KickTxCount++;
2156                 pAd->RalinkCounters.OneSecTxDoneCount++;
2157
2158         }
2159
2160         HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
2161         HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
2162
2163         //
2164         // Kick out Tx
2165         //
2166 #ifdef RT2860
2167         if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_DISABLE_TX))
2168 #endif
2169                 HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
2170
2171 }
2172
2173
2174 VOID STA_Fragment_Frame_Tx(
2175         IN RTMP_ADAPTER *pAd,
2176         IN TX_BLK               *pTxBlk)
2177 {
2178         HEADER_802_11   *pHeader_802_11;
2179         PUCHAR                  pHeaderBufPtr;
2180         USHORT                  FreeNumber;
2181         UCHAR                   fragNum = 0;
2182         PACKET_INFO             PacketInfo;
2183         USHORT                  EncryptionOverhead = 0;
2184         UINT32                  FreeMpduSize, SrcRemainingBytes;
2185         USHORT                  AckDuration;
2186         UINT                    NextMpduSize;
2187         BOOLEAN                 bVLANPkt;
2188         PQUEUE_ENTRY    pQEntry;
2189
2190
2191         ASSERT(pTxBlk);
2192
2193         pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
2194         pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
2195         if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
2196         {
2197                 RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
2198                 return;
2199         }
2200
2201         ASSERT(TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag));
2202         bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
2203
2204         STAFindCipherAlgorithm(pAd, pTxBlk);
2205         STABuildCommon802_11Header(pAd, pTxBlk);
2206
2207         if (pTxBlk->CipherAlg == CIPHER_TKIP)
2208         {
2209                 pTxBlk->pPacket = duplicate_pkt_with_TKIP_MIC(pAd, pTxBlk->pPacket);
2210                 if (pTxBlk->pPacket == NULL)
2211                         return;
2212                 RTMP_QueryPacketInfo(pTxBlk->pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
2213         }
2214
2215         // skip 802.3 header
2216         pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
2217         pTxBlk->SrcBufLen  -= LENGTH_802_3;
2218
2219
2220         // skip vlan tag
2221         if (bVLANPkt)
2222         {
2223                 pTxBlk->pSrcBufData     += LENGTH_802_1Q;
2224                 pTxBlk->SrcBufLen       -= LENGTH_802_1Q;
2225         }
2226
2227         pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
2228         pHeader_802_11 = (HEADER_802_11 *)pHeaderBufPtr;
2229
2230
2231         // skip common header
2232         pHeaderBufPtr += pTxBlk->MpduHeaderLen;
2233
2234         if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
2235         {
2236                 //
2237                 // build QOS Control bytes
2238                 //
2239                 *pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
2240
2241                 *(pHeaderBufPtr+1) = 0;
2242                 pHeaderBufPtr +=2;
2243                 pTxBlk->MpduHeaderLen += 2;
2244         }
2245
2246         //
2247         // padding at front of LLC header
2248         // LLC header should locate at 4-octets aligment
2249         //
2250         pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
2251         pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
2252         pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
2253
2254
2255
2256         //
2257         // Insert LLC-SNAP encapsulation - 8 octets
2258         //
2259         //
2260         // if original Ethernet frame contains no LLC/SNAP,
2261         // then an extra LLC/SNAP encap is required
2262         //
2263         EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
2264         if (pTxBlk->pExtraLlcSnapEncap)
2265         {
2266                 UCHAR vlan_size;
2267
2268                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
2269                 pHeaderBufPtr += 6;
2270                 // skip vlan tag
2271                 vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
2272                 // get 2 octets (TypeofLen)
2273                 NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
2274                 pHeaderBufPtr += 2;
2275                 pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
2276         }
2277
2278
2279         // If TKIP is used and fragmentation is required. Driver has to
2280         //      append TKIP MIC at tail of the scatter buffer
2281         //      MAC ASIC will only perform IV/EIV/ICV insertion but no TKIP MIC
2282         if (pTxBlk->CipherAlg == CIPHER_TKIP)
2283         {
2284
2285                 // NOTE: DON'T refer the skb->len directly after following copy. Becasue the length is not adjust
2286                 //                      to correct lenght, refer to pTxBlk->SrcBufLen for the packet length in following progress.
2287                 NdisMoveMemory(pTxBlk->pSrcBufData + pTxBlk->SrcBufLen, &pAd->PrivateInfo.Tx.MIC[0], 8);
2288                 //skb_put((RTPKT_TO_OSPKT(pTxBlk->pPacket))->tail, 8);
2289                 pTxBlk->SrcBufLen += 8;
2290                 pTxBlk->TotalFrameLen += 8;
2291                 pTxBlk->CipherAlg = CIPHER_TKIP_NO_MIC;
2292         }
2293
2294         //
2295         // calcuate the overhead bytes that encryption algorithm may add. This
2296         // affects the calculate of "duration" field
2297         //
2298         if ((pTxBlk->CipherAlg == CIPHER_WEP64) || (pTxBlk->CipherAlg == CIPHER_WEP128))
2299                 EncryptionOverhead = 8; //WEP: IV[4] + ICV[4];
2300         else if (pTxBlk->CipherAlg == CIPHER_TKIP_NO_MIC)
2301                 EncryptionOverhead = 12;//TKIP: IV[4] + EIV[4] + ICV[4], MIC will be added to TotalPacketLength
2302         else if (pTxBlk->CipherAlg == CIPHER_TKIP)
2303                 EncryptionOverhead = 20;//TKIP: IV[4] + EIV[4] + ICV[4] + MIC[8]
2304         else if (pTxBlk->CipherAlg == CIPHER_AES)
2305                 EncryptionOverhead = 16;        // AES: IV[4] + EIV[4] + MIC[8]
2306         else
2307                 EncryptionOverhead = 0;
2308
2309         // decide how much time an ACK/CTS frame will consume in the air
2310         AckDuration = RTMPCalcDuration(pAd, pAd->CommonCfg.ExpectedACKRate[pTxBlk->TxRate], 14);
2311
2312         // Init the total payload length of this frame.
2313         SrcRemainingBytes = pTxBlk->SrcBufLen;
2314
2315         pTxBlk->TotalFragNum = 0xff;
2316
2317         do {
2318
2319                 FreeMpduSize = pAd->CommonCfg.FragmentThreshold - LENGTH_CRC;
2320
2321                 FreeMpduSize -= pTxBlk->MpduHeaderLen;
2322
2323                 if (SrcRemainingBytes <= FreeMpduSize)
2324                 {       // this is the last or only fragment
2325
2326                         pTxBlk->SrcBufLen = SrcRemainingBytes;
2327
2328                         pHeader_802_11->FC.MoreFrag = 0;
2329                         pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + AckDuration;
2330
2331                         // Indicate the lower layer that this's the last fragment.
2332                         pTxBlk->TotalFragNum = fragNum;
2333                 }
2334                 else
2335                 {       // more fragment is required
2336
2337                         pTxBlk->SrcBufLen = FreeMpduSize;
2338
2339                         NextMpduSize = min(((UINT)SrcRemainingBytes - pTxBlk->SrcBufLen), ((UINT)pAd->CommonCfg.FragmentThreshold));
2340                         pHeader_802_11->FC.MoreFrag = 1;
2341                         pHeader_802_11->Duration = (3 * pAd->CommonCfg.Dsifs) + (2 * AckDuration) + RTMPCalcDuration(pAd, pTxBlk->TxRate, NextMpduSize + EncryptionOverhead);
2342                 }
2343
2344                 if (fragNum == 0)
2345                         pTxBlk->FrameGap = IFS_HTTXOP;
2346                 else
2347                         pTxBlk->FrameGap = IFS_SIFS;
2348
2349                 RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
2350
2351                 HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, &FreeNumber);
2352
2353                 pAd->RalinkCounters.KickTxCount++;
2354                 pAd->RalinkCounters.OneSecTxDoneCount++;
2355
2356                 // Update the frame number, remaining size of the NDIS packet payload.
2357
2358                 // space for 802.11 header.
2359                 if (fragNum == 0 && pTxBlk->pExtraLlcSnapEncap)
2360                         pTxBlk->MpduHeaderLen -= LENGTH_802_1_H;
2361
2362                 fragNum++;
2363                 SrcRemainingBytes -= pTxBlk->SrcBufLen;
2364                 pTxBlk->pSrcBufData += pTxBlk->SrcBufLen;
2365
2366                 pHeader_802_11->Frag++;  // increase Frag #
2367
2368         }while(SrcRemainingBytes > 0);
2369
2370         //
2371         // Kick out Tx
2372         //
2373         HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
2374 }
2375
2376
2377 #define RELEASE_FRAMES_OF_TXBLK(_pAd, _pTxBlk, _pQEntry, _Status)                                                                               \
2378                 while(_pTxBlk->TxPacketList.Head)                                                                                                               \
2379                 {                                                                                                                                                                               \
2380                         _pQEntry = RemoveHeadQueue(&_pTxBlk->TxPacketList);                                                                     \
2381                         RELEASE_NDIS_PACKET(_pAd, QUEUE_ENTRY_TO_PACKET(_pQEntry), _Status);    \
2382                 }
2383
2384
2385 /*
2386         ========================================================================
2387
2388         Routine Description:
2389                 Copy frame from waiting queue into relative ring buffer and set
2390         appropriate ASIC register to kick hardware encryption before really
2391         sent out to air.
2392
2393         Arguments:
2394                 pAd     Pointer to our adapter
2395                 PNDIS_PACKET    Pointer to outgoing Ndis frame
2396                 NumberOfFrag    Number of fragment required
2397
2398         Return Value:
2399                 None
2400
2401         IRQL = DISPATCH_LEVEL
2402
2403         Note:
2404
2405         ========================================================================
2406 */
2407 NDIS_STATUS STAHardTransmit(
2408         IN PRTMP_ADAPTER        pAd,
2409         IN TX_BLK                       *pTxBlk,
2410         IN      UCHAR                   QueIdx)
2411 {
2412         NDIS_PACKET             *pPacket;
2413         PQUEUE_ENTRY    pQEntry;
2414
2415         // ---------------------------------------------
2416         // STEP 0. DO SANITY CHECK AND SOME EARLY PREPARATION.
2417         // ---------------------------------------------
2418         //
2419         ASSERT(pTxBlk->TxPacketList.Number);
2420         if (pTxBlk->TxPacketList.Head == NULL)
2421         {
2422                 DBGPRINT(RT_DEBUG_ERROR, ("pTxBlk->TotalFrameNum == %ld!\n", pTxBlk->TxPacketList.Number));
2423                 return NDIS_STATUS_FAILURE;
2424         }
2425
2426         pPacket = QUEUE_ENTRY_TO_PACKET(pTxBlk->TxPacketList.Head);
2427
2428         // ------------------------------------------------------------------
2429         // STEP 1. WAKE UP PHY
2430         //              outgoing frame always wakeup PHY to prevent frame lost and
2431         //              turn off PSM bit to improve performance
2432         // ------------------------------------------------------------------
2433         // not to change PSM bit, just send this frame out?
2434         if ((pAd->StaCfg.Psm == PWR_SAVE) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
2435         {
2436             DBGPRINT_RAW(RT_DEBUG_TRACE, ("AsicForceWakeup At HardTx\n"));
2437 #ifdef RT2860
2438                 AsicForceWakeup(pAd, FROM_TX);
2439 #endif
2440 #ifdef RT2870
2441                 AsicForceWakeup(pAd, TRUE);
2442 #endif
2443         }
2444
2445         // It should not change PSM bit, when APSD turn on.
2446         if ((!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable) && (pAd->CommonCfg.bAPSDForcePowerSave == FALSE))
2447                 || (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
2448                 || (RTMP_GET_PACKET_WAI(pTxBlk->pPacket)))
2449         {
2450                 if ((pAd->StaCfg.Psm == PWR_SAVE) &&
2451             (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeFast_PSP))
2452                         MlmeSetPsmBit(pAd, PWR_ACTIVE);
2453         }
2454
2455         switch (pTxBlk->TxFrameType)
2456         {
2457                 case TX_AMPDU_FRAME:
2458                                 STA_AMPDU_Frame_Tx(pAd, pTxBlk);
2459                         break;
2460                 case TX_AMSDU_FRAME:
2461                                 STA_AMSDU_Frame_Tx(pAd, pTxBlk);
2462                         break;
2463                 case TX_LEGACY_FRAME:
2464                                 STA_Legacy_Frame_Tx(pAd, pTxBlk);
2465                         break;
2466                 case TX_MCAST_FRAME:
2467                                 STA_Legacy_Frame_Tx(pAd, pTxBlk);
2468                         break;
2469                 case TX_RALINK_FRAME:
2470                                 STA_ARalink_Frame_Tx(pAd, pTxBlk);
2471                         break;
2472                 case TX_FRAG_FRAME:
2473                                 STA_Fragment_Frame_Tx(pAd, pTxBlk);
2474                         break;
2475                 default:
2476                         {
2477                                 // It should not happened!
2478                                 DBGPRINT(RT_DEBUG_ERROR, ("Send a pacekt was not classified!! It should not happen!\n"));
2479                                 while(pTxBlk->TxPacketList.Number)
2480                                 {
2481                                         pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
2482                                         pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
2483                                         if (pPacket)
2484                                                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
2485                                 }
2486                         }
2487                         break;
2488         }
2489
2490         return (NDIS_STATUS_SUCCESS);
2491
2492 }
2493
2494 ULONG  HashBytesPolynomial(UCHAR *value, unsigned int len)
2495 {
2496    unsigned char *word = value;
2497    unsigned int ret = 0;
2498    unsigned int i;
2499
2500    for(i=0; i < len; i++)
2501    {
2502           int mod = i % 32;
2503           ret ^=(unsigned int) (word[i]) << mod;
2504           ret ^=(unsigned int) (word[i]) >> (32 - mod);
2505    }
2506    return ret;
2507 }
2508
2509 VOID Sta_Announce_or_Forward_802_3_Packet(
2510         IN      PRTMP_ADAPTER   pAd,
2511         IN      PNDIS_PACKET    pPacket,
2512         IN      UCHAR                   FromWhichBSSID)
2513 {
2514         if (TRUE
2515                 )
2516         {
2517                 announce_802_3_packet(pAd, pPacket);
2518         }
2519         else
2520         {
2521                 // release packet
2522                 RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
2523         }
2524 }
2525