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