SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/staging/otus/80211core/cinit.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 : init.c                                                */
  18 /*                                                                      */
  19 /*  Abstract                                                            */
  20 /*      This module contains init functions.                            */
  21 /*                                                                      */
  22 /*  NOTES                                                               */
  23 /*      None                                                            */
  24 /*                                                                      */
  25 /************************************************************************/
  26 #include "cprecomp.h"
  27 #include "../hal/hpreg.h"
  28
  29 extern const u8_t zcUpToAc[8];
  30
  31 u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
  32                                24000, 12000, 6000, 54000, 36000, 18000, 9000};
  33 u32_t zcIndextoRateN20L[16] = {6500, 13000, 19500, 26000, 39000, 52000, 58500,
  34                               65000, 13000, 26000, 39000, 52000, 78000, 104000,
  35                               117000, 130000};
  36 u32_t zcIndextoRateN20S[16] = {7200, 14400, 21700, 28900, 43300, 57800, 65000,
  37                               72200, 14400, 28900, 43300, 57800, 86700, 115600,
  38                               130000, 144400};
  39 u32_t zcIndextoRateN40L[16] = {13500, 27000, 40500, 54000, 81000, 108000, 121500,
  40                               135000, 27000, 54000, 81000, 108000, 162000, 216000,
  41                               243000, 270000};
  42 u32_t zcIndextoRateN40S[16] = {15000, 30000, 45000, 60000, 90000, 120000, 135000,
  43                               150000, 30000, 60000, 90000, 120000, 180000, 240000,
  44                               270000, 300000};
  45
  46 /************************************************************************/
  47 /*                                                                      */
  48 /*    FUNCTION DESCRIPTION                  zfTxGenWlanHeader           */
  49 /*      Generate WLAN MAC header and LLC header.                        */
  50 /*                                                                      */
  51 /*    INPUTS                                                            */
  52 /*      dev : device pointer                                            */
  53 /*      buf : buffer pointer                                            */
  54 /*      id : Index of TxD                                               */
  55 /*      port : WLAN port                                                */
  56 /*                                                                      */
  57 /*    OUTPUTS                                                           */
  58 /*      length of removed Ethernet header                               */
  59 /*                                                                      */
  60 /*    AUTHOR                                                            */
  61 /*      Stephen             ZyDAS Technology Corporation    2005.5      */
  62 /*                                                                      */
  63 /************************************************************************/
  64 u16_t zfTxGenWlanHeader(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t seq,
  65                         u8_t flag, u16_t plusLen, u16_t minusLen, u16_t port,
  66                         u16_t* da, u16_t* sa, u8_t up, u16_t *micLen,
  67                         u16_t* snap, u16_t snapLen, struct aggControl *aggControl)
  68 {
  69
  70     u16_t len;
  71     u16_t macCtrl;
  72     u32_t phyCtrl;
  73     u16_t hlen = 16;
  74     u16_t icvLen = 0;
  75     u16_t wdsPortId;
  76     u16_t vap = 0;
  77     u16_t mcs = 0;
  78     u16_t mt = 0;
  79     u8_t  qosType;
  80     u8_t  b1, b2;
  81     u16_t wdsPort;
  82     u8_t  encExemptionActionType;
  83     u16_t rateProbingFlag = 0;
  84     u8_t  tkipFrameOffset = 0;
  85
  86 #ifdef ZM_ENABLE_IBSS_WPA2PSK
  87     u8_t    res, peerIdx;
  88     u8_t    userIdx=0;
  89     u16_t   *iv16;
  90     u32_t   *iv32;
  91 #endif
  92
  93     zmw_get_wlan_dev(dev);
  94
  95    /* Generate WLAN header */
  96     /* Frame control */
  97     header[4] = 0x0008 | (flag<<8);
  98     /* Duration */
  99     header[5] = 0x0000;
 100
 101     if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
 102     {
 103         /* ToDS bit */
 104         header[4] |= 0x0100;
 105
 106         /*Sometimes we wake up to tx/rx but AP still think we are sleeping, so still need to set this bit*/
 107         if ( zfPowerSavingMgrIsSleeping(dev) || wd->sta.psMgr.tempWakeUp == 1 )
 108         {
 109             header[4] |= 0x1000;
 110         }
 111
 112         /* Address 1 = BSSID */
 113         header[6] = wd->sta.bssid[0];
 114         header[7] = wd->sta.bssid[1];
 115         header[8] = wd->sta.bssid[2];
 116         /* Address 3 = DA */
 117         header[12] = da[0];
 118         header[13] = da[1];
 119         header[14] = da[2];
 120     }
 121     else if (wd->wlanMode == ZM_MODE_PSEUDO)
 122     {
 123         /* Address 1 = DA */
 124         header[6] = da[0];
 125         header[7] = da[1];
 126         header[8] = da[2];
 127         /* Address 3 = 00:00:00:00:00:00 */
 128         header[12] = 0;
 129         header[13] = 0;
 130         header[14] = 0;
 131
 132         /* PSEUDO test : WDS */
 133         if (wd->enableWDS)
 134         {
 135             /* ToDS and FromDS bit */
 136             header[4] |= 0x0300;
 137
 138             /* Address 4 = SA */
 139             header[16] = 0;
 140             header[17] = 0;
 141             header[18] = 0;
 142
 143             hlen = 19;
 144         }
 145     }
 146     else if (wd->wlanMode == ZM_MODE_IBSS)
 147     {
 148         /* Address 1 = DA */
 149         header[6] = da[0];
 150         header[7] = da[1];
 151         header[8] = da[2];
 152         /* Address 3 = BSSID */
 153         header[12] = wd->sta.bssid[0];
 154         header[13] = wd->sta.bssid[1];
 155         header[14] = wd->sta.bssid[2];
 156
 157 #ifdef ZM_ENABLE_IBSS_WPA2PSK
 158         zmw_enter_critical_section(dev);
 159         res = zfStaFindOppositeByMACAddr(dev, da, &peerIdx);
 160         if(res == 0) // Find opposite in our OppositeInfo Structure !
 161         {
 162             userIdx = peerIdx;
 163         }
 164         zmw_leave_critical_section(dev);
 165 #endif
 166     }
 167     else if (wd->wlanMode == ZM_MODE_AP)
 168     {
 169         if (port < 0x20)
 170         /* AP mode */
 171         {
 172             /* FromDS bit */
 173             header[4] |= 0x0200;
 174
 175             /* Address 1 = DA */
 176             header[6] = da[0];
 177             header[7] = da[1];
 178             header[8] = da[2];
 179             /* Address 3 = SA */
 180             header[12] = sa[0];
 181             header[13] = sa[1];
 182             header[14] = sa[2];
 183
 184             if (port < ZM_MAX_AP_SUPPORT)
 185             {
 186                 vap = port;
 187                 header[14] += (vap<<8);
 188             }
 189         }
 190         else
 191         /* WDS port */
 192         {
 193             /* ToDS and FromDS bit */
 194             header[4] |= 0x0300;
 195
 196             wdsPortId = port - 0x20;
 197
 198             /* Address 1 = RA */
 199             header[6] = wd->ap.wds.macAddr[wdsPortId][0];
 200             header[7] = wd->ap.wds.macAddr[wdsPortId][1];
 201             header[8] = wd->ap.wds.macAddr[wdsPortId][2];
 202             /* Address 3 = DA */
 203             header[12] = da[0];
 204             header[13] = da[1];
 205             header[14] = da[2];
 206             /* Address 4 = SA */
 207             header[16] = sa[0];
 208             header[17] = sa[1];
 209             header[18] = sa[2];
 210
 211             hlen = 19;
 212         }
 213     } /* else if (wd->wlanMode == ZM_MODE_AP) */
 214
 215     /* Address 2 = TA */
 216     header[9] = wd->macAddr[0];
 217     header[10] = wd->macAddr[1];
 218 #ifdef ZM_VAPMODE_MULTILE_SSID
 219     header[11] = wd->macAddr[2]; //Multiple SSID
 220 #else
 221     header[11] = wd->macAddr[2] + (vap<<8); //VAP
 222 #endif
 223
 224     if ( (wd->wlanMode == ZM_MODE_IBSS) && (wd->XLinkMode) )
 225     {
 226         header[9]  = sa[0];
 227         header[10] = sa[1];
 228         header[11] = sa[2];
 229     }
 230
 231     /* Sequence Control */
 232     header[15] = seq;
 233
 234
 235     if (wd->wlanMode == ZM_MODE_AP)
 236     {
 237         zfApGetStaTxRateAndQosType(dev, da, &phyCtrl, &qosType, &rateProbingFlag);
 238         mt = (u16_t)(phyCtrl & 0x3);
 239         mcs = (u16_t)((phyCtrl >> 16) & 0x3f);
 240 #if 1
 241         //zfApGetStaQosType(dev, da, &qosType);
 242
 243         /* if DA == WME STA */
 244         if (qosType == 1)
 245         {
 246             /* QoS data */
 247             header[4] |= 0x0080;
 248
 249             /* QoS Control */
 250             header[hlen] = up;
 251             hlen += 1;
 252         }
 253 #endif
 254     }
 255
 256 #if 0
 257     //AGG Test Code
 258     if (header[6] == 0x8000)
 259     {
 260         /* QoS data */
 261         header[4] |= 0x0080;
 262
 263         /* QoS Control */
 264         header[hlen] = 0;
 265         hlen += 1;
 266     }
 267 #endif
 268
 269     if (wd->wlanMode == ZM_MODE_AP) {
 270         /* Todo: rate control here for qos field */
 271     }
 272     else {
 273         /* Rate control */
 274         zfStaGetTxRate(dev, da, &phyCtrl, &rateProbingFlag);
 275         mt = (u16_t)(phyCtrl & 0x3);
 276         mcs = (u16_t)((phyCtrl >> 16) & 0x3f);
 277     }
 278
 279     if (wd->txMCS != 0xff)
 280     {
 281         /* fixed rate */
 282             phyCtrl = ((u32_t)wd->txMCS<<16) + wd->txMT;
 283         mcs = wd->txMCS;
 284         mt = wd->txMT;
 285     }
 286
 287     if (wd->enableAggregation)
 288     {
 289         /* force enable aggregation */
 290         if (wd->enableAggregation==2 && !(header[6]&0x1))
 291         {
 292             /* QoS data */
 293             header[4] |= 0x0080;
 294
 295             /* QoS Control */
 296             header[hlen] = 0;
 297             hlen += 1;
 298         }
 299         /* if wd->enableAggregation=1 => force disable */
 300         /* if wd->enableAggregation=0 => auto */
 301     }
 302
 303 #ifdef ZM_ENABLE_AGGREGATION
 304     /*
 305      * aggregation control
 306      */
 307
 308     /*
 309      * QoS data
 310      */
 311     if (wd->wlanMode == ZM_MODE_AP) {
 312         if (aggControl && mt == 2) {
 313             if (wd->enableAggregation==0 && !(header[6]&0x1))
 314             {
 315                 header[4] |= 0x0080;
 316
 317                 /*
 318                  * QoS Control
 319                  */
 320                 header[hlen] = 0;
 321                 hlen += 1;
 322             }
 323         }
 324     }
 325 #endif
 326
 327     // MSDU Length
 328     len = zfwBufGetSize(dev, buf);
 329
 330     /* Generate control setting */
 331     /* Backoff, Non-Burst and hardware duration */
 332     macCtrl = 0x208;
 333
 334     /* ACK */
 335     if ((header[6] & 0x1) == 0x1)
 336     {
 337         /* multicast frame : Set NO-ACK bit */
 338         macCtrl |= 0x4;
 339     }
 340     else
 341     {
 342         /* unicast frame */
 343     #if 0
 344         // Enable RTS according to MPDU Lengths ( not MSDU Lengths )
 345         if (len >= wd->rtsThreshold)
 346         {
 347             /* Enable RTS */
 348             macCtrl |= 1;
 349         }
 350     #endif
 351     }
 352     /* VAP test code */
 353     //macCtrl |= 0x4;
 354
 355     if (wd->wlanMode == ZM_MODE_AP)
 356     {
 357         u8_t encryType;
 358         u16_t iv16;
 359         u32_t iv32;
 360
 361         /* Check whether this is a multicast frame */
 362         if ((header[6] & 0x1) == 0x1)
 363         {
 364             /* multicast frame */
 365             if (wd->ap.encryMode[vap] == ZM_TKIP)
 366             {
 367                 wd->ap.iv16[vap]++;
 368
 369                 if(wd->ap.iv16[vap] == 0)
 370                 {
 371                     wd->ap.iv32[vap]++;
 372                 }
 373
 374                 b1 = (u8_t) (wd->ap.iv16[vap] >> 8);
 375                 b2 = (b1 | 0x20) & 0x7f;
 376                 header[hlen] = ((u16_t)b2 << 8) + b1;
 377                 b1 = (u8_t) wd->ap.iv16[vap];
 378                 b2 = 0x20 | (wd->ap.bcKeyIndex[vap] << 6);
 379                 header[hlen+1] = ((u16_t)b2 << 8) + b1;
 380                 header[hlen+2] = (u16_t) wd->ap.iv32[vap];
 381                 header[hlen+3] = (u16_t) (wd->ap.iv32[vap] >> 16);
 382
 383                 //macCtrl |= 0x80;
 384                 macCtrl |= 0x40;
 385                 icvLen = 4;
 386
 387                 /* set hardware MIC */
 388                 if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
 389                 {
 390                     macCtrl |= 0x100;
 391                     plusLen += 8;
 392                     *micLen = 8;
 393                 }
 394
 395                 header[4] |= 0x4000;
 396                 hlen += 4;
 397             }
 398             else if (wd->ap.encryMode[vap] == ZM_AES)
 399             {
 400                 wd->ap.iv16[vap]++;
 401
 402                 if(wd->ap.iv16[vap] == 0)
 403                 {
 404                     wd->ap.iv32[vap]++;
 405                 }
 406
 407                 b1 = (u8_t) wd->ap.iv16[vap];
 408                 b2 = (u8_t) (wd->ap.iv16[vap] >> 8);
 409                 header[hlen] = ((u16_t)b2 << 8) + b1;
 410                 header[hlen+1] = 0x2000 | (wd->ap.bcKeyIndex[vap] << 14);
 411                 header[hlen+2] = (u16_t) (wd->ap.iv32[vap]);
 412                 header[hlen+3] = (u16_t) (wd->ap.iv32[vap] >> 16);
 413
 414                 macCtrl |= 0xc0;
 415                 icvLen = 8;  /* MIC */
 416
 417                 header[4] |= 0x4000;
 418                 hlen += 4;
 419             }
 420             #ifdef ZM_ENABLE_CENC
 421             else if (wd->ap.encryMode[vap] == ZM_CENC)
 422             {
 423                 //u32_t txiv[4];
 424
 425                 wd->ap.txiv[vap][0]++;
 426
 427                 if (wd->ap.txiv[vap][0] == 0)
 428                 {
 429                     wd->ap.txiv[vap][1]++;
 430                 }
 431
 432                 if (wd->ap.txiv[vap][1] == 0)
 433                 {
 434                     wd->ap.txiv[vap][2]++;
 435                 }
 436
 437                 if (wd->ap.txiv[vap][2] == 0)
 438                 {
 439                     wd->ap.txiv[vap][3]++;
 440                 }
 441
 442                 if (wd->ap.txiv[vap][3] == 0)
 443                 {
 444                     wd->ap.txiv[vap][0] = 0;
 445                     wd->ap.txiv[vap][1] = 0;
 446                     wd->ap.txiv[vap][2] = 0;
 447                 }
 448
 449                 header[hlen] = (wd->ap.bcKeyIndex[vap] & 0x0001);    /* For Key Id and reserved field */
 450                 header[hlen+1] = (u16_t)wd->ap.txiv[vap][0];
 451                 header[hlen+2] = (u16_t)(wd->ap.txiv[vap][0] >> 16);
 452                 header[hlen+3] = (u16_t)wd->ap.txiv[vap][1];
 453                 header[hlen+4] = (u16_t)(wd->ap.txiv[vap][1] >> 16);
 454                 header[hlen+5] = (u16_t)wd->ap.txiv[vap][2];
 455                 header[hlen+6] = (u16_t)(wd->ap.txiv[vap][2] >> 16);
 456                 header[hlen+7] = (u16_t)wd->ap.txiv[vap][3];
 457                 header[hlen+8] = (u16_t)(wd->ap.txiv[vap][3] >> 16);
 458
 459                 macCtrl |= 0x80;
 460                 icvLen = 16;  /* MIC */
 461
 462                 header[4] |= 0x4000;
 463                 hlen += 9;
 464             }
 465             #endif //ZM_ENABLE_CENC
 466         }
 467         else
 468         {
 469             /* Get STA's encryption type */
 470             zfApGetStaEncryType(dev, da, &encryType);
 471
 472             if (encryType == ZM_TKIP)
 473             {
 474                 /* Get iv16 and iv32 */
 475                 zfApGetStaWpaIv(dev, da, &iv16, &iv32);
 476
 477                 iv16++;
 478                 if (iv16 == 0)
 479                 {
 480                     iv32++;
 481                 }
 482
 483                 b1 = (u8_t) (iv16 >> 8);
 484                 b2 = (b1 | 0x20) & 0x7f;
 485                 header[hlen] = ((u16_t)b2 << 8) + b1;
 486                 b1 = (u8_t) iv16;
 487                 b2 = 0x20;
 488                 header[hlen+1] = ((u16_t)b2 << 8) + b1;
 489                 header[hlen+2] = (u16_t) iv32;
 490                 header[hlen+3] = (u16_t) (iv32 >> 16);
 491
 492                 //macCtrl |= 0x80;
 493                 macCtrl |= 0x40;
 494                 icvLen = 4;
 495
 496                 /* set hardware MIC */
 497                 if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
 498                 {
 499                     macCtrl |= 0x100;
 500                     plusLen += 8;
 501                     *micLen = 8;
 502                 }
 503
 504                 header[4] |= 0x4000;
 505                 hlen += 4;
 506
 507                 /* Set iv16 and iv32 */
 508                 zfApSetStaWpaIv(dev, da, iv16, iv32);
 509             }
 510             else if (encryType == ZM_AES)
 511             {
 512                 /* Get iv16 and iv32 */
 513                 zfApGetStaWpaIv(dev, da, &iv16, &iv32);
 514
 515                 iv16++;
 516                 if (iv16 == 0)
 517                 {
 518                     iv32++;
 519                 }
 520
 521                 b1 = (u8_t) iv16;
 522                 b2 = (u8_t) (iv16 >> 8);
 523                 header[hlen] = ((u16_t)b2 << 8) + b1;
 524                 header[hlen+1] = 0x2000;
 525                 header[hlen+2] = (u16_t) (iv32);
 526                 header[hlen+3] = (u16_t) (iv32 >> 16);
 527
 528                 macCtrl |= 0xc0;
 529                 icvLen = 8;  /* MIC */
 530
 531                 header[4] |= 0x4000;
 532                 hlen += 4;
 533
 534                 /* Set iv16 and iv32 */
 535                 zfApSetStaWpaIv(dev, da, iv16, iv32);
 536             }
 537             #ifdef ZM_ENABLE_CENC
 538             else if (encryType == ZM_CENC)
 539             {
 540                 u32_t txiv[4];
 541                 u8_t keyIdx;
 542
 543                 /* Get CENC TxIV */
 544                 zfApGetStaCencIvAndKeyIdx(dev, da, txiv, &keyIdx);
 545
 546                 txiv[0] += 2;
 547
 548                 if (txiv[0] == 0 || txiv[0] == 1)
 549                 {
 550                     txiv[1]++;
 551                 }
 552
 553                 if (txiv[1] == 0)
 554                 {
 555                     txiv[2]++;
 556                 }
 557
 558                 if (txiv[2] == 0)
 559                 {
 560                     txiv[3]++;
 561                 }
 562
 563                 if (txiv[3] == 0)
 564                 {
 565                     txiv[0] = 0;
 566                     txiv[1] = 0;
 567                     txiv[2] = 0;
 568                 }
 569
 570                 header[hlen] = (keyIdx & 0x0001);    /* For Key Id and reserved field */
 571                 header[hlen+1] = (u16_t)txiv[0];
 572                 header[hlen+2] = (u16_t)(txiv[0] >> 16);
 573                 header[hlen+3] = (u16_t)txiv[1];
 574                 header[hlen+4] = (u16_t)(txiv[1] >> 16);
 575                 header[hlen+5] = (u16_t)txiv[2];
 576                 header[hlen+6] = (u16_t)(txiv[2] >> 16);
 577                 header[hlen+7] = (u16_t)txiv[3];
 578                 header[hlen+8] = (u16_t)(txiv[3] >> 16);
 579
 580                 macCtrl |= 0x80;
 581                 icvLen = 16;  /* MIC */
 582
 583                 header[4] |= 0x4000;
 584                 hlen += 9;
 585
 586                 /* Set CENC IV */
 587                 zfApSetStaCencIv(dev, da, txiv);
 588             }
 589             #endif //ZM_ENABLE_CENC
 590         }
 591
 592         /* protection mode */
 593         if (wd->ap.protectionMode == 1)
 594         {
 595             /* Enable Self-CTS */
 596             macCtrl &= 0xFFFC;
 597             macCtrl |= 2;
 598         }
 599
 600         /* Rate Control */
 601         if (port < 0x20)
 602         {
 603             /* AP */
 604             /* IV */
 605             if ((wd->ap.encryMode[vap] == ZM_WEP64) ||
 606                     (wd->ap.encryMode[vap] == ZM_WEP128) ||
 607                     (wd->ap.encryMode[vap] == ZM_WEP256))
 608             {
 609                 header[4] |= 0x4000;
 610                 header[hlen] = 0x0;   //IV
 611                 header[hlen+1] = wd->ap.bcKeyIndex[vap] << 14; //IV with Keyid--CWYang(m)
 612                 hlen += 2;
 613                 icvLen = 4;
 614                 macCtrl |= 0x40;
 615             }
 616         }
 617         else
 618         {
 619             /* WDS */
 620
 621             /* TODO : Fixed rate to 54M */
 622             phyCtrl = 0xc0001;   //PHY control L
 623
 624             /* WDS port checking */
 625             wdsPort = port - 0x20;
 626             if (wdsPort >= ZM_MAX_WDS_SUPPORT)
 627             {
 628                 wdsPort = 0;
 629             }
 630
 631             #if 1
 632             /* IV */
 633             switch (wd->ap.wds.encryMode[wdsPort])
 634             {
 635             case ZM_WEP64:
 636             case ZM_WEP128:
 637             case ZM_WEP256:
 638                     header[4] |= 0x4000;
 639                     header[hlen] = 0x0;   //IV
 640                     header[hlen+1] = wd->ap.bcKeyIndex[vap] << 14; //IV with Keyid
 641                     hlen += 2;
 642                     icvLen = 4;
 643                     macCtrl |= 0x40;
 644                     break;
 645
 646             case ZM_TKIP:
 647                     wd->sta.iv16++;
 648
 649                     if ( wd->sta.iv16 == 0 )
 650                     {
 651                         wd->sta.iv32++;
 652                     }
 653
 654                     b1 = (u8_t) (wd->sta.iv16 >> 8);
 655                     b2 = (b1 | 0x20) & 0x7f;
 656                     header[hlen] = ((u16_t)b2 << 8) + b1;
 657                     b1 = (u8_t) wd->sta.iv16;
 658                     b2 = 0x20;
 659                     header[hlen+1] = ((u16_t)b2 << 8) + b1;
 660                     header[hlen+2] = (u16_t) wd->sta.iv32;
 661                     header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
 662
 663                     //macCtrl |= 0x80;
 664                     macCtrl |= 0x40;
 665                     icvLen = 4;
 666
 667                     /* set hardware MIC */
 668                     if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
 669                     {
 670                         macCtrl |= 0x100;
 671                         plusLen += 8;
 672                         *micLen = 8;
 673                     }
 674
 675                     header[4] |= 0x4000;
 676                     hlen += 4;
 677                     break;
 678
 679             case ZM_AES:
 680                     wd->sta.iv16++;
 681                     if ( wd->sta.iv16 == 0 )
 682                     {
 683                         wd->sta.iv32++;
 684                     }
 685
 686                     b1 = (u8_t) wd->sta.iv16;
 687                     b2 = (u8_t) (wd->sta.iv16 >> 8);
 688                     header[hlen] = ((u16_t)b2 << 8) + b1;
 689                     header[hlen+1] = 0x2000;
 690                     header[hlen+2] = (u16_t) (wd->sta.iv32);
 691                     header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
 692
 693                     macCtrl |= 0xc0; /* Set to AES in control setting */
 694                     icvLen = 8;  /* MIC */
 695
 696                     header[4] |= 0x4000; /* Set WEP bit in wlan header */
 697                     hlen += 4; /* plus IV length */
 698                     break;
 699             }/* end of switch */
 700             #endif
 701         }
 702     }
 703     else   /* wd->wlanMode != ZM_MODE_AP */
 704     {
 705         encExemptionActionType = zfwGetPktEncExemptionActionType(dev, buf);
 706
 707         if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
 708         {
 709             #if 1
 710             /* if WME AP */
 711             if (wd->sta.wmeConnected != 0)
 712             {
 713                 /* QoS data */
 714                 header[4] |= 0x0080;
 715
 716                 /* QoS Control */
 717                 header[hlen] = up;
 718                 hlen += 1;
 719             }
 720             #endif
 721
 722             if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
 723             {
 724                 if ( wd->sta.authMode < ZM_AUTH_MODE_WPA )
 725                 {   /* non-WPA */
 726                     if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
 727                     {
 728                         if ( (wd->sta.encryMode == ZM_WEP64)||
 729                              (wd->sta.encryMode == ZM_WEP128)||
 730                              (wd->sta.encryMode == ZM_WEP256) )
 731                         {
 732                             header[4] |= 0x4000;
 733                             header[hlen] = 0x0;   //IV
 734                             header[hlen+1] = 0x0; //IV
 735                             header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
 736                             hlen += 2;
 737                             icvLen = 4;
 738
 739                             /* For Software WEP */
 740                             if ((wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) != 0)
 741                             {
 742                                 u8_t keyLen = 5;
 743                                 u8_t iv[3];
 744
 745                                 iv[0] = 0x0;
 746                                 iv[1] = 0x0;
 747                                 iv[2] = 0x0;
 748
 749                                 if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP64)
 750                                 {
 751                                     keyLen = 5;
 752                                 }
 753                                 else if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP128)
 754                                 {
 755                                     keyLen = 13;
 756                                 }
 757                                 else if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP256)
 758                                 {
 759                                     keyLen = 29;
 760                                 }
 761
 762                                 zfWEPEncrypt(dev, buf, (u8_t*) snap, snapLen, minusLen, keyLen,
 763                                         wd->sta.wepKey[wd->sta.keyId], iv);
 764                             }
 765                             else
 766                             {
 767                                 macCtrl |= 0x40;
 768                             }
 769                         }
 770                     }
 771                 }
 772                 else
 773                 {   /* WPA */
 774                     if ( wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK )
 775                     {
 776                         wd->sta.iv16++;
 777                         if ( wd->sta.iv16 == 0 )
 778                         {
 779                             wd->sta.iv32++;
 780                         }
 781
 782                         /* set encryption mode */
 783                         if ( wd->sta.encryMode == ZM_TKIP )
 784                         {
 785                             b1 = (u8_t) (wd->sta.iv16 >> 8);
 786                             b2 = (b1 | 0x20) & 0x7f;
 787                             header[hlen] = ((u16_t)b2 << 8) + b1;
 788                             b1 = (u8_t) wd->sta.iv16;
 789                             b2 = 0x20;
 790
 791                             // header[hlen+1] = (((u16_t) wd->sta.keyId) << 14) | (((u16_t)b2 << 8) + b1);
 792                             // STA in infrastructure mode should use keyId = 0 to transmit unicast !
 793                             header[hlen+1] = (((u16_t)b2 << 8) + b1);
 794                             header[hlen+2] = (u16_t) wd->sta.iv32;
 795                             header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
 796
 797                             /* If software encryption enable */
 798                             if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0)
 799                             {
 800                                 //macCtrl |= 0x80;
 801                                 /* TKIP same to WEP */
 802                                 macCtrl |= 0x40;
 803                                 icvLen = 4;
 804
 805                                 /* set hardware MIC */
 806                                 if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
 807                                 {
 808                                     macCtrl |= 0x100;
 809                                     plusLen += 8;
 810                                     *micLen = 8;
 811                                 }
 812                             }
 813                             else
 814                             {
 815                                 u8_t mic[8];
 816                                 u16_t offset;
 817                                 u32_t icv;
 818                                 u8_t RC4Key[16];
 819
 820                                 /* TODO: Remove the criticial section here. */
 821                                 zmw_declare_for_critical_section();
 822
 823                                 zmw_enter_critical_section(dev);
 824                                 /* Calculate MIC */
 825                                 zfCalTxMic(dev, buf, (u8_t *)snap, snapLen, minusLen, da, sa, up, mic);
 826
 827                                 offset = zfwBufGetSize(dev, buf);
 828
 829                                 /* Append MIC to the buffer */
 830                                 zfCopyToIntTxBuffer(dev, buf, mic, offset, 8);
 831                                 zfwBufSetSize(dev, buf, offset+8);
 832                                 zmw_leave_critical_section(dev);
 833
 834                                 /* TKIP Key Mixing */
 835                                 zfTkipPhase1KeyMix(wd->sta.iv32, &wd->sta.txSeed);
 836                                 zfTkipPhase2KeyMix(wd->sta.iv16, &wd->sta.txSeed);
 837                                 zfTkipGetseeds(wd->sta.iv16, RC4Key, &wd->sta.txSeed);
 838
 839                                 /* Encrypt Data */
 840                                 zfTKIPEncrypt(dev, buf, (u8_t *)snap, snapLen, minusLen, 16, RC4Key, &icv);
 841
 842                                 icvLen = 4;
 843                                 len += 8;
 844                             }
 845
 846                             header[4] |= 0x4000;
 847                             hlen += 4;
 848                         }
 849                         else if ( wd->sta.encryMode == ZM_AES )
 850                         {
 851                             b1 = (u8_t) wd->sta.iv16;
 852                             b2 = (u8_t) (wd->sta.iv16 >> 8);
 853                             header[hlen] = ((u16_t)b2 << 8) + b1;
 854                             // header[hlen+1] = (((u16_t) wd->sta.keyId) << 14) | (0x2000);
 855                             // STA in infrastructure mode should use keyId = 0 to transmit unicast !
 856                             header[hlen+1] = 0x2000;
 857                             header[hlen+2] = (u16_t) (wd->sta.iv32);
 858                             header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
 859
 860                             macCtrl |= 0xc0;
 861                             icvLen = 8;  /* MIC */
 862
 863                             header[4] |= 0x4000;
 864                             hlen += 4;
 865                         }
 866                         #ifdef ZM_ENABLE_CENC
 867                         else if ( wd->sta.encryMode == ZM_CENC )
 868                         {
 869                             /* Accumlate the PN sequence */
 870                             wd->sta.txiv[0] += 2;
 871
 872                             if (wd->sta.txiv[0] == 0 || wd->sta.txiv[0] == 1)
 873                             {
 874                                 wd->sta.txiv[1]++;
 875                             }
 876
 877                             if (wd->sta.txiv[1] == 0)
 878                             {
 879                                 wd->sta.txiv[2]++;
 880                             }
 881
 882                             if (wd->sta.txiv[2] == 0)
 883                             {
 884                                 wd->sta.txiv[3]++;
 885                             }
 886
 887                             if (wd->sta.txiv[3] == 0)
 888                             {
 889                                 wd->sta.txiv[0] = 0;
 890                                 wd->sta.txiv[1] = 0;
 891                                 wd->sta.txiv[2] = 0;
 892                             }
 893
 894                             header[hlen] = (wd->sta.cencKeyId & 0x0001);    /* For Key Id and reserved field */
 895                             header[hlen+1] = (u16_t) wd->sta.txiv[0];
 896                             header[hlen+2] = (u16_t) (wd->sta.txiv[0] >> 16);
 897                             header[hlen+3] = (u16_t) wd->sta.txiv[1];
 898                             header[hlen+4] = (u16_t) (wd->sta.txiv[1] >> 16);
 899                             header[hlen+5] = (u16_t) wd->sta.txiv[2];
 900                             header[hlen+6] = (u16_t) (wd->sta.txiv[2] >> 16);
 901                             header[hlen+7] = (u16_t) wd->sta.txiv[3];
 902                             header[hlen+8] = (u16_t) (wd->sta.txiv[3] >> 16);
 903
 904                             macCtrl |= 0x80;
 905                             icvLen = 16;  /* MIC */
 906
 907                             header[4] |= 0x4000;
 908                             hlen += 9;
 909                         }
 910                         #endif //ZM_ENABLE_CENC
 911                     }
 912                 }
 913             } // if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
 914         } /* if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE ) */
 915
 916         if ( wd->wlanMode == ZM_MODE_IBSS )
 917         {
 918             if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
 919             {
 920 #ifdef ZM_ENABLE_IBSS_WPA2PSK
 921                 if( wd->sta.oppositeInfo[userIdx].wpaState >= ZM_STA_WPA_STATE_PK_OK || wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK)
 922                 {
 923                     int isUnicast = 1 ;
 924
 925                     if((da[0]& 0x1))
 926                     {
 927                         isUnicast = 0 ; // Not unicast , is broadcast
 928                     }
 929
 930                     if( wd->sta.ibssWpa2Psk == 1 )
 931                     { /* The IV order is not the same between unicast and broadcast ! */
 932                         if ( isUnicast )
 933                         {
 934                             iv16 = &wd->sta.oppositeInfo[userIdx].iv16;
 935                             iv32 = &wd->sta.oppositeInfo[userIdx].iv32;
 936                         }
 937                         else
 938                         {
 939                             iv16 = &wd->sta.iv16;
 940                             iv32 = &wd->sta.iv32;
 941                         }
 942                     }
 943                     else
 944                     {
 945                         iv16 = &wd->sta.iv16;
 946                         iv32 = &wd->sta.iv32;
 947                     }
 948
 949                     (*iv16)++;
 950                     if ( *iv16 == 0 )
 951                     {
 952                         *iv32++;
 953                     }
 954
 955                     if ( wd->sta.oppositeInfo[userIdx].encryMode == ZM_AES || wd->sta.encryMode == ZM_AES)
 956                     {
 957                         //printk("Station encryption mode is AES-CCMP\n") ;
 958                         b1 = (u8_t) (*iv16);
 959                         b2 = (u8_t) ((*iv16) >> 8);
 960                         header[hlen] = ((u16_t)b2 << 8) + b1;
 961
 962                         if ( isUnicast )
 963                         {
 964                             header[hlen+1] = 0x2000;
 965                         }
 966                         else
 967                         {
 968                             header[hlen+1] = 0x2000 | (((u16_t) wd->sta.keyId) << 14);
 969                         }
 970
 971                         header[hlen+2] = (u16_t) (*iv32);
 972                         header[hlen+3] = (u16_t) ((*iv32) >> 16);
 973                         macCtrl |= 0xc0;
 974                         icvLen = 8;  /* MIC */
 975                     }
 976
 977                     header[4] |= 0x4000;
 978                     hlen += 4;
 979                 }
 980                 else if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED)
 981                 {
 982                     if ( (wd->sta.encryMode == ZM_WEP64)||
 983                         (wd->sta.encryMode == ZM_WEP128)||
 984                         (wd->sta.encryMode == ZM_WEP256) )
 985                     {
 986                         header[4] |= 0x4000;
 987                         header[hlen] = 0x0;   //IV
 988                         header[hlen+1] = 0x0; //IV
 989                         header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
 990                         hlen += 2;
 991                         icvLen = 4;
 992                         macCtrl |= 0x40;
 993                     }
 994                 }
 995 #else
 996                 /* ----- 20070405 add by Mxzeng ----- */
 997                 if( wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK )
 998                 {
 999                     int isUnicast = 1 ;
1000
1001                     if((da[0]& 0x1))
1002                     {
1003                         isUnicast = 0 ; // Not unicast , is broadcast
1004                     }
1005
1006                     wd->sta.iv16++;
1007                     if ( wd->sta.iv16 == 0 )
1008                     {
1009                         wd->sta.iv32++;
1010                     }
1011
1012                     if ( wd->sta.encryMode == ZM_AES )
1013                     {
1014                         //printk("Station encryption mode is AES-CCMP\n") ;
1015                         b1 = (u8_t) wd->sta.iv16;
1016                         b2 = (u8_t) (wd->sta.iv16 >> 8);
1017                         header[hlen] = ((u16_t)b2 << 8) + b1;
1018
1019                         if ( isUnicast )
1020                         {
1021                             header[hlen+1] = 0x2000;
1022                         }
1023                         else
1024                         {
1025                             header[hlen+1] = 0x2000 | (((u16_t) wd->sta.keyId) << 14);
1026                         }
1027
1028                             header[hlen+2] = (u16_t) (wd->sta.iv32);
1029                             header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
1030                             macCtrl |= 0xc0;
1031                             icvLen = 8;  /* MIC */
1032                     }
1033
1034                     header[4] |= 0x4000;
1035                     hlen += 4;
1036                 }
1037                 else if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED)
1038                 {
1039                     if ( (wd->sta.encryMode == ZM_WEP64)||
1040                          (wd->sta.encryMode == ZM_WEP128)||
1041                          (wd->sta.encryMode == ZM_WEP256) )
1042                     {
1043                         header[4] |= 0x4000;
1044                         header[hlen] = 0x0;   //IV
1045                         header[hlen+1] = 0x0; //IV
1046                         header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
1047                         hlen += 2;
1048                         icvLen = 4;
1049                         macCtrl |= 0x40;
1050                     }
1051                 }
1052 #endif
1053             }   // End if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
1054         }   // End if ( wd->wlanMode == ZM_MODE_IBSS )
1055         else if ( wd->wlanMode == ZM_MODE_PSEUDO )
1056         {
1057             switch (wd->sta.encryMode)
1058                 {
1059             case ZM_WEP64:
1060             case ZM_WEP128:
1061             case ZM_WEP256:
1062                 header[4] |= 0x4000;
1063                 header[hlen] = 0x0;   //IV
1064                 header[hlen+1] = 0x0; //IV
1065                 hlen += 2;
1066                 icvLen = 4;
1067                 macCtrl |= 0x40;
1068                 break;
1069
1070             case ZM_TKIP:
1071             {
1072                 wd->sta.iv16++;
1073                 if ( wd->sta.iv16 == 0 )
1074                 {
1075                     wd->sta.iv32++;
1076                 }
1077
1078                 b1 = (u8_t) (wd->sta.iv16 >> 8);
1079                 b2 = (b1 | 0x20) & 0x7f;
1080                 header[hlen] = ((u16_t)b2 << 8) + b1;
1081                 b1 = (u8_t) wd->sta.iv16;
1082                 b2 = 0x20;
1083                 header[hlen+1] = ((u16_t)b2 << 8) + b1;
1084                 header[hlen+2] = (u16_t) wd->sta.iv32;
1085                 header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
1086
1087                 //macCtrl |= 0x80;
1088                 macCtrl |= 0x40;
1089                 icvLen = 4;
1090
1091                 /* set hardware MIC */
1092                 if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
1093                 {
1094                     macCtrl |= 0x100;
1095                     plusLen += 8;
1096                     *micLen = 8;
1097                 }
1098
1099                 header[4] |= 0x4000;
1100                 hlen += 4;
1101             }/* end of PSEUDO TKIP */
1102                 break;
1103
1104             case ZM_AES:
1105             {
1106                 wd->sta.iv16++;
1107                 if ( wd->sta.iv16 == 0 )
1108                 {
1109                     wd->sta.iv32++;
1110                 }
1111
1112                 b1 = (u8_t) wd->sta.iv16;
1113                 b2 = (u8_t) (wd->sta.iv16 >> 8);
1114                 header[hlen] = ((u16_t)b2 << 8) + b1;
1115                 header[hlen+1] = 0x2000;
1116                 header[hlen+2] = (u16_t) (wd->sta.iv32);
1117                 header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
1118                 macCtrl |= 0xc0;
1119                 icvLen = 8;  /* MIC */
1120                 header[4] |= 0x4000;
1121                 hlen += 4;
1122             }/* end of PSEUDO AES */
1123                     break;
1124
1125               #ifdef ZM_ENABLE_CENC
1126               case ZM_CENC:
1127                     /* Accumlate the PN sequence */
1128                     wd->sta.txiv[0] += 2;
1129
1130                     if (wd->sta.txiv[0] == 0 || wd->sta.txiv[0] == 1)
1131                     {
1132                         wd->sta.txiv[1]++;
1133                     }
1134
1135                     if (wd->sta.txiv[1] == 0)
1136                     {
1137                         wd->sta.txiv[2]++;
1138                     }
1139
1140                     if (wd->sta.txiv[2] == 0)
1141                     {
1142                         wd->sta.txiv[3]++;
1143                     }
1144
1145                     if (wd->sta.txiv[3] == 0)
1146                     {
1147                         wd->sta.txiv[0] = 0;
1148                         wd->sta.txiv[1] = 0;
1149                         wd->sta.txiv[2] = 0;
1150                     }
1151
1152                     header[hlen] = 0;
1153                     header[hlen+1] = (u16_t) wd->sta.txiv[0];
1154                     header[hlen+2] = (u16_t) (wd->sta.txiv[0] >> 16);
1155                     header[hlen+3] = (u16_t) wd->sta.txiv[1];
1156                     header[hlen+4] = (u16_t) (wd->sta.txiv[1] >> 16);
1157                     header[hlen+5] = (u16_t) wd->sta.txiv[2];
1158                     header[hlen+6] = (u16_t) (wd->sta.txiv[2] >> 16);
1159                     header[hlen+7] = (u16_t) wd->sta.txiv[3];
1160                     header[hlen+8] = (u16_t) (wd->sta.txiv[3] >> 16);
1161
1162                     macCtrl |= 0x80;
1163                     icvLen = 16;  /* MIC */
1164
1165                     header[4] |= 0x4000;
1166                     hlen += 9;
1167                                 break;
1168                     #endif //ZM_ENABLE_CENC
1169                         }/* end of switch */
1170                 }
1171
1172         /* Generate control setting */
1173
1174         /* protection mode */
1175         if (wd->enableProtectionMode)
1176         {
1177             if (wd->enableProtectionMode==2)
1178             {
1179                 /* Force enable protection: self cts  */
1180                 macCtrl &= 0xFFFC;
1181                 macCtrl |= 2;
1182             }
1183             /* if wd->enableProtectionMode=1 => force disable */
1184             /* if wd->enableProtectionMode=0 => auto */
1185         }
1186         else
1187         {
1188
1189             /* protection mode */
1190             if (wd->sta.bProtectionMode == TRUE)
1191             {
1192                 /* Enable Self-CTS */
1193                 macCtrl &= 0xFFFC;
1194                 macCtrl |= 2;
1195             }
1196         }
1197
1198     }
1199
1200     if (wd->txMCS != 0xff)
1201     {
1202         /* fixed rate */
1203             phyCtrl = ((u32_t)wd->txMCS<<16) + wd->txMT;
1204         mcs = wd->txMCS;
1205         mt = wd->txMT;
1206     }
1207
1208     if (mt == 2)
1209     {
1210 #if 0
1211         /* HT PT: 0 Mixed mode    1 Green field */
1212             if (wd->sta.preambleTypeHT == ZM_PREAMBLE_TYPE_GREEN_FIELD)
1213             {
1214             phyCtrl |= 0x4;     /* Bit 2 */
1215         }
1216 #endif
1217         /* Bandwidth */
1218         if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
1219         {
1220             phyCtrl |= (0x80<<16);  /* BIT 23 */
1221         }
1222 #if 0
1223         /* STBC */
1224         if (wd->sta.htCtrlSTBC<=0x3)
1225         {
1226             phyCtrl |= (wd->sta.htCtrlSTBC<<28);   /* BIT 23 */
1227         }
1228 #endif
1229         /* Short GI */
1230         if(wd->sta.htCtrlSG)
1231         {
1232             phyCtrl |= (0x8000<<16);         /* BIT 31 */
1233         }
1234
1235         /* TA */
1236         if ( ((mcs >=0x8) && (mcs<=0xf))  || (wd->sta.htCtrlSTBC) )
1237         {
1238             phyCtrl |= 0x1800;               /* BIT 11 12 */
1239         }
1240     }
1241     else if(mt == 1)
1242     {
1243         #if 0
1244         //bug that cause OFDM rate become duplicate legacy rate
1245         /* Bandwidth */
1246         if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
1247         {
1248             phyCtrl |= (0x80<<16);  /* BIT 23 */
1249             mt = 3;                 /* duplicate legacy */
1250             phyCtrl |= mt;
1251         }
1252         #endif
1253     }
1254     else if(mt == 0)
1255     {
1256         /* CCK PT: Legcy Preamble: 1 long preamble    2 short preamble */
1257         if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_SHORT)
1258         {
1259                //phyCtrl |= 0x4;    /* BIT 2 */
1260         }
1261     }
1262
1263     /* TA */
1264     if (wd->sta.defaultTA)
1265     {
1266         phyCtrl |= 0x1000;
1267     }
1268     else
1269     {
1270         phyCtrl |= 0x0800;
1271     }
1272
1273     //Get CurrentTxRate -- CWYang(+)
1274     if ((mt == 0) || (mt == 1)) //B,G Rate
1275     {
1276         if (mcs < 16)
1277         {
1278             wd->CurrentTxRateKbps = zcIndextoRateBG[mcs];
1279         }
1280     }
1281     else if (mt == 2)
1282     {
1283         if (mcs < 16)
1284         {
1285             if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
1286             {
1287                 if((phyCtrl & 0x80000000) != 0)
1288                 {
1289                     /* Short GI 40 MHz MIMO Rate */
1290                     wd->CurrentTxRateKbps = zcIndextoRateN40S[mcs];
1291                 }
1292                 else
1293                 {
1294                     /* Long GI 40 MHz MIMO Rate */
1295                     wd->CurrentTxRateKbps = zcIndextoRateN40L[mcs];
1296                 }
1297             }
1298             else
1299             {
1300                 if((phyCtrl & 0x80000000) != 0)
1301                 {
1302                     /* Short GI 20 MHz MIMO Rate */
1303                     wd->CurrentTxRateKbps = zcIndextoRateN20S[mcs];
1304                 }
1305                 else
1306                 {
1307                     /* Long GI 20 MHz MIMO Rate */
1308                     wd->CurrentTxRateKbps = zcIndextoRateN20L[mcs];
1309                 }
1310             }
1311         }
1312     }
1313
1314     //802.11 header(include IV) = (hlen<<1)-8
1315     //ethernet frame = len
1316     //snap + mic = plusLen
1317     //ethernet header = minusLen
1318     //icv = icvLen
1319     //crc32 = 4
1320     //length=802.11 header+snap+(ethernet frame-ethernet header)+mic+icv+crc32
1321     header[0] = ((hlen<<1)-8)+plusLen+(len-minusLen)+icvLen+4;  //Length
1322
1323     // header[0] : MPDU Lengths
1324     if ((header[6] & 0x1) != 0x1) // Unicast Frame
1325     {
1326         if (header[0] >= wd->rtsThreshold)
1327         {
1328             /* Enable RTS */
1329             macCtrl |= 1;
1330         }
1331     }
1332
1333     if ( wd->sta.encryMode == ZM_TKIP )
1334         tkipFrameOffset = 8;
1335
1336     if( wd->sta.EnableHT != 1 )
1337     { // Aggregation should not be fragmented !
1338         if ( header[0] > ( wd->fragThreshold + tkipFrameOffset ) )
1339         {
1340             return 0; // Need to be fragmented ! !
1341         }
1342     }
1343
1344     //if ( wd->sta.encryMode == ZM_TKIP )
1345     //{
1346     //    zm_debug_msg1("ctrl length = ", header[0]);
1347     //}
1348
1349     //MAC control
1350     if (rateProbingFlag != 0)
1351     {
1352         macCtrl |= 0x8000;
1353     }
1354     header[1] = macCtrl;
1355     //PHY control L
1356     header[2] = (u16_t) ((phyCtrl&0xffff) | 0x700 | (zcUpToAc[up&0x7]<<13));
1357     //PHY control H
1358     header[3] = (u16_t) ((phyCtrl>>16) | 0x700);
1359
1360     if (wd->enableAggregation)
1361     {
1362         /* force enable aggregation */
1363         if (wd->enableAggregation==2 && !(header[6]&0x1))
1364         {
1365             if (((header[2] & 0x3) == 2))
1366             {
1367                 /* Enable aggregation */
1368                 header[1] |= 0x20;
1369             }
1370         }
1371         /* if wd->enableAggregation=1 => force disable */
1372         /* if wd->enableAggregation=0 => auto */
1373     }
1374
1375 #ifdef ZM_ENABLE_AGGREGATION
1376     if (wd->addbaComplete) {
1377         #ifdef ZM_BYPASS_AGGR_SCHEDULING
1378         if (!(header[6]&0x1) && !rateProbingFlag && (wd->enableAggregation != 1))
1379         {
1380             if (((header[2] & 0x3) == 2))
1381             {
1382                 /* Unicast frame with HT rate => Enable aggregation */
1383                 /* We only support software encryption in single packet mode */
1384                 if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0 &&
1385                     (wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) == 0)
1386                 {
1387                     /* Set aggregation group bits per AC */
1388                     header[1] |= (0x20 | (zcUpToAc[up&0x7]<<10));
1389
1390                     //if (wd->sta.currentFrequency < 3000)
1391                     {
1392                         /* issue: -PB42 Enable RTS/CTS to prevent OWL Tx hang up */
1393                         /* If this is Owl Ap, enable RTS/CTS protect */
1394                         if ( (wd->sta.athOwlAp == 1) || (wd->sta.RTSInAGGMode == TRUE) )
1395                         {
1396                             header[1] &= 0xfffc;
1397                             header[1] |= 0x1;
1398                         }
1399
1400                         /* Enable RIFS : workaround 854T RTS/CTS */
1401                         /* Bit13 : TI enable RIFS */
1402                         //header[1] |= 0x2000;
1403                     }
1404                 }
1405             }
1406         }
1407         #else
1408         /*
1409          * aggregation ampduIndication control
1410          */
1411         if (aggControl && aggControl->aggEnabled) {
1412             if (wd->enableAggregation==0 && !(header[6]&0x1))
1413             {
1414                 if (((header[2] & 0x3) == 2))
1415                 {
1416                     /* Enable aggregation */
1417                     header[1] |= 0x20;
1418                     if (ZM_AGG_LAST_MPDU == aggControl->ampduIndication)
1419                         header[1] |= 0x4000;
1420                 }
1421                 else {
1422                     zm_debug_msg1("no aggr, header[2]&0x3 = ",header[2] & 0x3)
1423                     aggControl->aggEnabled = 0;
1424                 }
1425             }
1426             else {
1427                 zm_debug_msg1("no aggr, wd->enableAggregation = ", wd->enableAggregation);
1428                 zm_debug_msg1("no aggr, !header[6]&0x1 = ",!(header[6]&0x1));
1429                 aggControl->aggEnabled = 0;
1430             }
1431         }
1432         #endif
1433
1434         #ifdef ZM_AGGR_BIT_ON
1435         if (!(header[6]&0x1) && !rateProbingFlag)
1436         {
1437             if (((header[2] & 0x3) == 2))
1438             {
1439                 /* Unicast frame with HT rate => Enable aggregation */
1440                 /* Set aggregation group bits per AC */
1441                 header[1] |= (0x20 | (zcUpToAc[up&0x7]<<10));
1442
1443                 //if (wd->sta.currentFrequency < 3000)
1444                 {
1445                     /* Enable RTS/CTS to prevent OWL Tx hang up */
1446                     header[1] &= 0xfffc;
1447                     header[1] |= 0x1;
1448                 }
1449             }
1450         }
1451         #endif
1452     }
1453 #endif
1454
1455     return (hlen<<1);
1456 }
1457
1458
1459 u16_t zfTxGenMmHeader(zdev_t* dev, u8_t frameType, u16_t* dst,
1460         u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
1461 {
1462     //u16_t bodyLen;
1463     u8_t  hlen = 32;        // MAC ctrl + PHY ctrl + 802.11 MM header
1464
1465     zmw_get_wlan_dev(dev);
1466
1467     zmw_declare_for_critical_section();
1468
1469     /* Generate control setting */
1470     //bodyLen = zfwBufGetSize(dev, buf);
1471     header[0] = 24+len+4;   //Length
1472     if ((dst[0] & 0x1) != 0) //Broadcast, multicast frames
1473     {
1474         header[1] = 0xc;            //MAC control, backoff + noack
1475     }
1476     else
1477     {
1478         header[1] = 0x8;            //MAC control, backoff + (ack)
1479     }
1480     /* Dualband Management frame tx Rate */
1481     if (wd->wlanMode == ZM_MODE_AP)
1482     {
1483         if (wd->frequency < 3000)
1484         {
1485             /* CCK 1M */
1486             header[2] = 0x0f00;          //PHY control L
1487             header[3] = 0x0000;          //PHY control H
1488         }
1489         else
1490         {
1491             /* CCK 6M */
1492             header[2] = 0x0f01;          //PHY control L
1493             header[3] = 0x000B;          //PHY control H
1494         }
1495     }
1496     else
1497     {
1498         if (wd->sta.currentFrequency < 3000)
1499         {
1500             /* CCK 2M */
1501             header[2] = 0x0f00;          //PHY control L
1502             header[3] = 0x0001;          //PHY control H
1503         }
1504         else
1505         {
1506             /* CCK 6M */
1507             header[2] = 0x0f01;          //PHY control L
1508             header[3] = 0x000B;          //PHY control H
1509         }
1510     }
1511     /* Generate WLAN header */
1512     /* Frame control */
1513     header[4+0] = frameType;
1514     /* Duration */
1515     header[4+1] = 0;
1516
1517     if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
1518     {
1519         if ( frameType == ZM_WLAN_FRAME_TYPE_PROBEREQ )
1520         {
1521             header[4+8] = 0xFFFF;
1522             header[4+9] = 0xFFFF;
1523             header[4+10] = 0xFFFF;
1524         }
1525         else if ( frameType == ZM_WLAN_FRAME_TYPE_BA ) {
1526             /* do nothing */
1527         }
1528         else
1529         {
1530             header[4+8] = wd->sta.bssid[0];
1531             header[4+9] = wd->sta.bssid[1];
1532             header[4+10] = wd->sta.bssid[2];
1533         }
1534     }
1535     else if (wd->wlanMode == ZM_MODE_PSEUDO)
1536     {
1537         /* Address 3 = 00:00:00:00:00:00 */
1538         header[4+8] = 0;
1539         header[4+9] = 0;
1540         header[4+10] = 0;
1541     }
1542     else if (wd->wlanMode == ZM_MODE_IBSS)
1543     {
1544         header[4+8] = wd->sta.bssid[0];
1545         header[4+9] = wd->sta.bssid[1];
1546         header[4+10] = wd->sta.bssid[2];
1547
1548         if ( frameType == ZM_WLAN_FRAME_TYPE_ATIM )
1549         {
1550             /* put ATIM to queue 5th */
1551             //header[2] |= (ZM_BIT_13|ZM_BIT_14);
1552             header[2] |= ZM_BIT_15;
1553         }
1554     }
1555     else if (wd->wlanMode == ZM_MODE_AP)
1556     {
1557         /* Address 3 = BSSID */
1558         header[4+8] = wd->macAddr[0];
1559         header[4+9] = wd->macAddr[1];
1560 #ifdef ZM_VAPMODE_MULTILE_SSID
1561         header[4+10] = wd->macAddr[2]; //Multiple SSID
1562 #else
1563         header[4+10] = wd->macAddr[2] + (vap<<8); //VAP
1564 #endif
1565         //if in scan, must set address 3 to broadcast because of some ap would care this
1566         //if ((wd->heartBeatNotification & ZM_BSSID_LIST_SCAN)
1567         //        == ZM_BSSID_LIST_SCAN)
1568         //if FrameType is Probe Request, Address3 should be boradcast
1569         if (frameType == ZM_WLAN_FRAME_TYPE_PROBEREQ)
1570         {
1571             header[4+8] = 0xFFFF;
1572             header[4+9] = 0xFFFF;
1573             header[4+10] = 0xFFFF;
1574         }
1575     }
1576
1577     /* Address 1 = DA */
1578     header[4+2] = dst[0];
1579     header[4+3] = dst[1];
1580     header[4+4] = dst[2];
1581
1582     /* Address 2 = SA */
1583     header[4+5] = wd->macAddr[0];
1584     header[4+6] = wd->macAddr[1];
1585     if (wd->wlanMode == ZM_MODE_AP)
1586     {
1587 #ifdef ZM_VAPMODE_MULTILE_SSID
1588         header[4+7] = wd->macAddr[2]; //Multiple SSID
1589 #else
1590         header[4+7] = wd->macAddr[2] + (vap<<8); //VAP
1591 #endif
1592     }
1593     else
1594     {
1595         header[4+7] = wd->macAddr[2];
1596     }
1597
1598     /* Sequence Control */
1599     zmw_enter_critical_section(dev);
1600     header[4+11] = ((wd->mmseq++)<<4);
1601     zmw_leave_critical_section(dev);
1602
1603     if( frameType == ZM_WLAN_FRAME_TYPE_QOS_NULL )
1604     {
1605         /*Qos Control*/
1606         header[4+12] = 0x0;
1607         hlen+=2;
1608         header[0]+=2;
1609     }
1610
1611     if ( encrypt )
1612     {
1613         if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
1614         {
1615             if ( (wd->sta.encryMode == ZM_WEP64)||
1616                  (wd->sta.encryMode == ZM_WEP128)||
1617                  (wd->sta.encryMode == ZM_WEP256) )
1618             {
1619                 header[4] |= 0x4000;
1620                 header[16] = 0x0;   //IV
1621                 header[17] = 0x0; //IV
1622                 header[17] |= (((u16_t) wd->sta.keyId) << 14);
1623                 hlen += 4;
1624
1625                 header[0] += 8;         // icvLen = 4;
1626                 header[1] |= 0x40;      // enable encryption on macCtrl
1627              }
1628         }
1629     }
1630
1631     // Enable HW duration
1632     if ( frameType != ZM_WLAN_FRAME_TYPE_PSPOLL )
1633     {
1634         header[1] |= 0x200;
1635     }
1636
1637     return hlen;
1638 }
1639
1640 void zfInitMacApMode(zdev_t* dev)
1641 {
1642     u16_t i;
1643
1644     zmw_get_wlan_dev(dev);
1645
1646     zfHpEnableBeacon(dev, ZM_MODE_AP, (wd->beaconInterval/wd->ap.vapNumber), 1, 0);
1647
1648     /* AP mode */
1649     zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_AP);
1650
1651     /* VAP test code */
1652     /* AP + VAP mode */
1653     if (wd->ap.vapNumber >= 2)
1654     {
1655         for (i=1; i<ZM_MAX_AP_SUPPORT; i++)
1656         {
1657             if (((wd->ap.apBitmap >> i) & 0x1) != 0)
1658             {
1659                 u16_t mac[3];
1660                 mac[0] = wd->macAddr[0];
1661                 mac[1] = wd->macAddr[1];
1662 #ifdef ZM_VAPMODE_MULTILE_SSID
1663                 mac[2] = wd->macAddr[2]; //Multiple SSID
1664 #else
1665                 mac[2] = wd->macAddr[2] + (i<<8); //VAP
1666 #endif
1667                 zfHpSetMacAddress(dev, mac, i);
1668
1669             }
1670         }
1671     }
1672
1673     /* basic rate setting */
1674     zfHpSetBasicRateSet(dev, wd->bRateBasic, wd->gRateBasic);
1675
1676     /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME AP default. */
1677     zfUpdateDefaultQosParameter(dev, 1);
1678
1679     return;
1680 }
1681
1682 u16_t zfChGetNextChannel(zdev_t* dev, u16_t frequency, u8_t* pbPassive)
1683 {
1684     u8_t   i;
1685     u8_t   bPassive;
1686
1687     zmw_get_wlan_dev(dev);
1688
1689     /* Avoid NULL value */
1690     if ( pbPassive == NULL )
1691     {
1692         pbPassive = &bPassive;
1693     }
1694
1695     for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
1696     {
1697         if ( wd->regulationTable.allowChannel[i].channel == frequency )
1698         {
1699             if ( i == (wd->regulationTable.allowChannelCnt-1) )
1700             {
1701                 i = 0;
1702             }
1703             else
1704             {
1705                 i++;
1706             }
1707
1708             if ( wd->regulationTable.allowChannel[i].channelFlags
1709                     & ZM_REG_FLAG_CHANNEL_PASSIVE )
1710             {
1711                 *pbPassive = TRUE;
1712             }
1713             else
1714             {
1715                 *pbPassive = FALSE;
1716             }
1717
1718             return wd->regulationTable.allowChannel[i].channel;
1719         }
1720     }
1721
1722     return 0xffff;
1723 }
1724
1725 u16_t zfChGetFirstChannel(zdev_t* dev, u8_t* pbPassive)
1726 {
1727     u8_t   bPassive;
1728
1729     zmw_get_wlan_dev(dev);
1730
1731     /* Avoid NULL value */
1732     if ( pbPassive == NULL )
1733     {
1734         pbPassive = &bPassive;
1735     }
1736
1737    if ( wd->regulationTable.allowChannel[0].channelFlags & ZM_REG_FLAG_CHANNEL_PASSIVE )
1738     {
1739         *pbPassive = TRUE;
1740     }
1741     else
1742     {
1743         *pbPassive = FALSE;
1744     }
1745
1746     return wd->regulationTable.allowChannel[0].channel;
1747 }
1748
1749 u16_t zfChGetFirst2GhzChannel(zdev_t* dev)
1750 {
1751     u8_t    i;
1752
1753     zmw_get_wlan_dev(dev);
1754
1755     for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
1756     {
1757         if ( wd->regulationTable.allowChannel[i].channel < 3000 )
1758         {
1759             /* find the first 2Ghz channel */
1760             return wd->regulationTable.allowChannel[i].channel;
1761         }
1762     }
1763
1764     /* Can not find any 2Ghz channel */
1765     return 0;
1766 }
1767
1768 u16_t zfChGetFirst5GhzChannel(zdev_t* dev)
1769 {
1770     u8_t    i;
1771
1772     zmw_get_wlan_dev(dev);
1773
1774     for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
1775     {
1776         if ( wd->regulationTable.allowChannel[i].channel > 3000 )
1777         {
1778             /* find the first 5Ghz channel */
1779             return wd->regulationTable.allowChannel[i].channel;
1780         }
1781     }
1782
1783     /* Can not find any 5Ghz channel */
1784     return 0;
1785 }
1786
1787 u16_t zfChGetLastChannel(zdev_t* dev, u8_t* pbPassive)
1788 {
1789     u8_t   bPassive;
1790     u8_t   ChannelIndex;
1791
1792     zmw_get_wlan_dev(dev);
1793
1794     ChannelIndex = wd->regulationTable.allowChannelCnt-1;
1795
1796     /* Avoid NULL value */
1797     if ( pbPassive == NULL )
1798     {
1799         pbPassive = &bPassive;
1800     }
1801
1802     if ( wd->regulationTable.allowChannel[ChannelIndex].channelFlags
1803             & ZM_REG_FLAG_CHANNEL_PASSIVE )
1804     {
1805         *pbPassive = TRUE;
1806     }
1807     else
1808     {
1809         *pbPassive = FALSE;
1810     }
1811
1812     return wd->regulationTable.allowChannel[ChannelIndex].channel;
1813 }
1814
1815 u16_t zfChGetLast5GhzChannel(zdev_t* dev)
1816 {
1817     u8_t    i;
1818     u16_t   last5Ghzfrequency;
1819
1820     zmw_get_wlan_dev(dev);
1821
1822     last5Ghzfrequency = 0;
1823     for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
1824     {
1825         if ( wd->regulationTable.allowChannel[i].channel > 3000 )
1826         {
1827             last5Ghzfrequency = wd->regulationTable.allowChannel[i].channel;
1828         }
1829     }
1830
1831     return last5Ghzfrequency;
1832 }
1833
1834 /* freqBand = 0 => auto check   */
1835 /*          = 1 => 2.4 GHz band */
1836 /*          = 2 => 5 GHz band   */
1837 u16_t zfChNumToFreq(zdev_t* dev, u8_t ch, u8_t freqBand)
1838 {
1839     u16_t freq = 0xffff;
1840
1841     if ( freqBand == 0 )
1842     {
1843         if (ch > 14)
1844         {   /* adapter is at 5 GHz band */
1845             freqBand = 2;
1846         }
1847         else
1848         {
1849             freqBand = 1;
1850         }
1851     }
1852
1853     if ( freqBand == 2 )
1854     {   /* the channel belongs to 5 GHz band */
1855         if ( (ch >= 184)&&(ch <= 196) )
1856         {
1857             freq = 4000 + ch*5;
1858         }
1859         else
1860         {
1861             freq = 5000 + ch*5;
1862         }
1863     }
1864     else
1865     {   /* the channel belongs to 2.4 GHz band */
1866         if ( ch == 14 )
1867         {
1868             freq = ZM_CH_G_14;
1869         }
1870         else
1871         {
1872             freq = ZM_CH_G_1 + (ch-1)*5;
1873         }
1874     }
1875
1876     return freq;
1877 }
1878
1879 u8_t zfChFreqToNum(u16_t freq, u8_t* pbIs5GBand)
1880 {
1881     u8_t   ch;
1882     u8_t   Is5GBand;
1883
1884     /* to avoid NULL value */
1885     if ( pbIs5GBand == NULL )
1886     {
1887         pbIs5GBand = &Is5GBand;
1888     }
1889
1890     *pbIs5GBand = FALSE;
1891
1892     if ( freq == ZM_CH_G_14 )
1893     {
1894         ch = 14;
1895     }
1896     else if ( freq < 4000 )
1897     {
1898         ch = (freq - ZM_CH_G_1) / 5 + 1;
1899     }
1900     else if ( freq < 5000 )
1901     {
1902         ch = (freq - 4000) / 5;
1903         *pbIs5GBand = TRUE;
1904     }
1905     else
1906     {
1907         ch = (freq - 5000) / 5;
1908         *pbIs5GBand = TRUE;
1909     }
1910
1911     return ch;
1912 }