SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/staging/otus/80211core/cmm.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 : mm.c                                                  */
  18 /*                                                                      */
  19 /*  Abstract                                                            */
  20 /*      This module contains common functions for handle management     */
  21 /*      frame.                                                          */
  22 /*                                                                      */
  23 /*  NOTES                                                               */
  24 /*      None                                                            */
  25 /*                                                                      */
  26 /************************************************************************/
  27 #include "cprecomp.h"
  28 #include "../hal/hpreg.h"
  29
  30 /* TODO : put all constant tables to a file */
  31 const u8_t zg11bRateTbl[4] = {2, 4, 11, 22};
  32 const u8_t zg11gRateTbl[8] = {12, 18, 24, 36, 48, 72, 96, 108};
  33
  34 /* 0xff => element does not exist */
  35 const u8_t zgElementOffsetTable[] =
  36 {
  37     4,      /*  0 : asoc req */
  38     6,      /*  1 : asoc rsp */
  39     10,     /*  2 : reasoc req*/
  40     6,      /*  3 : reasoc rsp */
  41     0,      /*  4 : probe req */
  42     12,     /*  5 : probe rsp */
  43     0xff,   /*  6 : reserved */
  44     0xff,   /*  7 : reserved */
  45     12,     /*  8 : beacon */
  46     4,      /*  9 : ATIM */
  47     0xff,   /* 10 : disasoc */
  48     6,      /* 11 : auth */
  49     0xff,   /* 12 : deauth */
  50     4,      /* 13 : action */
  51     0xff,   /* 14 : reserved */
  52     0xff,   /* 15 : reserved */
  53 };
  54
  55 /************************************************************************/
  56 /*                                                                      */
  57 /*    FUNCTION DESCRIPTION                  zfFindElement               */
  58 /*      Find a specific element in management frame                     */
  59 /*                                                                      */
  60 /*    INPUTS                                                            */
  61 /*      dev : device pointer                                            */
  62 /*      buf : management frame buffer                                   */
  63 /*      eid : target element id                                         */
  64 /*                                                                      */
  65 /*    OUTPUTS                                                           */
  66 /*      byte offset of target element                                   */
  67 /*      or 0xffff if not found                                          */
  68 /*                                                                      */
  69 /*    AUTHOR                                                            */
  70 /*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
  71 /*                                                                      */
  72 /************************************************************************/
  73 u16_t zfFindElement(zdev_t* dev, zbuf_t* buf, u8_t eid)
  74 {
  75     u8_t subType;
  76     u16_t offset;
  77     u16_t bufLen;
  78     u16_t elen;
  79     u8_t id, HTEid=0;
  80     u8_t oui[4] = {0x00, 0x50, 0xf2, 0x01};
  81     u8_t oui11n[3] = {0x00,0x90,0x4C};
  82     u8_t HTType = 0;
  83
  84     /* Get offset of first element */
  85     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
  86     offset = zgElementOffsetTable[subType];
  87     if (offset == 0xff)
  88     {
  89         zm_assert(0);
  90     }
  91
  92     /* Plus wlan header */
  93     offset += 24;
  94
  95     // jhlee HT 0
  96
  97     if ((eid == ZM_WLAN_EID_HT_CAPABILITY) ||
  98         (eid == ZM_WLAN_EID_EXTENDED_HT_CAPABILITY))
  99     {
 100         HTEid = eid;
 101         eid = ZM_WLAN_EID_WPA_IE;
 102         HTType = 1;
 103     }
 104
 105
 106     bufLen = zfwBufGetSize(dev, buf);
 107     /* Search loop */
 108     while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
 109     {
 110         /* Search target element */
 111         id = zmw_rx_buf_readb(dev, buf, offset);
 112         if (id == eid)
 113         {
 114             /* Bingo */
 115             elen = zmw_rx_buf_readb(dev, buf, offset+1);
 116             if (elen > bufLen - offset)
 117             {
 118                 /* Element length error */
 119                 return 0xffff;
 120             }
 121
 122             if ( elen == 0 && eid != ZM_WLAN_EID_SSID)
 123             {
 124                 /* Element length error */
 125                 return 0xffff;
 126             }
 127
 128             if ( eid == ZM_WLAN_EID_WPA_IE )
 129             {
 130                 /* avoid sta to be thought use 11n when find a WPA_IE */
 131                 if ( (HTType == 0) && zfRxBufferEqualToStr(dev, buf, oui, offset+2, 4) )
 132                 {
 133                     return offset;
 134                 }
 135
 136                 // jhlee HT 0
 137                 // CWYang(+)
 138
 139                 if ((HTType == 1) && ( zfRxBufferEqualToStr(dev, buf, oui11n, offset+2, 3) ))
 140                 {
 141                     if ( zmw_rx_buf_readb(dev, buf, offset+5) == HTEid )
 142                     {
 143                         return offset + 5;
 144                     }
 145                 }
 146
 147             }
 148             else
 149             {
 150                 return offset;
 151             }
 152         }
 153         /* Advance to next element */
 154         #if 1
 155         elen = zmw_rx_buf_readb(dev, buf, offset+1);
 156         #else
 157         elen = zmw_rx_buf_readb(dev, buf, offset+1);
 158         if (elen == 0)
 159         {
 160             return 0xffff;
 161         }
 162         #endif
 163
 164         offset += (elen+2);
 165     }
 166     return 0xffff;
 167 }
 168
 169
 170 /************************************************************************/
 171 /*                                                                      */
 172 /*    FUNCTION DESCRIPTION                  zfFindWifiElement           */
 173 /*      Find a specific Wifi element in management frame                */
 174 /*                                                                      */
 175 /*    INPUTS                                                            */
 176 /*      dev : device pointer                                            */
 177 /*      buf : management frame buffer                                   */
 178 /*      type : OUI type                                                 */
 179 /*      subType : OUI subtype                                           */
 180 /*                                                                      */
 181 /*    OUTPUTS                                                           */
 182 /*      byte offset of target element                                   */
 183 /*      or 0xffff if not found                                          */
 184 /*                                                                      */
 185 /*    AUTHOR                                                            */
 186 /*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
 187 /*                                                                      */
 188 /************************************************************************/
 189 u16_t zfFindWifiElement(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype)
 190 {
 191     u8_t subType;
 192     u16_t offset;
 193     u16_t bufLen;
 194     u16_t elen;
 195     u8_t id;
 196     u8_t tmp;
 197
 198     /* Get offset of first element */
 199     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
 200
 201     offset = zgElementOffsetTable[subType];
 202     if (offset == 0xff)
 203     {
 204         zm_assert(0);
 205     }
 206
 207     /* Plus wlan header */
 208     offset += 24;
 209
 210     bufLen = zfwBufGetSize(dev, buf);
 211     /* Search loop */
 212     while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
 213     {
 214         /* Search target element */
 215         id = zmw_rx_buf_readb(dev, buf, offset);
 216         if (id == ZM_WLAN_EID_WIFI_IE)
 217         {
 218             /* Bingo */
 219             elen = zmw_rx_buf_readb(dev, buf, offset+1);
 220             if (elen > bufLen - offset)
 221             {
 222                 /* Element length error */
 223                 return 0xffff;
 224             }
 225
 226             if ( elen == 0 )
 227             {
 228                 return 0xffff;
 229             }
 230
 231             if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
 232                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
 233                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0xF2)
 234                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+5)) == type))
 235
 236             {
 237                 if ( subtype != 0xff )
 238                 {
 239                     tmp = zmw_rx_buf_readb(dev, buf, offset+6);
 240                     if (tmp == subtype)
 241                     {
 242                         return offset;
 243                     }
 244                 }
 245                 else
 246                 {
 247                     return offset;
 248                 }
 249             }
 250         }
 251         /* Advance to next element */
 252         elen = zmw_rx_buf_readb(dev, buf, offset+1);
 253         if (elen == 0)
 254         {
 255             return 0xffff;
 256         }
 257         offset += (elen+2);
 258     }
 259     return 0xffff;
 260 }
 261
 262 u16_t zfRemoveElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t eid)
 263 {
 264     u16_t offset = 0;
 265     u16_t elen;
 266     u8_t  HTEid = 0;
 267     u8_t  oui[4] = {0x00, 0x50, 0xf2, 0x01};
 268     u8_t  oui11n[3] = {0x00,0x90,0x4C};
 269     u8_t  HTType = 0;
 270
 271     if ((eid == ZM_WLAN_EID_HT_CAPABILITY) ||
 272         (eid == ZM_WLAN_EID_EXTENDED_HT_CAPABILITY))
 273     {
 274         HTEid = eid;
 275         eid = ZM_WLAN_EID_WPA_IE;
 276         HTType = 1;
 277     }
 278
 279     while (offset < size)
 280     {
 281         elen = *(buf+offset+1);
 282
 283         if (*(buf+offset) == eid)
 284         {
 285             if ( eid == ZM_WLAN_EID_WPA_IE )
 286             {
 287                 if ( (HTType == 0)
 288                      && (*(buf+offset+2) == oui[0])
 289                      && (*(buf+offset+3) == oui[1])
 290                      && (*(buf+offset+4) == oui[2])
 291                      && (*(buf+offset+5) == oui[3]) )
 292                 {
 293                     zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
 294                     return (size-elen-2);
 295                 }
 296
 297                 if ( (HTType == 1)
 298                     && (*(buf+offset+2) == oui11n[0])
 299                     && (*(buf+offset+3) == oui11n[1])
 300                     && (*(buf+offset+4) == oui11n[2])
 301                     && (*(buf+offset+5) == HTEid) )
 302                 {
 303                     zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
 304                     return (size-elen-2);
 305                 }
 306             }
 307             else
 308             {
 309                 zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
 310                 return (size-elen-2);
 311             }
 312         }
 313
 314         offset += (elen+2);
 315     }
 316
 317     return size;
 318 }
 319
 320 u16_t zfUpdateElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t* updateeid)
 321 {
 322     u16_t offset = 0;
 323     u16_t elen;
 324
 325     while (offset < size) {
 326         elen = *(buf+offset+1);
 327
 328         if (*(buf+offset) == updateeid[0]) {
 329             if (updateeid[1] <= elen) {
 330                 zfMemoryMove(buf+offset, updateeid, updateeid[1]+2);
 331                 zfMemoryMove(buf+offset+updateeid[1]+2, buf+offset+elen+2, size-offset-elen-2);
 332
 333                 return size-(elen-updateeid[1]);
 334             } else {
 335                 zfMemoryMove(buf+offset+updateeid[1]+2, buf+offset+elen+2, size-offset-elen-2);
 336                 zfMemoryMove(buf+offset, updateeid, updateeid[1]+2);
 337
 338                 return size+(updateeid[1]-elen);
 339             }
 340         }
 341
 342         offset += (elen+2);
 343     }
 344
 345     return size;
 346 }
 347
 348 u16_t zfFindSuperGElement(zdev_t* dev, zbuf_t* buf, u8_t type)
 349 {
 350     u8_t subType;
 351     u16_t offset;
 352     u16_t bufLen;
 353     u16_t elen;
 354     u8_t id;
 355     u8_t super_feature;
 356     u8_t ouiSuperG[6] = {0x00,0x03,0x7f,0x01, 0x01, 0x00};
 357
 358     /* Get offset of first element */
 359     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
 360     offset = zgElementOffsetTable[subType];
 361     if (offset == 0xff)
 362     {
 363         zm_assert(0);
 364     }
 365
 366     /* Plus wlan header */
 367     offset += 24;
 368
 369     bufLen = zfwBufGetSize(dev, buf);
 370     /* Search loop */
 371     while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
 372     {
 373         /* Search target element */
 374         id = zmw_rx_buf_readb(dev, buf, offset);
 375         if (id == ZM_WLAN_EID_VENDOR_PRIVATE)
 376         {
 377             /* Bingo */
 378             elen = zmw_rx_buf_readb(dev, buf, offset+1);
 379             if (elen > bufLen - offset)
 380             {
 381                 /* Element length error */
 382                 return 0xffff;
 383             }
 384
 385             if ( elen == 0 )
 386             {
 387                 return 0xffff;
 388             }
 389
 390             if (zfRxBufferEqualToStr(dev, buf, ouiSuperG, offset+2, 6) && ( zmw_rx_buf_readb(dev, buf, offset+1) >= 6))
 391             {
 392                 /* super_feature 0:useFastFrame, 1:useCompression, 2:useTurboPrime */
 393                 super_feature= zmw_rx_buf_readb(dev, buf, offset+8);
 394                 if ((super_feature & 0x01) || (super_feature & 0x02) || (super_feature & 0x04))
 395                 {
 396                     return offset;
 397                 }
 398             }
 399         }
 400         /* Advance to next element */
 401         #if 1
 402         elen = zmw_rx_buf_readb(dev, buf, offset+1);
 403         #else
 404         elen = zmw_rx_buf_readb(dev, buf, offset+1);
 405         if (elen == 0)
 406         {
 407             return 0xffff;
 408         }
 409         #endif
 410
 411         offset += (elen+2);
 412     }
 413     return 0xffff;
 414 }
 415
 416 u16_t zfFindXRElement(zdev_t* dev, zbuf_t* buf, u8_t type)
 417 {
 418     u8_t subType;
 419     u16_t offset;
 420     u16_t bufLen;
 421     u16_t elen;
 422     u8_t id;
 423     u8_t ouixr[6] = {0x00,0x03,0x7f,0x03, 0x01, 0x00};
 424
 425     /* Get offset of first element */
 426     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
 427     offset = zgElementOffsetTable[subType];
 428     if (offset == 0xff)
 429     {
 430         zm_assert(0);
 431     }
 432
 433     /* Plus wlan header */
 434     offset += 24;
 435
 436     bufLen = zfwBufGetSize(dev, buf);
 437     /* Search loop */
 438     while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
 439     {
 440         /* Search target element */
 441         id = zmw_rx_buf_readb(dev, buf, offset);
 442         if (id == ZM_WLAN_EID_VENDOR_PRIVATE)
 443         {
 444             /* Bingo */
 445             elen = zmw_rx_buf_readb(dev, buf, offset+1);
 446             if (elen > bufLen - offset)
 447             {
 448                 /* Element length error */
 449                 return 0xffff;
 450             }
 451
 452             if ( elen == 0 )
 453             {
 454                 return 0xffff;
 455             }
 456
 457             if (zfRxBufferEqualToStr(dev, buf, ouixr, offset+2, 6) && ( zmw_rx_buf_readb(dev, buf, offset+1) >= 6))
 458             {
 459                 return offset;
 460             }
 461         }
 462         /* Advance to next element */
 463         #if 1
 464         elen = zmw_rx_buf_readb(dev, buf, offset+1);
 465         #else
 466         elen = zmw_rx_buf_readb(dev, buf, offset+1);
 467         if (elen == 0)
 468         {
 469             return 0xffff;
 470         }
 471         #endif
 472
 473         offset += (elen+2);
 474     }
 475     return 0xffff;
 476 }
 477
 478 /************************************************************************/
 479 /*                                                                      */
 480 /*    FUNCTION DESCRIPTION                  zfMmAddIeSupportRate        */
 481 /*      Add information element Support Rate to buffer.                 */
 482 /*                                                                      */
 483 /*    INPUTS                                                            */
 484 /*      dev : device pointer                                            */
 485 /*      buf : buffer to add information element                         */
 486 /*      offset : add information element from this offset               */
 487 /*      eid : element ID                                                */
 488 /*      rateSet :  CCK or OFDM                                          */
 489 /*                                                                      */
 490 /*    OUTPUTS                                                           */
 491 /*      buffer offset after adding information element                  */
 492 /*                                                                      */
 493 /*    AUTHOR                                                            */
 494 /*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
 495 /*                                                                      */
 496 /************************************************************************/
 497 u16_t zfMmAddIeSupportRate(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t eid, u8_t rateSet)
 498 {
 499     u8_t len = 0;
 500     u16_t i;
 501
 502     zmw_get_wlan_dev(dev);
 503
 504     //if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
 505     //{
 506     //    return offset;
 507     //}
 508
 509     /* Information : Support Rate */
 510     if ( rateSet == ZM_RATE_SET_CCK )
 511     {
 512         for (i=0; i<4; i++)
 513         {
 514             if ((wd->bRate & (0x1<<i)) == (0x1<<i))
 515             //if ((0xf & (0x1<<i)) == (0x1<<i))
 516             {
 517                 zmw_tx_buf_writeb(dev, buf, offset+len+2,
 518                                      zg11bRateTbl[i]+((wd->bRateBasic & (0x1<<i))<<(7-i)));
 519                 len++;
 520             }
 521         }
 522     }
 523     else if ( rateSet == ZM_RATE_SET_OFDM )
 524     {
 525         for (i=0; i<8; i++)
 526         {
 527             if ((wd->gRate & (0x1<<i)) == (0x1<<i))
 528             //if ((0xff & (0x1<<i)) == (0x1<<i))
 529             {
 530                 zmw_tx_buf_writeb(dev, buf, offset+len+2,
 531                                      zg11gRateTbl[i]+((wd->gRateBasic & (0x1<<i))<<(7-i)));
 532                 len++;
 533             }
 534         }
 535     }
 536
 537     if (len > 0)
 538     {
 539         /* Element ID */
 540         zmw_tx_buf_writeb(dev, buf, offset, eid);
 541
 542         /* Element Length */
 543         zmw_tx_buf_writeb(dev, buf, offset+1, len);
 544
 545         /* Return value */
 546         offset += (2+len);
 547     }
 548
 549     return offset;
 550 }
 551
 552 /************************************************************************/
 553 /*                                                                      */
 554 /*    FUNCTION DESCRIPTION                  zfMmAddIeDs                 */
 555 /*      Add information element DS to buffer.                           */
 556 /*                                                                      */
 557 /*    INPUTS                                                            */
 558 /*      dev : device pointer                                            */
 559 /*      buf : buffer to add information element                         */
 560 /*      offset : add information element from this offset               */
 561 /*                                                                      */
 562 /*    OUTPUTS                                                           */
 563 /*      buffer offset after adding information element                  */
 564 /*                                                                      */
 565 /*    AUTHOR                                                            */
 566 /*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
 567 /*                                                                      */
 568 /************************************************************************/
 569 u16_t zfMmAddIeDs(zdev_t* dev, zbuf_t* buf, u16_t offset)
 570 {
 571     zmw_get_wlan_dev(dev);
 572
 573     /* Element ID */
 574     zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_DS);
 575
 576     /* Element Length */
 577     zmw_tx_buf_writeb(dev, buf, offset++, 1);
 578
 579     /* Information : DS */
 580     zmw_tx_buf_writeb(dev, buf, offset++,
 581                          zfChFreqToNum(wd->frequency, NULL));
 582
 583     return offset;
 584 }
 585
 586
 587 /************************************************************************/
 588 /*                                                                      */
 589 /*    FUNCTION DESCRIPTION                  zfMmAddIeErp                */
 590 /*      Add information element ERP to buffer.                          */
 591 /*                                                                      */
 592 /*    INPUTS                                                            */
 593 /*      dev : device pointer                                            */
 594 /*      buf : buffer to add information element                         */
 595 /*      offset : add information element from this offset               */
 596 /*                                                                      */
 597 /*    OUTPUTS                                                           */
 598 /*      buffer offset after adding information element                  */
 599 /*                                                                      */
 600 /*    AUTHOR                                                            */
 601 /*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
 602 /*                                                                      */
 603 /************************************************************************/
 604 u16_t zfMmAddIeErp(zdev_t* dev, zbuf_t* buf, u16_t offset)
 605 {
 606     zmw_get_wlan_dev(dev);
 607
 608     /* Element ID */
 609     zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_ERP);
 610
 611     /* Element Length */
 612     zmw_tx_buf_writeb(dev, buf, offset++, 1);
 613
 614     /* Information : ERP */
 615     zmw_tx_buf_writeb(dev, buf, offset++, wd->erpElement);
 616
 617     return offset;
 618 }
 619
 620
 621 /************************************************************************/
 622 /*                                                                      */
 623 /*    FUNCTION DESCRIPTION                  zfMmAddIeWpa                */
 624 /*      Add information element WPA to buffer.                          */
 625 /*                                                                      */
 626 /*    INPUTS                                                            */
 627 /*      dev : device pointer                                            */
 628 /*      buf : buffer to add information element                         */
 629 /*      offset : add information element from this offset               */
 630 /*                                                                      */
 631 /*    OUTPUTS                                                           */
 632 /*      buffer offset after adding information element                  */
 633 /*                                                                      */
 634 /*    AUTHOR                                                            */
 635 /*      Yuan-Gu Wei         ZyDAS Technology Corporation    2006.2      */
 636 /*                                                                      */
 637 /************************************************************************/
 638 u16_t zfMmAddIeWpa(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t apId)
 639 {
 640     //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
 641     int i;
 642
 643     zmw_get_wlan_dev(dev);
 644
 645     /* Element ID */
 646     //zmw_inttx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
 647
 648     /* Element Length */
 649     //zmw_inttx_buf_writeb(dev, buf, offset++, wd->ap.wpaLen);
 650     for(i = 0; i < wd->ap.wpaLen[apId]; i++)
 651     {
 652         /* Information : WPA */
 653         zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.wpaIe[apId][i]);
 654     }
 655
 656     return offset;
 657 }
 658
 659 /************************************************************************/
 660 /*                                                                      */
 661 /*    FUNCTION DESCRIPTION                  zfMmAddHTCapability         */
 662 /*      Add HT Capability Infomation to buffer.                         */
 663 /*                                                                      */
 664 /*    INPUTS                                                            */
 665 /*      dev : device pointer                                            */
 666 /*      buf : buffer to add information element                         */
 667 /*      offset : add information element from this offset               */
 668 /*                                                                      */
 669 /*    OUTPUTS                                                           */
 670 /*      buffer offset after adding information element                  */
 671 /*                                                                      */
 672 /*    AUTHOR                                                            */
 673 /*      Chao-Wen Yang     ZyDAS Technology Corporation       2006.06    */
 674 /*                                                                      */
 675 /************************************************************************/
 676 u16_t zfMmAddHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
 677 {
 678     u8_t OUI[3] = {0x0,0x90,0x4C};
 679     u16_t i;
 680
 681     zmw_get_wlan_dev(dev);
 682
 683     /* Prob ID */
 684     zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
 685
 686     if ( wd->wlanMode == ZM_MODE_AP )
 687     {
 688         /* Element Length */
 689         zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.Length + 4);
 690
 691         /* OUI Data */
 692         for (i = 0; i < 3; i++)
 693         {
 694             zmw_buf_writeb(dev, buf, offset++, OUI[i]);
 695         }
 696
 697         /* Element Type ID */
 698         zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.ElementID);
 699
 700         /* HT Capability Data */
 701         for (i = 0; i < 26; i++)
 702         {
 703             zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Byte[i+2]);
 704         }
 705     }
 706     else
 707     {
 708         /* Element Length */
 709         zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.Length + 4);
 710
 711         /* OUI Data */
 712         for (i = 0; i < 3; i++)
 713         {
 714             zmw_buf_writeb(dev, buf, offset++, OUI[i]);
 715         }
 716
 717         /* Element Type ID */
 718         zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.ElementID);
 719
 720         /* HT Capability Data */
 721         for (i = 0; i < 26; i++)
 722         {
 723             zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Byte[i+2]);
 724         }
 725     }
 726
 727     return offset;
 728 }
 729
 730
 731 u16_t zfMmAddPreNHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
 732 {
 733     //u8_t OUI[3] = {0x0,0x90,0x4C};
 734     u16_t i;
 735
 736     zmw_get_wlan_dev(dev);
 737
 738     /* Prob ID */
 739     zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_PREN2_EID_HTCAPABILITY);
 740
 741     if ( wd->wlanMode == ZM_MODE_AP )
 742     {
 743         /* Element Length */
 744         zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.Length);
 745
 746         /* HT Capability Data */
 747         for (i = 0; i < 26; i++)
 748         {
 749             zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Byte[i+2]);
 750         }
 751     }
 752     else
 753     {
 754         /* Element Length */
 755         zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.Length);
 756
 757         /* HT Capability Data */
 758         for (i = 0; i < 26; i++)
 759         {
 760             zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Byte[i+2]);
 761         }
 762     }
 763
 764     return offset;
 765 }
 766
 767 /************************************************************************/
 768 /*                                                                      */
 769 /*    FUNCTION DESCRIPTION                zfMmAddExtendedHTCapability   */
 770 /*      Add Extended HT Capability Infomation to buffer.                */
 771 /*                                                                      */
 772 /*    INPUTS                                                            */
 773 /*      dev : device pointer                                            */
 774 /*      buf : buffer to add information element                         */
 775 /*      offset : add information element from this offset               */
 776 /*                                                                      */
 777 /*    OUTPUTS                                                           */
 778 /*      buffer offset after adding information element                  */
 779 /*                                                                      */
 780 /*    AUTHOR                                                            */
 781 /*      Chao-Wen Yang     ZyDAS Technology Corporation       2006.06    */
 782 /*                                                                      */
 783 /************************************************************************/
 784 u16_t zfMmAddExtendedHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
 785 {
 786     u8_t OUI[3] = {0x0,0x90,0x4C};
 787     u16_t i;
 788
 789     zmw_get_wlan_dev(dev);
 790
 791     /* Prob ID */
 792     zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
 793
 794     if ( wd->wlanMode == ZM_MODE_AP )
 795     {
 796         /* Element Length */
 797         zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Data.Length + 4);
 798
 799         /* OUI Data */
 800         for (i = 0; i < 3; i++)
 801         {
 802             zmw_buf_writeb(dev, buf, offset++, OUI[i]);
 803         }
 804
 805         /* Element Type ID */
 806         zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Data.ElementID);
 807
 808         /* HT Capability Data */
 809         for (i = 0; i < 22; i++)
 810         {
 811             zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Byte[i+2]);
 812         }
 813     }
 814     else
 815     {
 816         /* Element Length */
 817         zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Data.Length + 4);
 818
 819         /* OUI Data */
 820         for (i = 0; i < 3; i++)
 821         {
 822             zmw_buf_writeb(dev, buf, offset++, OUI[i]);
 823         }
 824
 825         /* Element Type ID */
 826         zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Data.ElementID);
 827
 828         /* HT Capability Data */
 829         for (i = 0; i < 22; i++)
 830         {
 831             zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Byte[i+2]);
 832         }
 833     }
 834
 835     return offset;
 836 }
 837
 838
 839
 840 /************************************************************************/
 841 /*                                                                      */
 842 /*    FUNCTION DESCRIPTION                  zfSendMmFrame               */
 843 /*      Send management frame.                                          */
 844 /*                                                                      */
 845 /*    INPUTS                                                            */
 846 /*      dev : device pointer                                            */
 847 /*      frameType : management frame type                               */
 848 /*      dst : destination MAC address                                   */
 849 /*      p1 : parameter 1                                                */
 850 /*      p2 : parameter 2                                                */
 851 /*      p3 : parameter 3                                                */
 852 /*                                                                      */
 853 /*    OUTPUTS                                                           */
 854 /*      none                                                            */
 855 /*                                                                      */
 856 /*    AUTHOR                                                            */
 857 /*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
 858 /*                                                                      */
 859 /************************************************************************/
 860 /* probe req : p1=> bWithSSID, p2=>R, p3=>R                                      */
 861 /* probe rsp : p1=>R, p2=>R, p3=>VAP ID(AP)                             */
 862 /* deauth : p1=>Reason Code, p2=>R, p3=>VAP ID(AP)                      */
 863 /* Disasoc : p1=>Reason Code, p2=>R, p3=>VAP ID(AP)                     */
 864 /* ATIM : p1=>R, p2=>R, p3=>R                                           */
 865 /* (re)asoc rsp : p1=>Status Code, p2=>AID, p3=>VAP ID(AP)              */
 866 /* asoc req : p1=>R, p2=>R, p3=>R                                       */
 867 /* reasoc req : p1=>AP MAC[0], p2=>AP MAC[1], p3=>AP MAC[2]             */
 868 /* auth : p1=>low=Algorithm, high=Transaction, p2=>Status, p3=>VAP ID   */
 869 void zfSendMmFrame(zdev_t* dev, u8_t frameType, u16_t* dst,
 870                    u32_t p1, u32_t p2, u32_t p3)
 871 {
 872     zbuf_t* buf;
 873     //u16_t addrTblSize;
 874     //struct zsAddrTbl addrTbl;
 875     u16_t offset = 0;
 876     u16_t hlen = 32;
 877     u16_t header[(24+25+1)/2];
 878     u16_t vap = 0;
 879     u16_t i;
 880     u8_t encrypt = 0;
 881     u16_t aid;
 882
 883     zmw_get_wlan_dev(dev);
 884     zmw_declare_for_critical_section();
 885
 886     zm_msg2_mm(ZM_LV_2, "Send mm frame, type=", frameType);
 887     /* TBD : Maximum size of management frame */
 888     buf = zfwBufAllocate(dev, 1024);
 889     if (buf == NULL)
 890     {
 891         zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
 892         return;
 893     }
 894
 895     //Reserve room for wlan header
 896     offset = hlen;
 897
 898     switch (frameType)
 899     {
 900         case ZM_WLAN_FRAME_TYPE_PROBEREQ :
 901             offset = zfSendProbeReq(dev, buf, offset, (u8_t) p1);
 902             break;
 903
 904         case ZM_WLAN_FRAME_TYPE_PROBERSP :
 905             zm_msg0_mm(ZM_LV_3, "probe rsp");
 906             /* 24-31 Time Stamp : hardware WON'T fill this field */
 907             zmw_tx_buf_writeh(dev, buf, offset, 0);
 908             zmw_tx_buf_writeh(dev, buf, offset+2, 0);
 909             zmw_tx_buf_writeh(dev, buf, offset+4, 0);
 910             zmw_tx_buf_writeh(dev, buf, offset+6, 0);
 911             offset+=8;
 912
 913             /* Beacon Interval */
 914             zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
 915             offset+=2;
 916
 917             if (wd->wlanMode == ZM_MODE_AP)
 918             {
 919                 vap = (u16_t) p3;
 920                 /* Capability */
 921                 zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
 922                 offset+=2;
 923                 /* SSID */
 924                 offset = zfApAddIeSsid(dev, buf, offset, vap);
 925             }
 926             else
 927             {
 928                 /* Capability */
 929                 zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
 930                 zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
 931                 /* SSID */
 932                 offset = zfStaAddIeSsid(dev, buf, offset);
 933             }
 934
 935             /* Support Rate */
 936             if ( wd->frequency < 3000 )
 937             {
 938             offset = zfMmAddIeSupportRate(dev, buf, offset,
 939                                           ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
 940             }
 941             else
 942             {
 943                 offset = zfMmAddIeSupportRate(dev, buf, offset,
 944                                           ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
 945             }
 946
 947             /* DS parameter set */
 948             offset = zfMmAddIeDs(dev, buf, offset);
 949
 950             /* TODO ¡G IBSS */
 951             if ( wd->wlanMode == ZM_MODE_IBSS )
 952             {
 953                 offset = zfStaAddIeIbss(dev, buf, offset);
 954
 955                 if (wd->frequency < 3000)
 956                 {
 957                     if( wd->wfc.bIbssGMode
 958                         && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
 959                     {
 960                         /* ERP Information */
 961                         wd->erpElement = 0;
 962                         offset = zfMmAddIeErp(dev, buf, offset);
 963
 964                         /* Enable G Mode */
 965                         /* Extended Supported Rates */
 966                             offset = zfMmAddIeSupportRate(dev, buf, offset,
 967                                 ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
 968                     }
 969                     }
 970             }
 971
 972
 973             if ((wd->wlanMode == ZM_MODE_AP)
 974                  && (wd->ap.wlanType[vap] != ZM_WLAN_TYPE_PURE_B))
 975             {
 976                 /* ERP Information */
 977                 offset = zfMmAddIeErp(dev, buf, offset);
 978
 979                 /* Extended Supported Rates */
 980                 if ( wd->frequency < 3000 )
 981                 {
 982                 offset = zfMmAddIeSupportRate(dev, buf, offset,
 983                                    ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
 984             }
 985             }
 986
 987             /* ERP Information */
 988             //offset = zfMmAddIeErp(dev, buf, offset);
 989
 990             /* Extended Supported Rates */
 991             //offset = zfMmAddIeSupportRate(dev, buf, offset,
 992             //                              ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
 993
 994             /* TODO : RSN */
 995             if (wd->wlanMode == ZM_MODE_AP && wd->ap.wpaSupport[vap] == 1)
 996             {
 997                 offset = zfMmAddIeWpa(dev, buf, offset, vap);
 998             }
 999             else if ( wd->wlanMode == ZM_MODE_IBSS && wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK)
1000             {
1001                 offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
1002             }
1003
1004             /* WME Parameters */
1005             if (wd->wlanMode == ZM_MODE_AP)
1006             {
1007                 if (wd->ap.qosMode == 1)
1008                 {
1009                     offset = zfApAddIeWmePara(dev, buf, offset, vap);
1010                 }
1011             }
1012
1013             if ( wd->wlanMode != ZM_MODE_IBSS )
1014             {
1015             // jhlee HT 0
1016             //CWYang(+)
1017                 /* TODO : Need to check if it is ok */
1018             /* HT Capabilities Info */
1019             offset = zfMmAddHTCapability(dev, buf, offset);
1020             //CWYang(+)
1021             /* Extended HT Capabilities Info */
1022             offset = zfMmAddExtendedHTCapability(dev, buf, offset);
1023             }
1024
1025             if ( wd->sta.ibssAdditionalIESize )
1026                 offset = zfStaAddIbssAdditionalIE(dev, buf, offset);
1027             break;
1028
1029         case ZM_WLAN_FRAME_TYPE_AUTH :
1030             if (p1 == 0x30001)
1031             {
1032                 hlen += 4;
1033                 offset += 4;        // for reserving wep header
1034                 encrypt = 1;
1035             }
1036
1037             /* Algotrithm Number */
1038             zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p1&0xffff));
1039             offset+=2;
1040
1041             /* Transaction Number */
1042             zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p1>>16));
1043             offset+=2;
1044
1045             /* Status Code */
1046             zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p2);
1047             offset+=2;
1048
1049             if (wd->wlanMode == ZM_MODE_AP)
1050             {
1051                 vap = (u16_t) p3;
1052             }
1053
1054             /* Challenge Text => share-2 or share-3 */
1055             if (p1 == 0x20001)
1056             {
1057                 if (p2 == 0) //Status == success
1058                 {
1059                     zmw_buf_writeh(dev, buf, offset, 0x8010);
1060                     offset+=2;
1061                     /* share-2 : AP generate challenge text */
1062                     for (i=0; i<128; i++)
1063                     {
1064                         wd->ap.challengeText[i] = (u8_t)zfGetRandomNumber(dev, 0);
1065                     }
1066                     zfCopyToIntTxBuffer(dev, buf, wd->ap.challengeText, offset, 128);
1067                     offset += 128;
1068                 }
1069             }
1070             else if (p1 == 0x30001)
1071             {
1072                 /* share-3 : STA return challenge Text */
1073                 zfCopyToIntTxBuffer(dev, buf, wd->sta.challengeText, offset, wd->sta.challengeText[1]+2);
1074                 offset += (wd->sta.challengeText[1]+2);
1075             }
1076
1077             break;
1078
1079         case ZM_WLAN_FRAME_TYPE_ASOCREQ :
1080         case ZM_WLAN_FRAME_TYPE_REASOCREQ :
1081             /* Capability */
1082             zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
1083             zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
1084
1085             /* Listen Interval */
1086             zmw_tx_buf_writeh(dev, buf, offset, 0x0005);
1087             offset+=2;
1088
1089             /* Reassocaited Request : Current AP address */
1090             if (frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ)
1091             {
1092             zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[0]);
1093                 offset+=2;
1094             zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[1]);
1095                 offset+=2;
1096             zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[2]);
1097                 offset+=2;
1098             }
1099
1100             /* SSID */
1101             offset = zfStaAddIeSsid(dev, buf, offset);
1102
1103
1104             if ( wd->sta.currentFrequency < 3000 )
1105             {
1106                 /* Support Rate */
1107                 offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
1108             }
1109             else
1110             {
1111                 /* Support Rate */
1112                 offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
1113             }
1114
1115             if ((wd->sta.capability[1] & ZM_BIT_0) == 1)
1116             {   //spectrum management flag enable
1117                 offset = zfStaAddIePowerCap(dev, buf, offset);
1118                 offset = zfStaAddIeSupportCh(dev, buf, offset);
1119             }
1120
1121             if (wd->sta.currentFrequency < 3000)
1122             {
1123                 /* Extended Supported Rates */
1124                 if (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N))
1125                 {
1126                     offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
1127                 }
1128             }
1129
1130
1131             //offset = zfStaAddIeWpaRsn(dev, buf, offset, frameType);
1132             //Move to wrapper function, for OS difference--CWYang(m)
1133             //for windows wrapper, zfwStaAddIeWpaRsn() should be below:
1134             //u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
1135             //{
1136             //    return zfStaAddIeWpaRsn(dev, buf, offset, frameType);
1137             //}
1138             offset = zfwStaAddIeWpaRsn(dev, buf, offset, frameType);
1139
1140 #ifdef ZM_ENABLE_CENC
1141             /* CENC */
1142             //if (wd->sta.encryMode == ZM_CENC)
1143             offset = zfStaAddIeCenc(dev, buf, offset);
1144 #endif //ZM_ENABLE_CENC
1145             if (((wd->sta.wmeEnabled & ZM_STA_WME_ENABLE_BIT) != 0) //WME enabled
1146               && ((wd->sta.apWmeCapability & 0x1) != 0)) //WME AP
1147             {
1148                 if (((wd->sta.apWmeCapability & 0x80) != 0) //UAPSD AP
1149                  && ((wd->sta.wmeEnabled & ZM_STA_UAPSD_ENABLE_BIT) != 0)) //UAPSD enabled
1150                 {
1151                     offset = zfStaAddIeWmeInfo(dev, buf, offset, wd->sta.wmeQosInfo);
1152                 }
1153                 else
1154                 {
1155                     offset = zfStaAddIeWmeInfo(dev, buf, offset, 0);
1156                 }
1157             }
1158             // jhlee HT 0
1159             //CWYang(+)
1160             if (wd->sta.EnableHT != 0)
1161             {
1162                 #ifndef ZM_DISABLE_AMSDU8K_SUPPORT
1163                     //Support 8K A-MSDU
1164                     if (wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED)
1165                     {
1166                         wd->sta.HTCap.Data.HtCapInfo |= HTCAP_MaxAMSDULength;
1167                     }
1168                     else
1169                     {
1170                         wd->sta.HTCap.Data.HtCapInfo &= (~HTCAP_MaxAMSDULength);
1171                     }
1172                 #else
1173                     //Support 4K A-MSDU
1174                     wd->sta.HTCap.Data.HtCapInfo &= (~HTCAP_MaxAMSDULength);
1175                 #endif
1176
1177                 /* HT Capabilities Info */
1178                 if (wd->BandWidth40 == 1) {
1179                     wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
1180                 }
1181                 else {
1182                     wd->sta.HTCap.Data.HtCapInfo &= ~HTCAP_SupChannelWidthSet;
1183                     //wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
1184                 }
1185
1186                 wd->sta.HTCap.Data.AMPDUParam &= ~HTCAP_MaxRxAMPDU3;
1187                 wd->sta.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
1188                 wd->sta.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
1189                 offset = zfMmAddHTCapability(dev, buf, offset);
1190                 offset = zfMmAddPreNHTCapability(dev, buf, offset);
1191                 //CWYang(+)
1192                 /* Extended HT Capabilities Info */
1193                 //offset = zfMmAddExtendedHTCapability(dev, buf, offset);
1194             }
1195
1196
1197             //Store asoc request frame body, for VISTA only
1198             wd->sta.asocReqFrameBodySize = ((offset - hlen) >
1199                     ZM_CACHED_FRAMEBODY_SIZE)?
1200                     ZM_CACHED_FRAMEBODY_SIZE:(offset - hlen);
1201             for (i=0; i<wd->sta.asocReqFrameBodySize; i++)
1202             {
1203                 wd->sta.asocReqFrameBody[i] = zmw_tx_buf_readb(dev, buf, i + hlen);
1204             }
1205             break;
1206
1207         case ZM_WLAN_FRAME_TYPE_ASOCRSP :
1208         case ZM_WLAN_FRAME_TYPE_REASOCRSP :
1209             vap = (u16_t) p3;
1210
1211             /* Capability */
1212             zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
1213             offset+=2;
1214
1215             /* Status Code */
1216             zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p1);
1217             offset+=2;
1218
1219             /* AID */
1220             zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p2|0xc000));
1221             offset+=2;
1222
1223
1224             if ( wd->frequency < 3000 )
1225             {
1226             /* Support Rate */
1227             offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
1228
1229             /* Extended Supported Rates */
1230             offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
1231             }
1232             else
1233             {
1234                 /* Support Rate */
1235                 offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
1236             }
1237
1238
1239
1240             /* WME Parameters */
1241             if (wd->wlanMode == ZM_MODE_AP)
1242             {
1243                 /* TODO : if WME STA then send WME parameter element */
1244                 if (wd->ap.qosMode == 1)
1245                 {
1246                     offset = zfApAddIeWmePara(dev, buf, offset, vap);
1247                 }
1248             }
1249             // jhlee HT 0
1250             //CWYang(+)
1251             /* HT Capabilities Info */
1252             offset = zfMmAddHTCapability(dev, buf, offset);
1253             //CWYang(+)
1254             /* Extended HT Capabilities Info */
1255             offset = zfMmAddExtendedHTCapability(dev, buf, offset);
1256             break;
1257
1258         case ZM_WLAN_FRAME_TYPE_ATIM :
1259             /* NULL frame */
1260             /* TODO : add two dumb bytes temporarily */
1261             offset += 2;
1262             break;
1263
1264         case ZM_WLAN_FRAME_TYPE_QOS_NULL :
1265             zmw_buf_writeh(dev, buf, offset, 0x0010);
1266             offset += 2;
1267             break;
1268
1269         case ZM_WLAN_DATA_FRAME :
1270             break;
1271
1272         case ZM_WLAN_FRAME_TYPE_DISASOC :
1273         case ZM_WLAN_FRAME_TYPE_DEAUTH :
1274             if (wd->wlanMode == ZM_MODE_AP)
1275             {
1276               vap = (u16_t) p3;
1277
1278               aid = zfApFindSta(dev, dst);
1279               if (aid != 0xffff)
1280               {
1281                   zmw_enter_critical_section(dev);
1282                   /* Clear STA table */
1283                   wd->ap.staTable[aid].valid = 0;
1284
1285                   zmw_leave_critical_section(dev);
1286
1287                   if (wd->zfcbDisAsocNotify != NULL)
1288                   {
1289                       wd->zfcbDisAsocNotify(dev, (u8_t*)dst, vap);
1290                   }
1291               }
1292             }
1293             /* Reason Code */
1294             zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p1);
1295             offset+=2;
1296             break;
1297     }
1298
1299     zfwBufSetSize(dev, buf, offset);
1300
1301     zm_msg2_mm(ZM_LV_2, "management frame body size=", offset-hlen);
1302
1303     //Copy wlan header
1304     zfTxGenMmHeader(dev, frameType, dst, header, offset-hlen, buf, vap, encrypt);
1305     for (i=0; i<(hlen>>1); i++)
1306     {
1307         zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
1308     }
1309
1310     /* Get buffer DMA address */
1311     //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
1312     //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
1313     //{
1314     //    goto zlError;
1315     //}
1316
1317     zm_msg2_mm(ZM_LV_2, "offset=", offset);
1318     zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
1319     //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
1320     //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
1321     //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
1322     //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
1323
1324     #if 0
1325     err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
1326                    ZM_INTERNAL_ALLOC_BUF, 0, 0xff);
1327     if (err != ZM_SUCCESS)
1328     {
1329         goto zlError;
1330     }
1331     #else
1332     zfPutVmmq(dev, buf);
1333     zfPushVtxq(dev);
1334     #endif
1335
1336     return;
1337 #if 0
1338 zlError:
1339
1340     zfwBufFree(dev, buf, 0);
1341     return;
1342 #endif
1343 }
1344
1345
1346 /************************************************************************/
1347 /*                                                                      */
1348 /*    FUNCTION DESCRIPTION                  zfProcessManagement         */
1349 /*      Process received management frame.                              */
1350 /*                                                                      */
1351 /*    INPUTS                                                            */
1352 /*      dev : device pointer                                            */
1353 /*      buf : received management frame buffer                           */
1354 /*                                                                      */
1355 /*    OUTPUTS                                                           */
1356 /*      none                                                            */
1357 /*                                                                      */
1358 /*    AUTHOR                                                            */
1359 /*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
1360 /*                                                                      */
1361 /************************************************************************/
1362 void zfProcessManagement(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo) //CWYang(m)
1363 {
1364     u8_t frameType;
1365     u16_t ta[3];
1366     u16_t ra[3];
1367     u16_t vap = 0, index = 0;
1368     //u16_t i;
1369
1370     zmw_get_wlan_dev(dev);
1371
1372     ra[0] = zmw_rx_buf_readh(dev, buf, 4);
1373     ra[1] = zmw_rx_buf_readh(dev, buf, 6);
1374     ra[2] = zmw_rx_buf_readh(dev, buf, 8);
1375
1376     ta[0] = zmw_rx_buf_readh(dev, buf, 10);
1377     ta[1] = zmw_rx_buf_readh(dev, buf, 12);
1378     ta[2] = zmw_rx_buf_readh(dev, buf, 14);
1379
1380     frameType = zmw_rx_buf_readb(dev, buf, 0);
1381
1382     if (wd->wlanMode == ZM_MODE_AP)
1383     {
1384 #if 1
1385         vap = 0;
1386         if ((ra[0] & 0x1) != 1)
1387         {
1388             /* AP : Find virtual AP */
1389             index = zfApFindSta(dev, ta);
1390             if (index != 0xffff)
1391             {
1392                 vap = wd->ap.staTable[index].vap;
1393             }
1394         }
1395         zm_msg2_mm(ZM_LV_2, "vap=", vap);
1396 #endif
1397
1398         /* Dispatch by frame type */
1399         switch (frameType)
1400         {
1401                 /* Beacon */
1402             case ZM_WLAN_FRAME_TYPE_BEACON :
1403                 zfApProcessBeacon(dev, buf);
1404                 break;
1405                 /* Authentication */
1406             case ZM_WLAN_FRAME_TYPE_AUTH :
1407                 zfApProcessAuth(dev, buf, ta, vap);
1408                 break;
1409                 /* Association request */
1410             case ZM_WLAN_FRAME_TYPE_ASOCREQ :
1411                 /* Reassociation request */
1412             case ZM_WLAN_FRAME_TYPE_REASOCREQ :
1413                 zfApProcessAsocReq(dev, buf, ta, vap);
1414                 break;
1415                 /* Association response */
1416             case ZM_WLAN_FRAME_TYPE_ASOCRSP :
1417                 //zfApProcessAsocRsp(dev, buf);
1418                 break;
1419                 /* Deauthentication */
1420             case ZM_WLAN_FRAME_TYPE_DEAUTH :
1421                 zfApProcessDeauth(dev, buf, ta, vap);
1422                 break;
1423                 /* Disassociation */
1424             case ZM_WLAN_FRAME_TYPE_DISASOC :
1425                 zfApProcessDisasoc(dev, buf, ta, vap);
1426                 break;
1427                 /* Probe request */
1428             case ZM_WLAN_FRAME_TYPE_PROBEREQ :
1429                 zfProcessProbeReq(dev, buf, ta);
1430                 break;
1431                 /* Probe response */
1432             case ZM_WLAN_FRAME_TYPE_PROBERSP :
1433                 zfApProcessProbeRsp(dev, buf, AddInfo);
1434                 break;
1435                 /* Action */
1436             case ZM_WLAN_FRAME_TYPE_ACTION :
1437                 zfApProcessAction(dev, buf);
1438                 break;
1439         }
1440     }
1441     else //if ((wd->wlanMode == ZM_MODE_INFRASTRUCTURE) || (wd->wlanMode == ZM_MODE_IBSS))
1442     {
1443         /* Dispatch by frame type */
1444         switch (frameType)
1445         {
1446                 /* Beacon */
1447             case ZM_WLAN_FRAME_TYPE_BEACON :
1448                 /* if enable 802.11h and current channel is silent but receive beacon from other AP */
1449                 if (((wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
1450                         & ZM_REG_FLAG_CHANNEL_CSA) != 0) && wd->sta.DFSEnable)
1451                 {
1452                     wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
1453                             &= ~(ZM_REG_FLAG_CHANNEL_CSA & ZM_REG_FLAG_CHANNEL_PASSIVE);
1454                 }
1455                 zfStaProcessBeacon(dev, buf, AddInfo); //CWYang(m)
1456                 break;
1457                 /* Authentication */
1458             case ZM_WLAN_FRAME_TYPE_AUTH :
1459                 /* TODO : vap parameter is useless in STA mode, get rid of it */
1460                 zfStaProcessAuth(dev, buf, ta, 0);
1461                 break;
1462                 /* Association request */
1463             case ZM_WLAN_FRAME_TYPE_ASOCREQ :
1464                 /* TODO : vap parameter is useless in STA mode, get rid of it */
1465                 zfStaProcessAsocReq(dev, buf, ta, 0);
1466                 break;
1467                 /* Association response */
1468             case ZM_WLAN_FRAME_TYPE_ASOCRSP :
1469                 /* Reassociation request */
1470             case ZM_WLAN_FRAME_TYPE_REASOCRSP :
1471                 zfStaProcessAsocRsp(dev, buf);
1472                 break;
1473                 /* Deauthentication */
1474             case ZM_WLAN_FRAME_TYPE_DEAUTH :
1475                 zm_debug_msg0("Deauthentication received");
1476                 zfStaProcessDeauth(dev, buf);
1477                 break;
1478                 /* Disassociation */
1479             case ZM_WLAN_FRAME_TYPE_DISASOC :
1480                 zm_debug_msg0("Disassociation received");
1481                 zfStaProcessDisasoc(dev, buf);
1482                 break;
1483                 /* Probe request */
1484             case ZM_WLAN_FRAME_TYPE_PROBEREQ :
1485                 zfProcessProbeReq(dev, buf, ta);
1486                 break;
1487                 /* Probe response */
1488             case ZM_WLAN_FRAME_TYPE_PROBERSP :
1489                 /* if enable 802.11h and current channel is silent but receive probe response from other AP */
1490                 if (((wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
1491                         & ZM_REG_FLAG_CHANNEL_CSA) != 0) && wd->sta.DFSEnable)
1492                 {
1493                     wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
1494                             &= ~(ZM_REG_FLAG_CHANNEL_CSA & ZM_REG_FLAG_CHANNEL_PASSIVE);
1495                 }
1496                 zfStaProcessProbeRsp(dev, buf, AddInfo);
1497                 break;
1498
1499             case ZM_WLAN_FRAME_TYPE_ATIM:
1500                 zfStaProcessAtim(dev, buf);
1501                 break;
1502                 /* Action */
1503             case ZM_WLAN_FRAME_TYPE_ACTION :
1504                 zm_msg0_mm(ZM_LV_2, "ProcessActionMgtFrame");
1505                 zfStaProcessAction(dev, buf);
1506                 break;
1507         }
1508     }
1509 }
1510
1511 /************************************************************************/
1512 /*                                                                      */
1513 /*    FUNCTION DESCRIPTION                  zfProcessProbeReq           */
1514 /*      Process probe request management frame.                         */
1515 /*                                                                      */
1516 /*    INPUTS                                                            */
1517 /*      dev : device pointer                                            */
1518 /*      buf : auth frame buffer                                         */
1519 /*                                                                      */
1520 /*    OUTPUTS                                                           */
1521 /*      none                                                            */
1522 /*                                                                      */
1523 /*    AUTHOR                                                            */
1524 /*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
1525 /*                                                                      */
1526 /************************************************************************/
1527 void zfProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src)
1528 {
1529     u16_t offset;
1530     u8_t len;
1531     u16_t i, j;
1532     u8_t ch;
1533     u16_t sendFlag;
1534
1535     zmw_get_wlan_dev(dev);
1536
1537     /* check mode : AP/IBSS */
1538     if ((wd->wlanMode != ZM_MODE_AP) && (wd->wlanMode != ZM_MODE_IBSS))
1539     {
1540         zm_msg0_mm(ZM_LV_3, "Ignore probe req");
1541         return;
1542     }
1543
1544     if ((wd->wlanMode != ZM_MODE_AP) && (wd->sta.adapterState == ZM_STA_STATE_DISCONNECT))
1545     {
1546         zm_msg0_mm(ZM_LV_3, "Packets dropped due to disconnect state");
1547         return;
1548     }
1549
1550     if ( wd->wlanMode == ZM_MODE_IBSS )
1551     {
1552         zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, 0, 0, 0);
1553
1554         return;
1555     }
1556
1557     /* check SSID */
1558     offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID);
1559     if (offset == 0xffff)
1560     {
1561         zm_msg0_mm(ZM_LV_3, "probe req SSID not found");
1562         return;
1563     }
1564
1565     len = zmw_rx_buf_readb(dev, buf, offset+1);
1566
1567     for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
1568     {
1569         if ((wd->ap.apBitmap & (1<<i)) != 0)
1570         {
1571             zm_msg1_mm(ZM_LV_3, "len=", len);
1572             sendFlag = 0;
1573             /* boardcast SSID */
1574             if (len == 0)
1575             {
1576                 if (wd->ap.hideSsid[i] == 0)
1577                 {
1578                     sendFlag = 1;
1579                 }
1580             }
1581             /* Not broadcast SSID */
1582             else if (wd->ap.ssidLen[i] == len)
1583             {
1584                 for (j=0; j<len; j++)
1585                 {
1586                     ch = zmw_rx_buf_readb(dev, buf, offset+2+j);
1587                     if (ch != wd->ap.ssid[i][j])
1588                     {
1589                         break;
1590                     }
1591                 }
1592                 if (j == len)
1593                 {
1594                     sendFlag = 1;
1595                 }
1596             }
1597             if (sendFlag == 1)
1598             {
1599                 /* Send probe response */
1600                 zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, i, 0, i);
1601             }
1602         }
1603     }
1604 }
1605
1606 /************************************************************************/
1607 /*                                                                      */
1608 /*    FUNCTION DESCRIPTION                  zfProcessProbeRsp           */
1609 /*      Process probe response management frame.                        */
1610 /*                                                                      */
1611 /*    INPUTS                                                            */
1612 /*      dev : device pointer                                            */
1613 /*      buf : auth frame buffer                                         */
1614 /*  AddInfo : Rx Header and Rx Mac Status                               */
1615 /*                                                                      */
1616 /*    OUTPUTS                                                           */
1617 /*      none                                                            */
1618 /*                                                                      */
1619 /*    AUTHOR                                                            */
1620 /*      Aress Yang          ZyDAS Technology Corporation    2006.11     */
1621 /*                                                                      */
1622 /************************************************************************/
1623 void zfProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
1624 {
1625     /* Gather scan result */
1626     /* Parse TIM and send PS-POLL in power saving mode */
1627     struct zsWlanProbeRspFrameHeader*  pProbeRspHeader;
1628     struct zsBssInfo* pBssInfo;
1629     u8_t   pBuf[sizeof(struct zsWlanProbeRspFrameHeader)];
1630     int    res;
1631
1632     zmw_get_wlan_dev(dev);
1633
1634     zmw_declare_for_critical_section();
1635
1636     zfCopyFromRxBuffer(dev, buf, pBuf, 0,
1637                        sizeof(struct zsWlanProbeRspFrameHeader));
1638     pProbeRspHeader = (struct zsWlanProbeRspFrameHeader*) pBuf;
1639
1640     zmw_enter_critical_section(dev);
1641
1642     //zm_debug_msg1("bss count = ", wd->sta.bssList.bssCount);
1643
1644     pBssInfo = zfStaFindBssInfo(dev, buf, pProbeRspHeader);
1645
1646     //if ( i == wd->sta.bssList.bssCount )
1647     if ( pBssInfo == NULL )
1648     {
1649         /* Allocate a new entry if BSS not in the scan list */
1650         pBssInfo = zfBssInfoAllocate(dev);
1651         if (pBssInfo != NULL)
1652         {
1653             res = zfStaInitBssInfo(dev, buf, pProbeRspHeader, pBssInfo, AddInfo, 0);
1654             //zfDumpSSID(pBssInfo->ssid[1], &(pBssInfo->ssid[2]));
1655             if ( res != 0 )
1656             {
1657                 zfBssInfoFree(dev, pBssInfo);
1658             }
1659             else
1660             {
1661                 zfBssInfoInsertToList(dev, pBssInfo);
1662             }
1663         }
1664     }
1665     else
1666     {
1667         res = zfStaInitBssInfo(dev, buf, pProbeRspHeader, pBssInfo, AddInfo, 1);
1668         if (res == 2)
1669         {
1670             zfBssInfoRemoveFromList(dev, pBssInfo);
1671             zfBssInfoFree(dev, pBssInfo);
1672         }
1673         else if ( wd->wlanMode == ZM_MODE_IBSS )
1674         {
1675             int idx;
1676
1677             // It would reset the alive counter if the peer station is found!
1678             zfStaFindFreeOpposite(dev, (u16_t *)pBssInfo->macaddr, &idx);
1679         }
1680     }
1681
1682     zmw_leave_critical_section(dev);
1683
1684     return;
1685 }
1686
1687 /************************************************************************/
1688 /*                                                                      */
1689 /*    FUNCTION DESCRIPTION                  zfSendProbeReq              */
1690 /*      Send probe request management frame.                            */
1691 /*                                                                      */
1692 /*    INPUTS                                                            */
1693 /*      dev : device pointer                                            */
1694 /*                                                                      */
1695 /*                                                                      */
1696 /*    OUTPUTS                                                           */
1697 /*      none                                                            */
1698 /*                                                                      */
1699 /*    AUTHOR                                                            */
1700 /*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.11     */
1701 /*                                                                      */
1702 /************************************************************************/
1703
1704 u16_t zfSendProbeReq(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t bWithSSID)
1705 {
1706     zmw_get_wlan_dev(dev);
1707     zmw_declare_for_critical_section();
1708
1709
1710     /* SSID */
1711     if (bWithSSID == 0)  /* broadcast ssid */
1712     {
1713         //zmw_leave_critical_section(dev);
1714         zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
1715         zmw_tx_buf_writeb(dev, buf, offset++, 0);   /* length = 0 */
1716     }
1717     else
1718     {
1719         zmw_enter_critical_section(dev);
1720         if (wd->ws.probingSsidList[bWithSSID-1].ssidLen == 0)
1721         {
1722             zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
1723             zmw_tx_buf_writeb(dev, buf, offset++, 0);   /* length = 0 */
1724         }
1725         else
1726         {
1727             zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
1728             zmw_tx_buf_writeb(dev, buf, offset++,
1729                     wd->ws.probingSsidList[bWithSSID-1].ssidLen);
1730             zfCopyToIntTxBuffer(dev, buf,
1731                     wd->ws.probingSsidList[bWithSSID-1].ssid,
1732                     offset,
1733                     wd->ws.probingSsidList[bWithSSID-1].ssidLen); /* ssid */
1734             offset += wd->ws.probingSsidList[bWithSSID-1].ssidLen;
1735         }
1736         zmw_leave_critical_section(dev);
1737     }
1738
1739     /* Supported rates */
1740     if ( wd->sta.currentFrequency < 3000 )
1741     {   /* 802.11b+g */
1742         offset = zfMmAddIeSupportRate(dev, buf, offset,
1743                                       ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
1744
1745         if (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) {
1746             if (wd->wlanMode == ZM_MODE_IBSS) {
1747                 if (wd->wfc.bIbssGMode) {
1748                     offset = zfMmAddIeSupportRate(dev, buf, offset,
1749                                       ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
1750                 }
1751             } else {
1752                 offset = zfMmAddIeSupportRate(dev, buf, offset,
1753                                       ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
1754             }
1755         }
1756     }
1757     else
1758     {  /* 802.11a */
1759         offset = zfMmAddIeSupportRate(dev, buf, offset,
1760                                       ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
1761     }
1762
1763     return offset;
1764 }
1765
1766
1767 /************************************************************************/
1768 /*                                                                      */
1769 /*    FUNCTION DESCRIPTION                  zfUpdateDefaultQosParameter */
1770 /*      Update TxQs CWMIN, CWMAX, AIFS and TXO to WME default value.    */
1771 /*                                                                      */
1772 /*    INPUTS                                                            */
1773 /*      dev : device pointer                                            */
1774 /*      mode : 0=>STA, 1=>AP                                            */
1775 /*                                                                      */
1776 /*    OUTPUTS                                                           */
1777 /*      none                                                            */
1778 /*                                                                      */
1779 /*    AUTHOR                                                            */
1780 /*      Stephen             ZyDAS Technology Corporation    2006.6      */
1781 /*                                                                      */
1782 /************************************************************************/
1783 void zfUpdateDefaultQosParameter(zdev_t* dev, u8_t mode)
1784 {
1785     u16_t cwmin[5];
1786     u16_t cwmax[5];
1787     u16_t aifs[5];
1788     u16_t txop[5];
1789
1790     /* WMM parameter for STA */
1791     /* Best Effor */
1792     cwmin[0] = 15;
1793     cwmax[0] = 1023;
1794     aifs[0] = 3 * 9 + 10;
1795     txop[0] = 0;
1796     /* Back Ground */
1797     cwmin[1] = 15;
1798     cwmax[1] = 1023;
1799     aifs[1] = 7 * 9 + 10;
1800     txop[1] = 0;
1801     /* VIDEO */
1802     cwmin[2] = 7;
1803     cwmax[2] = 15;
1804     aifs[2] = 2 * 9 + 10;
1805     txop[2] = 94;
1806     /* VOICE */
1807     cwmin[3] = 3;
1808     cwmax[3] = 7;
1809     aifs[3] = 2 * 9 + 10;
1810     txop[3] = 47;
1811     /* Special TxQ */
1812     cwmin[4] = 3;
1813     cwmax[4] = 7;
1814     aifs[4] = 2 * 9 + 10;
1815     txop[4] = 0;
1816
1817     /* WMM parameter for AP */
1818     if (mode == 1)
1819     {
1820         cwmax[0] = 63;
1821         aifs[3] = 1 * 9 + 10;
1822         aifs[4] = 1 * 9 + 10;
1823     }
1824     zfHpUpdateQosParameter(dev, cwmin, cwmax, aifs, txop);
1825 }
1826
1827 u16_t zfFindATHExtCap(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype)
1828 {
1829     u8_t subType;
1830     u16_t offset;
1831     u16_t bufLen;
1832     u16_t elen;
1833     u8_t id;
1834     u8_t tmp;
1835
1836     /* Get offset of first element */
1837     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
1838
1839     offset = zgElementOffsetTable[subType];
1840     if (offset == 0xff)
1841     {
1842         zm_assert(0);
1843     }
1844
1845     /* Plus wlan header */
1846     offset += 24;
1847
1848     bufLen = zfwBufGetSize(dev, buf);
1849
1850     /* Search loop */
1851     while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
1852     {
1853         /* Search target element */
1854         id = zmw_rx_buf_readb(dev, buf, offset);
1855         if (id == ZM_WLAN_EID_WIFI_IE)
1856         {
1857             /* Bingo */
1858             elen = zmw_rx_buf_readb(dev, buf, offset+1);
1859             if (elen > bufLen - offset)
1860             {
1861                 /* Element length error */
1862                 return 0xffff;
1863             }
1864
1865             if ( elen == 0 )
1866             {
1867                 return 0xffff;
1868             }
1869
1870             if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
1871                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x03)
1872                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x7f)
1873                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+5)) == type))
1874
1875             {
1876                 if ( subtype != 0xff )
1877                 {
1878                     tmp = zmw_rx_buf_readb(dev, buf, offset+6);
1879                     if (tmp == subtype  )
1880                     {
1881                         return offset;
1882                     }
1883                 }
1884                 else
1885                 {
1886                     return offset;
1887                 }
1888             }
1889         }
1890
1891         /* Advance to next element */
1892         elen = zmw_rx_buf_readb(dev, buf, offset+1);
1893         if (elen == 0)
1894         {
1895             return 0xffff;
1896         }
1897         offset += (elen+2);
1898     }
1899     return 0xffff;
1900 }
1901
1902 u16_t zfFindBrdcmMrvlRlnkExtCap(zdev_t* dev, zbuf_t* buf)
1903 {
1904     u8_t subType;
1905     u16_t offset;
1906     u16_t bufLen;
1907     u16_t elen;
1908     u8_t id;
1909     u8_t tmp;
1910
1911     /* Get offset of first element */
1912     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
1913
1914     offset = zgElementOffsetTable[subType];
1915     if (offset == 0xff)
1916     {
1917         zm_assert(0);
1918     }
1919
1920     /* Plus wlan header */
1921     offset += 24;
1922
1923     bufLen = zfwBufGetSize(dev, buf);
1924
1925     /* Search loop */
1926     while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
1927     {
1928         /* Search target element */
1929         id = zmw_rx_buf_readb(dev, buf, offset);
1930         if (id == ZM_WLAN_EID_WIFI_IE)
1931         {
1932             /* Bingo */
1933             elen = zmw_rx_buf_readb(dev, buf, offset+1);
1934             if (elen > bufLen - offset)
1935             {
1936                 /* Element length error */
1937                 return 0xffff;
1938             }
1939
1940             if ( elen == 0 )
1941             {
1942                 return 0xffff;
1943             }
1944
1945             if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
1946                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x10)
1947                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x18))
1948
1949             {
1950                 return offset;
1951             }
1952             else if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
1953                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
1954                     && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x43))
1955
1956             {
1957                 return offset;
1958             }
1959         }
1960         else if ((id = zmw_rx_buf_readb(dev, buf, offset)) == 0x7F)
1961         {
1962             /* Bingo */
1963             elen = zmw_rx_buf_readb(dev, buf, offset+1);
1964             if (elen > bufLen - offset)
1965             {
1966                 /* Element length error */
1967                 return 0xffff;
1968             }
1969
1970             if ( elen == 0 )
1971             {
1972                 return 0xffff;
1973             }
1974
1975             tmp = zmw_rx_buf_readb(dev, buf, offset+2);
1976             if (tmp == 0x01)
1977
1978             {
1979                 return offset;
1980             }
1981         }
1982
1983         /* Advance to next element */
1984         elen = zmw_rx_buf_readb(dev, buf, offset+1);
1985         if (elen == 0)
1986         {
1987             return 0xffff;
1988         }
1989         offset += (elen+2);
1990     }
1991     return 0xffff;
1992 }
1993
1994 u16_t zfFindMarvelExtCap(zdev_t* dev, zbuf_t* buf)
1995 {
1996     u8_t subType;
1997     u16_t offset;
1998     u16_t bufLen;
1999     u16_t elen;
2000     u8_t id;
2001     u8_t tmp;
2002
2003     /* Get offset of first element */
2004     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
2005
2006     offset = zgElementOffsetTable[subType];
2007     if (offset == 0xff)
2008     {
2009         zm_assert(0);
2010     }
2011
2012     /* Plus wlan header */
2013     offset += 24;
2014
2015     bufLen = zfwBufGetSize(dev, buf);
2016
2017     /* Search loop */
2018     while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
2019     {
2020         /* Search target element */
2021         id = zmw_rx_buf_readb(dev, buf, offset);
2022         if (id == ZM_WLAN_EID_WIFI_IE)
2023         {
2024             /* Bingo */
2025             elen = zmw_rx_buf_readb(dev, buf, offset+1);
2026             if (elen>(bufLen - offset))
2027             {
2028                 /* Element length error */
2029                 return 0xffff;
2030             }
2031
2032             if ( elen == 0 )
2033             {
2034                 return 0xffff;
2035             }
2036
2037             if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
2038                   && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
2039                   && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x43))
2040
2041             {
2042                 return offset;
2043             }
2044         }
2045
2046         /* Advance to next element */
2047         elen = zmw_rx_buf_readb(dev, buf, offset+1);
2048         if (elen == 0)
2049         {
2050             return 0xffff;
2051         }
2052         offset += (elen+2);
2053     }
2054     return 0xffff;
2055 }
2056
2057 u16_t zfFindBroadcomExtCap(zdev_t* dev, zbuf_t* buf)
2058 {
2059     u8_t subType;
2060     u16_t offset;
2061     u16_t bufLen;
2062     u16_t elen;
2063     u8_t id;
2064     u8_t tmp;
2065
2066     /* Get offset of first element */
2067     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
2068
2069     offset = zgElementOffsetTable[subType];
2070     if (offset == 0xff)
2071     {
2072         zm_assert(0);
2073     }
2074
2075     /* Plus wlan header */
2076     offset += 24;
2077
2078     bufLen = zfwBufGetSize(dev, buf);
2079
2080     /* Search loop */
2081     while((offset+2) < bufLen)                   // including element ID and length (2bytes)
2082     {
2083         /* Search target element */
2084         id = zmw_rx_buf_readb(dev, buf, offset);
2085         if (id == ZM_WLAN_EID_WIFI_IE)
2086         {
2087             /* Bingo */
2088             elen = zmw_rx_buf_readb(dev, buf, offset+1);
2089             if (elen > (bufLen - offset))
2090             {
2091                 /* Element length error */
2092                 return 0xffff;
2093             }
2094
2095             if (elen == 0)
2096             {
2097                 return 0xffff;
2098             }
2099
2100             if ( ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
2101                  && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x10)
2102                  && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x18) )
2103             {
2104                 return offset;
2105             }
2106         }
2107
2108         /* Advance to next element */
2109         elen = zmw_rx_buf_readb(dev, buf, offset+1);
2110         if (elen == 0)
2111         {
2112             return 0xffff;
2113         }
2114
2115         offset += (elen+2);
2116     }
2117
2118     return 0xffff;
2119 }
2120
2121 u16_t zfFindRlnkExtCap(zdev_t* dev, zbuf_t* buf)
2122 {
2123     u8_t subType;
2124     u16_t offset;
2125     u16_t bufLen;
2126     u16_t elen;
2127     u8_t id;
2128     u8_t tmp;
2129
2130     /* Get offset of first element */
2131     subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
2132
2133     offset = zgElementOffsetTable[subType];
2134     if (offset == 0xff)
2135     {
2136         zm_assert(0);
2137     }
2138
2139     /* Plus wlan header */
2140     offset += 24;
2141
2142     bufLen = zfwBufGetSize(dev, buf);
2143
2144     /* Search loop */
2145     while((offset+2) < bufLen)                   // including element ID and length (2bytes)
2146     {
2147         /* Search target element */
2148         id = zmw_rx_buf_readb(dev, buf, offset);
2149         if (id == 0x7F)
2150         {
2151             /* Bingo */
2152             elen = zmw_rx_buf_readb(dev, buf, offset+1);
2153             if (elen > bufLen - offset)
2154             {
2155                 /* Element length error */
2156                 return 0xffff;
2157             }
2158
2159             if ( elen == 0 )
2160             {
2161                 return 0xffff;
2162             }
2163
2164             tmp = zmw_rx_buf_readb(dev, buf, offset+2);
2165             if (tmp == 0x01)
2166
2167             {
2168                 return offset;
2169             }
2170         }
2171
2172         /* Advance to next element */
2173         elen = zmw_rx_buf_readb(dev, buf, offset+1);
2174         if (elen == 0)
2175         {
2176             return 0xffff;
2177         }
2178
2179         offset += (elen+2);
2180     }
2181
2182     return 0xffff;
2183 }