LXR linux/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 */
  31const u8_t zg11bRateTbl[4] = {2, 4, 11, 22};
  32const u8_t zg11gRateTbl[8] = {12, 18, 24, 36, 48, 72, 96, 108};
  33
  34/* 0xff => element does not exist */
  35const 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/************************************************************************/
  73u16_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/************************************************************************/
 185u16_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
 253u16_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
 311u16_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
 339u16_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
 405u16_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/************************************************************************/
 484u16_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/************************************************************************/
 556u16_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/************************************************************************/
 591u16_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/************************************************************************/
 625u16_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/************************************************************************/
 663u16_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
 718u16_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/************************************************************************/
 771u16_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   */
 856void 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
1322zlError:
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/************************************************************************/
1346void 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/************************************************************************/
1510void 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/************************************************************************/
1605void 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
1686u16_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/************************************************************************/
1765void 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
1809u16_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
1879u16_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
1965u16_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
2024u16_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
2084u16_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}
2142