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