SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/staging/otus/80211core/ctxrx.c

   1 /*
   2  * Copyright (c) 2007-2008 Atheros Communications Inc.
   3  *
   4  * Permission to use, copy, modify, and/or distribute this software for any
   5  * purpose with or without fee is hereby granted, provided that the above
   6  * copyright notice and this permission notice appear in all copies.
   7  *
   8  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
   9  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  10  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
  11  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  12  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  13  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  14  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  15  */
  16 /*                                                                      */
  17 /*  Module Name : htr.c                                                 */
  18 /*                                                                      */
  19 /*  Abstract                                                            */
  20 /*      This module contains Tx and Rx functions.                       */
  21 /*                                                                      */
  22 /*  NOTES                                                               */
  23 /*      None                                                            */
  24 /*                                                                      */
  25 /************************************************************************/
  26 #include "cprecomp.h"
  27
  28 u16_t zfWlanRxValidate(zdev_t* dev, zbuf_t* buf);
  29 u16_t zfWlanRxFilter(zdev_t* dev, zbuf_t* buf);
  30
  31
  32
  33 const u8_t zgSnapBridgeTunnel[6] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8 };
  34 const u8_t zgSnap8021h[6] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00 };
  35 /* Table for converting IP DSCP P2-P0 bits to 802.11e Access Category */
  36 const u8_t zcUpToAc[8] = {0, 1, 1, 0, 2, 2, 3, 3}; //WMM default
  37 //const u8_t zcUpToAc[8] = {0, 1, 1, 0, 0, 0, 0, 0}; //For 2 TxQ
  38 //const u8_t zcUpToAc[8] = {0, 0, 0, 0, 0, 0, 0, 0}; //For single TxQ
  39 const u8_t zcMaxspToPktNum[4] = {8, 2, 4, 6};
  40
  41 u8_t zfGetEncryModeFromRxStatus(struct zsAdditionInfo* addInfo)
  42 {
  43     u8_t securityByte;
  44     u8_t encryMode;
  45
  46     securityByte = (addInfo->Tail.Data.SAIndex & 0xc0) >> 4;  /* byte4 */
  47     securityByte |= (addInfo->Tail.Data.DAIndex & 0xc0) >> 6; /* byte5 */
  48
  49     switch( securityByte )
  50     {
  51         case ZM_NO_WEP:
  52         case ZM_WEP64:
  53         case ZM_WEP128:
  54         case ZM_WEP256:
  55 #ifdef ZM_ENABLE_CENC
  56         case ZM_CENC:
  57 #endif //ZM_ENABLE_CENC
  58         case ZM_TKIP:
  59         case ZM_AES:
  60
  61             encryMode = securityByte;
  62             break;
  63
  64         default:
  65
  66             if ( (securityByte & 0xf8) == 0x08 )
  67             {
  68                 // decrypted by software
  69             }
  70
  71             encryMode = ZM_NO_WEP;
  72             break;
  73     }
  74
  75     return encryMode;
  76 }
  77
  78 void zfGetRxIvIcvLength(zdev_t* dev, zbuf_t* buf, u8_t vap, u16_t* pIvLen,
  79                         u16_t* pIcvLen, struct zsAdditionInfo* addInfo)
  80 {
  81     u16_t wdsPort;
  82     u8_t  encryMode;
  83
  84     zmw_get_wlan_dev(dev);
  85
  86     *pIvLen = 0;
  87     *pIcvLen = 0;
  88
  89     encryMode = zfGetEncryModeFromRxStatus(addInfo);
  90
  91     if ( wd->wlanMode == ZM_MODE_AP )
  92     {
  93         if (vap < ZM_MAX_AP_SUPPORT)
  94         {
  95             if (( wd->ap.encryMode[vap] == ZM_WEP64 ) ||
  96                     ( wd->ap.encryMode[vap] == ZM_WEP128 ) ||
  97                     ( wd->ap.encryMode[vap] == ZM_WEP256 ))
  98             {
  99                 *pIvLen = 4;
 100                 *pIcvLen = 4;
 101             }
 102             else
 103             {
 104                 u16_t id;
 105                 u16_t addr[3];
 106
 107                 addr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
 108                 addr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
 109                 addr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
 110
 111                 /* Find STA's information */
 112                 if ((id = zfApFindSta(dev, addr)) != 0xffff)
 113                 {
 114                     if (wd->ap.staTable[id].encryMode == ZM_TKIP)
 115                     {
 116                         *pIvLen = 8;
 117                         *pIcvLen = 4;
 118                     }
 119                     else if (wd->ap.staTable[id].encryMode == ZM_AES)
 120                     {
 121                         *pIvLen = 8;
 122                         *pIcvLen = 8; // AES MIC
 123                         //*pIcvLen = 0;
 124                     }
 125 #ifdef ZM_ENABLE_CENC
 126                     else if (wd->ap.staTable[id].encryMode == ZM_CENC)
 127                     {
 128                         *pIvLen = 18;
 129                         *pIcvLen= 16;
 130                     }
 131 #endif //ZM_ENABLE_CENC
 132                 }
 133             }
 134             /* WDS port checking */
 135             if ((wdsPort = vap - 0x20) >= ZM_MAX_WDS_SUPPORT)
 136             {
 137                 wdsPort = 0;
 138             }
 139
 140             switch (wd->ap.wds.encryMode[wdsPort])
 141                         {
 142                         case ZM_WEP64:
 143                         case ZM_WEP128:
 144                         case ZM_WEP256:
 145                 *pIvLen = 4;
 146                 *pIcvLen = 4;
 147                                 break;
 148                         case ZM_TKIP:
 149                 *pIvLen = 8;
 150                 *pIcvLen = 4;
 151                                 break;
 152                         case ZM_AES:
 153                 *pIvLen = 8;
 154                 *pIcvLen = 0;
 155                                 break;
 156 #ifdef ZM_ENABLE_CENC
 157             case ZM_CENC:
 158                 *pIvLen = 18;
 159                 *pIcvLen = 16;
 160                                 break;
 161 #endif //ZM_ENABLE_CENC
 162                         }/* end of switch */
 163         }
 164     }
 165         else if ( wd->wlanMode == ZM_MODE_PSEUDO)
 166     {
 167         /* test: 6518 for QA auto test */
 168         switch (encryMode)
 169                 {
 170         case ZM_WEP64:
 171         case ZM_WEP128:
 172         case ZM_WEP256:
 173             *pIvLen = 4;
 174             *pIcvLen = 4;
 175                         break;
 176                 case ZM_TKIP:
 177             *pIvLen = 8;
 178             *pIcvLen = 4;
 179                         break;
 180                 case ZM_AES:
 181             *pIvLen = 8;
 182             *pIcvLen = 0;
 183                         break;
 184 #ifdef ZM_ENABLE_CENC
 185         case ZM_CENC:
 186             *pIvLen = 18;
 187             *pIcvLen = 16;
 188 #endif //ZM_ENABLE_CENC
 189                 }/* end of switch */
 190     }
 191     else
 192     {
 193         if ( (encryMode == ZM_WEP64)||
 194              (encryMode == ZM_WEP128)||
 195              (encryMode == ZM_WEP256) )
 196         {
 197             *pIvLen = 4;
 198             *pIcvLen = 4;
 199         }
 200         else if ( encryMode == ZM_TKIP )
 201         {
 202             *pIvLen = 8;
 203             *pIcvLen = 4;
 204         }
 205         else if ( encryMode == ZM_AES )
 206         {
 207             *pIvLen = 8;
 208             *pIcvLen = 8; // AES MIC
 209         }
 210 #ifdef ZM_ENABLE_CENC
 211         else if ( encryMode == ZM_CENC)
 212         {
 213             *pIvLen = 18;
 214             *pIcvLen= 16;
 215         }
 216 #endif //ZM_ENABLE_CENC
 217     }
 218 }
 219
 220
 221 /************************************************************************/
 222 /*                                                                      */
 223 /*    FUNCTION DESCRIPTION                  zfAgingDefragList           */
 224 /*      Force flushing whole defrag list or aging the buffer            */
 225 /*      in the defrag list.                                             */
 226 /*                                                                      */
 227 /*    INPUTS                                                            */
 228 /*      dev : device pointer                                            */
 229 /*      flushFlag : 1=>flushing, 0=>Aging                               */
 230 /*                                                                      */
 231 /*    OUTPUTS                                                           */
 232 /*      None                                                            */
 233 /*                                                                      */
 234 /*    AUTHOR                                                            */
 235 /*      Stephen Chen        Atheros Communications, INC.    2007.1      */
 236 /*                                                                      */
 237 /************************************************************************/
 238 void zfAgingDefragList(zdev_t* dev, u16_t flushFlag)
 239 {
 240     u16_t i, j;
 241     zmw_get_wlan_dev(dev);
 242     zmw_declare_for_critical_section();
 243
 244     zmw_enter_critical_section(dev);
 245
 246     for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
 247     {
 248         if (wd->defragTable.defragEntry[i].fragCount != 0 )
 249         {
 250             if (((wd->tick - wd->defragTable.defragEntry[i].tick) >
 251                         (ZM_DEFRAG_AGING_TIME_SEC * ZM_TICK_PER_SECOND))
 252                || (flushFlag != 0))
 253             {
 254                 zm_msg1_rx(ZM_LV_2, "Aging defrag list :", i);
 255                 /* Free the buffers in the defrag list */
 256                 for (j=0; j<wd->defragTable.defragEntry[i].fragCount; j++)
 257                 {
 258                     zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[j], 0);
 259                 }
 260             }
 261         }
 262         wd->defragTable.defragEntry[i].fragCount = 0;
 263     }
 264
 265     zmw_leave_critical_section(dev);
 266
 267     return;
 268 }
 269
 270
 271 /************************************************************************/
 272 /*                                                                      */
 273 /*    FUNCTION DESCRIPTION                  zfAddFirstFragToDefragList  */
 274 /*      Add first fragment to defragment list, the first empty entry    */
 275 /*      will be selected. If the list is full, sequentially select      */
 276 /*      one entry for replacement.                                      */
 277 /*                                                                      */
 278 /*    INPUTS                                                            */
 279 /*      dev : device pointer                                            */
 280 /*      buf : first fragment buffer                                     */
 281 /*      addr : address of first fragment buffer                         */
 282 /*      seqNum : sequence of first fragment buffer                      */
 283 /*                                                                      */
 284 /*    OUTPUTS                                                           */
 285 /*      None                                                            */
 286 /*                                                                      */
 287 /*    AUTHOR                                                            */
 288 /*      Stephen Chen        Atheros Communications, INC.    2007.1      */
 289 /*                                                                      */
 290 /************************************************************************/
 291 void zfAddFirstFragToDefragList(zdev_t* dev, zbuf_t* buf, u8_t* addr, u16_t seqNum)
 292 {
 293     u16_t i, j;
 294     zmw_get_wlan_dev(dev);
 295     zmw_declare_for_critical_section();
 296
 297     zmw_enter_critical_section(dev);
 298
 299     /* Find an empty one in defrag list */
 300     for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
 301     {
 302         if ( wd->defragTable.defragEntry[i].fragCount == 0 )
 303         {
 304             break;
 305         }
 306     }
 307
 308     /* If full, sequentially replace existing one */
 309     if (i == ZM_MAX_DEFRAG_ENTRIES)
 310     {
 311         i = wd->defragTable.replaceNum++ & (ZM_MAX_DEFRAG_ENTRIES-1);
 312         /* Free the buffers in the defrag list to be replaced */
 313         for (j=0; j<wd->defragTable.defragEntry[i].fragCount; j++)
 314         {
 315             zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[j], 0);
 316         }
 317     }
 318
 319     wd->defragTable.defragEntry[i].fragCount = 1;
 320     wd->defragTable.defragEntry[i].fragment[0] = buf;
 321     wd->defragTable.defragEntry[i].seqNum = seqNum;
 322     wd->defragTable.defragEntry[i].tick = wd->tick;
 323
 324     for (j=0; j<6; j++)
 325     {
 326         wd->defragTable.defragEntry[i].addr[j] = addr[j];
 327     }
 328
 329     zmw_leave_critical_section(dev);
 330
 331     return;
 332 }
 333
 334
 335 /************************************************************************/
 336 /*                                                                      */
 337 /*    FUNCTION DESCRIPTION                  zfAddFragToDefragList       */
 338 /*      Add middle or last fragment to defragment list.                 */
 339 /*                                                                      */
 340 /*    INPUTS                                                            */
 341 /*      dev : device pointer                                            */
 342 /*      buf : first fragment buffer                                     */
 343 /*      addr : address of fragment buffer                               */
 344 /*      seqNum : sequence fragment buffer                               */
 345 /*      fragNum : fragment number of fragment buffer                    */
 346 /*      moreFrag : more frag bit of fragment buffer                     */
 347 /*      addInfo : addition info of fragment buffer                      */
 348 /*                                                                      */
 349 /*    OUTPUTS                                                           */
 350 /*      None                                                            */
 351 /*                                                                      */
 352 /*    AUTHOR                                                            */
 353 /*      Stephen Chen        Atheros Communications, INC.    2007.1      */
 354 /*                                                                      */
 355 /************************************************************************/
 356 zbuf_t* zfAddFragToDefragList(zdev_t* dev, zbuf_t* buf, u8_t* addr,
 357         u16_t seqNum, u8_t fragNum, u8_t moreFrag,
 358         struct zsAdditionInfo* addInfo)
 359 {
 360     u16_t i, j, k;
 361     zbuf_t* returnBuf = NULL;
 362     u16_t defragDone = 0;
 363     u16_t lenErr = 0;
 364     u16_t startAddr, fragHead, frameLen, ivLen, icvLen;
 365     zmw_get_wlan_dev(dev);
 366     zmw_declare_for_critical_section();
 367
 368     zmw_enter_critical_section(dev);
 369
 370     /* Find frag in the defrag list */
 371     for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
 372     {
 373         if ( wd->defragTable.defragEntry[i].fragCount != 0 )
 374         {
 375             /* Compare address */
 376             for (j=0; j<6; j++)
 377             {
 378                 if (addr[j] != wd->defragTable.defragEntry[i].addr[j])
 379                 {
 380                     break;
 381                 }
 382             }
 383             if (j == 6)
 384             {
 385                 /* Compare sequence and fragment number */
 386                 if (seqNum == wd->defragTable.defragEntry[i].seqNum)
 387                 {
 388                     if ((fragNum == wd->defragTable.defragEntry[i].fragCount)
 389                                 && (fragNum < 8))
 390                     {
 391                         /* Add frag frame to defrag list */
 392                         wd->defragTable.defragEntry[i].fragment[fragNum] = buf;
 393                         wd->defragTable.defragEntry[i].fragCount++;
 394                         defragDone = 1;
 395
 396                         if (moreFrag == 0)
 397                         {
 398                             /* merge all fragment if more data bit is cleared */
 399                             returnBuf = wd->defragTable.defragEntry[i].fragment[0];
 400                             startAddr = zfwBufGetSize(dev, returnBuf);
 401                             /* skip WLAN header 24(Data) or 26(QoS Data) */
 402                             fragHead = 24 + ((zmw_rx_buf_readh(dev, returnBuf, 0) & 0x80) >> 6);
 403                             zfGetRxIvIcvLength(dev, returnBuf, 0, &ivLen, &icvLen, addInfo);
 404                             fragHead += ivLen; /* skip IV */
 405                             for(k=1; k<wd->defragTable.defragEntry[i].fragCount; k++)
 406                             {
 407                                 frameLen = zfwBufGetSize(dev,
 408                                                          wd->defragTable.defragEntry[i].fragment[k]);
 409                                 if ((startAddr+frameLen-fragHead) < 1560)
 410                                 {
 411                                     zfRxBufferCopy(dev, returnBuf, wd->defragTable.defragEntry[i].fragment[k],
 412                                                startAddr, fragHead, frameLen-fragHead);
 413                                     startAddr += (frameLen-fragHead);
 414                                 }
 415                                 else
 416                                 {
 417                                     lenErr = 1;
 418                                 }
 419                                 zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[k], 0);
 420                             }
 421
 422                             wd->defragTable.defragEntry[i].fragCount = 0;
 423                             zfwBufSetSize(dev, returnBuf, startAddr);
 424                         }
 425                         break;
 426                     }
 427                 }
 428             }
 429         }
 430     }
 431
 432     zmw_leave_critical_section(dev);
 433
 434     if (lenErr == 1)
 435     {
 436         zfwBufFree(dev, returnBuf, 0);
 437         return NULL;
 438     }
 439     if (defragDone == 0)
 440     {
 441         zfwBufFree(dev, buf, 0);
 442         return NULL;
 443     }
 444
 445     return returnBuf;
 446 }
 447
 448
 449 /* return value = NULL => save or free this frame         */
 450 zbuf_t* zfDefragment(zdev_t* dev, zbuf_t* buf, u8_t* pbIsDefrag,
 451                      struct zsAdditionInfo* addInfo)
 452 {
 453     u8_t fragNum;
 454     u16_t seqNum;
 455     u8_t moreFragBit;
 456     u8_t addr[6];
 457     u16_t i;
 458     zmw_get_wlan_dev(dev);
 459
 460     ZM_BUFFER_TRACE(dev, buf)
 461
 462     *pbIsDefrag = FALSE;
 463     seqNum = zmw_buf_readh(dev, buf, 22);
 464     fragNum = (u8_t)(seqNum & 0xf);
 465     moreFragBit = (zmw_buf_readb(dev, buf, 1) & ZM_BIT_2) >> 2;
 466
 467     if ((fragNum == 0) && (moreFragBit == 0))
 468     {
 469         /* Not part of a fragmentation */
 470
 471         return buf;
 472     }
 473     else
 474     {
 475         wd->commTally.swRxFragmentCount++;
 476         seqNum = seqNum >> 4;
 477         for (i=0; i<6; i++)
 478         {
 479             addr[i] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+i);
 480         }
 481
 482         if (fragNum == 0)
 483         {
 484             /* more frag = 1 */
 485             /* First part of a fragmentation */
 486             zm_msg1_rx(ZM_LV_2, "First Frag, seq=", seqNum);
 487             zfAddFirstFragToDefragList(dev, buf, addr, seqNum);
 488             buf = NULL;
 489         }
 490         else
 491         {
 492             /* Middle or last part of a fragmentation */
 493             zm_msg1_rx(ZM_LV_2, "Frag seq=", seqNum);
 494             zm_msg1_rx(ZM_LV_2, "Frag moreFragBit=", moreFragBit);
 495             buf = zfAddFragToDefragList(dev, buf, addr, seqNum, fragNum, moreFragBit, addInfo);
 496             if (buf != NULL)
 497             {
 498                 *pbIsDefrag = TRUE;
 499             }
 500         }
 501     }
 502
 503     return buf;
 504 }
 505
 506
 507 #if ZM_PROTOCOL_RESPONSE_SIMULATION
 508 u16_t zfSwap(u16_t num)
 509 {
 510     return ((num >> 8) + ((num & 0xff) << 8));
 511 }
 512
 513
 514 void zfProtRspSim(zdev_t* dev, zbuf_t* buf)
 515 {
 516     u16_t ethType;
 517     u16_t arpOp;
 518     u16_t prot;
 519     u16_t temp;
 520     u16_t i;
 521     u16_t dip[2];
 522     u16_t dstPort;
 523     u16_t srcPort;
 524
 525     ethType = zmw_rx_buf_readh(dev, buf, 12);
 526     zm_msg2_rx(ZM_LV_2, "ethType=", ethType);
 527
 528     /* ARP */
 529     if (ethType == 0x0608)
 530     {
 531         arpOp = zmw_rx_buf_readh(dev, buf, 20);
 532         dip[0] = zmw_rx_buf_readh(dev, buf, 38);
 533         dip[1] = zmw_rx_buf_readh(dev, buf, 40);
 534         zm_msg2_rx(ZM_LV_2, "arpOp=", arpOp);
 535         zm_msg2_rx(ZM_LV_2, "ip0=", dip[0]);
 536         zm_msg2_rx(ZM_LV_2, "ip1=", dip[1]);
 537
 538         //ARP request to 192.168.1.15
 539         if ((arpOp == 0x0100) && (dip[0] == 0xa8c0) && (dip[1] == 0x0f01));
 540         {
 541             zm_msg0_rx(ZM_LV_2, "ARP");
 542             /* ARP response */
 543             zmw_rx_buf_writeh(dev, buf, 20, 0x0200);
 544
 545             /* dst hardware address */
 546
 547             /* src hardware address */
 548             //zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
 549             //zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
 550             //zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
 551
 552             /* dst ip address */
 553             for (i=0; i<5; i++)
 554             {
 555                 temp = zmw_rx_buf_readh(dev, buf, 22+(i*2));
 556                 zmw_rx_buf_writeh(dev, buf, 32+(i*2), temp);
 557             }
 558
 559             /* src hardware address */
 560             zmw_rx_buf_writeh(dev, buf, 22, 0xa000);
 561             zmw_rx_buf_writeh(dev, buf, 24, 0x0000);
 562             zmw_rx_buf_writeh(dev, buf, 26, 0x0000);
 563
 564             /* src ip address */
 565             zmw_rx_buf_writeh(dev, buf, 28, 0xa8c0);
 566             zmw_rx_buf_writeh(dev, buf, 30, 0x0f01);
 567         }
 568     }
 569     /* ICMP */
 570     else if (ethType == 0x0008)
 571     {
 572         zm_msg0_rx(ZM_LV_2, "IP");
 573         prot = zmw_rx_buf_readb(dev, buf, 23);
 574         dip[0] = zmw_rx_buf_readh(dev, buf, 30);
 575         dip[1] = zmw_rx_buf_readh(dev, buf, 32);
 576         zm_msg2_rx(ZM_LV_2, "prot=", prot);
 577         zm_msg2_rx(ZM_LV_2, "ip0=", dip[0]);
 578         zm_msg2_rx(ZM_LV_2, "ip1=", dip[1]);
 579
 580         /* PING request to 192.168.1.15 */
 581         if ((prot == 0x1) && (dip[0] == 0xa8c0) && (dip[1] == 0x0f01))
 582         {
 583             zm_msg0_rx(ZM_LV_2, "ICMP");
 584             /* change dst */
 585             for (i=0; i<3; i++)
 586             {
 587                 temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
 588                 zmw_rx_buf_writeh(dev, buf, i*2, temp);
 589             }
 590             /* change src */
 591             zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
 592             zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
 593             zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
 594
 595             /* exchange src ip and dst ip */
 596             for (i=0; i<2; i++)
 597             {
 598                 temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
 599                 zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
 600             }
 601             zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
 602             zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
 603
 604             /* change icmp type to echo reply */
 605             zmw_rx_buf_writeb(dev, buf, 34, 0x0);
 606
 607             /* update icmp checksum */
 608             temp = zmw_rx_buf_readh(dev, buf, 36);
 609             temp += 8;
 610             zmw_rx_buf_writeh(dev, buf, 36, temp);
 611         }
 612         else if (prot == 0x6)
 613         {
 614             zm_msg0_rx(ZM_LV_2, "TCP");
 615             srcPort = zmw_rx_buf_readh(dev, buf, 34);
 616             dstPort = zmw_rx_buf_readh(dev, buf, 36);
 617             zm_msg2_rx(ZM_LV_2, "Src Port=", srcPort);
 618             zm_msg2_rx(ZM_LV_2, "Dst Port=", dstPort);
 619             if ((dstPort == 0x1500) || (srcPort == 0x1500))
 620             {
 621                 zm_msg0_rx(ZM_LV_2, "FTP");
 622
 623                 /* change dst */
 624                 for (i=0; i<3; i++)
 625                 {
 626                     temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
 627                     zmw_rx_buf_writeh(dev, buf, i*2, temp);
 628                 }
 629                 /* change src */
 630                 zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
 631                 zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
 632                 zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
 633
 634                 /* exchange src ip and dst ip */
 635                 for (i=0; i<2; i++)
 636                 {
 637                     temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
 638                     zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
 639                 }
 640                 zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
 641                 zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
 642 #if 0
 643                 /* Patch src port */
 644                 temp = zmw_rx_buf_readh(dev, buf, 34);
 645                 temp = zfSwap(zfSwap(temp) + 1);
 646                 zmw_rx_buf_writeh(dev, buf, 34, temp);
 647                 temp = zmw_rx_buf_readh(dev, buf, 38);
 648                 temp = zfSwap(zfSwap(temp) + 1);
 649                 zmw_rx_buf_writeh(dev, buf, 38, temp);
 650
 651                 /* Patch checksum */
 652                 temp = zmw_rx_buf_readh(dev, buf, 50);
 653                 temp = zfSwap(temp);
 654                 temp = ~temp;
 655                 temp += 2;
 656                 temp = ~temp;
 657                 temp = zfSwap(temp);
 658                 zmw_rx_buf_writeh(dev, buf, 50, temp);
 659 #endif
 660             }
 661
 662         }
 663         else if (prot == 0x11)
 664         {
 665             /* change dst */
 666             for (i=0; i<3; i++)
 667             {
 668                 temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
 669                 zmw_rx_buf_writeh(dev, buf, i*2, temp);
 670             }
 671             /* change src */
 672             zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
 673             zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
 674             zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
 675
 676             zm_msg0_rx(ZM_LV_2, "UDP");
 677             srcPort = zmw_rx_buf_readh(dev, buf, 34);
 678             dstPort = zmw_rx_buf_readh(dev, buf, 36);
 679             zm_msg2_rx(ZM_LV_2, "Src Port=", srcPort);
 680             zm_msg2_rx(ZM_LV_2, "Dst Port=", dstPort);
 681
 682             /* exchange src ip and dst ip */
 683             for (i=0; i<2; i++)
 684             {
 685                 temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
 686                 zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
 687             }
 688             zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
 689             zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
 690
 691             /* exchange port */
 692             zmw_rx_buf_writeh(dev, buf, 34, srcPort+1);
 693             zmw_rx_buf_writeh(dev, buf, 36, dstPort);
 694
 695             /* checksum = 0 */
 696             zmw_rx_buf_writeh(dev, buf, 40, 0);
 697         }
 698
 699     }
 700     else if (ethType == 0x0060) /* =>0x0060 is port */
 701     {
 702         /* change src for Evl tool loop back receive */
 703         zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
 704         zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
 705         zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
 706     }
 707
 708 }
 709 #endif
 710
 711 /************************************************************************/
 712 /*                                                                      */
 713 /*    FUNCTION DESCRIPTION                  zfiTxSendEth                */
 714 /*      Called to native 802.11 management frames                       */
 715 /*                                                                      */
 716 /*    INPUTS                                                            */
 717 /*      dev : device pointer                                            */
 718 /*      buf : buffer pointer                                            */
 719 /*      port : WLAN port, 0=>standard, 0x1-0x7=>VAP, 0x20-0x25=>WDS     */
 720 /*                                                                      */
 721 /*    OUTPUTS                                                           */
 722 /*      error code                                                      */
 723 /*                                                                      */
 724 /*    AUTHOR                                                            */
 725 /*      Ray             ZyDAS Technology Corporation    2005.5      */
 726 /*                                                                      */
 727 /************************************************************************/
 728 u16_t zfiTxSend80211Mgmt(zdev_t* dev, zbuf_t* buf, u16_t port)
 729 {
 730     u16_t err;
 731     //u16_t addrTblSize = 0;
 732     //struct zsAddrTbl addrTbl;
 733     u16_t hlen;
 734     u16_t header[(24+25+1)/2];
 735     int i;
 736
 737     for(i=0;i<12;i++)
 738     {
 739         header[i] = zmw_buf_readh(dev, buf, i);
 740     }
 741     hlen = 24;
 742
 743     zfwBufRemoveHead(dev, buf, 24);
 744
 745     if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
 746             ZM_EXTERNAL_ALLOC_BUF, 0, 0)) != ZM_SUCCESS)
 747     {
 748         goto zlError;
 749     }
 750
 751     return 0;
 752
 753 zlError:
 754
 755     zfwBufFree(dev, buf, 0);
 756     return 0;
 757 }
 758
 759 u8_t zfiIsTxQueueFull(zdev_t* dev)
 760 {
 761     zmw_get_wlan_dev(dev);
 762     zmw_declare_for_critical_section();
 763
 764     zmw_enter_critical_section(dev);
 765     if ((((wd->vtxqHead[0] + 1) & ZM_VTXQ_SIZE_MASK) != wd->vtxqTail[0]) )
 766     {
 767         zmw_leave_critical_section(dev);
 768         return 0;
 769     }
 770     else
 771     {
 772         zmw_leave_critical_section(dev);
 773         return 1;
 774     }
 775 }
 776
 777 /************************************************************************/
 778 /*                                                                      */
 779 /*    FUNCTION DESCRIPTION                  zfiTxSendEth                */
 780 /*      Called to transmit Ethernet frame from upper layer.             */
 781 /*                                                                      */
 782 /*    INPUTS                                                            */
 783 /*      dev : device pointer                                            */
 784 /*      buf : buffer pointer                                            */
 785 /*      port : WLAN port, 0=>standard, 0x1-0x7=>VAP, 0x20-0x25=>WDS     */
 786 /*                                                                      */
 787 /*    OUTPUTS                                                           */
 788 /*      error code                                                      */
 789 /*                                                                      */
 790 /*    AUTHOR                                                            */
 791 /*      Stephen             ZyDAS Technology Corporation    2005.5      */
 792 /*                                                                      */
 793 /************************************************************************/
 794 u16_t zfiTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port)
 795 {
 796     u16_t err, ret;
 797
 798     zmw_get_wlan_dev(dev);
 799
 800     ZM_PERFORMANCE_TX_MSDU(dev, wd->tick);
 801     zm_msg1_tx(ZM_LV_2, "zfiTxSendEth(), port=", port);
 802     /* Return error if port is disabled */
 803     if ((err = zfTxPortControl(dev, buf, port)) == ZM_PORT_DISABLED)
 804     {
 805         err = ZM_ERR_TX_PORT_DISABLED;
 806         goto zlError;
 807     }
 808
 809 #if 1
 810     if ((wd->wlanMode == ZM_MODE_AP) && (port < 0x20))
 811     {
 812         /* AP : Buffer frame for power saving STA */
 813         if ((ret = zfApBufferPsFrame(dev, buf, port)) == 1)
 814         {
 815             return ZM_SUCCESS;
 816         }
 817     }
 818     else
 819 #endif
 820     if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
 821     {
 822         if ( zfPowerSavingMgrIsSleeping(dev) )
 823         {
 824             /*check ZM_ENABLE_POWER_SAVE flag*/
 825             zfPowerSavingMgrWakeup(dev);
 826         }
 827     }
 828 #ifdef ZM_ENABLE_IBSS_PS
 829     /* IBSS power-saving mode */
 830     else if ( wd->wlanMode == ZM_MODE_IBSS )
 831     {
 832         if ( zfStaIbssPSQueueData(dev, buf) )
 833         {
 834             return ZM_SUCCESS;
 835         }
 836     }
 837 #endif
 838
 839 #if 1
 840     //if ( wd->bQoSEnable )
 841     if (1)
 842     {
 843         /* Put to VTXQ[ac] */
 844         ret = zfPutVtxq(dev, buf);
 845
 846         /* Push VTXQ[ac] */
 847         zfPushVtxq(dev);
 848     }
 849     else
 850     {
 851         ret = zfTxSendEth(dev, buf, port, ZM_EXTERNAL_ALLOC_BUF, 0);
 852     }
 853
 854     return ret;
 855 #else
 856     return zfTxSendEth(dev, buf, port, ZM_EXTERNAL_ALLOC_BUF, 0);
 857 #endif
 858
 859 zlError:
 860     zm_msg2_tx(ZM_LV_1, "Tx Comp err=", err);
 861
 862     zfwBufFree(dev, buf, err);
 863     return err;
 864 }
 865
 866
 867 /************************************************************************/
 868 /*                                                                      */
 869 /*    FUNCTION DESCRIPTION                  zfTxSendEth                 */
 870 /*      Called to transmit Ethernet frame from upper layer.             */
 871 /*                                                                      */
 872 /*    INPUTS                                                            */
 873 /*      dev : device pointer                                            */
 874 /*      buf : buffer pointer                                            */
 875 /*      port : WLAN port, 0=>standard, 0x10-0x17=>VAP, 0x20-0x25=>WDS   */
 876 /*                                                                      */
 877 /*    OUTPUTS                                                           */
 878 /*      error code                                                      */
 879 /*                                                                      */
 880 /*    AUTHOR                                                            */
 881 /*      Stephen             ZyDAS Technology Corporation    2005.5      */
 882 /*                                                                      */
 883 /************************************************************************/
 884 u16_t zfTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u16_t flag)
 885 {
 886     u16_t err;
 887     //u16_t addrTblSize;
 888     //struct zsAddrTbl addrTbl;
 889     u16_t removeLen;
 890     u16_t header[(8+30+2+18)/2];    /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
 891     u16_t headerLen;
 892     u16_t mic[8/2];
 893     u16_t micLen;
 894     u16_t snap[8/2];
 895     u16_t snapLen;
 896     u16_t fragLen;
 897     u16_t frameLen;
 898     u16_t fragNum;
 899     struct zsFrag frag;
 900     u16_t i, j, id;
 901     u16_t offset;
 902     u16_t da[3];
 903     u16_t sa[3];
 904     u8_t up;
 905     u8_t qosType, keyIdx = 0;
 906     u16_t fragOff;
 907     u16_t newFlag;
 908     struct zsMicVar*  pMicKey;
 909     u8_t tkipFrameOffset = 0;
 910
 911     zmw_get_wlan_dev(dev);
 912
 913     zmw_declare_for_critical_section();
 914
 915     newFlag = flag & 0xff00;
 916     flag = flag & 0xff;
 917
 918     zm_msg1_tx(ZM_LV_2, "zfTxSendEth(), port=", port);
 919
 920     /* Get IP TOS for QoS AC and IP frag offset */
 921     zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
 922
 923     //EOSP bit
 924     if (newFlag & 0x100)
 925     {
 926         up |= 0x10;
 927     }
 928
 929 #ifdef ZM_ENABLE_NATIVE_WIFI
 930     if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
 931     {
 932         /* DA */
 933         da[0] = zmw_tx_buf_readh(dev, buf, 16);
 934         da[1] = zmw_tx_buf_readh(dev, buf, 18);
 935         da[2] = zmw_tx_buf_readh(dev, buf, 20);
 936         /* SA */
 937         sa[0] = zmw_tx_buf_readh(dev, buf, 10);
 938         sa[1] = zmw_tx_buf_readh(dev, buf, 12);
 939         sa[2] = zmw_tx_buf_readh(dev, buf, 14);
 940     }
 941     else if ( wd->wlanMode == ZM_MODE_IBSS )
 942     {
 943         /* DA */
 944         da[0] = zmw_tx_buf_readh(dev, buf, 4);
 945         da[1] = zmw_tx_buf_readh(dev, buf, 6);
 946         da[2] = zmw_tx_buf_readh(dev, buf, 8);
 947         /* SA */
 948         sa[0] = zmw_tx_buf_readh(dev, buf, 10);
 949         sa[1] = zmw_tx_buf_readh(dev, buf, 12);
 950         sa[2] = zmw_tx_buf_readh(dev, buf, 14);
 951     }
 952     else if ( wd->wlanMode == ZM_MODE_AP )
 953     {
 954         /* DA */
 955         da[0] = zmw_tx_buf_readh(dev, buf, 4);
 956         da[1] = zmw_tx_buf_readh(dev, buf, 6);
 957         da[2] = zmw_tx_buf_readh(dev, buf, 8);
 958         /* SA */
 959         sa[0] = zmw_tx_buf_readh(dev, buf, 16);
 960         sa[1] = zmw_tx_buf_readh(dev, buf, 18);
 961         sa[2] = zmw_tx_buf_readh(dev, buf, 20);
 962     }
 963     else
 964     {
 965         //
 966     }
 967 #else
 968     /* DA */
 969     da[0] = zmw_tx_buf_readh(dev, buf, 0);
 970     da[1] = zmw_tx_buf_readh(dev, buf, 2);
 971     da[2] = zmw_tx_buf_readh(dev, buf, 4);
 972     /* SA */
 973     sa[0] = zmw_tx_buf_readh(dev, buf, 6);
 974     sa[1] = zmw_tx_buf_readh(dev, buf, 8);
 975     sa[2] = zmw_tx_buf_readh(dev, buf, 10);
 976 #endif
 977     //Decide Key Index in ATOM, No meaning in OTUS--CWYang(m)
 978     if (wd->wlanMode == ZM_MODE_AP)
 979     {
 980         keyIdx = wd->ap.bcHalKeyIdx[port];
 981         id = zfApFindSta(dev, da);
 982         if (id != 0xffff)
 983         {
 984             switch (wd->ap.staTable[id].encryMode)
 985             {
 986             case ZM_AES:
 987             case ZM_TKIP:
 988 #ifdef ZM_ENABLE_CENC
 989             case ZM_CENC:
 990 #endif //ZM_ENABLE_CENC
 991                 keyIdx = wd->ap.staTable[id].keyIdx;
 992                 break;
 993             }
 994         }
 995     }
 996     else
 997     {
 998         switch (wd->sta.encryMode)
 999         {
1000         case ZM_WEP64:
1001         case ZM_WEP128:
1002         case ZM_WEP256:
1003             keyIdx = wd->sta.keyId;
1004             break;
1005         case ZM_AES:
1006         case ZM_TKIP:
1007             if ((da[0] & 0x1))
1008                 keyIdx = 5;
1009             else
1010                 keyIdx = 4;
1011             break;
1012 #ifdef ZM_ENABLE_CENC
1013         case ZM_CENC:
1014             keyIdx = wd->sta.cencKeyId;
1015             break;
1016 #endif //ZM_ENABLE_CENC
1017         }
1018     }
1019
1020     /* Create SNAP */
1021     removeLen = zfTxGenWlanSnap(dev, buf, snap, &snapLen);
1022     //zm_msg1_tx(ZM_LV_0, "fragOff=", fragOff);
1023
1024
1025 /* ********************************************************************************************** */
1026 /* Add 20071025 Mxzeng                                                                            */
1027 /* ********************************************************************************************** */
1028 /* ---------------------------------------------------------------------------------------------- */
1029 /*  Ethernet : frameLen = zfwBufGetSize(dev, buf);                                                */
1030 /* ---+--6--+--6--+--2--+-----20-----+-------------------------+------ Variable -------+--------- */
1031 /*    |  DA |  SA | Type|  IP Header | TCP(20) UDP(12) ICMP(8) | Application Payload L |          */
1032 /* ---+-----+-----+-----+------------+-------------------------+-----------------------+--------- */
1033 /*  MSDU = 6 + 6 + 2 + ( Network Layer header ) + ( Transport Layer header ) + L                  */
1034 /*                                                                                                */
1035 /*  MSDU - DA - SA : frameLen -= removeLen;                                                       */
1036 /* ---+--2--+-----20-----+-------------------------+------ Variable -------+--------------------- */
1037 /*    | Type| IP Header  | TCP(20) UDP(12) ICMP(8) | Application Payload L |                      */
1038 /* ---+-----+------------+-------------------------+-----------------------+--------------------- */
1039 /*                                                                                                */
1040 /*  MPDU : frameLen + mpduLengthOffset ;                                                          */
1041 /* -+---2---+----2---+-6-+-6-+--6--+---2----+--1--+--1-+---1---+-------3------+-frameLen-+---4--+- */
1042 /*  | frame |duration| DA|SA |BSSID|sequence|SNAP |SNAP|Control|    RFC 1042  |          |  FCS |  */
1043 /*  |Control|        |   |   |     | number |DSAP |SSAP|       | encapsulation|          |      |  */
1044 /* -+-------+--------+---+---+-----+--------+-----+----+-------+--------------+----------+------+- */
1045 /* ----------------------------------------------------------------------------------------------- */
1046
1047     if ( wd->sta.encryMode == ZM_TKIP )
1048         tkipFrameOffset = 8;
1049
1050     fragLen = wd->fragThreshold + tkipFrameOffset;   // Fragmentation threshold for MPDU Lengths
1051     frameLen = zfwBufGetSize(dev, buf);    // MSDU Lengths
1052     frameLen -= removeLen;                 // MSDU Lengths - DA - SA
1053
1054     /* #1st create MIC Length manually */
1055     micLen = 0;
1056
1057     /* Access Category */
1058     if (wd->wlanMode == ZM_MODE_AP)
1059     {
1060         zfApGetStaQosType(dev, da, &qosType);
1061         if (qosType == 0)
1062         {
1063             up = 0;
1064         }
1065     }
1066     else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
1067     {
1068         if (wd->sta.wmeConnected == 0)
1069         {
1070             up = 0;
1071         }
1072     }
1073     else
1074     {
1075         /* TODO : STA QoS control field */
1076         up = 0;
1077     }
1078
1079     /* #2nd Assign sequence number */
1080     zmw_enter_critical_section(dev);
1081     frag.seq[0] = ((wd->seq[zcUpToAc[up&0x7]]++) << 4);
1082     zmw_leave_critical_section(dev);
1083
1084     /* #3rd Pass the total payload to generate MPDU length ! */
1085     frag.buf[0] = buf;
1086     frag.bufType[0] = bufType;
1087     frag.flag[0] = (u8_t)flag;
1088     fragNum = 1;
1089
1090     headerLen = zfTxGenWlanHeader(dev, frag.buf[0], header, frag.seq[0],
1091                                   frag.flag[0], snapLen+micLen, removeLen, port, da, sa,
1092                                   up, &micLen, snap, snapLen, NULL);
1093
1094     //zm_debug_msg1("#1 headerLen = ", headerLen);
1095
1096     /* #4th Check the HeaderLen and determine whether the MPDU Lengths bigger than Fragmentation threshold  */
1097     /* If MPDU Lengths large than fragmentation threshold --> headerLen = 0 */
1098     if( headerLen != 0 )
1099     {
1100         zf80211FrameSend(dev, frag.buf[0], header, snapLen, da, sa, up,
1101                          headerLen, snap, mic, micLen, removeLen, frag.bufType[0],
1102                          zcUpToAc[up&0x7], keyIdx);
1103     }
1104     else //if( headerLen == 0 ) // Need to be fragmented
1105     {
1106         u16_t mpduLengthOffset;
1107         u16_t pseudSnapLen = 0;
1108
1109         mpduLengthOffset = header[0] - frameLen; // For fragmentation threshold !
1110
1111         micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic); // Get snap and mic information
1112
1113         fragLen = fragLen - mpduLengthOffset;
1114
1115         //zm_debug_msg1("#2 frameLen = ", frameLen);
1116         //zm_debug_msg1("#3 fragThreshold = ", fragLen);
1117
1118         /* fragmentation */
1119         if (frameLen >= fragLen)
1120         {
1121             //copy fragLen to frag
1122             i = 0;
1123             while( frameLen > 0 )
1124             {
1125                 if ((frag.buf[i] = zfwBufAllocate(dev, fragLen+32)) != NULL)
1126                 {
1127                     frag.bufType[i] = ZM_INTERNAL_ALLOC_BUF;
1128                     frag.seq[i] = frag.seq[0] + i;
1129                     offset = removeLen + i*fragLen;
1130
1131                     /* Consider the offset if we consider snap length to the other fragmented frame */
1132                     if ( i >= 1 )
1133                         offset = offset + pseudSnapLen*(i-1);
1134
1135                     if (frameLen > fragLen + pseudSnapLen)
1136                     {
1137                         frag.flag[i] = flag | 0x4; /* More data */
1138                         /* First fragment */
1139                         if (i == 0)
1140                         {
1141                             /* Add SNAP */
1142                             for (j=0; j<snapLen; j+=2)
1143                             {
1144                                 zmw_tx_buf_writeh(dev, frag.buf[i], j, snap[(j>>1)]);
1145                             }
1146                             zfTxBufferCopy(dev, frag.buf[i], buf, snapLen, offset, fragLen);
1147                             zfwBufSetSize(dev, frag.buf[i], snapLen+fragLen);
1148
1149                             /* Add pseud snap length to the other fragmented frame */
1150                             pseudSnapLen = snapLen;
1151
1152                             frameLen -= fragLen;
1153                         }
1154                         /* Intermediate Fragment */
1155                         else
1156                         {
1157                             //zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, fragLen);
1158                             //zfwBufSetSize(dev, frag.buf[i], fragLen);
1159
1160                             zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, fragLen+pseudSnapLen );
1161                             zfwBufSetSize(dev, frag.buf[i], fragLen+pseudSnapLen);
1162
1163                             frameLen -= (fragLen+pseudSnapLen);
1164                         }
1165                         //frameLen -= fragLen;
1166                     }
1167                     else
1168                     {
1169                         /* Last fragment  */
1170                         zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, frameLen);
1171                         /* Add MIC if need */
1172                         if ( micLen )
1173                         {
1174                             zfCopyToRxBuffer(dev, frag.buf[i], (u8_t*) mic, frameLen, micLen);
1175                         }
1176                         zfwBufSetSize(dev, frag.buf[i], frameLen+micLen);
1177                         frameLen = 0;
1178                         frag.flag[i] = (u8_t)flag; /* No more data */
1179                     }
1180                     i++;
1181                 }
1182                 else
1183                 {
1184                     break;
1185                 }
1186
1187                 // Please pay attention to the index of the buf !!!
1188                 // If write to null buf , the OS will crash !!!
1189                 zfwCopyBufContext(dev, buf, frag.buf[i-1]);
1190             }
1191             fragNum = i;
1192             snapLen = micLen = removeLen = 0;
1193
1194             zfwBufFree(dev, buf, 0);
1195         }
1196
1197         for (i=0; i<fragNum; i++)
1198         {
1199             /* Create WLAN header(Control Setting + 802.11 header + IV) */
1200             headerLen = zfTxGenWlanHeader(dev, frag.buf[i], header, frag.seq[i],
1201                                     frag.flag[i], snapLen+micLen, removeLen, port, da, sa, up, &micLen,
1202                                     snap, snapLen, NULL);
1203
1204             zf80211FrameSend(dev, frag.buf[i], header, snapLen, da, sa, up,
1205                              headerLen, snap, mic, micLen, removeLen, frag.bufType[i],
1206                              zcUpToAc[up&0x7], keyIdx);
1207
1208         } /* for (i=0; i<fragNum; i++) */
1209     }
1210
1211     return ZM_SUCCESS;
1212 }
1213
1214
1215 /************************************************************************/
1216 /*                                                                      */
1217 /*    FUNCTION DESCRIPTION                  zfTxPortControl             */
1218 /*      Check port status.                                              */
1219 /*                                                                      */
1220 /*    INPUTS                                                            */
1221 /*      buf : buffer pointer                                            */
1222 /*      port : port number, 0=>standard, 10-17=>Virtual AP, 20-25=>WDS  */
1223 /*                                                                      */
1224 /*    OUTPUTS                                                           */
1225 /*      ZM_PORT_ENABLED or ZM_PORT_DISABLE                              */
1226 /*                                                                      */
1227 /*    AUTHOR                                                            */
1228 /*      Signature           ZyDAS Technology Corporation    2005.4      */
1229 /*                                                                      */
1230 /************************************************************************/
1231 u16_t zfTxPortControl(zdev_t* dev, zbuf_t* buf, u16_t port)
1232 {
1233     zmw_get_wlan_dev(dev);
1234
1235     if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
1236     {
1237         if ( wd->sta.adapterState == ZM_STA_STATE_DISCONNECT )
1238         {
1239             zm_msg0_tx(ZM_LV_3, "Packets dropped due to disconnect state");
1240             return ZM_PORT_DISABLED;
1241         }
1242     }
1243
1244     return ZM_PORT_ENABLED;
1245 }
1246
1247
1248
1249 /************************************************************************/
1250 /*                                                                      */
1251 /*    FUNCTION DESCRIPTION                  zfIdlRecv                   */
1252 /*      Do frame validation and filtering then pass to zfwRecv80211().  */
1253 /*                                                                      */
1254 /*    INPUTS                                                            */
1255 /*      dev : device pointer                                            */
1256 /*      buf : received 802.11 frame buffer.                             */
1257 /*                                                                      */
1258 /*    OUTPUTS                                                           */
1259 /*      None                                                            */
1260 /*                                                                      */
1261 /*    AUTHOR                                                            */
1262 /*      Stephen             ZyDAS Technology Corporation    2005.10     */
1263 /*                                                                      */
1264 /************************************************************************/
1265 void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
1266 {
1267     u16_t ret = 0;
1268     u16_t bssid[3];
1269     struct agg_tid_rx *tid_rx;
1270     zmw_get_wlan_dev(dev);
1271
1272     ZM_BUFFER_TRACE(dev, buf)
1273
1274     /* tally */
1275     wd->commTally.DriverRxFrmCnt++;
1276
1277     bssid[0] = zmw_buf_readh(dev, buf, 16);
1278     bssid[1] = zmw_buf_readh(dev, buf, 18);
1279     bssid[2] = zmw_buf_readh(dev, buf, 20);
1280
1281     /* Validate Rx frame */
1282     if ((ret = zfWlanRxValidate(dev, buf)) != ZM_SUCCESS)
1283     {
1284         zm_msg1_rx(ZM_LV_1, "Rx invalid:", ret);
1285         goto zlError;
1286     }
1287
1288 #ifdef ZM_ENABLE_AGGREGATION
1289     //#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION
1290     /*
1291      * add by honda
1292      */
1293     tid_rx = zfAggRxEnabled(dev, buf);
1294     if (tid_rx && wd->reorder)
1295     {
1296         zfAggRx(dev, buf, addInfo, tid_rx);
1297
1298         return;
1299     }
1300     /*
1301      * end of add by honda
1302      */
1303     //#endif
1304 #endif
1305
1306     /* Filter Rx frame */
1307     if ((ret = zfWlanRxFilter(dev, buf)) != ZM_SUCCESS)
1308     {
1309         zm_msg1_rx(ZM_LV_1, "Rx duplicated:", ret);
1310         goto zlError;
1311     }
1312
1313     /* Discard error frame except mic failure */
1314     if ((addInfo->Tail.Data.ErrorIndication & 0x3f) != 0)
1315     {
1316         if ( wd->XLinkMode && ((addInfo->Tail.Data.ErrorIndication & 0x3f)==0x10) &&
1317              zfCompareWithBssid(dev, bssid) )
1318         {
1319             // Bypass frames !!!
1320         }
1321         else
1322         {
1323             goto zlError;
1324         }
1325     }
1326
1327
1328     /* OTUS command-8212 dump rx packet */
1329     if (wd->rxPacketDump)
1330     {
1331         zfwDumpBuf(dev, buf);
1332     }
1333
1334     /* Call zfwRecv80211() wrapper function to deliver Rx packet */
1335     /* to driver framework.                                      */
1336
1337     if (wd->zfcbRecv80211 != NULL)
1338     {
1339         wd->zfcbRecv80211(dev, buf, addInfo); //CWYang(m)
1340     }
1341     else
1342     {
1343         zfiRecv80211(dev, buf, addInfo);
1344     }
1345     return;
1346
1347 zlError:
1348     zm_msg1_rx(ZM_LV_1, "Free packet, error code:", ret);
1349
1350     wd->commTally.DriverDiscardedFrm++;
1351
1352     /* Free Rx buffer */
1353     zfwBufFree(dev, buf, 0);
1354
1355     return;
1356 }
1357
1358
1359 void zfShowRxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset)
1360 {
1361     u8_t   packetType, keyType, code, identifier, type, flags;
1362     u16_t  packetLen, keyInfo, keyLen, keyDataLen, length, Op_Code;
1363     u32_t  replayCounterH, replayCounterL, vendorId, VendorType;
1364
1365     /* EAPOL packet type */
1366     packetType = zmw_rx_buf_readb(dev, buf, offset+1); // 0: EAP-Packet
1367                                                        // 1: EAPOL-Start
1368                                                        // 2: EAPOL-Logoff
1369                                                        // 3: EAPOL-Key
1370                                                        // 4: EAPOL-Encapsulated-ASF-Alert
1371
1372     /* EAPOL frame format */
1373     /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
1374     /* -----------------------------------------------   */
1375     /*            PAE Ethernet Type (0x888e)             */
1376     /* ----------------------------------------------- 2 */
1377     /*     Protocol Version    |         Type            */
1378     /* ----------------------------------------------- 4 */
1379     /*                       Length                      */
1380     /* ----------------------------------------------- 6 */
1381     /*                    Packet Body                    */
1382     /* ----------------------------------------------- N */
1383
1384     /* EAPOL body length */
1385     packetLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+2)) << 8) +
1386                 zmw_rx_buf_readb(dev, buf, offset+3);
1387
1388     if( packetType == 0 )
1389     { // EAP-Packet
1390
1391         /* EAP-Packet Code */
1392         code = zmw_rx_buf_readb(dev, buf, offset+4); // 1 : Request
1393                                                      // 2 : Response
1394                                                      // 3 : Success
1395                                                      // 4 : Failure
1396         // An EAP packet of the type of Success and Failure has no Data field, and has a length of 4.
1397
1398         /* EAP Packet format */
1399         /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
1400         /* -----------------------------------------------   */
1401         /*           Code          |        Identifier       */
1402         /* ----------------------------------------------- 2 */
1403         /*                       Length                      */
1404         /* ----------------------------------------------- 4 */
1405         /*                        Data                       */
1406         /* ----------------------------------------------- N */
1407
1408         zm_debug_msg0("EAP-Packet");
1409         zm_debug_msg1("Packet Length = ", packetLen);
1410         zm_debug_msg1("EAP-Packet Code = ", code);
1411
1412         if( code == 1 )
1413         {
1414             zm_debug_msg0("EAP-Packet Request");
1415
1416             /* EAP-Packet Identifier */
1417             identifier = zmw_rx_buf_readb(dev, buf, offset+5);
1418             /* EAP-Packet Length */
1419             length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
1420                       zmw_rx_buf_readb(dev, buf, offset+7);
1421             /* EAP-Packet Type */
1422             type = zmw_rx_buf_readb(dev, buf, offset+8); // 1   : Identity
1423                                                          // 2   : Notification
1424                                                          // 3   : Nak (Response Only)
1425                                                          // 4   : MD5-Challenge
1426                                                          // 5   : One Time Password (OTP)
1427                                                          // 6   : Generic Token Card (GTC)
1428                                                          // 254 : (Expanded Types)Wi-Fi Protected Setup
1429                                                          // 255 : Experimental Use
1430
1431             /* The data field in an EAP packet of the type of Request or Response is in the format shown bellowing */
1432             /*  0  1  2  3  4  5  6  7             N             */
1433             /* -----------------------------------------------   */
1434             /*           Type          |        Type Data        */
1435             /* -----------------------------------------------   */
1436
1437             zm_debug_msg1("EAP-Packet Identifier = ", identifier);
1438             zm_debug_msg1("EAP-Packet Length = ", length);
1439             zm_debug_msg1("EAP-Packet Type = ", type);
1440
1441             if( type == 1 )
1442             {
1443                 zm_debug_msg0("EAP-Packet Request Identity");
1444             }
1445             else if( type == 2 )
1446             {
1447                 zm_debug_msg0("EAP-Packet Request Notification");
1448             }
1449             else if( type == 4 )
1450             {
1451                 zm_debug_msg0("EAP-Packet Request MD5-Challenge");
1452             }
1453             else if( type == 5 )
1454             {
1455                 zm_debug_msg0("EAP-Packet Request One Time Password");
1456             }
1457             else if( type == 6 )
1458             {
1459                 zm_debug_msg0("EAP-Packet Request Generic Token Card");
1460             }
1461             else if( type == 254 )
1462             {
1463                 zm_debug_msg0("EAP-Packet Request Wi-Fi Protected Setup");
1464
1465                 /* 0                   1                   2                   3   */
1466                 /* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 */
1467                 /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
1468                 /*|     Type      |               Vendor-Id                       |*/
1469                 /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
1470                 /*|                          Vendor-Type                          |*/
1471                 /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
1472                 /*|              Vendor data...                                    */
1473                 /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+                        */
1474
1475                 /* EAP-Packet Vendor ID */
1476                 vendorId = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 16) +
1477                            (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 8) +
1478                            zmw_rx_buf_readb(dev, buf, offset+11);
1479                 /* EAP-Packet Vendor Type */
1480                 VendorType = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+12)) << 24) +
1481                              (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 16) +
1482                              (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 8) +
1483                              zmw_rx_buf_readb(dev, buf, offset+15);
1484                 /* EAP-Packet Op Code */
1485                 Op_Code = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+16)) << 8) +
1486                           zmw_rx_buf_readb(dev, buf, offset+17);
1487                 /* EAP-Packet Flags */
1488                 flags = zmw_rx_buf_readb(dev, buf, offset+18);
1489
1490                 zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
1491                 zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
1492                 zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
1493                 zm_debug_msg1("EAP-Packet Flags = ", flags);
1494             }
1495         }
1496         else if( code == 2 )
1497         {
1498             zm_debug_msg0("EAP-Packet Response");
1499
1500             /* EAP-Packet Identifier */
1501             identifier = zmw_rx_buf_readb(dev, buf, offset+5);
1502             /* EAP-Packet Length */
1503             length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
1504                       zmw_rx_buf_readb(dev, buf, offset+7);
1505             /* EAP-Packet Type */
1506             type = zmw_rx_buf_readb(dev, buf, offset+8);
1507
1508             zm_debug_msg1("EAP-Packet Identifier = ", identifier);
1509             zm_debug_msg1("EAP-Packet Length = ", length);
1510             zm_debug_msg1("EAP-Packet Type = ", type);
1511
1512             if( type == 1 )
1513             {
1514                 zm_debug_msg0("EAP-Packet Response Identity");
1515             }
1516             else if( type == 2 )
1517             {
1518                 zm_debug_msg0("EAP-Packet Request Notification");
1519             }
1520             else if( type == 3 )
1521             {
1522                 zm_debug_msg0("EAP-Packet Request Nak");
1523             }
1524             else if( type == 4 )
1525             {
1526                 zm_debug_msg0("EAP-Packet Request MD5-Challenge");
1527             }
1528             else if( type == 5 )
1529             {
1530                 zm_debug_msg0("EAP-Packet Request One Time Password");
1531             }
1532             else if( type == 6 )
1533             {
1534                 zm_debug_msg0("EAP-Packet Request Generic Token Card");
1535             }
1536             else if( type == 254 )
1537             {
1538                 zm_debug_msg0("EAP-Packet Response Wi-Fi Protected Setup");
1539
1540                 /* EAP-Packet Vendor ID */
1541                 vendorId = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 16) +
1542                            (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 8) +
1543                            zmw_rx_buf_readb(dev, buf, offset+11);
1544                 /* EAP-Packet Vendor Type */
1545                 VendorType = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+12)) << 24) +
1546                              (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 16) +
1547                              (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 8) +
1548                              zmw_rx_buf_readb(dev, buf, offset+15);
1549                 /* EAP-Packet Op Code */
1550                 Op_Code = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+16)) << 8) +
1551                           zmw_rx_buf_readb(dev, buf, offset+17);
1552                 /* EAP-Packet Flags */
1553                 flags = zmw_rx_buf_readb(dev, buf, offset+18);
1554
1555                 zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
1556                 zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
1557                 zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
1558                 zm_debug_msg1("EAP-Packet Flags = ", flags);
1559             }
1560         }
1561         else if( code == 3 )
1562         {
1563             zm_debug_msg0("EAP-Packet Success");
1564
1565             /* EAP-Packet Identifier */
1566             identifier = zmw_rx_buf_readb(dev, buf, offset+5);
1567             /* EAP-Packet Length */
1568             length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
1569                       zmw_rx_buf_readb(dev, buf, offset+7);
1570
1571             zm_debug_msg1("EAP-Packet Identifier = ", identifier);
1572             zm_debug_msg1("EAP-Packet Length = ", length);
1573         }
1574         else if( code == 4 )
1575         {
1576             zm_debug_msg0("EAP-Packet Failure");
1577
1578             /* EAP-Packet Identifier */
1579             identifier = zmw_rx_buf_readb(dev, buf, offset+5);
1580             /* EAP-Packet Length */
1581             length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
1582                       zmw_rx_buf_readb(dev, buf, offset+7);
1583
1584             zm_debug_msg1("EAP-Packet Identifier = ", identifier);
1585             zm_debug_msg1("EAP-Packet Length = ", length);
1586         }
1587     }
1588     else if( packetType == 1 )
1589     { // EAPOL-Start
1590         zm_debug_msg0("EAPOL-Start");
1591     }
1592     else if( packetType == 2 )
1593     { // EAPOL-Logoff
1594         zm_debug_msg0("EAPOL-Logoff");
1595     }
1596     else if( packetType == 3 )
1597     { // EAPOL-Key
1598         /* EAPOL-Key type */
1599         keyType = zmw_rx_buf_readb(dev, buf, offset+4);
1600         /* EAPOL-Key information */
1601         keyInfo = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+5)) << 8) +
1602                   zmw_rx_buf_readb(dev, buf, offset+6);
1603         /* EAPOL-Key length */
1604         keyLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+7)) << 8) +
1605                  zmw_rx_buf_readb(dev, buf, offset+8);
1606         /* EAPOL-Key replay counter (high double word) */
1607         replayCounterH = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 24) +
1608                          (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 16) +
1609                          (((u32_t) zmw_rx_buf_readb(dev, buf, offset+11)) << 8) +
1610                          zmw_rx_buf_readb(dev, buf, offset+12);
1611         /* EAPOL-Key replay counter (low double word) */
1612         replayCounterL = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 24) +
1613                          (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 16) +
1614                          (((u32_t) zmw_rx_buf_readb(dev, buf, offset+15)) << 8) +
1615                          zmw_rx_buf_readb(dev, buf, offset+16);
1616         /* EAPOL-Key data length */
1617         keyDataLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+97)) << 8) +
1618                      zmw_rx_buf_readb(dev, buf, offset+98);
1619
1620         zm_debug_msg0("EAPOL-Key");
1621         zm_debug_msg1("packet length = ", packetLen);
1622
1623         if ( keyType == 254 )
1624         {
1625             zm_debug_msg0("key type = 254 (SSN key descriptor)");
1626         }
1627         else
1628         {
1629             zm_debug_msg2("key type = 0x", keyType);
1630         }
1631
1632         zm_debug_msg2("replay counter(L) = ", replayCounterL);
1633
1634         zm_debug_msg2("key information = ", keyInfo);
1635
1636         if ( keyInfo & ZM_BIT_3 )
1637         {
1638             zm_debug_msg0("    - pairwise key");
1639         }
1640         else
1641         {
1642             zm_debug_msg0("    - group key");
1643         }
1644
1645         if ( keyInfo & ZM_BIT_6 )
1646         {
1647             zm_debug_msg0("    - Tx key installed");
1648         }
1649         else
1650         {
1651             zm_debug_msg0("    - Tx key not set");
1652         }
1653
1654         if ( keyInfo & ZM_BIT_7 )
1655         {
1656             zm_debug_msg0("    - Ack needed");
1657         }
1658         else
1659         {
1660             zm_debug_msg0("    - Ack not needed");
1661         }
1662
1663         if ( keyInfo & ZM_BIT_8 )
1664         {
1665             zm_debug_msg0("    - MIC set");
1666         }
1667         else
1668         {
1669             zm_debug_msg0("    - MIC not set");
1670         }
1671
1672         if ( keyInfo & ZM_BIT_9 )
1673         {
1674             zm_debug_msg0("    - packet encrypted");
1675         }
1676         else
1677         {
1678             zm_debug_msg0("    - packet not encrypted");
1679         }
1680
1681         zm_debug_msg1("keyLen = ", keyLen);
1682         zm_debug_msg1("keyDataLen = ", keyDataLen);
1683     }
1684     else if( packetType == 4 )
1685     {
1686         zm_debug_msg0("EAPOL-Encapsulated-ASF-Alert");
1687     }
1688 }
1689
1690 void zfShowTxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset)
1691 {
1692     u8_t   packetType, keyType, code, identifier, type, flags;
1693     u16_t  packetLen, keyInfo, keyLen, keyDataLen, length, Op_Code;
1694     u32_t  replayCounterH, replayCounterL, vendorId, VendorType;
1695
1696     zmw_get_wlan_dev(dev);
1697
1698     zm_debug_msg1("EAPOL Packet size = ", zfwBufGetSize(dev, buf));
1699
1700     /* EAPOL packet type */
1701     // 0: EAP-Packet
1702     // 1: EAPOL-Start
1703     // 2: EAPOL-Logoff
1704     // 3: EAPOL-Key
1705     // 4: EAPOL-Encapsulated-ASF-Alert
1706
1707     /* EAPOL frame format */
1708     /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
1709     /* -----------------------------------------------   */
1710     /*            PAE Ethernet Type (0x888e)             */
1711     /* ----------------------------------------------- 2 */
1712     /*     Protocol Version    |         Type            */
1713     /* ----------------------------------------------- 4 */
1714     /*                       Length                      */
1715     /* ----------------------------------------------- 6 */
1716     /*                    Packet Body                    */
1717     /* ----------------------------------------------- N */
1718
1719     packetType = zmw_tx_buf_readb(dev, buf, offset+1);
1720     /* EAPOL body length */
1721     packetLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+2)) << 8) +
1722                 zmw_tx_buf_readb(dev, buf, offset+3);
1723
1724     if( packetType == 0 )
1725     { // EAP-Packet
1726         /* EAP-Packet Code */
1727         code = zmw_tx_buf_readb(dev, buf, offset+4); // 1 : Request
1728                                                      // 2 : Response
1729                                                      // 3 : Success
1730                                                      // 4 : Failure
1731
1732         // An EAP packet of the type of Success and Failure has no Data field, and has a length of 4.
1733
1734         /* EAP Packet format */
1735         /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
1736         /* -----------------------------------------------   */
1737         /*           Code          |        Identifier       */
1738         /* ----------------------------------------------- 2 */
1739         /*                       Length                      */
1740         /* ----------------------------------------------- 4 */
1741         /*                        Data                       */
1742         /* ----------------------------------------------- N */
1743
1744         zm_debug_msg0("EAP-Packet");
1745         zm_debug_msg1("Packet Length = ", packetLen);
1746         zm_debug_msg1("EAP-Packet Code = ", code);
1747
1748         if( code == 1 )
1749         {
1750             zm_debug_msg0("EAP-Packet Request");
1751
1752             /* EAP-Packet Identifier */
1753             identifier = zmw_tx_buf_readb(dev, buf, offset+5);
1754             /* EAP-Packet Length */
1755             length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
1756                       zmw_tx_buf_readb(dev, buf, offset+7);
1757             /* EAP-Packet Type */
1758             type = zmw_tx_buf_readb(dev, buf, offset+8); // 1   : Identity
1759                                                          // 2   : Notification
1760                                                          // 3   : Nak (Response Only)
1761                                                          // 4   : MD5-Challenge
1762                                                          // 5   : One Time Password (OTP)
1763                                                          // 6   : Generic Token Card (GTC)
1764                                                          // 254 : (Expanded Types)Wi-Fi Protected Setup
1765                                                          // 255 : Experimental Use
1766
1767             /* The data field in an EAP packet of the type of Request or Response is in the format shown bellowing */
1768             /*  0  1  2  3  4  5  6  7             N             */
1769             /* -----------------------------------------------   */
1770             /*           Type          |        Type Data        */
1771             /* -----------------------------------------------   */
1772
1773             zm_debug_msg1("EAP-Packet Identifier = ", identifier);
1774             zm_debug_msg1("EAP-Packet Length = ", length);
1775             zm_debug_msg1("EAP-Packet Type = ", type);
1776
1777             if( type == 1 )
1778             {
1779                 zm_debug_msg0("EAP-Packet Request Identity");
1780             }
1781             else if( type == 2 )
1782             {
1783                 zm_debug_msg0("EAP-Packet Request Notification");
1784             }
1785             else if( type == 4 )
1786             {
1787                 zm_debug_msg0("EAP-Packet Request MD5-Challenge");
1788             }
1789             else if( type == 5 )
1790             {
1791                 zm_debug_msg0("EAP-Packet Request One Time Password");
1792             }
1793             else if( type == 6 )
1794             {
1795                 zm_debug_msg0("EAP-Packet Request Generic Token Card");
1796             }
1797             else if( type == 254 )
1798             {
1799                 zm_debug_msg0("EAP-Packet Request Wi-Fi Protected Setup");
1800
1801                 /* 0                   1                   2                   3   */
1802                 /* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 */
1803                 /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
1804                 /*|     Type      |               Vendor-Id                       |*/
1805                 /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
1806                 /*|                          Vendor-Type                          |*/
1807                 /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
1808                 /*|              Vendor data...                                    */
1809                 /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+                        */
1810
1811                 /* EAP-Packet Vendor ID */
1812                 vendorId = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 16) +
1813                            (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 8) +
1814                            zmw_tx_buf_readb(dev, buf, offset+11);
1815                 /* EAP-Packet Vendor Type */
1816                 VendorType = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+12)) << 24) +
1817                              (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 16) +
1818                              (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 8) +
1819                              zmw_tx_buf_readb(dev, buf, offset+15);
1820                 /* EAP-Packet Op Code */
1821                 Op_Code = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+16)) << 8) +
1822                           zmw_tx_buf_readb(dev, buf, offset+17);
1823                 /* EAP-Packet Flags */
1824                 flags = zmw_tx_buf_readb(dev, buf, offset+18);
1825
1826                 zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
1827                 zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
1828                 zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
1829                 zm_debug_msg1("EAP-Packet Flags = ", flags);
1830             }
1831         }
1832         else if( code == 2 )
1833         {
1834             zm_debug_msg0("EAP-Packet Response");
1835
1836             /* EAP-Packet Identifier */
1837             identifier = zmw_tx_buf_readb(dev, buf, offset+5);
1838             /* EAP-Packet Length */
1839             length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
1840                       zmw_tx_buf_readb(dev, buf, offset+7);
1841             /* EAP-Packet Type */
1842             type = zmw_tx_buf_readb(dev, buf, offset+8);
1843
1844             zm_debug_msg1("EAP-Packet Identifier = ", identifier);
1845             zm_debug_msg1("EAP-Packet Length = ", length);
1846             zm_debug_msg1("EAP-Packet Type = ", type);
1847
1848             if( type == 1 )
1849             {
1850                 zm_debug_msg0("EAP-Packet Response Identity");
1851             }
1852             else if( type == 2 )
1853             {
1854                 zm_debug_msg0("EAP-Packet Request Notification");
1855             }
1856             else if( type == 3 )
1857             {
1858                 zm_debug_msg0("EAP-Packet Request Nak");
1859             }
1860             else if( type == 4 )
1861             {
1862                 zm_debug_msg0("EAP-Packet Request MD5-Challenge");
1863             }
1864             else if( type == 5 )
1865             {
1866                 zm_debug_msg0("EAP-Packet Request One Time Password");
1867             }
1868             else if( type == 6 )
1869             {
1870                 zm_debug_msg0("EAP-Packet Request Generic Token Card");
1871             }
1872             else if( type == 254 )
1873             {
1874                 zm_debug_msg0("EAP-Packet Response Wi-Fi Protected Setup");
1875
1876                 /* EAP-Packet Vendor ID */
1877                 vendorId = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 16) +
1878                            (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 8) +
1879                            zmw_tx_buf_readb(dev, buf, offset+11);
1880                 /* EAP-Packet Vendor Type */
1881                 VendorType = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+12)) << 24) +
1882                              (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 16) +
1883                              (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 8) +
1884                              zmw_tx_buf_readb(dev, buf, offset+15);
1885                 /* EAP-Packet Op Code */
1886                 Op_Code = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+16)) << 8) +
1887                           zmw_tx_buf_readb(dev, buf, offset+17);
1888                 /* EAP-Packet Flags */
1889                 flags = zmw_tx_buf_readb(dev, buf, offset+18);
1890
1891                 zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
1892                 zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
1893                 zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
1894                 zm_debug_msg1("EAP-Packet Flags = ", flags);
1895             }
1896         }
1897         else if( code == 3 )
1898         {
1899             zm_debug_msg0("EAP-Packet Success");
1900
1901             /* EAP-Packet Identifier */
1902             identifier = zmw_rx_buf_readb(dev, buf, offset+5);
1903             /* EAP-Packet Length */
1904             length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
1905                       zmw_rx_buf_readb(dev, buf, offset+7);
1906
1907             zm_debug_msg1("EAP-Packet Identifier = ", identifier);
1908             zm_debug_msg1("EAP-Packet Length = ", length);
1909         }
1910         else if( code == 4 )
1911         {
1912             zm_debug_msg0("EAP-Packet Failure");
1913
1914             /* EAP-Packet Identifier */
1915             identifier = zmw_tx_buf_readb(dev, buf, offset+5);
1916             /* EAP-Packet Length */
1917             length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
1918                       zmw_tx_buf_readb(dev, buf, offset+7);
1919
1920             zm_debug_msg1("EAP-Packet Identifier = ", identifier);
1921             zm_debug_msg1("EAP-Packet Length = ", length);
1922         }
1923     }
1924     else if( packetType == 1 )
1925     { // EAPOL-Start
1926         zm_debug_msg0("EAPOL-Start");
1927     }
1928     else if( packetType == 2 )
1929     { // EAPOL-Logoff
1930         zm_debug_msg0("EAPOL-Logoff");
1931     }
1932     else if( packetType == 3 )
1933     { // EAPOL-Key
1934         /* EAPOL-Key type */
1935         keyType = zmw_tx_buf_readb(dev, buf, offset+4);
1936         /* EAPOL-Key information */
1937         keyInfo = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+5)) << 8) +
1938                   zmw_tx_buf_readb(dev, buf, offset+6);
1939         /* EAPOL-Key length */
1940         keyLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+7)) << 8) +
1941                  zmw_tx_buf_readb(dev, buf, offset+8);
1942         /* EAPOL-Key replay counter (high double word) */
1943         replayCounterH = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 24) +
1944                          (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 16) +
1945                          (((u32_t) zmw_tx_buf_readb(dev, buf, offset+11)) << 8) +
1946                          zmw_tx_buf_readb(dev, buf, offset+12);
1947         /* EAPOL-Key replay counter (low double word) */
1948         replayCounterL = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 24) +
1949                          (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 16) +
1950                          (((u32_t) zmw_tx_buf_readb(dev, buf, offset+15)) << 8) +
1951                          zmw_tx_buf_readb(dev, buf, offset+16);
1952         /* EAPOL-Key data length */
1953         keyDataLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+97)) << 8) +
1954                      zmw_tx_buf_readb(dev, buf, offset+98);
1955
1956         zm_debug_msg0("EAPOL-Key");
1957         zm_debug_msg1("packet length = ", packetLen);
1958
1959         if ( keyType == 254 )
1960         {
1961             zm_debug_msg0("key type = 254 (SSN key descriptor)");
1962         }
1963         else
1964         {
1965             zm_debug_msg2("key type = 0x", keyType);
1966         }
1967
1968         zm_debug_msg2("replay counter(L) = ", replayCounterL);
1969
1970         zm_debug_msg2("key information = ", keyInfo);
1971
1972         if ( keyInfo & ZM_BIT_3 )
1973         {
1974             zm_debug_msg0("    - pairwise key");
1975         }
1976         else
1977         {
1978             zm_debug_msg0("    - group key");
1979         }
1980
1981         if ( keyInfo & ZM_BIT_6 )
1982         {
1983             zm_debug_msg0("    - Tx key installed");
1984         }
1985         else
1986         {
1987             zm_debug_msg0("    - Tx key not set");
1988         }
1989
1990         if ( keyInfo & ZM_BIT_7 )
1991         {
1992             zm_debug_msg0("    - Ack needed");
1993         }
1994         else
1995         {
1996             zm_debug_msg0("    - Ack not needed");
1997         }
1998
1999         if ( keyInfo & ZM_BIT_8 )
2000         {
2001             zm_debug_msg0("    - MIC set");
2002         }
2003         else
2004         {
2005             zm_debug_msg0("    - MIC not set");
2006         }
2007
2008         if ( keyInfo & ZM_BIT_9 )
2009         {
2010             zm_debug_msg0("    - packet encrypted");
2011         }
2012         else
2013         {
2014             zm_debug_msg0("    - packet not encrypted");
2015         }
2016
2017         zm_debug_msg1("keyLen = ", keyLen);
2018         zm_debug_msg1("keyDataLen = ", keyDataLen);
2019     }
2020     else if( packetType == 4 )
2021     {
2022         zm_debug_msg0("EAPOL-Encapsulated-ASF-Alert");
2023     }
2024 }
2025
2026
2027 /************************************************************************/
2028 /*                                                                      */
2029 /*    FUNCTION DESCRIPTION                  zfiRecv80211                */
2030 /*      Called to receive 802.11 frame.                                 */
2031 /*                                                                      */
2032 /*    INPUTS                                                            */
2033 /*      dev : device pointer                                            */
2034 /*      buf : received 802.11 frame buffer.                             */
2035 /*                                                                      */
2036 /*    OUTPUTS                                                           */
2037 /*      None                                                            */
2038 /*                                                                      */
2039 /*    AUTHOR                                                            */
2040 /*      Stephen             ZyDAS Technology Corporation    2005.5      */
2041 /*                                                                      */
2042 /************************************************************************/
2043 void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
2044 {
2045     u8_t snapCase=0, encryMode;
2046     u16_t frameType, typeLengthField;
2047     u16_t frameCtrl;
2048     u16_t frameSubtype;
2049     u16_t ret;
2050     u16_t len;
2051     u8_t bIsDefrag = 0;
2052     u16_t offset, tailLen;
2053     u8_t vap = 0;
2054     u16_t da[3], sa[3];
2055     u16_t ii;
2056     u8_t uapsdTrig = 0;
2057     zbuf_t* psBuf;
2058 #ifdef ZM_ENABLE_NATIVE_WIFI
2059     u8_t i;
2060 #endif
2061
2062     zmw_get_wlan_dev(dev);
2063
2064     ZM_BUFFER_TRACE(dev, buf)
2065
2066     //zm_msg2_rx(ZM_LV_2, "zfiRecv80211(), buf=", buf);
2067
2068     //zm_msg2_rx(ZM_LV_0, "h[0]=", zmw_rx_buf_readh(dev, buf, 0));
2069     //zm_msg2_rx(ZM_LV_0, "h[2]=", zmw_rx_buf_readh(dev, buf, 2));
2070     //zm_msg2_rx(ZM_LV_0, "h[4]=", zmw_rx_buf_readh(dev, buf, 4));
2071
2072     frameCtrl = zmw_rx_buf_readb(dev, buf, 0);
2073     frameType = frameCtrl & 0xf;
2074     frameSubtype = frameCtrl & 0xf0;
2075
2076 #if 0   // Move to ProcessBeacon to judge if there's a new peer station
2077     if ( (wd->wlanMode == ZM_MODE_IBSS)&&
2078          (wd->sta.ibssPartnerStatus != ZM_IBSS_PARTNER_ALIVE) )
2079     {
2080         zfStaIbssMonitoring(dev, buf);
2081     }
2082 #endif
2083
2084     /* If data frame */
2085     if (frameType == ZM_WLAN_DATA_FRAME)
2086     {
2087         wd->sta.TotalNumberOfReceivePackets++;
2088         wd->sta.TotalNumberOfReceiveBytes += zfwBufGetSize(dev, buf);
2089         //zm_debug_msg1("Receive packets     = ", wd->sta.TotalNumberOfReceivePackets);
2090
2091         //zm_msg0_rx(ZM_LV_0, "Rx data");
2092         if (wd->wlanMode == ZM_MODE_AP)
2093         {
2094             if ((ret = zfApUpdatePsBit(dev, buf, &vap, &uapsdTrig)) != ZM_SUCCESS)
2095             {
2096                 zfwBufFree(dev, buf, 0);
2097                 return;
2098             }
2099
2100             if (((uapsdTrig&0xf) != 0) && ((frameSubtype & 0x80) != 0))
2101             {
2102                 u8_t ac = zcUpToAc[zmw_buf_readb(dev, buf, 24)&0x7];
2103                 u8_t pktNum;
2104                 u8_t mb;
2105                 u16_t flag;
2106                 u8_t src[6];
2107
2108                 //printk("QoS ctrl=%d\n", zmw_buf_readb(dev, buf, 24));
2109                 //printk("UAPSD trigger, ac=%d\n", ac);
2110
2111                 if (((0x8>>ac) & uapsdTrig) != 0)
2112                 {
2113                     pktNum = zcMaxspToPktNum[(uapsdTrig>>4) & 0x3];
2114
2115                     for (ii=0; ii<6; ii++)
2116                     {
2117                         src[ii] = zmw_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+ii);
2118                     }
2119
2120                     for (ii=0; ii<pktNum; ii++)
2121                     {
2122                         //if ((psBuf = zfQueueGet(dev, wd->ap.uapsdQ)) != NULL)
2123                         if ((psBuf = zfQueueGetWithMac(dev, wd->ap.uapsdQ, src, &mb)) != NULL)
2124                         {
2125                             if ((ii+1) == pktNum)
2126                             {
2127                                 //EOSP anyway
2128                                 flag = 0x100 | (mb<<5);
2129                             }
2130                             else
2131                             {
2132                                 if (mb != 0)
2133                                 {
2134                                     //more data, not EOSP
2135                                     flag = 0x20;
2136                                 }
2137                                 else
2138                                 {
2139                                     //no more data, EOSP
2140                                     flag = 0x100;
2141                                 }
2142                             }
2143                             zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, flag);
2144                         }
2145
2146                         if ((psBuf == NULL) || (mb == 0))
2147                         {
2148                             if ((ii == 0) && (psBuf == NULL))
2149                             {
2150                                 zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_QOS_NULL, (u16_t*)src, 0, 0, 0);
2151                             }
2152                             break;
2153                         }
2154                     }
2155                 }
2156             }
2157
2158         }
2159         else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
2160         {
2161             u16_t frameCtrlMSB;
2162                 u8_t   bssid[6];
2163
2164             /* Check Is RIFS frame and decide to enable RIFS or not */
2165             if( wd->sta.EnableHT )
2166                 zfCheckIsRIFSFrame(dev, buf, frameSubtype);
2167
2168             if ( zfPowerSavingMgrIsSleeping(dev) || wd->sta.psMgr.tempWakeUp == 1)
2169             {
2170                 frameCtrlMSB = zmw_rx_buf_readb(dev, buf, 1);
2171
2172                 /* check more data */
2173                 if ( frameCtrlMSB & ZM_BIT_5 )
2174                 {
2175                     //if rx frame's AC is not delivery-enabled
2176                     if ((wd->sta.qosInfo&0xf) != 0xf)
2177                     {
2178                         u8_t rxAc = 0;
2179                         if ((frameSubtype & 0x80) != 0)
2180                         {
2181                             rxAc = zcUpToAc[zmw_buf_readb(dev, buf, 24)&0x7];
2182                         }
2183
2184                         if (((0x8>>rxAc) & wd->sta.qosInfo) == 0)
2185                         {
2186                             zfSendPSPoll(dev);
2187                             wd->sta.psMgr.tempWakeUp = 0;
2188                         }
2189                     }
2190                 }
2191             }
2192                         /*increase beacon count when receive vaild data frame from AP*/
2193                 ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
2194
2195                         if (zfStaIsConnected(dev)&&
2196                                 zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6))
2197                         {
2198                 wd->sta.rxBeaconCount++;
2199                         }
2200         }
2201
2202         zm_msg1_rx(ZM_LV_2, "Rx VAP=", vap);
2203
2204         /* handle IV, EXT-IV, ICV, and EXT-ICV */
2205         zfGetRxIvIcvLength(dev, buf, vap, &offset, &tailLen, addInfo);
2206
2207         zfStaIbssPSCheckState(dev, buf);
2208         //QoS data frame
2209         if ((frameSubtype & 0x80) == 0x80)
2210         {
2211             offset += 2;
2212         }
2213
2214         len = zfwBufGetSize(dev, buf);
2215         /* remove ICV */
2216         if (tailLen > 0)
2217         {
2218             if (len > tailLen)
2219             {
2220                 len -= tailLen;
2221                 zfwBufSetSize(dev, buf, len);
2222             }
2223         }
2224
2225         /* Filter NULL data */
2226         if (((frameSubtype&0x40) != 0) || ((len = zfwBufGetSize(dev, buf))<=24))
2227         {
2228             zm_msg1_rx(ZM_LV_1, "Free Rx NULL data, len=", len);
2229             zfwBufFree(dev, buf, 0);
2230             return;
2231         }
2232
2233         /* check and handle defragmentation */
2234         if ( wd->sta.bSafeMode && (wd->sta.wepStatus == ZM_ENCRYPTION_AES) && wd->sta.SWEncryptEnable )
2235         {
2236             zm_msg0_rx(ZM_LV_1, "Bypass defragmentation packets in safe mode");
2237         }
2238         else
2239         {
2240             if ( (buf = zfDefragment(dev, buf, &bIsDefrag, addInfo)) == NULL )
2241             {
2242                 /* In this case, the buffer has been freed in zfDefragment */
2243                 return;
2244             }
2245         }
2246
2247         ret = ZM_MIC_SUCCESS;
2248
2249         /* If SW WEP/TKIP are not turned on */
2250         if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_DECRY_EN) == 0 &&
2251             (wd->sta.SWEncryptEnable & ZM_SW_WEP_DECRY_EN) == 0)
2252         {
2253             encryMode = zfGetEncryModeFromRxStatus(addInfo);
2254
2255             /* check if TKIP */
2256             if ( encryMode == ZM_TKIP )
2257             {
2258                 if ( bIsDefrag )
2259                 {
2260                     ret = zfMicRxVerify(dev, buf);
2261                 }
2262                 else
2263                 {
2264                     /* check MIC failure bit */
2265                     if ( ZM_RX_STATUS_IS_MIC_FAIL(addInfo) )
2266                     {
2267                         ret = ZM_MIC_FAILURE;
2268                     }
2269                 }
2270
2271                 if ( ret == ZM_MIC_FAILURE )
2272                 {
2273                     u8_t Unicast_Pkt = 0x0;
2274
2275                     if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
2276                     {
2277                         wd->commTally.swRxUnicastMicFailCount++;
2278                         Unicast_Pkt = 0x1;
2279                     }/*
2280                     else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
2281                     {
2282                         wd->commTally.swRxMulticastMicFailCount++;
2283                     }*/
2284                     else
2285                     {
2286                         wd->commTally.swRxMulticastMicFailCount++;
2287                     }
2288                     if ( wd->wlanMode == ZM_MODE_AP )
2289                     {
2290                         u16_t idx;
2291                         u8_t addr[6];
2292
2293                         for (idx=0; idx<6; idx++)
2294                         {
2295                             addr[idx] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+idx);
2296                         }
2297
2298                         if (wd->zfcbApMicFailureNotify != NULL)
2299                         {
2300                             wd->zfcbApMicFailureNotify(dev, addr, buf);
2301                         }
2302                     }
2303                     else
2304                     {
2305                         if(Unicast_Pkt)
2306                         {
2307                             zm_debug_msg0("Countermeasure : Unicast_Pkt ");
2308                         }
2309                         else
2310                         {
2311                             zm_debug_msg0("Countermeasure : Non-Unicast_Pkt ");
2312                         }
2313
2314                         if((wd->TKIP_Group_KeyChanging == 0x0) || (Unicast_Pkt == 0x1))
2315                         {
2316                             zm_debug_msg0("Countermeasure : Do MIC Check ");
2317                             zfStaMicFailureHandling(dev, buf);
2318                         }
2319                         else
2320                         {
2321                             zm_debug_msg0("Countermeasure : SKIP MIC Check due to Group Keychanging ");
2322                         }
2323                     }
2324                     /* Discard MIC failed frame */
2325                     zfwBufFree(dev, buf, 0);
2326                     return;
2327                 }
2328             }
2329         }
2330         else
2331         {
2332             u8_t IsEncryFrame;
2333
2334             /* TODO: Check whether WEP bit is turned on in MAC header */
2335             encryMode = ZM_NO_WEP;
2336
2337             IsEncryFrame = (zmw_rx_buf_readb(dev, buf, 1) & 0x40);
2338
2339             if (IsEncryFrame)
2340             {
2341                 /* Software decryption for TKIP */
2342                 if (wd->sta.SWEncryptEnable & ZM_SW_TKIP_DECRY_EN)
2343                 {
2344                     u16_t iv16;
2345                     u16_t iv32;
2346                     u8_t RC4Key[16];
2347                     u16_t IvOffset;
2348                     struct zsTkipSeed *rxSeed;
2349
2350                     IvOffset = offset + ZM_SIZE_OF_WLAN_DATA_HEADER;
2351
2352                     rxSeed = zfStaGetRxSeed(dev, buf);
2353
2354                     if (rxSeed == NULL)
2355                     {
2356                         zm_debug_msg0("rxSeed is NULL");
2357
2358                         /* Discard this frame */
2359                         zfwBufFree(dev, buf, 0);
2360                         return;
2361                     }
2362
2363                     iv16 = (zmw_rx_buf_readb(dev, buf, IvOffset) << 8) + zmw_rx_buf_readb(dev, buf, IvOffset+2);
2364                     iv32 = zmw_rx_buf_readb(dev, buf, IvOffset+4) +
2365                            (zmw_rx_buf_readb(dev, buf, IvOffset+5) << 8) +
2366                            (zmw_rx_buf_readb(dev, buf, IvOffset+6) << 16) +
2367                            (zmw_rx_buf_readb(dev, buf, IvOffset+7) << 24);
2368
2369                     /* TKIP Key Mixing */
2370                     zfTkipPhase1KeyMix(iv32, rxSeed);
2371                     zfTkipPhase2KeyMix(iv16, rxSeed);
2372                     zfTkipGetseeds(iv16, RC4Key, rxSeed);
2373
2374                     /* Decrypt Data */
2375                     ret = zfTKIPDecrypt(dev, buf, IvOffset+ZM_SIZE_OF_IV+ZM_SIZE_OF_EXT_IV, 16, RC4Key);
2376
2377                     if (ret == ZM_ICV_FAILURE)
2378                     {
2379                         zm_debug_msg0("TKIP ICV fail");
2380
2381                         /* Discard ICV failed frame */
2382                         zfwBufFree(dev, buf, 0);
2383                         return;
2384                     }
2385
2386                     /* Remove ICV from buffer */
2387                     zfwBufSetSize(dev, buf, len-4);
2388
2389                     /* Check MIC */
2390                     ret = zfMicRxVerify(dev, buf);
2391
2392                     if (ret == ZM_MIC_FAILURE)
2393                     {
2394                         if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
2395                         {
2396                             wd->commTally.swRxUnicastMicFailCount++;
2397                         }
2398                         else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
2399                         {
2400                             wd->commTally.swRxMulticastMicFailCount++;
2401                         }
2402                         else
2403                         {
2404                             wd->commTally.swRxMulticastMicFailCount++;
2405                         }
2406                         if ( wd->wlanMode == ZM_MODE_AP )
2407                         {
2408                             u16_t idx;
2409                             u8_t addr[6];
2410
2411                             for (idx=0; idx<6; idx++)
2412                             {
2413                                 addr[idx] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+idx);
2414                             }
2415
2416                             if (wd->zfcbApMicFailureNotify != NULL)
2417                             {
2418                                 wd->zfcbApMicFailureNotify(dev, addr, buf);
2419                             }
2420                         }
2421                         else
2422                         {
2423                             zfStaMicFailureHandling(dev, buf);
2424                         }
2425
2426                         zm_debug_msg0("MIC fail");
2427                         /* Discard MIC failed frame */
2428                         zfwBufFree(dev, buf, 0);
2429                         return;
2430                     }
2431
2432                     encryMode = ZM_TKIP;
2433                     offset += ZM_SIZE_OF_IV + ZM_SIZE_OF_EXT_IV;
2434                 }
2435                 else if(wd->sta.SWEncryptEnable & ZM_SW_WEP_DECRY_EN)
2436                 {
2437                     u16_t IvOffset;
2438                     u8_t keyLen = 5;
2439                     u8_t iv[3];
2440                     u8_t *wepKey;
2441                     u8_t keyIdx;
2442
2443                     IvOffset = offset + ZM_SIZE_OF_WLAN_DATA_HEADER;
2444
2445                     /* Retrieve IV */
2446                     iv[0] = zmw_rx_buf_readb(dev, buf, IvOffset);
2447                     iv[1] = zmw_rx_buf_readb(dev, buf, IvOffset+1);
2448                     iv[2] = zmw_rx_buf_readb(dev, buf, IvOffset+2);
2449
2450                     keyIdx = ((zmw_rx_buf_readb(dev, buf, IvOffset+3) >> 6) & 0x03);
2451
2452                     IvOffset += ZM_SIZE_OF_IV;
2453
2454                     if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP64)
2455                     {
2456                         keyLen = 5;
2457                     }
2458                     else if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP128)
2459                     {
2460                         keyLen = 13;
2461                     }
2462                     else if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP256)
2463                     {
2464                         keyLen = 29;
2465                     }
2466
2467                     zfWEPDecrypt(dev, buf, IvOffset, keyLen, wd->sta.wepKey[keyIdx], iv);
2468
2469                     if (ret == ZM_ICV_FAILURE)
2470                     {
2471                         zm_debug_msg0("WEP ICV fail");
2472
2473                         /* Discard ICV failed frame */
2474                         zfwBufFree(dev, buf, 0);
2475                         return;
2476                     }
2477
2478                     encryMode = wd->sta.SWEncryMode[keyIdx];
2479
2480                     /* Remove ICV from buffer */
2481                     zfwBufSetSize(dev, buf, len-4);
2482
2483                     offset += ZM_SIZE_OF_IV;
2484                 }
2485             }
2486         }
2487
2488 #ifdef ZM_ENABLE_CENC
2489         //else if ( encryMode == ZM_CENC ) /* check if CENC */
2490         if ( encryMode == ZM_CENC )
2491         {
2492             u32_t rxIV[4];
2493
2494             rxIV[0] = (zmw_rx_buf_readh(dev, buf, 28) << 16)
2495                      + zmw_rx_buf_readh(dev, buf, 26);
2496             rxIV[1] = (zmw_rx_buf_readh(dev, buf, 32) << 16)
2497                      + zmw_rx_buf_readh(dev, buf, 30);
2498             rxIV[2] = (zmw_rx_buf_readh(dev, buf, 36) << 16)
2499                      + zmw_rx_buf_readh(dev, buf, 34);
2500             rxIV[3] = (zmw_rx_buf_readh(dev, buf, 40) << 16)
2501                      + zmw_rx_buf_readh(dev, buf, 38);
2502
2503             //zm_debug_msg2("rxIV[0] = 0x", rxIV[0]);
2504             //zm_debug_msg2("rxIV[1] = 0x", rxIV[1]);
2505             //zm_debug_msg2("rxIV[2] = 0x", rxIV[2]);
2506             //zm_debug_msg2("rxIV[3] = 0x", rxIV[3]);
2507
2508             /* destination address*/
2509             da[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
2510             da[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+2);
2511             da[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+4);
2512
2513             if ( wd->wlanMode == ZM_MODE_AP )
2514             {
2515             }
2516             else
2517             {
2518                 if ((da[0] & 0x1))
2519                 { //multicast frame
2520                     /* Accumlate the PN sequence */
2521                     wd->sta.rxivGK[0] ++;
2522
2523                     if (wd->sta.rxivGK[0] == 0)
2524                     {
2525                         wd->sta.rxivGK[1]++;
2526                     }
2527
2528                     if (wd->sta.rxivGK[1] == 0)
2529                     {
2530                         wd->sta.rxivGK[2]++;
2531                     }
2532
2533                     if (wd->sta.rxivGK[2] == 0)
2534                     {
2535                         wd->sta.rxivGK[3]++;
2536                     }
2537
2538                     if (wd->sta.rxivGK[3] == 0)
2539                     {
2540                         wd->sta.rxivGK[0] = 0;
2541                         wd->sta.rxivGK[1] = 0;
2542                         wd->sta.rxivGK[2] = 0;
2543                     }
2544
2545                     //zm_debug_msg2("wd->sta.rxivGK[0] = 0x", wd->sta.rxivGK[0]);
2546                     //zm_debug_msg2("wd->sta.rxivGK[1] = 0x", wd->sta.rxivGK[1]);
2547                     //zm_debug_msg2("wd->sta.rxivGK[2] = 0x", wd->sta.rxivGK[2]);
2548                     //zm_debug_msg2("wd->sta.rxivGK[3] = 0x", wd->sta.rxivGK[3]);
2549
2550                     if ( !((wd->sta.rxivGK[0] == rxIV[0])
2551                         && (wd->sta.rxivGK[1] == rxIV[1])
2552                         && (wd->sta.rxivGK[2] == rxIV[2])
2553                         && (wd->sta.rxivGK[3] == rxIV[3])))
2554                     {
2555                         u8_t PacketDiscard = 0;
2556                         /* Discard PN Code Error frame */
2557                         if (rxIV[0] < wd->sta.rxivGK[0])
2558                         {
2559                             PacketDiscard = 1;
2560                         }
2561                         if (wd->sta.rxivGK[0] > 0xfffffff0)
2562                         { //boundary case
2563                             if ((rxIV[0] < 0xfffffff0)
2564                                 && (((0xffffffff - wd->sta.rxivGK[0]) + rxIV[0]) > 16))
2565                             {
2566                                 PacketDiscard = 1;
2567                             }
2568                         }
2569                         else
2570                         { //normal case
2571                             if ((rxIV[0] - wd->sta.rxivGK[0]) > 16)
2572                             {
2573                                 PacketDiscard = 1;
2574                             }
2575                         }
2576                         // sync sta pn code with ap because of losting some packets
2577                         wd->sta.rxivGK[0] = rxIV[0];
2578                         wd->sta.rxivGK[1] = rxIV[1];
2579                         wd->sta.rxivGK[2] = rxIV[2];
2580                         wd->sta.rxivGK[3] = rxIV[3];
2581                         if (PacketDiscard)
2582                         {
2583                             zm_debug_msg0("Discard PN Code lost too much multicast frame");
2584                             zfwBufFree(dev, buf, 0);
2585                             return;
2586                         }
2587                     }
2588                 }
2589                 else
2590                 { //unicast frame
2591                     /* Accumlate the PN sequence */
2592                     wd->sta.rxiv[0] += 2;
2593
2594                     if (wd->sta.rxiv[0] == 0 || wd->sta.rxiv[0] == 1)
2595                     {
2596                         wd->sta.rxiv[1]++;
2597                     }
2598
2599                     if (wd->sta.rxiv[1] == 0)
2600                     {
2601                         wd->sta.rxiv[2]++;
2602                     }
2603
2604                     if (wd->sta.rxiv[2] == 0)
2605                     {
2606                         wd->sta.rxiv[3]++;
2607                     }
2608
2609                     if (wd->sta.rxiv[3] == 0)
2610                     {
2611                         wd->sta.rxiv[0] = 0;
2612                         wd->sta.rxiv[1] = 0;
2613                         wd->sta.rxiv[2] = 0;
2614                     }
2615
2616                     //zm_debug_msg2("wd->sta.rxiv[0] = 0x", wd->sta.rxiv[0]);
2617                     //zm_debug_msg2("wd->sta.rxiv[1] = 0x", wd->sta.rxiv[1]);
2618                     //zm_debug_msg2("wd->sta.rxiv[2] = 0x", wd->sta.rxiv[2]);
2619                     //zm_debug_msg2("wd->sta.rxiv[3] = 0x", wd->sta.rxiv[3]);
2620
2621                     if ( !((wd->sta.rxiv[0] == rxIV[0])
2622                         && (wd->sta.rxiv[1] == rxIV[1])
2623                         && (wd->sta.rxiv[2] == rxIV[2])
2624                         && (wd->sta.rxiv[3] == rxIV[3])))
2625                     {
2626                         zm_debug_msg0("PN Code mismatch, lost unicast frame, sync pn code to recv packet");
2627                         // sync sta pn code with ap because of losting some packets
2628                         wd->sta.rxiv[0] = rxIV[0];
2629                         wd->sta.rxiv[1] = rxIV[1];
2630                         wd->sta.rxiv[2] = rxIV[2];
2631                         wd->sta.rxiv[3] = rxIV[3];
2632                         /* Discard PN Code Error frame */
2633                         //zm_debug_msg0("Discard PN Code mismatch unicast frame");
2634                         //zfwBufFree(dev, buf, 0);
2635                         //return;
2636                     }
2637                 }
2638             }
2639         }
2640 #endif //ZM_ENABLE_CENC
2641
2642         /* for tally */
2643         if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
2644         {
2645             /* for ACU to display RxRate */
2646             zfWlanUpdateRxRate(dev, addInfo);
2647
2648             wd->commTally.rxUnicastFrm++;
2649             wd->commTally.rxUnicastOctets += (len-24);
2650         }
2651         else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
2652         {
2653             wd->commTally.rxBroadcastFrm++;
2654             wd->commTally.rxBroadcastOctets += (len-24);
2655         }
2656         else
2657         {
2658             wd->commTally.rxMulticastFrm++;
2659             wd->commTally.rxMulticastOctets += (len-24);
2660         }
2661         wd->ledStruct.rxTraffic++;
2662
2663         if ((frameSubtype & 0x80) == 0x80)
2664         {
2665             /* if QoS control bit-7 is 1 => A-MSDU frame */
2666             if ((zmw_rx_buf_readh(dev, buf, 24) & 0x80) != 0)
2667             {
2668                 zfDeAmsdu(dev, buf, vap, encryMode);
2669                 return;
2670             }
2671         }
2672
2673         // Remove MIC of TKIP
2674         if ( encryMode == ZM_TKIP )
2675         {
2676             zfwBufSetSize(dev, buf, zfwBufGetSize(dev, buf) - 8);
2677         }
2678
2679         /* Convert 802.11 and SNAP header to ethernet header */
2680         if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)||
2681              (wd->wlanMode == ZM_MODE_IBSS) )
2682         {
2683             /* destination address*/
2684             da[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
2685             da[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+2);
2686             da[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+4);
2687
2688             /* check broadcast frame */
2689             if ( (da[0] == 0xffff) && (da[1] == 0xffff) && (da[2] == 0xffff) )
2690             {
2691                 // Ap send broadcast frame to the DUT !
2692             }
2693             /* check multicast frame */
2694             /* TODO : Remove these code, hardware should be able to block */
2695             /*        multicast frame on the multicast address list       */
2696             /*        or bypass all multicast packet by flag bAllMulticast */
2697             else if ((da[0] & 0x01) && (wd->sta.bAllMulticast == 0))
2698             {
2699                 for(ii=0; ii<wd->sta.multicastList.size; ii++)
2700                 {
2701                     if ( zfMemoryIsEqual(wd->sta.multicastList.macAddr[ii].addr,
2702                                          (u8_t*) da, 6))
2703                     {
2704                         break;
2705                     }
2706                 }
2707
2708                 if ( ii == wd->sta.multicastList.size )
2709                 {   /* not found */
2710                     zm_debug_msg0("discard unknown multicast frame");
2711
2712                     zfwBufFree(dev, buf, 0);
2713                     return;
2714                 }
2715             }
2716
2717 #ifdef ZM_ENABLE_NATIVE_WIFI //Native Wifi : 1, Ethernet format : 0
2718             //To remove IV
2719             if (offset > 0)
2720             {
2721                 for (i=12; i>0; i--)
2722                 {
2723                     zmw_rx_buf_writeh(dev, buf, ((i-1)*2)+offset,
2724                             zmw_rx_buf_readh(dev, buf, (i-1)*2));
2725                 }
2726                 zfwBufRemoveHead(dev, buf, offset);
2727             }
2728 #else
2729
2730             if (zfRxBufferEqualToStr(dev, buf, zgSnapBridgeTunnel,
2731                                      24+offset, 6))
2732             {
2733                 snapCase = 1;
2734             }
2735             else if ( zfRxBufferEqualToStr(dev, buf, zgSnap8021h,
2736                                            24+offset, 6) )
2737             {
2738                 typeLengthField =
2739                     (((u16_t) zmw_rx_buf_readb(dev, buf, 30+offset)) << 8) +
2740                     zmw_rx_buf_readb(dev, buf, 31+offset);
2741
2742                 //zm_debug_msg2("tpyeLengthField = ", typeLengthField);
2743
2744                 //8137 : IPX, 80F3 : Appletalk
2745                 if ( (typeLengthField != 0x8137)&&
2746                      (typeLengthField != 0x80F3) )
2747                 {
2748                     snapCase = 2;
2749                 }
2750
2751                 if ( typeLengthField == 0x888E )
2752                 {
2753                     zfShowRxEAPOL(dev, buf, 32);
2754                 }
2755             }
2756             else
2757             {
2758                 //zfwDumpBuf(dev, buf);
2759             }
2760
2761             /* source address */
2762             if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
2763             {
2764                 /* SA = Address 3 */
2765                 sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
2766                 sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
2767                 sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
2768             }
2769             else
2770             {
2771                 /* SA = Address 2 */
2772                 sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
2773                 sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
2774                 sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
2775             }
2776
2777             if ( snapCase )
2778             {
2779                 /* SA */
2780                 zmw_rx_buf_writeh(dev, buf, 24+offset, sa[0]);
2781                 zmw_rx_buf_writeh(dev, buf, 26+offset, sa[1]);
2782                 zmw_rx_buf_writeh(dev, buf, 28+offset, sa[2]);
2783
2784                 /* DA = Address 1 */
2785                 zmw_rx_buf_writeh(dev, buf, 18+offset, da[0]);
2786                 zmw_rx_buf_writeh(dev, buf, 20+offset, da[1]);
2787                 zmw_rx_buf_writeh(dev, buf, 22+offset, da[2]);
2788                 zfwBufRemoveHead(dev, buf, 18+offset);
2789             }
2790             else
2791             {
2792                 /* SA */
2793                 zmw_rx_buf_writeh(dev, buf, 16+offset, sa[0]);
2794                 zmw_rx_buf_writeh(dev, buf, 18+offset, sa[1]);
2795                 zmw_rx_buf_writeh(dev, buf, 20+offset, sa[2]);
2796
2797                 /* DA = Address 1 */
2798                 zmw_rx_buf_writeh(dev, buf, 10+offset, da[0]);
2799                 zmw_rx_buf_writeh(dev, buf, 12+offset, da[1]);
2800                 zmw_rx_buf_writeh(dev, buf, 14+offset, da[2]);
2801                 zfwBufRemoveHead(dev, buf, 10+offset);
2802                 /* Ethernet payload length */
2803                 typeLengthField = zfwBufGetSize(dev, buf) - 14;
2804                 zmw_rx_buf_writeh(dev, buf, 12, (typeLengthField<<8)+(typeLengthField>>8));
2805             }
2806 #endif  // ZM_ENABLE_NATIVE_WIFI
2807         }
2808         else if (wd->wlanMode == ZM_MODE_AP)
2809         {
2810             //if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3)
2811             if (vap < ZM_MAX_AP_SUPPORT)
2812             /* AP mode */
2813             {
2814 #ifdef ZM_ENABLE_NATIVE_WIFI //Native Wifi : 1, Ethernet format : 0
2815                 //To remove IV
2816                 if (offset > 0)
2817                 {
2818                     for (i=12; i>0; i--)
2819                     {
2820                         zmw_rx_buf_writeh(dev, buf, ((i-1)*2)+offset,
2821                                 zmw_rx_buf_readh(dev, buf, (i-1)*2));
2822                     }
2823                     zfwBufRemoveHead(dev, buf, offset);
2824                 }
2825 #else
2826                 /* SA = Address 2 */
2827                 zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
2828                         ZM_WLAN_HEADER_A2_OFFSET));
2829                 zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
2830                         ZM_WLAN_HEADER_A2_OFFSET+2));
2831                 zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
2832                         ZM_WLAN_HEADER_A2_OFFSET+4));
2833                 /* DA = Address 3 */
2834                 /* Seq : Read 20 write 22, read 18 write 20, read 16 write 18 */
2835                 /* sequence must not be inverted */
2836                 zmw_rx_buf_writeh(dev, buf, 22+offset, zmw_rx_buf_readh(dev, buf,
2837                         ZM_WLAN_HEADER_A3_OFFSET+4));
2838                 zmw_rx_buf_writeh(dev, buf, 20+offset, zmw_rx_buf_readh(dev, buf,
2839                         ZM_WLAN_HEADER_A3_OFFSET+2));
2840                 zmw_rx_buf_writeh(dev, buf, 18+offset, zmw_rx_buf_readh(dev, buf,
2841                         ZM_WLAN_HEADER_A3_OFFSET));
2842                 zfwBufRemoveHead(dev, buf, 18+offset);
2843 #endif  // ZM_ENABLE_NATIVE_WIFI
2844                 #if 1
2845                 if ((ret = zfIntrabssForward(dev, buf, vap)) == 1)
2846                 {
2847                     /* Free Rx buffer if intra-BSS unicast frame */
2848                     zm_msg0_rx(ZM_LV_2, "Free intra-BSS unicast frame");
2849                     zfwBufFree(dev, buf, 0);
2850                     return;
2851                 }
2852                 #endif
2853             }
2854             else
2855             /* WDS mode */
2856             {
2857                 zm_msg0_rx(ZM_LV_2, "Rx WDS data");
2858
2859                 /* SA = Address 4 */
2860                 zmw_rx_buf_writeh(dev, buf, 30+offset, zmw_rx_buf_readh(dev, buf,
2861                         ZM_WLAN_HEADER_A4_OFFSET));
2862                 zmw_rx_buf_writeh(dev, buf, 32+offset, zmw_rx_buf_readh(dev, buf,
2863                         ZM_WLAN_HEADER_A4_OFFSET+2));
2864                 zmw_rx_buf_writeh(dev, buf, 34+offset, zmw_rx_buf_readh(dev, buf,
2865                         ZM_WLAN_HEADER_A4_OFFSET+4));
2866                 /* DA = Address 3 */
2867                 /* Seq : Read 20 write 22, read 18 write 20, read 16 write 18 */
2868                 /* sequence must not be inverted */
2869                 zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
2870                         ZM_WLAN_HEADER_A3_OFFSET+4));
2871                 zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
2872                         ZM_WLAN_HEADER_A3_OFFSET+2));
2873                 zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
2874                         ZM_WLAN_HEADER_A3_OFFSET));
2875                 zfwBufRemoveHead(dev, buf, 24+offset);
2876             }
2877         }
2878         else if (wd->wlanMode == ZM_MODE_PSEUDO)
2879         {
2880                         /* WDS test: remove add4 */
2881             if (wd->enableWDS)
2882             {
2883                 offset += 6;
2884             }
2885
2886             /* SA = Address 2 */
2887             zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
2888                               ZM_WLAN_HEADER_A2_OFFSET));
2889             zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
2890                               ZM_WLAN_HEADER_A2_OFFSET+2));
2891             zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
2892                               ZM_WLAN_HEADER_A2_OFFSET+4));
2893             /* DA = Address 1 */
2894             zmw_rx_buf_writeh(dev, buf, 18+offset, zmw_rx_buf_readh(dev, buf,
2895                               ZM_WLAN_HEADER_A1_OFFSET));
2896             zmw_rx_buf_writeh(dev, buf, 20+offset, zmw_rx_buf_readh(dev, buf,
2897                               ZM_WLAN_HEADER_A1_OFFSET+2));
2898             zmw_rx_buf_writeh(dev, buf, 22+offset, zmw_rx_buf_readh(dev, buf,
2899                               ZM_WLAN_HEADER_A1_OFFSET+4));
2900             zfwBufRemoveHead(dev, buf, 18+offset);
2901         }
2902         else
2903         {
2904             zm_assert(0);
2905         }
2906
2907         /* Call zfwRecvEth() to notify upper layer */
2908         //zm_msg2_rx(ZM_LV_2, "Call zfwRecvEth(), buf=", buf);
2909         //zfwDumpBuf(dev, buf);
2910
2911         #if ZM_PROTOCOL_RESPONSE_SIMULATION == 1
2912         zfProtRspSim(dev, buf);
2913         #endif
2914         //zfwDumpBuf(dev, buf);
2915
2916         /* tally */
2917         wd->commTally.NotifyNDISRxFrmCnt++;
2918
2919         if (wd->zfcbRecvEth != NULL)
2920         {
2921             wd->zfcbRecvEth(dev, buf, vap);
2922             ZM_PERFORMANCE_RX_MSDU(dev, wd->tick)
2923         }
2924     }
2925     /* if management frame */
2926     else if (frameType == ZM_WLAN_MANAGEMENT_FRAME)
2927     {
2928         zm_msg2_rx(ZM_LV_2, "Rx management,FC=", frameCtrl);
2929         /* Call zfProcessManagement() to handle management frame */
2930         zfProcessManagement(dev, buf, addInfo); //CWYang(m)
2931         zfwBufFree(dev, buf, 0);
2932     }
2933     /* PsPoll */
2934     else if ((wd->wlanMode == ZM_MODE_AP) && (frameCtrl == 0xa4))
2935     {
2936         zm_msg0_rx(ZM_LV_0, "Rx PsPoll");
2937         zfApProcessPsPoll(dev, buf);
2938         zfwBufFree(dev, buf, 0);
2939     }
2940     else
2941     {
2942         zm_msg0_rx(ZM_LV_1, "Rx discard!!");
2943         wd->commTally.DriverDiscardedFrm++;
2944
2945         zfwBufFree(dev, buf, 0);
2946     }
2947     return;
2948 }
2949
2950
2951 /************************************************************************/
2952 /*                                                                      */
2953 /*    FUNCTION DESCRIPTION                  zfWlanRxValidate            */
2954 /*      Validate Rx frame.                                              */
2955 /*                                                                      */
2956 /*    INPUTS                                                            */
2957 /*      dev : device pointer                                            */
2958 /*      buf : received 802.11 frame buffer.                             */
2959 /*                                                                      */
2960 /*    OUTPUTS                                                           */
2961 /*      Error code                                                      */
2962 /*                                                                      */
2963 /*    AUTHOR                                                            */
2964 /*      Stephen             ZyDAS Technology Corporation    2005.10     */
2965 /*                                                                      */
2966 /************************************************************************/
2967 u16_t zfWlanRxValidate(zdev_t* dev, zbuf_t* buf)
2968 {
2969     u16_t frameType;
2970     u16_t frameCtrl;
2971     u16_t frameLen;
2972     u16_t ret;
2973     u8_t  frameSubType;
2974
2975     zmw_get_wlan_dev(dev);
2976
2977     frameCtrl = zmw_rx_buf_readh(dev, buf, 0);
2978     frameType = frameCtrl & 0xC;
2979     frameSubType = (frameCtrl & 0xF0) >> 4;
2980
2981     frameLen = zfwBufGetSize(dev, buf);
2982
2983     /* Accept Data/Management frame with protocol version = 0 */
2984     if ((frameType == 0x8) || (frameType == 0x0))
2985     {
2986
2987         /* TODO : check rx status => erro bit */
2988
2989         /* Check Minimum Length with Wep */
2990         if ((frameCtrl & 0x4000) != 0)
2991         {
2992             /* Minimum Length =                                       */
2993             /*     PLCP(5)+Header(24)+IV(4)+ICV(4)+CRC(4)+RxStatus(8) */
2994             if (frameLen < 32)
2995             {
2996                 return ZM_ERR_MIN_RX_ENCRYPT_FRAME_LENGTH;
2997             }
2998         }
2999         else if ( frameSubType == 0x5 || frameSubType == 0x8 )
3000         {
3001             /* Minimum Length = PLCP(5)+MACHeader(24)+Timestamp(8)+BeaconInterval(2)+Cap(2)+CRC(4)+RxStatus(8) */
3002             if (frameLen < 36)
3003             {
3004                 return ZM_ERR_MIN_RX_FRAME_LENGTH;
3005             }
3006         }
3007         else
3008         {
3009             /* Minimum Length = PLCP(5)+MACHeader(24)+CRC(4)+RxStatus(8) */
3010             if (frameLen < 24)
3011             {
3012                 return ZM_ERR_MIN_RX_FRAME_LENGTH;
3013             }
3014         }
3015
3016         /* Check if frame Length > ZM_WLAN_MAX_RX_SIZE. */
3017         if (frameLen > ZM_WLAN_MAX_RX_SIZE)
3018         {
3019             return ZM_ERR_MAX_RX_FRAME_LENGTH;
3020         }
3021     }
3022     else if ((frameCtrl&0xff) == 0xa4)
3023     {
3024         /* PsPoll */
3025         //zm_msg0_rx(ZM_LV_0, "rx pspoll");
3026     }
3027     else if ((frameCtrl&0xff) == ZM_WLAN_FRAME_TYPE_BAR)
3028     {
3029         if (wd->sta.enableDrvBA == 1)
3030         {
3031             zfAggRecvBAR(dev, buf);
3032         }
3033
3034         return ZM_ERR_RX_BAR_FRAME;
3035     }
3036     else
3037     {
3038         return ZM_ERR_RX_FRAME_TYPE;
3039     }
3040
3041     if ( wd->wlanMode == ZM_MODE_AP )
3042     {
3043     }
3044     else if ( wd->wlanMode != ZM_MODE_PSEUDO )
3045     {
3046         if ( (ret=zfStaRxValidateFrame(dev, buf))!=ZM_SUCCESS )
3047         {
3048             //zm_debug_msg1("discard frame, code = ", ret);
3049             return ret;
3050         }
3051     }
3052
3053     return ZM_SUCCESS;
3054 }
3055
3056
3057 /************************************************************************/
3058 /*                                                                      */
3059 /*    FUNCTION DESCRIPTION                  zfWlanRxFilter              */
3060 /*      Filter duplicated frame.                                        */
3061 /*                                                                      */
3062 /*    INPUTS                                                            */
3063 /*      dev : device pointer                                            */
3064 /*      buf : received 802.11 frame buffer.                             */
3065 /*                                                                      */
3066 /*    OUTPUTS                                                           */
3067 /*      Error code                                                      */
3068 /*                                                                      */
3069 /*    AUTHOR                                                            */
3070 /*      Stephen             ZyDAS Technology Corporation    2005.10     */
3071 /*                                                                      */
3072 /************************************************************************/
3073 u16_t zfWlanRxFilter(zdev_t* dev, zbuf_t* buf)
3074 {
3075     u16_t src[3];
3076     u16_t dst0;
3077     u16_t frameType;
3078     u16_t seq;
3079     u16_t offset;
3080     u16_t index;
3081     u16_t col;
3082     u16_t i;
3083     u8_t up = 0; /* User priority */
3084
3085     zmw_get_wlan_dev(dev);
3086
3087     zmw_declare_for_critical_section();
3088
3089     ZM_BUFFER_TRACE(dev, buf)
3090
3091     /* RX PREFIX */
3092     offset = 0;
3093
3094     frameType = zmw_rx_buf_readh(dev, buf, offset);
3095
3096     // Don't divide 2^4 because we don't want the fragmentation pkt to be treated as
3097     // duplicated frames
3098     seq = zmw_rx_buf_readh(dev, buf, offset+22);
3099     dst0 = zmw_rx_buf_readh(dev, buf, offset+4);
3100     src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
3101     src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
3102     src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
3103
3104     /* QoS data frame */
3105     if ((frameType & 0x88) == 0x88)
3106     {
3107         up = zmw_rx_buf_readb(dev, buf, offset+24);
3108         up &= 0x7;
3109     }
3110
3111     index = (src[2]+up) & (ZM_FILTER_TABLE_ROW-1);
3112
3113     /* TBD : filter frame with source address == own MAC adress */
3114     if ((wd->macAddr[0] == src[0]) && (wd->macAddr[1] == src[1])
3115             && (wd->macAddr[2] == src[2]))
3116     {
3117         //zm_msg0_rx(ZM_LV_0, "Rx filter=>src is own MAC");
3118         wd->trafTally.rxSrcIsOwnMac++;
3119 #if 0
3120         return ZM_ERR_RX_SRC_ADDR_IS_OWN_MAC;
3121 #endif
3122     }
3123
3124     zm_msg2_rx(ZM_LV_2, "Rx seq=", seq);
3125
3126     /* Filter unicast frame only */
3127     if ((dst0 & 0x1) == 0)
3128     {
3129         zmw_enter_critical_section(dev);
3130
3131         for(i=0; i<ZM_FILTER_TABLE_COL; i++)
3132         {
3133             if ((wd->rxFilterTbl[i][index].addr[0] == src[0])
3134                     && (wd->rxFilterTbl[i][index].addr[1] == src[1])
3135                     && (wd->rxFilterTbl[i][index].addr[2] == src[2])
3136                     && (wd->rxFilterTbl[i][index].up == up))
3137             {
3138                 if (((frameType&0x800)==0x800)
3139                         &&(wd->rxFilterTbl[i][index].seq==seq))
3140                 {
3141                     zmw_leave_critical_section(dev);
3142                     /* hit : duplicated frame */
3143                     zm_msg0_rx(ZM_LV_1, "Rx filter hit=>duplicated");
3144                     wd->trafTally.rxDuplicate++;
3145                     return ZM_ERR_RX_DUPLICATE;
3146                 }
3147                 else
3148                 {
3149                     /* hit : not duplicated frame, update sequence number */
3150                     wd->rxFilterTbl[i][index].seq = seq;
3151                     zmw_leave_critical_section(dev);
3152                     zm_msg0_rx(ZM_LV_2, "Rx filter hit");
3153                     return ZM_SUCCESS;
3154                 }
3155             }
3156         } /* for(i=0; i<ZM_FILTER_TABLE_COL; i++) */
3157
3158         /* miss : add to table */
3159         zm_msg0_rx(ZM_LV_1, "Rx filter miss");
3160         /* TODO : Random select a column */
3161         col = (u16_t)(wd->tick & (ZM_FILTER_TABLE_COL-1));
3162         wd->rxFilterTbl[col][index].addr[0] = src[0];
3163         wd->rxFilterTbl[col][index].addr[1] = src[1];
3164         wd->rxFilterTbl[col][index].addr[2] = src[2];
3165         wd->rxFilterTbl[col][index].seq = seq;
3166         wd->rxFilterTbl[col][index].up = up;
3167
3168         zmw_leave_critical_section(dev);
3169     } /* if ((dst0 & 0x1) == 0) */
3170
3171     return ZM_SUCCESS;
3172 }
3173
3174
3175
3176 u16_t zfTxGenWlanTail(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t snaplen,
3177                       u16_t* mic)
3178 {
3179     struct zsMicVar*  pMicKey;
3180     u16_t  i, length, payloadOffset;
3181     u8_t   bValue, qosType = 0;
3182     u8_t   snapByte[12];
3183
3184     zmw_get_wlan_dev(dev);
3185
3186     if ( wd->wlanMode == ZM_MODE_AP )
3187     {
3188         pMicKey = zfApGetTxMicKey(dev, buf, &qosType);
3189
3190         if ( pMicKey == NULL )
3191         {
3192             return 0;
3193         }
3194     }
3195     else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
3196     {
3197         pMicKey = zfStaGetTxMicKey(dev, buf);
3198
3199         if ( pMicKey == NULL )
3200         {
3201             return 0;
3202         }
3203     }
3204     else
3205     {
3206         return 0;
3207     }
3208
3209     length = zfwBufGetSize(dev, buf);
3210
3211     zfMicClear(pMicKey);
3212
3213     /* append DA and SA */
3214 #ifdef ZM_ENABLE_NATIVE_WIFI
3215     for(i=16; i<22; i++)
3216     { // VISTA DA
3217         bValue = zmw_tx_buf_readb(dev, buf, i);
3218         zfMicAppendByte(bValue, pMicKey);
3219     }
3220     for(i=10; i<16; i++)
3221     { // VISTA SA
3222         bValue = zmw_tx_buf_readb(dev, buf, i);
3223         zfMicAppendByte(bValue, pMicKey);
3224     }
3225 #else
3226     for(i=0; i<12; i++)
3227     {
3228         bValue = zmw_tx_buf_readb(dev, buf, i);
3229         zfMicAppendByte(bValue, pMicKey);
3230     }
3231 #endif
3232
3233     /* append for alignment */
3234     if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
3235     {
3236         if (wd->sta.wmeConnected != 0)
3237             zfMicAppendByte(zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1) >> 5, pMicKey);
3238         else
3239             zfMicAppendByte(0, pMicKey);
3240     }
3241     else if ( wd->wlanMode == ZM_MODE_AP )
3242     {
3243         if (qosType == 1)
3244             zfMicAppendByte(zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1) >> 5, pMicKey);
3245         else
3246             zfMicAppendByte(0, pMicKey);
3247     }
3248     else
3249     {
3250         /* TODO : Qos Software MIC in IBSS Mode */
3251         zfMicAppendByte(0, pMicKey);
3252     }
3253     zfMicAppendByte(0, pMicKey);
3254     zfMicAppendByte(0, pMicKey);
3255     zfMicAppendByte(0, pMicKey);
3256
3257     if ( snaplen == 0 )
3258     {
3259         payloadOffset = ZM_80211_FRAME_IP_OFFSET;
3260     }
3261     else
3262     {
3263         payloadOffset = ZM_80211_FRAME_TYPE_OFFSET;
3264
3265         for(i=0; i<(snaplen>>1); i++)
3266         {
3267             snapByte[i*2] = (u8_t) (snap[i] & 0xff);
3268             snapByte[i*2+1] = (u8_t) ((snap[i] >> 8) & 0xff);
3269         }
3270
3271         for(i=0; i<snaplen; i++)
3272         {
3273             zfMicAppendByte(snapByte[i], pMicKey);
3274         }
3275     }
3276
3277     for(i=payloadOffset; i<length; i++)
3278     {
3279         bValue = zmw_tx_buf_readb(dev, buf, i);
3280         zfMicAppendByte(bValue, pMicKey);
3281     }
3282
3283     zfMicGetMic( (u8_t*) mic, pMicKey);
3284
3285     return ZM_SIZE_OF_MIC;
3286 }
3287
3288
3289 /************************************************************************/
3290 /*                                                                      */
3291 /*    FUNCTION DESCRIPTION                  zfTxGetIpTosAndFrag         */
3292 /*      Get IP TOS and frag offset from Tx buffer                       */
3293 /*                                                                      */
3294 /*    INPUTS                                                            */
3295 /*      dev : device pointer                                            */
3296 /*      buf : Tx buffer pointer                                         */
3297 /*      up : pointer for returning user priority                        */
3298 /*      fragOff : pointer for returning ip frag offset                  */
3299 /*                                                                      */
3300 /*    OUTPUTS                                                           */
3301 /*      None                                                            */
3302 /*                                                                      */
3303 /*    AUTHOR                                                            */
3304 /*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
3305 /*                                                                      */
3306 /************************************************************************/
3307 void zfTxGetIpTosAndFrag(zdev_t* dev, zbuf_t* buf, u8_t* up, u16_t* fragOff)
3308 {
3309     u8_t ipv;
3310     u16_t len;
3311         u16_t etherType;
3312     u8_t tos;
3313
3314     *up = 0;
3315     *fragOff = 0;
3316
3317     len = zfwBufGetSize(dev, buf);
3318
3319     if (len >= 34) //Minimum IPv4 packet size, 14(Ether header)+20(IPv4 header)
3320     {
3321         etherType = (((u16_t)zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_TYPE_OFFSET))<<8)
3322                     + zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_TYPE_OFFSET + 1);
3323
3324         /* protocol type = IP */
3325         if (etherType == 0x0800)
3326         {
3327             ipv = zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET) >> 4;
3328             if (ipv == 0x4) //IPv4
3329             {
3330                 tos = zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1);
3331                 *up = (tos >> 5);
3332                 *fragOff = zmw_tx_buf_readh(dev, buf, ZM_80211_FRAME_IP_OFFSET + 6);
3333             }
3334             /* TODO : handle VLAN tag and IPv6 packet */
3335         }
3336     }
3337     return;
3338 }
3339
3340 #ifdef ZM_ENABLE_NATIVE_WIFI
3341 u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen)
3342 {
3343     snap[0] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 0);
3344     snap[1] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 2);
3345     snap[2] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 4);
3346     *snaplen = 6;
3347
3348     return ZM_80211_FRAME_HEADER_LEN + *snaplen;
3349 }
3350 #else
3351 u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen)
3352 {
3353     u16_t removed;
3354            u16_t etherType;
3355         u16_t len;
3356
3357            len = zfwBufGetSize(dev, buf);
3358     if (len < 14) //Minimum Ethernet packet size, 14(Ether header)
3359     {
3360         /* TODO : Assert? */
3361         *snaplen = 0;
3362         return 0;
3363     }
3364
3365     /* Generate RFC1042 header */
3366     etherType = (((u16_t)zmw_tx_buf_readb(dev, buf, 12))<<8)
3367                 + zmw_tx_buf_readb(dev, buf, 13);
3368
3369     //zm_debug_msg2("ethernet type or length = ", etherType);
3370
3371     if (etherType > 1500)
3372     {
3373         /* ETHERNET format */
3374         removed = 12;
3375         snap[0] = 0xaaaa;
3376         snap[1] = 0x0003;
3377         if ((etherType ==0x8137) || (etherType == 0x80f3))
3378         {
3379             /* Bridge Tunnel */
3380             snap[2] = 0xF800;
3381         }
3382         else
3383         {
3384             /* RFC 1042 */
3385             snap[2] = 0x0000;
3386         }
3387         *snaplen = 6;
3388
3389         if ( etherType == 0x888E )
3390         {
3391             zfShowTxEAPOL(dev, buf, 14);
3392         }
3393     }
3394     else
3395     {
3396         /* 802.3 format */
3397         removed = 14;
3398         *snaplen = 0;
3399     }
3400
3401     return removed;
3402 }
3403 #endif
3404
3405 u8_t zfIsVtxqEmpty(zdev_t* dev)
3406 {
3407     u8_t isEmpty = TRUE;
3408     u8_t i;
3409
3410     zmw_get_wlan_dev(dev);
3411
3412     zmw_declare_for_critical_section();
3413
3414     zmw_enter_critical_section(dev);
3415
3416     if (wd->vmmqHead != wd->vmmqTail)
3417     {
3418         isEmpty = FALSE;
3419         goto check_done;
3420     }
3421
3422     for(i=0; i < 4; i++)
3423     {
3424         if (wd->vtxqHead[i] != wd->vtxqTail[i])
3425         {
3426             isEmpty = FALSE;
3427             goto check_done;
3428         }
3429     }
3430
3431 check_done:
3432     zmw_leave_critical_section(dev);
3433     return isEmpty;
3434 }
3435
3436 /************************************************************************/
3437 /*                                                                      */
3438 /*    FUNCTION DESCRIPTION                  zfPutVtxq                   */
3439 /*      Put Tx buffer to virtual TxQ                                    */
3440 /*                                                                      */
3441 /*    INPUTS                                                            */
3442 /*      dev : device pointer                                            */
3443 /*      buf : Tx buffer pointer                                         */
3444 /*                                                                      */
3445 /*    OUTPUTS                                                           */
3446 /*      ZM_SUCCESS or error code                                        */
3447 /*                                                                      */
3448 /*    AUTHOR                                                            */
3449 /*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
3450 /*                                                                      */
3451 /************************************************************************/
3452 u16_t zfPutVtxq(zdev_t* dev, zbuf_t* buf)
3453 {
3454     u8_t ac;
3455     u8_t up;
3456     u16_t fragOff;
3457 #ifdef ZM_AGG_TALLY
3458     struct aggTally *agg_tal;
3459 #endif
3460 #ifdef ZM_ENABLE_AGGREGATION
3461     #ifndef ZM_BYPASS_AGGR_SCHEDULING
3462         u16_t ret;
3463     u16_t tid;
3464     #endif
3465 #endif
3466
3467     zmw_get_wlan_dev(dev);
3468
3469     zmw_declare_for_critical_section();
3470
3471     zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
3472
3473     if ( wd->zfcbClassifyTxPacket != NULL )
3474     {
3475         ac = wd->zfcbClassifyTxPacket(dev, buf);
3476     }
3477     else
3478     {
3479         ac = zcUpToAc[up&0x7] & 0x3;
3480     }
3481
3482     /*
3483      * add by honda
3484      * main A-MPDU aggregation function
3485      */
3486 #ifdef ZM_AGG_TALLY
3487     agg_tal = &wd->agg_tal;
3488     agg_tal->got_packets_sum++;
3489
3490 #endif
3491
3492 #ifdef ZM_ENABLE_AGGREGATION
3493     #ifndef ZM_BYPASS_AGGR_SCHEDULING
3494     tid = up&0x7;
3495     if(wd->enableAggregation==0)
3496     {
3497         if( (wd->wlanMode == ZM_MODE_AP) ||
3498             (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
3499             (wd->wlanMode == ZM_MODE_PSEUDO) ) {
3500             // (infrastructure_mode && connect_to_11n_ap) || (ap_mode && is_11n_ap)
3501             //ret = zfAggPutVtxq(dev, buf);
3502
3503
3504             ret = zfAggTx(dev, buf, tid);
3505             if (ZM_SUCCESS == ret)
3506             {
3507                 //zfwBufFree(dev, buf, ZM_SUCCESS);
3508
3509                 return ZM_SUCCESS;
3510             }
3511             if (ZM_ERR_EXCEED_PRIORITY_THRESHOLD == ret)
3512             {
3513                 wd->commTally.txQosDropCount[ac]++;
3514                 zfwBufFree(dev, buf, ZM_SUCCESS);
3515
3516                 zm_msg1_tx(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
3517
3518                 return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
3519             }
3520             if (ZM_ERR_TX_BUFFER_UNAVAILABLE == ret)
3521             {
3522                 /*
3523                 * do nothing
3524                 * continue following procession, put into VTXQ
3525                 * return ZM_SUCCESS;
3526                 */
3527             }
3528         }
3529     }
3530     #endif
3531 #endif
3532     /*
3533      * end of add by honda
3534      */
3535
3536     /* First Ip frag */
3537     if ((fragOff & 0xff3f) == 0x0020)
3538     {
3539         /* Don't let ip frag in if VTXQ unable to hold */
3540         /* whole ip frag burst(assume 20 frag)         */
3541         zmw_enter_critical_section(dev);
3542         if (((wd->vtxqHead[ac] - wd->vtxqTail[ac])& ZM_VTXQ_SIZE_MASK)
3543                 > (ZM_VTXQ_SIZE-20))
3544         {
3545             wd->qosDropIpFrag[ac] = 1;
3546         }
3547         else
3548         {
3549             wd->qosDropIpFrag[ac] = 0;
3550         }
3551         zmw_leave_critical_section(dev);
3552
3553         if (wd->qosDropIpFrag[ac] == 1)
3554         {
3555             //zm_debug_msg2("vtQ full, drop buf = ", buf);
3556             wd->commTally.txQosDropCount[ac]++;
3557             zfwBufFree(dev, buf, ZM_SUCCESS);
3558             zm_msg1_tx(ZM_LV_1, "Packet discarded, first ip frag, ac=", ac);
3559             //VTXQ[] can not hold whold ip frag burst(assume 20 frags)
3560             return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
3561         }
3562     }
3563     else if ((fragOff & 0xff3f) == 0)
3564     {
3565         wd->qosDropIpFrag[ac] = 0;
3566     }
3567
3568     if (((fragOff &= 0xff1f) != 0) && (wd->qosDropIpFrag[ac] == 1))
3569     {
3570         wd->commTally.txQosDropCount[ac]++;
3571         zfwBufFree(dev, buf, ZM_SUCCESS);
3572         zm_msg1_tx(ZM_LV_1, "Packet discarded, ip frag, ac=", ac);
3573         //Discard following ip frags
3574         return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
3575     }
3576
3577     zmw_enter_critical_section(dev);
3578     if (((wd->vtxqHead[ac] + 1) & ZM_VTXQ_SIZE_MASK) != wd->vtxqTail[ac])
3579     {
3580         wd->vtxq[ac][wd->vtxqHead[ac]] = buf;
3581         wd->vtxqHead[ac] = ((wd->vtxqHead[ac] + 1) & ZM_VTXQ_SIZE_MASK);
3582         zmw_leave_critical_section(dev);
3583         return ZM_SUCCESS;
3584     }
3585     else
3586     {
3587         zmw_leave_critical_section(dev);
3588
3589         wd->commTally.txQosDropCount[ac]++;
3590         zfwBufFree(dev, buf, ZM_SUCCESS);
3591         zm_msg1_tx(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
3592         return ZM_ERR_EXCEED_PRIORITY_THRESHOLD; //VTXQ[] Full
3593     }
3594 }
3595
3596
3597 /************************************************************************/
3598 /*                                                                      */
3599 /*    FUNCTION DESCRIPTION                  zfGetVtxq                   */
3600 /*      Get Tx buffer from virtual TxQ                                  */
3601 /*                                                                      */
3602 /*    INPUTS                                                            */
3603 /*      dev : device pointer                                            */
3604 /*                                                                      */
3605 /*    OUTPUTS                                                           */
3606 /*      Tx buffer pointer                                               */
3607 /*                                                                      */
3608 /*    AUTHOR                                                            */
3609 /*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
3610 /*                                                                      */
3611 /************************************************************************/
3612 zbuf_t* zfGetVtxq(zdev_t* dev, u8_t ac)
3613 {
3614     zbuf_t* buf;
3615
3616     zmw_get_wlan_dev(dev);
3617
3618     zmw_declare_for_critical_section();
3619
3620     ac &= 0x3;
3621     zmw_enter_critical_section(dev);
3622     if (wd->vtxqHead[ac] != wd->vtxqTail[ac])
3623     {
3624         buf = wd->vtxq[ac][wd->vtxqTail[ac]];
3625         wd->vtxqTail[ac] = ((wd->vtxqTail[ac] + 1) & ZM_VTXQ_SIZE_MASK);
3626         zmw_leave_critical_section(dev);
3627         return buf;
3628     }
3629     else
3630     {
3631         zmw_leave_critical_section(dev);
3632         return 0; //VTXQ[] empty
3633     }
3634 }
3635
3636 /************************************************************************/
3637 /*                                                                      */
3638 /*    FUNCTION DESCRIPTION                  zfPutVmmq                   */
3639 /*      Put Tx buffer to virtual MmQ                                    */
3640 /*                                                                      */
3641 /*    INPUTS                                                            */
3642 /*      dev : device pointer                                            */
3643 /*      buf : Tx buffer pointer                                         */
3644 /*                                                                      */
3645 /*    OUTPUTS                                                           */
3646 /*      ZM_SUCCESS or error code                                        */
3647 /*                                                                      */
3648 /*    AUTHOR                                                            */
3649 /*      Stephen Chen        ZyDAS Technology Corporation    2006.12     */
3650 /*                                                                      */
3651 /************************************************************************/
3652 u16_t zfPutVmmq(zdev_t* dev, zbuf_t* buf)
3653 {
3654     zmw_get_wlan_dev(dev);
3655     zmw_declare_for_critical_section();
3656
3657     zmw_enter_critical_section(dev);
3658     if (((wd->vmmqHead + 1) & ZM_VMMQ_SIZE_MASK) != wd->vmmqTail)
3659     {
3660         wd->vmmq[wd->vmmqHead] = buf;
3661         wd->vmmqHead = ((wd->vmmqHead + 1) & ZM_VMMQ_SIZE_MASK);
3662         zmw_leave_critical_section(dev);
3663         return ZM_SUCCESS;
3664     }
3665     else
3666     {
3667         zmw_leave_critical_section(dev);
3668
3669         zfwBufFree(dev, buf, ZM_SUCCESS);
3670         zm_msg0_mm(ZM_LV_0, "Packet discarded, VMmQ full");
3671         return ZM_ERR_VMMQ_FULL; //VTXQ[] Full
3672     }
3673 }
3674
3675
3676 /************************************************************************/
3677 /*                                                                      */
3678 /*    FUNCTION DESCRIPTION                  zfGetVmmq                   */
3679 /*      Get Tx buffer from virtual MmQ                                  */
3680 /*                                                                      */
3681 /*    INPUTS                                                            */
3682 /*      dev : device pointer                                            */
3683 /*                                                                      */
3684 /*    OUTPUTS                                                           */
3685 /*      Tx buffer pointer                                               */
3686 /*                                                                      */
3687 /*    AUTHOR                                                            */
3688 /*      Stephen Chen        ZyDAS Technology Corporation    2006.12     */
3689 /*                                                                      */
3690 /************************************************************************/
3691 zbuf_t* zfGetVmmq(zdev_t* dev)
3692 {
3693     zbuf_t* buf;
3694     zmw_get_wlan_dev(dev);
3695     zmw_declare_for_critical_section();
3696
3697     zmw_enter_critical_section(dev);
3698     if (wd->vmmqHead != wd->vmmqTail)
3699     {
3700         buf = wd->vmmq[wd->vmmqTail];
3701         wd->vmmqTail = ((wd->vmmqTail + 1) & ZM_VMMQ_SIZE_MASK);
3702         zmw_leave_critical_section(dev);
3703         return buf;
3704     }
3705     else
3706     {
3707         zmw_leave_critical_section(dev);
3708         return 0; //VTXQ[] empty
3709     }
3710 }
3711
3712 /************************************************************************/
3713 /*                                                                      */
3714 /*    FUNCTION DESCRIPTION                  zfPushVtxq                  */
3715 /*      Service Virtual TxQ (weighted round robin)                      */
3716 /*      Get Tx buffer form virtual TxQ and put to hardware TxD queue    */
3717 /*                                                                      */
3718 /*    INPUTS                                                            */
3719 /*      dev : device pointer                                            */
3720 /*                                                                      */
3721 /*    OUTPUTS                                                           */
3722 /*      None                                                            */
3723 /*                                                                      */
3724 /*    AUTHOR                                                            */
3725 /*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
3726 /*                                                                      */
3727 /************************************************************************/
3728 void zfPushVtxq(zdev_t* dev)
3729 {
3730     zbuf_t* buf;
3731     u16_t i;
3732     u16_t txed;
3733     u32_t freeTxd;
3734     u16_t err;
3735     u16_t skipFlag = 0;
3736     zmw_get_wlan_dev(dev);
3737     zmw_declare_for_critical_section();
3738
3739
3740
3741     //zm_debug_msg1("zfHpGetFreeTxdCount = ", zfHpGetFreeTxdCount(dev));
3742
3743     if (wd->halState == ZM_HAL_STATE_INIT)
3744     {
3745         if (!wd->modeMDKEnable)
3746         {
3747             zm_debug_msg0("HAL is not ready for Tx");
3748         }
3749         return;
3750     }
3751     else if (wd->sta.DFSDisableTx)
3752     {
3753         zm_debug_msg0("return because 802.11h DFS Disable Tx");
3754         return;
3755     }
3756     else if (wd->sta.flagFreqChanging != 0)
3757     {
3758         //Hold until RF frequency changed
3759         return;
3760     }
3761     else if (( wd->sta.flagKeyChanging ) && ( wd->wlanMode != ZM_MODE_AP ))
3762     {
3763         return;
3764     }
3765 #ifdef ZM_ENABLE_POWER_SAVE
3766     else if ( zfPowerSavingMgrIsSleeping(dev) )
3767     {
3768         //zm_debug_msg0("Packets queued since the MAC is in power-saving mode\n");
3769         return;
3770     }
3771 #endif
3772
3773     zmw_enter_critical_section(dev);
3774     if (wd->vtxqPushing != 0)
3775     {
3776         skipFlag = 1;
3777     }
3778     else
3779     {
3780         wd->vtxqPushing = 1;
3781     }
3782     zmw_leave_critical_section(dev);
3783
3784     if (skipFlag == 1)
3785     {
3786         return;
3787     }
3788
3789     while (1)
3790     {
3791         txed = 0;
3792
3793         /* 2006.12.20, Serve Management queue */
3794         while( zfHpGetFreeTxdCount(dev) > 0 )
3795         {
3796             if ((buf = zfGetVmmq(dev)) != 0)
3797             {
3798                 txed = 1;
3799                 //zm_debug_msg2("send buf = ", buf);
3800                 if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
3801                         ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
3802                 {
3803                     zfwBufFree(dev, buf, 0);
3804                 }
3805             }
3806             else
3807             {
3808                 break;
3809             }
3810         }
3811         if ((wd->sta.bScheduleScan) || ((wd->sta.bChannelScan == TRUE) && (zfStaIsConnected(dev))))
3812         {
3813             //Hold until Scan Stop
3814             wd->vtxqPushing = 0;
3815             return;
3816         }
3817
3818 #ifdef ZM_ENABLE_AGGREGATION
3819     #ifndef ZM_BYPASS_AGGR_SCHEDULING
3820         if( (wd->wlanMode == ZM_MODE_AP) ||
3821             (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
3822             (wd->wlanMode == ZM_MODE_PSEUDO) ) {
3823
3824             zfAggTxScheduler(dev, 0);
3825
3826             if (txed == 0) {
3827                 wd->vtxqPushing = 0;
3828                 return;
3829             }
3830             else {
3831                 continue;
3832             }
3833         }
3834     #endif
3835 #endif
3836
3837         /* Service VTxQ[3] */
3838         for (i=0; i<4; i++)
3839         {
3840             if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= 3)
3841             {
3842                 if ((buf = zfGetVtxq(dev, 3)) != 0)
3843                 {
3844                     txed = 1;
3845                     //zm_debug_msg2("send buf = ", buf);
3846                     zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
3847                     ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
3848                 }
3849             }
3850             else
3851             {
3852                 break;
3853             }
3854         }
3855
3856         /* Service VTxQ[2] */
3857         for (i=0; i<3; i++)
3858         {
3859             if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*1/4))
3860             {
3861                 if ((buf = zfGetVtxq(dev, 2)) != 0)
3862                 {
3863                     txed = 1;
3864                     zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
3865                     ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
3866                 }
3867                 if (wd->sta.ac0PriorityHigherThanAc2 == 1)
3868                 {
3869                     if ((buf = zfGetVtxq(dev, 0)) != 0)
3870                     {
3871                         txed = 1;
3872                         zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
3873                         ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
3874                     }
3875                 }
3876             }
3877             else
3878             {
3879                 break;
3880             }
3881         }
3882
3883         /* Service VTxQ[0] */
3884         for (i=0; i<2; i++)
3885         {
3886             if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*2/4))
3887             {
3888                 if ((buf = zfGetVtxq(dev, 0)) != 0)
3889                 {
3890                     txed = 1;
3891                     zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
3892                     ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
3893                 }
3894             }
3895             else
3896             {
3897                 break;
3898             }
3899
3900         }
3901
3902         /* Service VTxQ[1] */
3903         if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*3/4))
3904         {
3905             if ((buf = zfGetVtxq(dev, 1)) != 0)
3906             {
3907                 txed = 1;
3908                 zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
3909                 ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
3910             }
3911         }
3912
3913         /* All VTxQs are either empty or exceed their threshold */
3914         if (txed == 0)
3915         {
3916             wd->vtxqPushing = 0;
3917             return;
3918         }
3919     } //while (1)
3920 }
3921
3922
3923 /************************************************************************/
3924 /*                                                                      */
3925 /*    FUNCTION DESCRIPTION                  zfFlushVtxq                 */
3926 /*      Flush Virtual TxQ and MmQ                                       */
3927 /*                                                                      */
3928 /*    INPUTS                                                            */
3929 /*      dev : device pointer                                            */
3930 /*                                                                      */
3931 /*    OUTPUTS                                                           */
3932 /*      None                                                            */
3933 /*                                                                      */
3934 /*    AUTHOR                                                            */
3935 /*      Stephen Chen        Atheros Communications, INC.    2007.1      */
3936 /*                                                                      */
3937 /************************************************************************/
3938 void zfFlushVtxq(zdev_t* dev)
3939 {
3940     zbuf_t* buf;
3941     u8_t i;
3942     zmw_get_wlan_dev(dev);
3943
3944     /* Flush MmQ */
3945     while ((buf = zfGetVmmq(dev)) != 0)
3946     {
3947         zfwBufFree(dev, buf, 0);
3948         zm_debug_msg0("zfFlushVtxq: [Vmmq]");
3949         wd->queueFlushed  |= 0x10;
3950     }
3951
3952     /* Flush VTxQ */
3953     for (i=0; i<4; i++)
3954     {
3955         while ((buf = zfGetVtxq(dev, i)) != 0)
3956         {
3957             zfwBufFree(dev, buf, 0);
3958             zm_debug_msg1("zfFlushVtxq: [zfGetVtxq]- ", i);
3959             wd->queueFlushed |= (1<<i);
3960         }
3961     }
3962 }
3963
3964 void zf80211FrameSend(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t snapLen,
3965                            u16_t* da, u16_t* sa, u8_t up, u16_t headerLen, u16_t* snap,
3966                            u16_t* tail, u16_t tailLen, u16_t offset, u16_t bufType,
3967                            u8_t ac, u8_t keyIdx)
3968 {
3969     u16_t err;
3970     u16_t fragLen;
3971
3972     zmw_get_wlan_dev(dev);
3973
3974     fragLen = zfwBufGetSize(dev, buf);
3975     if ((da[0]&0x1) == 0)
3976     {
3977         wd->commTally.txUnicastFrm++;
3978         wd->commTally.txUnicastOctets += (fragLen+snapLen);
3979     }
3980     else if (da[0] == 0xffff)
3981     {
3982         wd->commTally.txBroadcastFrm++;
3983         wd->commTally.txBroadcastOctets += (fragLen+snapLen);
3984     }
3985     else
3986     {
3987         wd->commTally.txMulticastFrm++;
3988         wd->commTally.txMulticastOctets += (fragLen+snapLen);
3989     }
3990     wd->ledStruct.txTraffic++;
3991
3992     if ((err = zfHpSend(dev, header, headerLen, snap, snapLen,
3993                         tail, tailLen, buf, offset,
3994                         bufType, ac, keyIdx)) != ZM_SUCCESS)
3995     {
3996         if (bufType == ZM_EXTERNAL_ALLOC_BUF)
3997         {
3998             zfwBufFree(dev, buf, err);
3999         }
4000         else if (bufType == ZM_INTERNAL_ALLOC_BUF)
4001         {
4002             zfwBufFree(dev, buf, 0);
4003         }
4004         else
4005         {
4006             zm_assert(0);
4007         }
4008     }
4009 }
4010
4011 void zfCheckIsRIFSFrame(zdev_t* dev, zbuf_t* buf, u16_t frameSubtype)
4012 {
4013     zmw_get_wlan_dev(dev);
4014
4015     /* #2 Record the sequence number to determine whether the unicast frame is separated by RIFS or not */
4016     if (frameSubtype & 0x80)
4017     {   //QoS data frame
4018         u16_t sequenceNum;
4019         u16_t qosControlField;
4020
4021         sequenceNum = ( zmw_buf_readh(dev, buf, 22) >> 4 ); // Discard fragment number !
4022         qosControlField = zmw_buf_readh(dev, buf, 24); // Don't consider WDS (Wireless Distribution System)
4023         //DbgPrint("The QoS Control Field                              : %d", qosControlField);
4024         //DbgPrint("The RIFS Count                                     : %d", wd->sta.rifsCount);
4025
4026         if( qosControlField & ZM_BIT_5 )
4027         {// ACK policy is "No ACK"
4028             /* RIFS-Like frame */
4029             wd->sta.rifsLikeFrameSequence[wd->sta.rifsLikeFrameCnt]   = sequenceNum;
4030
4031             if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTING )
4032             {
4033                 if( wd->sta.rifsLikeFrameSequence[2] != 0 )
4034                 {// RIFS-like Pattern collected
4035                     if( ( wd->sta.rifsLikeFrameSequence[2] - wd->sta.rifsLikeFrameSequence[1] == 2 ) &&
4036                         ( wd->sta.rifsLikeFrameSequence[1] - wd->sta.rifsLikeFrameSequence[0] == 2 ) )
4037                     {
4038                         /* RIFS pattern matched */
4039
4040                         /* #3 Enable RIFS function if the RIFS pattern matched  */
4041                         zfHpEnableRifs(dev, ((wd->sta.currentFrequency<3000)?1:0), wd->sta.EnableHT, wd->sta.HT2040);
4042
4043                         // Set RIFS timer
4044                         wd->sta.rifsTimer = wd->tick;
4045
4046                         wd->sta.rifsCount++;
4047
4048                         // Set state to be Detected
4049                         wd->sta.rifsState = ZM_RIFS_STATE_DETECTED;
4050                     }
4051                 }
4052             }
4053             else
4054             {// state = Detected
4055                 // Reset RIFS timer
4056                 if( (wd->tick - wd->sta.rifsTimer) < ZM_RIFS_TIMER_TIMEOUT )
4057                     wd->sta.rifsTimer = wd->tick;
4058             }
4059
4060             //DbgPrint("SN1 = %d, SN2 = %d, SN3 = %d\n", wd->sta.rifsLikeFrameSequence[0],
4061             //                                           wd->sta.rifsLikeFrameSequence[1],
4062             //                                           wd->sta.rifsLikeFrameSequence[2]);
4063
4064             // Update RIFS-like sequence number
4065             if( wd->sta.rifsLikeFrameSequence[2] != 0 )
4066             {
4067                 wd->sta.rifsLikeFrameSequence[0] = wd->sta.rifsLikeFrameSequence[1];
4068                 wd->sta.rifsLikeFrameSequence[1] = wd->sta.rifsLikeFrameSequence[2];
4069                 wd->sta.rifsLikeFrameSequence[2] = 0;
4070             }
4071
4072             // Only record three adjacent frame
4073             if( wd->sta.rifsLikeFrameCnt < 2 )
4074                 wd->sta.rifsLikeFrameCnt++;
4075         }
4076     }
4077
4078     /* #4 Disable RIFS function if the timer TIMEOUT  */
4079     if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
4080     {
4081         if( ( wd->tick - wd->sta.rifsTimer ) > ZM_RIFS_TIMER_TIMEOUT )
4082         {// TIMEOUT
4083             // Disable RIFS
4084             zfHpDisableRifs(dev);
4085
4086             // Reset RIFS-like sequence number FIFO
4087             wd->sta.rifsLikeFrameSequence[0] = 0;
4088             wd->sta.rifsLikeFrameSequence[1] = 0;
4089             wd->sta.rifsLikeFrameSequence[2] = 0;
4090             wd->sta.rifsLikeFrameCnt = 0;
4091
4092             // Set state to be Detecting
4093             wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
4094         }
4095     }
4096 }