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