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