drivers/staging/vt6656/dpc.c

   1 /*
   2  * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
   3  * All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License along
  16  * with this program; if not, write to the Free Software Foundation, Inc.,
  17  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  18  *
  19  * File: dpc.c
  20  *
  21  * Purpose: handle dpc rx functions
  22  *
  23  * Author: Lyndon Chen
  24  *
  25  * Date: May 20, 2003
  26  *
  27  * Functions:
  28  *      device_receive_frame - Rcv 802.11 frame function
  29  *      s_bAPModeRxCtl- AP Rcv frame filer Ctl.
  30  *      s_bAPModeRxData- AP Rcv data frame handle
  31  *      s_bHandleRxEncryption- Rcv decrypted data via on-fly
  32  *      s_bHostWepRxEncryption- Rcv encrypted data via host
  33  *      s_byGetRateIdx- get rate index
  34  *      s_vGetDASA- get data offset
  35  *      s_vProcessRxMACHeader- Rcv 802.11 and translate to 802.3
  36  *
  37  * Revision History:
  38  *
  39  */
  40
  41 #include "device.h"
  42 #include "rxtx.h"
  43 #include "tether.h"
  44 #include "card.h"
  45 #include "bssdb.h"
  46 #include "mac.h"
  47 #include "baseband.h"
  48 #include "michael.h"
  49 #include "tkip.h"
  50 #include "tcrc.h"
  51 #include "wctl.h"
  52 #include "tbit.h"
  53 #include "hostap.h"
  54 #include "rf.h"
  55 #include "iowpa.h"
  56 #include "aes_ccmp.h"
  57 #include "datarate.h"
  58 #include "usbpipe.h"
  59
  60 /*---------------------  Static Definitions -------------------------*/
  61
  62 /*---------------------  Static Classes  ----------------------------*/
  63
  64 /*---------------------  Static Variables  --------------------------*/
  65 //static int          msglevel                =MSG_LEVEL_DEBUG;
  66 static int          msglevel                =MSG_LEVEL_INFO;
  67
  68 const BYTE acbyRxRate[MAX_RATE] =
  69 {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108};
  70
  71
  72 /*---------------------  Static Functions  --------------------------*/
  73
  74 /*---------------------  Static Definitions -------------------------*/
  75
  76 /*---------------------  Static Functions  --------------------------*/
  77
  78 static BYTE s_byGetRateIdx(IN BYTE byRate);
  79
  80
  81 static
  82 VOID
  83 s_vGetDASA(
  84     IN  PBYTE pbyRxBufferAddr,
  85     OUT PUINT pcbHeaderSize,
  86     OUT PSEthernetHeader psEthHeader
  87     );
  88
  89 static
  90 VOID
  91 s_vProcessRxMACHeader (
  92     IN  PSDevice pDevice,
  93     IN  PBYTE pbyRxBufferAddr,
  94     IN  UINT cbPacketSize,
  95     IN  BOOL bIsWEP,
  96     IN  BOOL bExtIV,
  97     OUT PUINT pcbHeadSize
  98     );
  99
 100 static BOOL s_bAPModeRxCtl(
 101     IN PSDevice pDevice,
 102     IN PBYTE    pbyFrame,
 103     IN INT      iSANodeIndex
 104     );
 105
 106
 107
 108 static BOOL s_bAPModeRxData (
 109     IN PSDevice pDevice,
 110     IN struct sk_buff* skb,
 111     IN UINT     FrameSize,
 112     IN UINT     cbHeaderOffset,
 113     IN INT      iSANodeIndex,
 114     IN INT      iDANodeIndex
 115     );
 116
 117
 118 static BOOL s_bHandleRxEncryption(
 119     IN PSDevice     pDevice,
 120     IN PBYTE        pbyFrame,
 121     IN UINT         FrameSize,
 122     IN PBYTE        pbyRsr,
 123     OUT PBYTE       pbyNewRsr,
 124     OUT PSKeyItem   *pKeyOut,
 125     int *       pbExtIV,
 126     OUT PWORD       pwRxTSC15_0,
 127     OUT PDWORD      pdwRxTSC47_16
 128     );
 129
 130 static BOOL s_bHostWepRxEncryption(
 131
 132     IN PSDevice     pDevice,
 133     IN PBYTE        pbyFrame,
 134     IN UINT         FrameSize,
 135     IN PBYTE        pbyRsr,
 136     IN BOOL         bOnFly,
 137     IN PSKeyItem    pKey,
 138     OUT PBYTE       pbyNewRsr,
 139     int *       pbExtIV,
 140     OUT PWORD       pwRxTSC15_0,
 141     OUT PDWORD      pdwRxTSC47_16
 142
 143     );
 144
 145 /*---------------------  Export Variables  --------------------------*/
 146
 147 /*+
 148  *
 149  * Description:
 150  *    Translate Rcv 802.11 header to 802.3 header with Rx buffer
 151  *
 152  * Parameters:
 153  *  In:
 154  *      pDevice
 155  *      dwRxBufferAddr  - Address of Rcv Buffer
 156  *      cbPacketSize    - Rcv Packet size
 157  *      bIsWEP          - If Rcv with WEP
 158  *  Out:
 159  *      pcbHeaderSize   - 802.11 header size
 160  *
 161  * Return Value: None
 162  *
 163 -*/
 164 static
 165 VOID
 166 s_vProcessRxMACHeader (
 167     IN  PSDevice pDevice,
 168     IN  PBYTE pbyRxBufferAddr,
 169     IN  UINT cbPacketSize,
 170     IN  BOOL bIsWEP,
 171     IN  BOOL bExtIV,
 172     OUT PUINT pcbHeadSize
 173     )
 174 {
 175     PBYTE           pbyRxBuffer;
 176     UINT            cbHeaderSize = 0;
 177     PWORD           pwType;
 178     PS802_11Header  pMACHeader;
 179     int             ii;
 180
 181
 182     pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
 183
 184     s_vGetDASA((PBYTE)pMACHeader, &cbHeaderSize, &pDevice->sRxEthHeader);
 185
 186     if (bIsWEP) {
 187         if (bExtIV) {
 188             // strip IV&ExtIV , add 8 byte
 189             cbHeaderSize += (WLAN_HDR_ADDR3_LEN + 8);
 190         } else {
 191             // strip IV , add 4 byte
 192             cbHeaderSize += (WLAN_HDR_ADDR3_LEN + 4);
 193         }
 194     }
 195     else {
 196         cbHeaderSize += WLAN_HDR_ADDR3_LEN;
 197     };
 198
 199     pbyRxBuffer = (PBYTE) (pbyRxBufferAddr + cbHeaderSize);
 200     if (IS_ETH_ADDRESS_EQUAL(pbyRxBuffer, &pDevice->abySNAP_Bridgetunnel[0])) {
 201         cbHeaderSize += 6;
 202     }
 203     else if (IS_ETH_ADDRESS_EQUAL(pbyRxBuffer, &pDevice->abySNAP_RFC1042[0])) {
 204         cbHeaderSize += 6;
 205         pwType = (PWORD) (pbyRxBufferAddr + cbHeaderSize);
 206         if ((*pwType!= TYPE_PKT_IPX) && (*pwType != cpu_to_le16(0xF380))) {
 207         }
 208         else {
 209             cbHeaderSize -= 8;
 210             pwType = (PWORD) (pbyRxBufferAddr + cbHeaderSize);
 211             if (bIsWEP) {
 212                 if (bExtIV) {
 213                     *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
 214                 } else {
 215                     *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
 216                 }
 217             }
 218             else {
 219                 *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
 220             }
 221         }
 222     }
 223     else {
 224         cbHeaderSize -= 2;
 225         pwType = (PWORD) (pbyRxBufferAddr + cbHeaderSize);
 226         if (bIsWEP) {
 227             if (bExtIV) {
 228                 *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 8);    // 8 is IV&ExtIV
 229             } else {
 230                 *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN - 4);    // 4 is IV
 231             }
 232         }
 233         else {
 234             *pwType = htons(cbPacketSize - WLAN_HDR_ADDR3_LEN);
 235         }
 236     }
 237
 238     cbHeaderSize -= (U_ETHER_ADDR_LEN * 2);
 239     pbyRxBuffer = (PBYTE) (pbyRxBufferAddr + cbHeaderSize);
 240     for(ii=0;ii<U_ETHER_ADDR_LEN;ii++)
 241         *pbyRxBuffer++ = pDevice->sRxEthHeader.abyDstAddr[ii];
 242     for(ii=0;ii<U_ETHER_ADDR_LEN;ii++)
 243         *pbyRxBuffer++ = pDevice->sRxEthHeader.abySrcAddr[ii];
 244
 245     *pcbHeadSize = cbHeaderSize;
 246 }
 247
 248
 249
 250
 251 static BYTE s_byGetRateIdx (IN BYTE byRate)
 252 {
 253     BYTE    byRateIdx;
 254
 255     for (byRateIdx = 0; byRateIdx <MAX_RATE ; byRateIdx++) {
 256         if (acbyRxRate[byRateIdx%MAX_RATE] == byRate)
 257             return byRateIdx;
 258     }
 259     return 0;
 260 }
 261
 262
 263 static
 264 VOID
 265 s_vGetDASA (
 266     IN  PBYTE pbyRxBufferAddr,
 267     OUT PUINT pcbHeaderSize,
 268     OUT PSEthernetHeader psEthHeader
 269     )
 270 {
 271     UINT            cbHeaderSize = 0;
 272     PS802_11Header  pMACHeader;
 273     int             ii;
 274
 275     pMACHeader = (PS802_11Header) (pbyRxBufferAddr + cbHeaderSize);
 276
 277     if ((pMACHeader->wFrameCtl & FC_TODS) == 0) {
 278         if (pMACHeader->wFrameCtl & FC_FROMDS) {
 279             for(ii=0;ii<U_ETHER_ADDR_LEN;ii++) {
 280                 psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr1[ii];
 281                 psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr3[ii];
 282             }
 283         }
 284         else {
 285             // IBSS mode
 286             for(ii=0;ii<U_ETHER_ADDR_LEN;ii++) {
 287                 psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr1[ii];
 288                 psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr2[ii];
 289             }
 290         }
 291     }
 292     else {
 293         // Is AP mode..
 294         if (pMACHeader->wFrameCtl & FC_FROMDS) {
 295             for(ii=0;ii<U_ETHER_ADDR_LEN;ii++) {
 296                 psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr3[ii];
 297                 psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr4[ii];
 298                 cbHeaderSize += 6;
 299             }
 300         }
 301         else {
 302             for(ii=0;ii<U_ETHER_ADDR_LEN;ii++) {
 303                 psEthHeader->abyDstAddr[ii] = pMACHeader->abyAddr3[ii];
 304                 psEthHeader->abySrcAddr[ii] = pMACHeader->abyAddr2[ii];
 305             }
 306         }
 307     };
 308     *pcbHeaderSize = cbHeaderSize;
 309 }
 310
 311
 312
 313
 314 BOOL
 315 RXbBulkInProcessData (
 316     IN PSDevice         pDevice,
 317     IN PRCB             pRCB,
 318     IN ULONG            BytesToIndicate
 319     )
 320 {
 321
 322     struct net_device_stats* pStats=&pDevice->stats;
 323     struct sk_buff* skb;
 324     PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
 325     PSRxMgmtPacket  pRxPacket = &(pMgmt->sRxPacket);
 326     PS802_11Header  p802_11Header;
 327     PBYTE           pbyRsr;
 328     PBYTE           pbyNewRsr;
 329     PBYTE           pbyRSSI;
 330     PQWORD          pqwTSFTime;
 331     PBYTE           pbyFrame;
 332     BOOL            bDeFragRx = FALSE;
 333     UINT            cbHeaderOffset;
 334     UINT            FrameSize;
 335     WORD            wEtherType = 0;
 336     INT             iSANodeIndex = -1;
 337     INT             iDANodeIndex = -1;
 338     UINT            ii;
 339     UINT            cbIVOffset;
 340     PBYTE           pbyRxSts;
 341     PBYTE           pbyRxRate;
 342     PBYTE           pbySQ;
 343 #ifdef Calcu_LinkQual
 344     PBYTE           pby3SQ;
 345 #endif
 346     UINT            cbHeaderSize;
 347     PSKeyItem       pKey = NULL;
 348     WORD            wRxTSC15_0 = 0;
 349     DWORD           dwRxTSC47_16 = 0;
 350     SKeyItem        STempKey;
 351     // 802.11h RPI
 352     //LONG            ldBm = 0;
 353     BOOL            bIsWEP = FALSE;
 354     BOOL            bExtIV = FALSE;
 355     DWORD           dwWbkStatus;
 356     PRCB            pRCBIndicate = pRCB;
 357     PBYTE           pbyDAddress;
 358     PWORD           pwPLCP_Length;
 359     BYTE            abyVaildRate[MAX_RATE] = {2,4,11,22,12,18,24,36,48,72,96,108};
 360     WORD            wPLCPwithPadding;
 361     PS802_11Header  pMACHeader;
 362     BOOL            bRxeapol_key = FALSE;
 363
 364
 365
 366     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---------- RXbBulkInProcessData---\n");
 367
 368     skb = pRCB->skb;
 369
 370     //[31:16]RcvByteCount ( not include 4-byte Status )
 371     dwWbkStatus =  *( (PDWORD)(skb->data) );
 372     FrameSize = (UINT)(dwWbkStatus >> 16);
 373     FrameSize += 4;
 374
 375     if (BytesToIndicate != FrameSize) {
 376         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---------- WRONG Length 1 \n");
 377         return FALSE;
 378     }
 379
 380     if ((BytesToIndicate > 2372)||(BytesToIndicate <= 40)) {
 381         // Frame Size error drop this packet.
 382         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---------- WRONG Length 2 \n");
 383         return FALSE;
 384     }
 385
 386     pbyDAddress = (PBYTE)(skb->data);
 387     pbyRxSts = pbyDAddress+4;
 388     pbyRxRate = pbyDAddress+5;
 389
 390     //real Frame Size = USBFrameSize -4WbkStatus - 4RxStatus - 8TSF - 4RSR - 4SQ3 - ?Padding
 391     //if SQ3 the range is 24~27, if no SQ3 the range is 20~23
 392     //real Frame size in PLCPLength field.
 393     pwPLCP_Length = (PWORD) (pbyDAddress + 6);
 394     //Fix hardware bug => PLCP_Length error
 395     if ( ((BytesToIndicate - (*pwPLCP_Length)) > 27) ||
 396          ((BytesToIndicate - (*pwPLCP_Length)) < 24) ||
 397          (BytesToIndicate < (*pwPLCP_Length)) ) {
 398
 399         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Wrong PLCP Length %x\n", (int) *pwPLCP_Length);
 400         ASSERT(0);
 401         return FALSE;
 402     }
 403     for ( ii=RATE_1M;ii<MAX_RATE;ii++) {
 404         if ( *pbyRxRate == abyVaildRate[ii] ) {
 405             break;
 406         }
 407     }
 408     if ( ii==MAX_RATE ) {
 409         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Wrong RxRate %x\n",(int) *pbyRxRate);
 410         return FALSE;
 411     }
 412
 413     wPLCPwithPadding = ( (*pwPLCP_Length / 4) + ( (*pwPLCP_Length % 4) ? 1:0 ) ) *4;
 414
 415     pqwTSFTime = (PQWORD) (pbyDAddress + 8 + wPLCPwithPadding);
 416 #ifdef Calcu_LinkQual
 417   if(pDevice->byBBType == BB_TYPE_11G)  {
 418       pby3SQ = pbyDAddress + 8 + wPLCPwithPadding + 12;
 419       pbySQ = pby3SQ;
 420     }
 421   else {
 422    pbySQ = pbyDAddress + 8 + wPLCPwithPadding + 8;
 423    pby3SQ = pbySQ;
 424   }
 425 #else
 426     pbySQ = pbyDAddress + 8 + wPLCPwithPadding + 8;
 427 #endif
 428     pbyNewRsr = pbyDAddress + 8 + wPLCPwithPadding + 9;
 429     pbyRSSI = pbyDAddress + 8 + wPLCPwithPadding + 10;
 430     pbyRsr = pbyDAddress + 8 + wPLCPwithPadding + 11;
 431
 432     FrameSize = *pwPLCP_Length;
 433
 434     pbyFrame = pbyDAddress + 8;
 435     // update receive statistic counter
 436
 437     STAvUpdateRDStatCounter(&pDevice->scStatistic,
 438                             *pbyRsr,
 439                             *pbyNewRsr,
 440                             *pbyRxSts,
 441                             *pbyRxRate,
 442                             pbyFrame,
 443                             FrameSize
 444                             );
 445
 446
 447     pMACHeader = (PS802_11Header) pbyFrame;
 448
 449 //mike add: to judge if current AP is activated?
 450     if ((pMgmt->eCurrMode == WMAC_MODE_STANDBY) ||
 451         (pMgmt->eCurrMode == WMAC_MODE_ESS_STA)) {
 452        if (pMgmt->sNodeDBTable[0].bActive) {
 453          if(IS_ETH_ADDRESS_EQUAL (pMgmt->abyCurrBSSID, pMACHeader->abyAddr2) ) {
 454             if (pMgmt->sNodeDBTable[0].uInActiveCount != 0)
 455                   pMgmt->sNodeDBTable[0].uInActiveCount = 0;
 456            }
 457        }
 458     }
 459
 460     if (!IS_MULTICAST_ADDRESS(pMACHeader->abyAddr1) && !IS_BROADCAST_ADDRESS(pMACHeader->abyAddr1)) {
 461         if ( WCTLbIsDuplicate(&(pDevice->sDupRxCache), (PS802_11Header) pbyFrame) ) {
 462             pDevice->s802_11Counter.FrameDuplicateCount++;
 463             return FALSE;
 464         }
 465
 466         if ( !IS_ETH_ADDRESS_EQUAL (pDevice->abyCurrentNetAddr, pMACHeader->abyAddr1) ) {
 467             return FALSE;
 468         }
 469     }
 470
 471
 472     // Use for TKIP MIC
 473     s_vGetDASA(pbyFrame, &cbHeaderSize, &pDevice->sRxEthHeader);
 474
 475     if (IS_ETH_ADDRESS_EQUAL((PBYTE)&(pDevice->sRxEthHeader.abySrcAddr[0]), pDevice->abyCurrentNetAddr))
 476         return FALSE;
 477
 478     if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) || (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)) {
 479         if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
 480             p802_11Header = (PS802_11Header) (pbyFrame);
 481             // get SA NodeIndex
 482             if (BSSbIsSTAInNodeDB(pDevice, (PBYTE)(p802_11Header->abyAddr2), &iSANodeIndex)) {
 483                 pMgmt->sNodeDBTable[iSANodeIndex].ulLastRxJiffer = jiffies;
 484                 pMgmt->sNodeDBTable[iSANodeIndex].uInActiveCount = 0;
 485             }
 486         }
 487     }
 488
 489     if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
 490         if (s_bAPModeRxCtl(pDevice, pbyFrame, iSANodeIndex) == TRUE) {
 491             return FALSE;
 492         }
 493     }
 494
 495
 496     if (IS_FC_WEP(pbyFrame)) {
 497         BOOL     bRxDecryOK = FALSE;
 498
 499         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"rx WEP pkt\n");
 500         bIsWEP = TRUE;
 501         if ((pDevice->bEnableHostWEP) && (iSANodeIndex >= 0)) {
 502             pKey = &STempKey;
 503             pKey->byCipherSuite = pMgmt->sNodeDBTable[iSANodeIndex].byCipherSuite;
 504             pKey->dwKeyIndex = pMgmt->sNodeDBTable[iSANodeIndex].dwKeyIndex;
 505             pKey->uKeyLength = pMgmt->sNodeDBTable[iSANodeIndex].uWepKeyLength;
 506             pKey->dwTSC47_16 = pMgmt->sNodeDBTable[iSANodeIndex].dwTSC47_16;
 507             pKey->wTSC15_0 = pMgmt->sNodeDBTable[iSANodeIndex].wTSC15_0;
 508             memcpy(pKey->abyKey,
 509                 &pMgmt->sNodeDBTable[iSANodeIndex].abyWepKey[0],
 510                 pKey->uKeyLength
 511                 );
 512
 513             bRxDecryOK = s_bHostWepRxEncryption(pDevice,
 514                                                 pbyFrame,
 515                                                 FrameSize,
 516                                                 pbyRsr,
 517                                                 pMgmt->sNodeDBTable[iSANodeIndex].bOnFly,
 518                                                 pKey,
 519                                                 pbyNewRsr,
 520                                                 &bExtIV,
 521                                                 &wRxTSC15_0,
 522                                                 &dwRxTSC47_16);
 523         } else {
 524             bRxDecryOK = s_bHandleRxEncryption(pDevice,
 525                                                 pbyFrame,
 526                                                 FrameSize,
 527                                                 pbyRsr,
 528                                                 pbyNewRsr,
 529                                                 &pKey,
 530                                                 &bExtIV,
 531                                                 &wRxTSC15_0,
 532                                                 &dwRxTSC47_16);
 533         }
 534
 535         if (bRxDecryOK) {
 536             if ((*pbyNewRsr & NEWRSR_DECRYPTOK) == 0) {
 537                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV Fail\n");
 538                 if ( (pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
 539                     (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
 540                     (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
 541                     (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
 542                     (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
 543
 544                     if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
 545                         pDevice->s802_11Counter.TKIPICVErrors++;
 546                     } else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP)) {
 547                         pDevice->s802_11Counter.CCMPDecryptErrors++;
 548                     } else if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_WEP)) {
 549 //                      pDevice->s802_11Counter.WEPICVErrorCount.QuadPart++;
 550                     }
 551                 }
 552                 return FALSE;
 553             }
 554         } else {
 555             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"WEP Func Fail\n");
 556             return FALSE;
 557         }
 558         if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP))
 559             FrameSize -= 8;         // Message Integrity Code
 560         else
 561             FrameSize -= 4;         // 4 is ICV
 562     }
 563
 564
 565     //
 566     // RX OK
 567     //
 568     //remove the CRC length
 569     FrameSize -= U_CRC_LEN;
 570
 571     if ((BITbIsAllBitsOff(*pbyRsr, (RSR_ADDRBROAD | RSR_ADDRMULTI))) && // unicast address
 572         (IS_FRAGMENT_PKT((pbyFrame)))
 573         ) {
 574         // defragment
 575         bDeFragRx = WCTLbHandleFragment(pDevice, (PS802_11Header) (pbyFrame), FrameSize, bIsWEP, bExtIV);
 576         pDevice->s802_11Counter.ReceivedFragmentCount++;
 577         if (bDeFragRx) {
 578             // defrag complete
 579             // TODO skb, pbyFrame
 580             skb = pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].skb;
 581             FrameSize = pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx].cbFrameLength;
 582             pbyFrame = skb->data + 8;
 583         }
 584         else {
 585             return FALSE;
 586         }
 587     }
 588
 589     //
 590     // Management & Control frame Handle
 591     //
 592     if ((IS_TYPE_DATA((pbyFrame))) == FALSE) {
 593         // Handle Control & Manage Frame
 594
 595         if (IS_TYPE_MGMT((pbyFrame))) {
 596             PBYTE pbyData1;
 597             PBYTE pbyData2;
 598
 599             pRxPacket = &(pRCB->sMngPacket);
 600             pRxPacket->p80211Header = (PUWLAN_80211HDR)(pbyFrame);
 601             pRxPacket->cbMPDULen = FrameSize;
 602             pRxPacket->uRSSI = *pbyRSSI;
 603             pRxPacket->bySQ = *pbySQ;
 604             HIDWORD(pRxPacket->qwLocalTSF) = cpu_to_le32(HIDWORD(*pqwTSFTime));
 605             LODWORD(pRxPacket->qwLocalTSF) = cpu_to_le32(LODWORD(*pqwTSFTime));
 606             if (bIsWEP) {
 607                 // strip IV
 608                 pbyData1 = WLAN_HDR_A3_DATA_PTR(pbyFrame);
 609                 pbyData2 = WLAN_HDR_A3_DATA_PTR(pbyFrame) + 4;
 610                 for (ii = 0; ii < (FrameSize - 4); ii++) {
 611                     *pbyData1 = *pbyData2;
 612                      pbyData1++;
 613                      pbyData2++;
 614                 }
 615             }
 616
 617             pRxPacket->byRxRate = s_byGetRateIdx(*pbyRxRate);
 618
 619             if ( *pbyRxSts == 0 ) {
 620                 //Discard beacon packet which channel is 0
 621                 if ( (WLAN_GET_FC_FSTYPE((pRxPacket->p80211Header->sA3.wFrameCtl)) == WLAN_FSTYPE_BEACON) ||
 622                      (WLAN_GET_FC_FSTYPE((pRxPacket->p80211Header->sA3.wFrameCtl)) == WLAN_FSTYPE_PROBERESP) ) {
 623                     return TRUE;
 624                 }
 625             }
 626             pRxPacket->byRxChannel = (*pbyRxSts) >> 2;
 627
 628             // hostap Deamon handle 802.11 management
 629             if (pDevice->bEnableHostapd) {
 630                     skb->dev = pDevice->apdev;
 631                     //skb->data += 4;
 632                     //skb->tail += 4;
 633                     skb->data += 8;
 634                     skb->tail += 8;
 635                 skb_put(skb, FrameSize);
 636                 skb_reset_mac_header(skb);
 637                     skb->pkt_type = PACKET_OTHERHOST;
 638                 skb->protocol = htons(ETH_P_802_2);
 639                     memset(skb->cb, 0, sizeof(skb->cb));
 640                     netif_rx(skb);
 641                 return TRUE;
 642                 }
 643
 644             //
 645             // Insert the RCB in the Recv Mng list
 646             //
 647             EnqueueRCB(pDevice->FirstRecvMngList, pDevice->LastRecvMngList, pRCBIndicate);
 648             pDevice->NumRecvMngList++;
 649             if ( bDeFragRx == FALSE) {
 650                 pRCB->Ref++;
 651             }
 652             if (pDevice->bIsRxMngWorkItemQueued == FALSE) {
 653                 pDevice->bIsRxMngWorkItemQueued = TRUE;
 654                 tasklet_schedule(&pDevice->RxMngWorkItem);
 655             }
 656
 657         }
 658         else {
 659             // Control Frame
 660         };
 661         return FALSE;
 662     }
 663     else {
 664         if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
 665             //In AP mode, hw only check addr1(BSSID or RA) if equal to local MAC.
 666             if (BITbIsBitOff(*pbyRsr, RSR_BSSIDOK)) {
 667                 if (bDeFragRx) {
 668                     if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
 669                         DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
 670                         pDevice->dev->name);
 671                     }
 672                 }
 673                 return FALSE;
 674             }
 675         }
 676         else {
 677             // discard DATA packet while not associate || BSSID error
 678             if ((pDevice->bLinkPass == FALSE) ||
 679                 BITbIsBitOff(*pbyRsr, RSR_BSSIDOK)) {
 680                 if (bDeFragRx) {
 681                     if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
 682                         DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
 683                         pDevice->dev->name);
 684                     }
 685                 }
 686                 return FALSE;
 687             }
 688    //mike add:station mode check eapol-key challenge--->
 689           {
 690             BYTE  Protocol_Version;    //802.1x Authentication
 691             BYTE  Packet_Type;           //802.1x Authentication
 692             BYTE  Descriptor_type;
 693              WORD Key_info;
 694               if (bIsWEP)
 695                   cbIVOffset = 8;
 696               else
 697                   cbIVOffset = 0;
 698               wEtherType = (skb->data[cbIVOffset + 8 + 24 + 6] << 8) |
 699                           skb->data[cbIVOffset + 8 + 24 + 6 + 1];
 700               Protocol_Version = skb->data[cbIVOffset + 8 + 24 + 6 + 1 +1];
 701               Packet_Type = skb->data[cbIVOffset + 8 + 24 + 6 + 1 +1+1];
 702              if (wEtherType == ETH_P_PAE) {         //Protocol Type in LLC-Header
 703                   if(((Protocol_Version==1) ||(Protocol_Version==2)) &&
 704                      (Packet_Type==3)) {  //802.1x OR eapol-key challenge frame receive
 705                         bRxeapol_key = TRUE;
 706                       Descriptor_type = skb->data[cbIVOffset + 8 + 24 + 6 + 1 +1+1+1+2];
 707                       Key_info = (skb->data[cbIVOffset + 8 + 24 + 6 + 1 +1+1+1+2+1]<<8) |skb->data[cbIVOffset + 8 + 24 + 6 + 1 +1+1+1+2+2] ;
 708                       if(Descriptor_type==2) {    //RSN
 709                          //  printk("WPA2_Rx_eapol-key_info<-----:%x\n",Key_info);
 710                       }
 711                      else  if(Descriptor_type==254) {
 712                         //  printk("WPA_Rx_eapol-key_info<-----:%x\n",Key_info);
 713                      }
 714                   }
 715               }
 716           }
 717     //mike add:station mode check eapol-key challenge<---
 718         }
 719     }
 720
 721
 722 // Data frame Handle
 723
 724
 725     if (pDevice->bEnablePSMode) {
 726         if (IS_FC_MOREDATA((pbyFrame))) {
 727             if (BITbIsBitOn(*pbyRsr, RSR_ADDROK)) {
 728                 //PSbSendPSPOLL((PSDevice)pDevice);
 729             }
 730         }
 731         else {
 732             if (pMgmt->bInTIMWake == TRUE) {
 733                 pMgmt->bInTIMWake = FALSE;
 734             }
 735         }
 736     };
 737
 738     // Now it only supports 802.11g Infrastructure Mode, and support rate must up to 54 Mbps
 739     if (pDevice->bDiversityEnable && (FrameSize>50) &&
 740        (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) &&
 741        (pDevice->bLinkPass == TRUE)) {
 742         BBvAntennaDiversity(pDevice, s_byGetRateIdx(*pbyRxRate), 0);
 743     }
 744
 745     // ++++++++ For BaseBand Algorithm +++++++++++++++
 746     pDevice->uCurrRSSI = *pbyRSSI;
 747     pDevice->byCurrSQ = *pbySQ;
 748
 749     // todo
 750 /*
 751     if ((*pbyRSSI != 0) &&
 752         (pMgmt->pCurrBSS!=NULL)) {
 753         RFvRSSITodBm(pDevice, *pbyRSSI, &ldBm);
 754         // Moniter if RSSI is too strong.
 755         pMgmt->pCurrBSS->byRSSIStatCnt++;
 756         pMgmt->pCurrBSS->byRSSIStatCnt %= RSSI_STAT_COUNT;
 757         pMgmt->pCurrBSS->ldBmAverage[pMgmt->pCurrBSS->byRSSIStatCnt] = ldBm;
 758         for(ii=0;ii<RSSI_STAT_COUNT;ii++) {
 759             if (pMgmt->pCurrBSS->ldBmAverage[ii] != 0) {
 760             pMgmt->pCurrBSS->ldBmMAX = max(pMgmt->pCurrBSS->ldBmAverage[ii], ldBm);
 761             }
 762         }
 763     }
 764 */
 765
 766
 767     // -----------------------------------------------
 768
 769     if ((pMgmt->eCurrMode == WMAC_MODE_ESS_AP) && (pDevice->bEnable8021x == TRUE)){
 770         BYTE    abyMacHdr[24];
 771
 772         // Only 802.1x packet incoming allowed
 773         if (bIsWEP)
 774             cbIVOffset = 8;
 775         else
 776             cbIVOffset = 0;
 777         wEtherType = (skb->data[cbIVOffset + 8 + 24 + 6] << 8) |
 778                     skb->data[cbIVOffset + 8 + 24 + 6 + 1];
 779
 780             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"wEtherType = %04x \n", wEtherType);
 781         if (wEtherType == ETH_P_PAE) {
 782             skb->dev = pDevice->apdev;
 783
 784             if (bIsWEP == TRUE) {
 785                 // strip IV header(8)
 786                 memcpy(&abyMacHdr[0], (skb->data + 8), 24);
 787                 memcpy((skb->data + 8 + cbIVOffset), &abyMacHdr[0], 24);
 788             }
 789
 790             skb->data +=  (cbIVOffset + 8);
 791             skb->tail +=  (cbIVOffset + 8);
 792             skb_put(skb, FrameSize);
 793             skb_reset_mac_header(skb);
 794             skb->pkt_type = PACKET_OTHERHOST;
 795             skb->protocol = htons(ETH_P_802_2);
 796             memset(skb->cb, 0, sizeof(skb->cb));
 797             netif_rx(skb);
 798             return TRUE;
 799
 800         }
 801         // check if 802.1x authorized
 802         if (!(pMgmt->sNodeDBTable[iSANodeIndex].dwFlags & WLAN_STA_AUTHORIZED))
 803             return FALSE;
 804     }
 805
 806
 807     if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
 808         if (bIsWEP) {
 809             FrameSize -= 8;  //MIC
 810         }
 811     }
 812
 813     //--------------------------------------------------------------------------------
 814     // Soft MIC
 815     if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_TKIP)) {
 816         if (bIsWEP) {
 817             PDWORD          pdwMIC_L;
 818             PDWORD          pdwMIC_R;
 819             DWORD           dwMIC_Priority;
 820             DWORD           dwMICKey0 = 0, dwMICKey1 = 0;
 821             DWORD           dwLocalMIC_L = 0;
 822             DWORD           dwLocalMIC_R = 0;
 823             viawget_wpa_header *wpahdr;
 824
 825
 826             if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
 827                 dwMICKey0 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[24]));
 828                 dwMICKey1 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[28]));
 829             }
 830             else {
 831                 if (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) {
 832                     dwMICKey0 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[16]));
 833                     dwMICKey1 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[20]));
 834                 } else if ((pKey->dwKeyIndex & BIT28) == 0) {
 835                     dwMICKey0 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[16]));
 836                     dwMICKey1 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[20]));
 837                 } else {
 838                     dwMICKey0 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[24]));
 839                     dwMICKey1 = cpu_to_le32(*(PDWORD)(&pKey->abyKey[28]));
 840                 }
 841             }
 842
 843             MIC_vInit(dwMICKey0, dwMICKey1);
 844             MIC_vAppend((PBYTE)&(pDevice->sRxEthHeader.abyDstAddr[0]), 12);
 845             dwMIC_Priority = 0;
 846             MIC_vAppend((PBYTE)&dwMIC_Priority, 4);
 847             // 4 is Rcv buffer header, 24 is MAC Header, and 8 is IV and Ext IV.
 848             MIC_vAppend((PBYTE)(skb->data + 8 + WLAN_HDR_ADDR3_LEN + 8),
 849                         FrameSize - WLAN_HDR_ADDR3_LEN - 8);
 850             MIC_vGetMIC(&dwLocalMIC_L, &dwLocalMIC_R);
 851             MIC_vUnInit();
 852
 853             pdwMIC_L = (PDWORD)(skb->data + 8 + FrameSize);
 854             pdwMIC_R = (PDWORD)(skb->data + 8 + FrameSize + 4);
 855
 856
 857             if ((cpu_to_le32(*pdwMIC_L) != dwLocalMIC_L) || (cpu_to_le32(*pdwMIC_R) != dwLocalMIC_R) ||
 858                 (pDevice->bRxMICFail == TRUE)) {
 859                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"MIC comparison is fail!\n");
 860                 pDevice->bRxMICFail = FALSE;
 861                 //pDevice->s802_11Counter.TKIPLocalMICFailures.QuadPart++;
 862                 pDevice->s802_11Counter.TKIPLocalMICFailures++;
 863                 if (bDeFragRx) {
 864                     if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
 865                         DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
 866                             pDevice->dev->name);
 867                     }
 868                 }
 869                 //2008-0409-07, <Add> by Einsn Liu
 870        #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 871                                 //send event to wpa_supplicant
 872                                 //if(pDevice->bWPASuppWextEnabled == TRUE)
 873                                 {
 874                                         union iwreq_data wrqu;
 875                                         struct iw_michaelmicfailure ev;
 876                                         int keyidx = pbyFrame[cbHeaderSize+3] >> 6; //top two-bits
 877                                         memset(&ev, 0, sizeof(ev));
 878                                         ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
 879                                         if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
 880                                                         (pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
 881                                                                 (*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
 882                                                 ev.flags |= IW_MICFAILURE_PAIRWISE;
 883                                         } else {
 884                                                 ev.flags |= IW_MICFAILURE_GROUP;
 885                                         }
 886
 887                                         ev.src_addr.sa_family = ARPHRD_ETHER;
 888                                         memcpy(ev.src_addr.sa_data, pMACHeader->abyAddr2, ETH_ALEN);
 889                                         memset(&wrqu, 0, sizeof(wrqu));
 890                                         wrqu.data.length = sizeof(ev);
 891                         PRINT_K("wireless_send_event--->IWEVMICHAELMICFAILURE\n");
 892                                         wireless_send_event(pDevice->dev, IWEVMICHAELMICFAILURE, &wrqu, (char *)&ev);
 893
 894                                 }
 895          #endif
 896
 897
 898                 if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
 899                      wpahdr = (viawget_wpa_header *)pDevice->skb->data;
 900                      if ((pMgmt->eCurrMode == WMAC_MODE_ESS_STA) &&
 901                          (pMgmt->eCurrState == WMAC_STATE_ASSOC) &&
 902                          (*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) {
 903                          //s802_11_Status.Flags = NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR;
 904                          wpahdr->type = VIAWGET_PTK_MIC_MSG;
 905                      } else {
 906                          //s802_11_Status.Flags = NDIS_802_11_AUTH_REQUEST_GROUP_ERROR;
 907                          wpahdr->type = VIAWGET_GTK_MIC_MSG;
 908                      }
 909                      wpahdr->resp_ie_len = 0;
 910                      wpahdr->req_ie_len = 0;
 911                      skb_put(pDevice->skb, sizeof(viawget_wpa_header));
 912                      pDevice->skb->dev = pDevice->wpadev;
 913                      skb_reset_mac_header(pDevice->skb);
 914                      pDevice->skb->pkt_type = PACKET_HOST;
 915                      pDevice->skb->protocol = htons(ETH_P_802_2);
 916                      memset(pDevice->skb->cb, 0, sizeof(pDevice->skb->cb));
 917                      netif_rx(pDevice->skb);
 918                      pDevice->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
 919                  };
 920
 921                 return FALSE;
 922
 923             }
 924         }
 925     } //---end of SOFT MIC-----------------------------------------------------------------------
 926
 927     // ++++++++++ Reply Counter Check +++++++++++++
 928
 929     if ((pKey != NULL) && ((pKey->byCipherSuite == KEY_CTL_TKIP) ||
 930                            (pKey->byCipherSuite == KEY_CTL_CCMP))) {
 931         if (bIsWEP) {
 932             WORD        wLocalTSC15_0 = 0;
 933             DWORD       dwLocalTSC47_16 = 0;
 934             ULONGLONG       RSC = 0;
 935             // endian issues
 936             RSC = *((ULONGLONG *) &(pKey->KeyRSC));
 937             wLocalTSC15_0 = (WORD) RSC;
 938             dwLocalTSC47_16 = (DWORD) (RSC>>16);
 939
 940             RSC = dwRxTSC47_16;
 941             RSC <<= 16;
 942             RSC += wRxTSC15_0;
 943             memcpy(&(pKey->KeyRSC), &RSC,  sizeof(QWORD));
 944
 945             if ( (pDevice->sMgmtObj.eCurrMode == WMAC_MODE_ESS_STA) &&
 946                  (pDevice->sMgmtObj.eCurrState == WMAC_STATE_ASSOC)) {
 947                 // check RSC
 948                 if ( (wRxTSC15_0 < wLocalTSC15_0) &&
 949                      (dwRxTSC47_16 <= dwLocalTSC47_16) &&
 950                      !((dwRxTSC47_16 == 0) && (dwLocalTSC47_16 == 0xFFFFFFFF))) {
 951                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC is illegal~~!\n ");
 952                     if (pKey->byCipherSuite == KEY_CTL_TKIP)
 953                         //pDevice->s802_11Counter.TKIPReplays.QuadPart++;
 954                         pDevice->s802_11Counter.TKIPReplays++;
 955                     else
 956                         //pDevice->s802_11Counter.CCMPReplays.QuadPart++;
 957                         pDevice->s802_11Counter.CCMPReplays++;
 958
 959                     if (bDeFragRx) {
 960                         if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
 961                             DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
 962                                 pDevice->dev->name);
 963                         }
 964                     }
 965                     return FALSE;
 966                 }
 967             }
 968         }
 969     } // ----- End of Reply Counter Check --------------------------
 970
 971
 972     s_vProcessRxMACHeader(pDevice, (PBYTE)(skb->data+8), FrameSize, bIsWEP, bExtIV, &cbHeaderOffset);
 973     FrameSize -= cbHeaderOffset;
 974     cbHeaderOffset += 8;        // 8 is Rcv buffer header
 975
 976     // Null data, framesize = 12
 977     if (FrameSize < 12)
 978         return FALSE;
 979
 980     if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
 981         if (s_bAPModeRxData(pDevice,
 982                             skb,
 983                             FrameSize,
 984                             cbHeaderOffset,
 985                             iSANodeIndex,
 986                             iDANodeIndex
 987                             ) == FALSE) {
 988
 989             if (bDeFragRx) {
 990                 if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
 991                     DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
 992                     pDevice->dev->name);
 993                 }
 994             }
 995             return FALSE;
 996         }
 997
 998     }
 999
1000         skb->data += cbHeaderOffset;
1001         skb->tail += cbHeaderOffset;
1002     skb_put(skb, FrameSize);
1003     skb->protocol=eth_type_trans(skb, skb->dev);
1004     skb->ip_summed=CHECKSUM_NONE;
1005     pStats->rx_bytes +=skb->len;
1006     pStats->rx_packets++;
1007     netif_rx(skb);
1008     if (bDeFragRx) {
1009         if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
1010             DBG_PRT(MSG_LEVEL_ERR,KERN_ERR "%s: can not alloc more frag bufs\n",
1011                 pDevice->dev->name);
1012         }
1013         return FALSE;
1014     }
1015
1016     return TRUE;
1017 }
1018
1019
1020 static BOOL s_bAPModeRxCtl (
1021     IN PSDevice pDevice,
1022     IN PBYTE    pbyFrame,
1023     IN INT      iSANodeIndex
1024     )
1025 {
1026     PS802_11Header      p802_11Header;
1027     CMD_STATUS          Status;
1028     PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
1029
1030
1031     if (IS_CTL_PSPOLL(pbyFrame) || !IS_TYPE_CONTROL(pbyFrame)) {
1032
1033         p802_11Header = (PS802_11Header) (pbyFrame);
1034         if (!IS_TYPE_MGMT(pbyFrame)) {
1035
1036             // Data & PS-Poll packet
1037             // check frame class
1038             if (iSANodeIndex > 0) {
1039                 // frame class 3 fliter & checking
1040                 if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_AUTH) {
1041                     // send deauth notification
1042                     // reason = (6) class 2 received from nonauth sta
1043                     vMgrDeAuthenBeginSta(pDevice,
1044                                          pMgmt,
1045                                          (PBYTE)(p802_11Header->abyAddr2),
1046                                          (WLAN_MGMT_REASON_CLASS2_NONAUTH),
1047                                          &Status
1048                                          );
1049                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 1\n");
1050                     return TRUE;
1051                 };
1052                 if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_ASSOC) {
1053                     // send deassoc notification
1054                     // reason = (7) class 3 received from nonassoc sta
1055                     vMgrDisassocBeginSta(pDevice,
1056                                          pMgmt,
1057                                          (PBYTE)(p802_11Header->abyAddr2),
1058                                          (WLAN_MGMT_REASON_CLASS3_NONASSOC),
1059                                          &Status
1060                                          );
1061                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDisassocBeginSta 2\n");
1062                     return TRUE;
1063                 };
1064
1065                 if (pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable) {
1066                     // delcare received ps-poll event
1067                     if (IS_CTL_PSPOLL(pbyFrame)) {
1068                         pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = TRUE;
1069                         bScheduleCommand((HANDLE)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
1070                         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 1\n");
1071                     }
1072                     else {
1073                         // check Data PS state
1074                         // if PW bit off, send out all PS bufferring packets.
1075                         if (!IS_FC_POWERMGT(pbyFrame)) {
1076                             pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = FALSE;
1077                             pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = TRUE;
1078                             bScheduleCommand((HANDLE)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
1079                             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 2\n");
1080                         }
1081                     }
1082                 }
1083                 else {
1084                    if (IS_FC_POWERMGT(pbyFrame)) {
1085                        pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = TRUE;
1086                        // Once if STA in PS state, enable multicast bufferring
1087                        pMgmt->sNodeDBTable[0].bPSEnable = TRUE;
1088                    }
1089                    else {
1090                       // clear all pending PS frame.
1091                       if (pMgmt->sNodeDBTable[iSANodeIndex].wEnQueueCnt > 0) {
1092                           pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = FALSE;
1093                           pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = TRUE;
1094                           bScheduleCommand((HANDLE)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
1095                          DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 3\n");
1096
1097                       }
1098                    }
1099                 }
1100             }
1101             else {
1102                   vMgrDeAuthenBeginSta(pDevice,
1103                                        pMgmt,
1104                                        (PBYTE)(p802_11Header->abyAddr2),
1105                                        (WLAN_MGMT_REASON_CLASS2_NONAUTH),
1106                                        &Status
1107                                        );
1108                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 3\n");
1109                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSSID:%02x-%02x-%02x=%02x-%02x-%02x \n",
1110                                 p802_11Header->abyAddr3[0],
1111                                 p802_11Header->abyAddr3[1],
1112                                 p802_11Header->abyAddr3[2],
1113                                 p802_11Header->abyAddr3[3],
1114                                 p802_11Header->abyAddr3[4],
1115                                 p802_11Header->abyAddr3[5]
1116                                );
1117                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR2:%02x-%02x-%02x=%02x-%02x-%02x \n",
1118                                 p802_11Header->abyAddr2[0],
1119                                 p802_11Header->abyAddr2[1],
1120                                 p802_11Header->abyAddr2[2],
1121                                 p802_11Header->abyAddr2[3],
1122                                 p802_11Header->abyAddr2[4],
1123                                 p802_11Header->abyAddr2[5]
1124                                );
1125                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR1:%02x-%02x-%02x=%02x-%02x-%02x \n",
1126                                 p802_11Header->abyAddr1[0],
1127                                 p802_11Header->abyAddr1[1],
1128                                 p802_11Header->abyAddr1[2],
1129                                 p802_11Header->abyAddr1[3],
1130                                 p802_11Header->abyAddr1[4],
1131                                 p802_11Header->abyAddr1[5]
1132                                );
1133                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: wFrameCtl= %x\n", p802_11Header->wFrameCtl );
1134                     return TRUE;
1135             }
1136         }
1137     }
1138     return FALSE;
1139
1140 }
1141
1142 static BOOL s_bHandleRxEncryption (
1143     IN PSDevice     pDevice,
1144     IN PBYTE        pbyFrame,
1145     IN UINT         FrameSize,
1146     IN PBYTE        pbyRsr,
1147     OUT PBYTE       pbyNewRsr,
1148     OUT PSKeyItem   *pKeyOut,
1149     int *       pbExtIV,
1150     OUT PWORD       pwRxTSC15_0,
1151     OUT PDWORD      pdwRxTSC47_16
1152     )
1153 {
1154     UINT            PayloadLen = FrameSize;
1155     PBYTE           pbyIV;
1156     BYTE            byKeyIdx;
1157     PSKeyItem       pKey = NULL;
1158     BYTE            byDecMode = KEY_CTL_WEP;
1159     PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
1160
1161
1162     *pwRxTSC15_0 = 0;
1163     *pdwRxTSC47_16 = 0;
1164
1165     pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
1166     if ( WLAN_GET_FC_TODS(*(PWORD)pbyFrame) &&
1167          WLAN_GET_FC_FROMDS(*(PWORD)pbyFrame) ) {
1168          pbyIV += 6;             // 6 is 802.11 address4
1169          PayloadLen -= 6;
1170     }
1171     byKeyIdx = (*(pbyIV+3) & 0xc0);
1172     byKeyIdx >>= 6;
1173     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\nKeyIdx: %d\n", byKeyIdx);
1174
1175     if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
1176         (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
1177         (pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
1178         (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) ||
1179         (pMgmt->eAuthenMode == WMAC_AUTH_WPA2PSK)) {
1180         if (((*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) &&
1181             (pMgmt->byCSSPK != KEY_CTL_NONE)) {
1182             // unicast pkt use pairwise key
1183             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"unicast pkt\n");
1184             if (KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, 0xFFFFFFFF, &pKey) == TRUE) {
1185                 if (pMgmt->byCSSPK == KEY_CTL_TKIP)
1186                     byDecMode = KEY_CTL_TKIP;
1187                 else if (pMgmt->byCSSPK == KEY_CTL_CCMP)
1188                     byDecMode = KEY_CTL_CCMP;
1189             }
1190             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"unicast pkt: %d, %p\n", byDecMode, pKey);
1191         } else {
1192             // use group key
1193             KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, byKeyIdx, &pKey);
1194             if (pMgmt->byCSSGK == KEY_CTL_TKIP)
1195                 byDecMode = KEY_CTL_TKIP;
1196             else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
1197                 byDecMode = KEY_CTL_CCMP;
1198             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"group pkt: %d, %d, %p\n", byKeyIdx, byDecMode, pKey);
1199         }
1200     }
1201     // our WEP only support Default Key
1202     if (pKey == NULL) {
1203         // use default group key
1204         KeybGetKey(&(pDevice->sKey), pDevice->abyBroadcastAddr, byKeyIdx, &pKey);
1205         if (pMgmt->byCSSGK == KEY_CTL_TKIP)
1206             byDecMode = KEY_CTL_TKIP;
1207         else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
1208             byDecMode = KEY_CTL_CCMP;
1209     }
1210     *pKeyOut = pKey;
1211
1212     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"AES:%d %d %d\n", pMgmt->byCSSPK, pMgmt->byCSSGK, byDecMode);
1213
1214     if (pKey == NULL) {
1215         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"pKey == NULL\n");
1216         if (byDecMode == KEY_CTL_WEP) {
1217 //            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
1218         } else if (pDevice->bLinkPass == TRUE) {
1219 //            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
1220         }
1221         return FALSE;
1222     }
1223     if (byDecMode != pKey->byCipherSuite) {
1224         if (byDecMode == KEY_CTL_WEP) {
1225 //            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
1226         } else if (pDevice->bLinkPass == TRUE) {
1227 //            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
1228         }
1229         *pKeyOut = NULL;
1230         return FALSE;
1231     }
1232     if (byDecMode == KEY_CTL_WEP) {
1233         // handle WEP
1234         if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
1235             (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == TRUE)) {
1236             // Software WEP
1237             // 1. 3253A
1238             // 2. WEP 256
1239
1240             PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
1241             memcpy(pDevice->abyPRNG, pbyIV, 3);
1242             memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
1243             rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
1244             rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
1245
1246             if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
1247                 *pbyNewRsr |= NEWRSR_DECRYPTOK;
1248             }
1249         }
1250     } else if ((byDecMode == KEY_CTL_TKIP) ||
1251                (byDecMode == KEY_CTL_CCMP)) {
1252         // TKIP/AES
1253
1254         PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
1255         *pdwRxTSC47_16 = cpu_to_le32(*(PDWORD)(pbyIV + 4));
1256         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ExtIV: %lx\n",*pdwRxTSC47_16);
1257         if (byDecMode == KEY_CTL_TKIP) {
1258             *pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
1259         } else {
1260             *pwRxTSC15_0 = cpu_to_le16(*(PWORD)pbyIV);
1261         }
1262         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC0_15: %x\n", *pwRxTSC15_0);
1263
1264         if ((byDecMode == KEY_CTL_TKIP) &&
1265             (pDevice->byLocalID <= REV_ID_VT3253_A1)) {
1266             // Software TKIP
1267             // 1. 3253 A
1268             PS802_11Header  pMACHeader = (PS802_11Header) (pbyFrame);
1269             TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
1270             rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
1271             rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
1272             if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
1273                 *pbyNewRsr |= NEWRSR_DECRYPTOK;
1274                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV OK!\n");
1275             } else {
1276                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV FAIL!!!\n");
1277                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"PayloadLen = %d\n", PayloadLen);
1278             }
1279         }
1280     }// end of TKIP/AES
1281
1282     if ((*(pbyIV+3) & 0x20) != 0)
1283         *pbExtIV = TRUE;
1284     return TRUE;
1285 }
1286
1287
1288 static BOOL s_bHostWepRxEncryption (
1289     IN PSDevice     pDevice,
1290     IN PBYTE        pbyFrame,
1291     IN UINT         FrameSize,
1292     IN PBYTE        pbyRsr,
1293     IN BOOL         bOnFly,
1294     IN PSKeyItem    pKey,
1295     OUT PBYTE       pbyNewRsr,
1296     int *       pbExtIV,
1297     OUT PWORD       pwRxTSC15_0,
1298     OUT PDWORD      pdwRxTSC47_16
1299     )
1300 {
1301     PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
1302     UINT            PayloadLen = FrameSize;
1303     PBYTE           pbyIV;
1304     BYTE            byKeyIdx;
1305     BYTE            byDecMode = KEY_CTL_WEP;
1306     PS802_11Header  pMACHeader;
1307
1308
1309
1310     *pwRxTSC15_0 = 0;
1311     *pdwRxTSC47_16 = 0;
1312
1313     pbyIV = pbyFrame + WLAN_HDR_ADDR3_LEN;
1314     if ( WLAN_GET_FC_TODS(*(PWORD)pbyFrame) &&
1315          WLAN_GET_FC_FROMDS(*(PWORD)pbyFrame) ) {
1316          pbyIV += 6;             // 6 is 802.11 address4
1317          PayloadLen -= 6;
1318     }
1319     byKeyIdx = (*(pbyIV+3) & 0xc0);
1320     byKeyIdx >>= 6;
1321     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"\nKeyIdx: %d\n", byKeyIdx);
1322
1323
1324     if (pMgmt->byCSSGK == KEY_CTL_TKIP)
1325         byDecMode = KEY_CTL_TKIP;
1326     else if (pMgmt->byCSSGK == KEY_CTL_CCMP)
1327         byDecMode = KEY_CTL_CCMP;
1328
1329     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"AES:%d %d %d\n", pMgmt->byCSSPK, pMgmt->byCSSGK, byDecMode);
1330
1331     if (byDecMode != pKey->byCipherSuite) {
1332         if (byDecMode == KEY_CTL_WEP) {
1333 //            pDevice->s802_11Counter.WEPUndecryptableCount.QuadPart++;
1334         } else if (pDevice->bLinkPass == TRUE) {
1335 //            pDevice->s802_11Counter.DecryptFailureCount.QuadPart++;
1336         }
1337         return FALSE;
1338     }
1339
1340     if (byDecMode == KEY_CTL_WEP) {
1341         // handle WEP
1342         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"byDecMode == KEY_CTL_WEP \n");
1343         if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
1344             (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == TRUE) ||
1345             (bOnFly == FALSE)) {
1346             // Software WEP
1347             // 1. 3253A
1348             // 2. WEP 256
1349             // 3. NotOnFly
1350
1351             PayloadLen -= (WLAN_HDR_ADDR3_LEN + 4 + 4); // 24 is 802.11 header,4 is IV, 4 is crc
1352             memcpy(pDevice->abyPRNG, pbyIV, 3);
1353             memcpy(pDevice->abyPRNG + 3, pKey->abyKey, pKey->uKeyLength);
1354             rc4_init(&pDevice->SBox, pDevice->abyPRNG, pKey->uKeyLength + 3);
1355             rc4_encrypt(&pDevice->SBox, pbyIV+4, pbyIV+4, PayloadLen);
1356
1357             if (ETHbIsBufferCrc32Ok(pbyIV+4, PayloadLen)) {
1358                 *pbyNewRsr |= NEWRSR_DECRYPTOK;
1359             }
1360         }
1361     } else if ((byDecMode == KEY_CTL_TKIP) ||
1362                (byDecMode == KEY_CTL_CCMP)) {
1363         // TKIP/AES
1364
1365         PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
1366         *pdwRxTSC47_16 = cpu_to_le32(*(PDWORD)(pbyIV + 4));
1367         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ExtIV: %lx\n",*pdwRxTSC47_16);
1368
1369         if (byDecMode == KEY_CTL_TKIP) {
1370             *pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
1371         } else {
1372             *pwRxTSC15_0 = cpu_to_le16(*(PWORD)pbyIV);
1373         }
1374         DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"TSC0_15: %x\n", *pwRxTSC15_0);
1375
1376         if (byDecMode == KEY_CTL_TKIP) {
1377
1378             if ((pDevice->byLocalID <= REV_ID_VT3253_A1) || (bOnFly == FALSE)) {
1379                 // Software TKIP
1380                 // 1. 3253 A
1381                 // 2. NotOnFly
1382                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"soft KEY_CTL_TKIP \n");
1383                 pMACHeader = (PS802_11Header) (pbyFrame);
1384                 TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
1385                 rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
1386                 rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
1387                 if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
1388                     *pbyNewRsr |= NEWRSR_DECRYPTOK;
1389                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV OK!\n");
1390                 } else {
1391                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"ICV FAIL!!!\n");
1392                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"PayloadLen = %d\n", PayloadLen);
1393                 }
1394             }
1395         }
1396
1397         if (byDecMode == KEY_CTL_CCMP) {
1398             if (bOnFly == FALSE) {
1399                 // Software CCMP
1400                 // NotOnFly
1401                 DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"soft KEY_CTL_CCMP\n");
1402                 if (AESbGenCCMP(pKey->abyKey, pbyFrame, FrameSize)) {
1403                     *pbyNewRsr |= NEWRSR_DECRYPTOK;
1404                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CCMP MIC compare OK!\n");
1405                 } else {
1406                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"CCMP MIC fail!\n");
1407                 }
1408             }
1409         }
1410
1411     }// end of TKIP/AES
1412
1413     if ((*(pbyIV+3) & 0x20) != 0)
1414         *pbExtIV = TRUE;
1415     return TRUE;
1416 }
1417
1418
1419
1420 static BOOL s_bAPModeRxData (
1421     IN PSDevice pDevice,
1422     IN struct sk_buff* skb,
1423     IN UINT     FrameSize,
1424     IN UINT     cbHeaderOffset,
1425     IN INT      iSANodeIndex,
1426     IN INT      iDANodeIndex
1427     )
1428
1429 {
1430     PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
1431     BOOL                bRelayAndForward = FALSE;
1432     BOOL                bRelayOnly = FALSE;
1433     BYTE                byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
1434     WORD                wAID;
1435
1436
1437     struct sk_buff* skbcpy = NULL;
1438
1439     if (FrameSize > CB_MAX_BUF_SIZE)
1440         return FALSE;
1441     // check DA
1442     if(IS_MULTICAST_ADDRESS((PBYTE)(skb->data+cbHeaderOffset))) {
1443        if (pMgmt->sNodeDBTable[0].bPSEnable) {
1444
1445            skbcpy = dev_alloc_skb((int)pDevice->rx_buf_sz);
1446
1447         // if any node in PS mode, buffer packet until DTIM.
1448            if (skbcpy == NULL) {
1449                DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "relay multicast no skb available \n");
1450            }
1451            else {
1452                skbcpy->dev = pDevice->dev;
1453                skbcpy->len = FrameSize;
1454                memcpy(skbcpy->data, skb->data+cbHeaderOffset, FrameSize);
1455                skb_queue_tail(&(pMgmt->sNodeDBTable[0].sTxPSQueue), skbcpy);
1456                pMgmt->sNodeDBTable[0].wEnQueueCnt++;
1457                // set tx map
1458                pMgmt->abyPSTxMap[0] |= byMask[0];
1459            }
1460        }
1461        else {
1462            bRelayAndForward = TRUE;
1463        }
1464     }
1465     else {
1466         // check if relay
1467         if (BSSbIsSTAInNodeDB(pDevice, (PBYTE)(skb->data+cbHeaderOffset), &iDANodeIndex)) {
1468             if (pMgmt->sNodeDBTable[iDANodeIndex].eNodeState >= NODE_ASSOC) {
1469                 if (pMgmt->sNodeDBTable[iDANodeIndex].bPSEnable) {
1470                     // queue this skb until next PS tx, and then release.
1471
1472                         skb->data += cbHeaderOffset;
1473                         skb->tail += cbHeaderOffset;
1474                     skb_put(skb, FrameSize);
1475                     skb_queue_tail(&pMgmt->sNodeDBTable[iDANodeIndex].sTxPSQueue, skb);
1476
1477                     pMgmt->sNodeDBTable[iDANodeIndex].wEnQueueCnt++;
1478                     wAID = pMgmt->sNodeDBTable[iDANodeIndex].wAID;
1479                     pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
1480                     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "relay: index= %d, pMgmt->abyPSTxMap[%d]= %d\n",
1481                                iDANodeIndex, (wAID >> 3), pMgmt->abyPSTxMap[wAID >> 3]);
1482                     return TRUE;
1483                 }
1484                 else {
1485                     bRelayOnly = TRUE;
1486                 }
1487             }
1488         };
1489     }
1490
1491     if (bRelayOnly || bRelayAndForward) {
1492         // relay this packet right now
1493         if (bRelayAndForward)
1494             iDANodeIndex = 0;
1495
1496         if ((pDevice->uAssocCount > 1) && (iDANodeIndex >= 0)) {
1497             bRelayPacketSend(pDevice, (PBYTE)(skb->data + cbHeaderOffset), FrameSize, (UINT)iDANodeIndex);
1498         }
1499
1500         if (bRelayOnly)
1501             return FALSE;
1502     }
1503     // none associate, don't forward
1504     if (pDevice->uAssocCount == 0)
1505         return FALSE;
1506
1507     return TRUE;
1508 }
1509
1510
1511
1512
1513 VOID
1514 RXvWorkItem(
1515     PVOID Context
1516     )
1517 {
1518     PSDevice pDevice = (PSDevice) Context;
1519     NTSTATUS        ntStatus;
1520     PRCB            pRCB=NULL;
1521
1522     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Rx Polling Thread\n");
1523     spin_lock_irq(&pDevice->lock);
1524     while ( MP_TEST_FLAG(pDevice, fMP_POST_READS) &&
1525             MP_IS_READY(pDevice) &&
1526             (pDevice->NumRecvFreeList != 0) ) {
1527         pRCB = pDevice->FirstRecvFreeList;
1528         pDevice->NumRecvFreeList--;
1529         ASSERT(pRCB);// cannot be NULL
1530         DequeueRCB(pDevice->FirstRecvFreeList, pDevice->LastRecvFreeList);
1531         ntStatus = PIPEnsBulkInUsbRead(pDevice, pRCB);
1532     }
1533     pDevice->bIsRxWorkItemQueued = FALSE;
1534     spin_unlock_irq(&pDevice->lock);
1535
1536 }
1537
1538
1539 VOID
1540 RXvFreeRCB(
1541     IN PRCB pRCB,
1542     IN BOOL bReAllocSkb
1543     )
1544 {
1545     PSDevice pDevice = (PSDevice)pRCB->pDevice;
1546
1547
1548     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->RXvFreeRCB\n");
1549
1550     ASSERT(!pRCB->Ref);     // should be 0
1551     ASSERT(pRCB->pDevice);  // shouldn't be NULL
1552
1553     if (bReAllocSkb == TRUE) {
1554         pRCB->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
1555         // todo error handling
1556         if (pRCB->skb == NULL) {
1557             DBG_PRT(MSG_LEVEL_ERR,KERN_ERR" Failed to re-alloc rx skb\n");
1558         }else {
1559             pRCB->skb->dev = pDevice->dev;
1560         }
1561     }
1562     //
1563     // Insert the RCB back in the Recv free list
1564     //
1565     EnqueueRCB(pDevice->FirstRecvFreeList, pDevice->LastRecvFreeList, pRCB);
1566     pDevice->NumRecvFreeList++;
1567
1568
1569     if (MP_TEST_FLAG(pDevice, fMP_POST_READS) && MP_IS_READY(pDevice) &&
1570         (pDevice->bIsRxWorkItemQueued == FALSE) ) {
1571
1572         pDevice->bIsRxWorkItemQueued = TRUE;
1573         tasklet_schedule(&pDevice->ReadWorkItem);
1574     }
1575     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"<----RXFreeRCB %d %d\n",pDevice->NumRecvFreeList, pDevice->NumRecvMngList);
1576 }
1577
1578
1579 VOID
1580 RXvMngWorkItem(
1581     PVOID Context
1582     )
1583 {
1584     PSDevice pDevice = (PSDevice) Context;
1585     PRCB            pRCB=NULL;
1586     PSRxMgmtPacket  pRxPacket;
1587     BOOL            bReAllocSkb = FALSE;
1588
1589     DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"---->Rx Mng Thread\n");
1590
1591     spin_lock_irq(&pDevice->lock);
1592     while (pDevice->NumRecvMngList!=0)
1593     {
1594         pRCB = pDevice->FirstRecvMngList;
1595         pDevice->NumRecvMngList--;
1596         DequeueRCB(pDevice->FirstRecvMngList, pDevice->LastRecvMngList);
1597         if(!pRCB){
1598             break;
1599         }
1600         ASSERT(pRCB);// cannot be NULL
1601         pRxPacket = &(pRCB->sMngPacket);
1602         vMgrRxManagePacket((HANDLE)pDevice, &(pDevice->sMgmtObj), pRxPacket);
1603         pRCB->Ref--;
1604         if(pRCB->Ref == 0) {
1605             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"RxvFreeMng %d %d\n",pDevice->NumRecvFreeList, pDevice->NumRecvMngList);
1606             RXvFreeRCB(pRCB, bReAllocSkb);
1607         } else {
1608             DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO"Rx Mng Only we have the right to free RCB\n");
1609         }
1610     }
1611
1612     pDevice->bIsRxMngWorkItemQueued = FALSE;
1613     spin_unlock_irq(&pDevice->lock);
1614
1615 }
1616
1617