Merge branch 'linus' into x86/setup-lzma

Conflicts:
	init/do_mounts_rd.c

34 files changed, 35332 insertions, 0 deletions

diff --git a/drivers/staging/otus/80211core/amsdu.c b/drivers/staging/otus/80211core/amsdu.c
new file mode 100644
index 000000000000..c9123d58b82f
--- /dev/null
+++ b/drivers/staging/otus/80211core/amsdu.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetAmsduSubFrame          */
+/*      Get a subframe from a-MSDU.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : A-MSDU frame buffer                                       */
+/*      offset : offset of subframe in the A-MSDU                       */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      NULL or subframe                                                */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetAmsduSubFrame(zdev_t* dev, zbuf_t* buf, u16_t* offset)
+{
+    u16_t subframeLen;
+    u16_t amsduLen = zfwBufGetSize(dev, buf);
+    zbuf_t* newBuf;
+
+    ZM_PERFORMANCE_RX_AMSDU(dev, buf, amsduLen);
+
+    /* Verify A-MSDU length */
+    if (amsduLen < (*offset + 14))
+    {
+        return NULL;
+    }
+
+    /* Locate A-MSDU subframe by offset and verify subframe length */
+    subframeLen = (zmw_buf_readb(dev, buf, *offset + 12) << 8) +
+                  zmw_buf_readb(dev, buf, *offset + 13);
+    if (subframeLen == 0)
+    {
+        return NULL;
+    }
+
+    /* Verify A-MSDU subframe length */
+    if ((*offset+14+subframeLen) <= amsduLen)
+    {
+        /* Allocate a new buffer */
+        if ((newBuf = zfwBufAllocate(dev, 24+2+subframeLen)) != NULL)
+        {
+#ifdef ZM_ENABLE_NATIVE_WIFI
+            /* Copy and convert subframe to wlan frame format */
+            /* SHALL NOT INCLUDE QOS and AMSDU header. Ray 20070807 For Vista */
+            zfRxBufferCopy(dev, newBuf, buf, 0, 0, 24);
+            zfRxBufferCopy(dev, newBuf, buf, 24, *offset+14, subframeLen);
+            zfwBufSetSize(dev, newBuf, 24+subframeLen);
+#else
+            /* Copy subframe to new buffer */
+            zfRxBufferCopy(dev, newBuf, buf, 0, *offset, 14+subframeLen);
+            zfwBufSetSize(dev, newBuf, 14+subframeLen);
+#endif
+            /* Update offset */
+            *offset += (((14+subframeLen)+3) & 0xfffc);
+
+            /* Return buffer pointer */
+            return newBuf;
+        }
+    }
+    return NULL;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDeAmsdu                   */
+/*      De-AMSDU.                                                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : A-MSDU frame buffer                                       */
+/*      vap : VAP port                                                  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfDeAmsdu(zdev_t* dev, zbuf_t* buf, u16_t vap, u8_t encryMode)
+{
+    u16_t offset = ZM_SIZE_OF_WLAN_DATA_HEADER+ZM_SIZE_OF_QOS_CTRL;
+    zbuf_t* subframeBuf;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    if (encryMode == ZM_AES || encryMode == ZM_TKIP)
+    {
+        offset += (ZM_SIZE_OF_IV + ZM_SIZE_OF_EXT_IV);
+    }
+    else if (encryMode == ZM_WEP64 || encryMode == ZM_WEP128)
+    {
+        offset += ZM_SIZE_OF_IV;
+    }
+
+    /* Repeatly calling zfGetAmsduSubFrame() until NULL returned */
+    while ((subframeBuf = zfGetAmsduSubFrame(dev, buf, &offset)) != NULL)
+    {
+        wd->commTally.NotifyNDISRxFrmCnt++;
+        if (wd->zfcbRecvEth != NULL)
+    	{
+            wd->zfcbRecvEth(dev, subframeBuf, (u8_t)vap);
+            ZM_PERFORMANCE_RX_MSDU(dev, wd->tick);
+        }
+    }
+    zfwBufFree(dev, buf, 0);
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/cagg.c b/drivers/staging/otus/80211core/cagg.c
new file mode 100644
index 000000000000..4942190747a1
--- /dev/null
+++ b/drivers/staging/otus/80211core/cagg.c
@@ -0,0 +1,3611 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cagg.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains A-MPDU aggregation related functions.      */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#include "cprecomp.h"
+
+extern u8_t zcUpToAc[8];
+const u8_t pri[] = {3,3,2,3,2,1,3,2,1,0};
+
+
+u16_t aggr_count;
+u32_t success_mpdu;
+u32_t total_mpdu;
+
+void zfAggInit(zdev_t* dev)
+{
+    u16_t i,j;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+    /*
+     * reset sta information
+     */
+
+    zmw_enter_critical_section(dev);
+    wd->aggInitiated = 0;
+    wd->addbaComplete = 0;
+    wd->addbaCount = 0;
+    wd->reorder = 1;
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        for (j=0; j<ZM_AC; j++)
+        {
+            //wd->aggSta[i].aggQNumber[j] = ZM_AGG_POOL_SIZE;
+            wd->aggSta[i].aggFlag[j] = wd->aggSta[i].count[j] = 0;
+            wd->aggSta[i].tid_tx[j] = NULL;
+            wd->aggSta[i].tid_tx[j+1] = NULL;
+
+        }
+    }
+
+    /*
+     * reset Tx/Rx aggregation queue information
+     */
+    wd->aggState = 0;
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        /*
+         * reset tx aggregation queue
+         */
+        wd->aggQPool[i] = zfwMemAllocate(dev, sizeof(struct aggQueue));
+        if(!wd->aggQPool[i])
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+        wd->aggQPool[i]->aggHead = wd->aggQPool[i]->aggTail =
+        wd->aggQPool[i]->aggQEnabled = wd->aggQPool[i]->aggReady =
+        wd->aggQPool[i]->clearFlag = wd->aggQPool[i]->deleteFlag = 0;
+        //wd->aggQPool[i]->aggSize = 16;
+
+        /*
+         * reset rx aggregation queue
+         */
+        wd->tid_rx[i] = zfwMemAllocate(dev, sizeof(struct agg_tid_rx));
+        if (!wd->tid_rx[i])
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+        wd->tid_rx[i]->aid = ZM_MAX_STA_SUPPORT;
+        wd->tid_rx[i]->seq_start = wd->tid_rx[i]->baw_head = \
+        wd->tid_rx[i]->baw_tail = 0;
+        wd->tid_rx[i]->sq_exceed_count = wd->tid_rx[i]->sq_behind_count = 0;
+        for (j=0; j<=ZM_AGG_BAW_SIZE; j++)
+            wd->tid_rx[i]->frame[j].buf = 0;
+        /*
+         * reset ADDBA exchange status code
+         * 0: NULL
+         * 1: ADDBA Request sent/received
+         * 2: ACK for ADDBA Request sent/received
+         * 3: ADDBA Response sent/received
+         * 4: ACK for ADDBA Response sent/received
+         */
+        wd->tid_rx[i]->addBaExchangeStatusCode = 0;
+
+    }
+    zmw_leave_critical_section(dev);
+    zfAggTallyReset(dev);
+    DESTQ.init = zfAggDestInit;
+    DESTQ.init(dev);
+    wd->aggInitiated = 1;
+    aggr_count = 0;
+    success_mpdu = 0;
+    total_mpdu = 0;
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+    BAW = zfwMemAllocate(dev, sizeof(struct baw_enabler));
+    if(!BAW)
+    {
+        return;
+    }
+    BAW->init = zfBawInit;
+    BAW->init(dev);
+#endif //disable BAW
+#endif
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggGetSta                 */
+/*      return STA AID.                                                 */
+/*      take buf as input, use the dest address of buf as index to      */
+/*      search STA AID.                                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer for one particular packet                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      AID                                                             */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+
+
+
+u16_t zfAggGetSta(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t id;
+    u16_t dst[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+    dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+
+    zmw_enter_critical_section(dev);
+
+    if(wd->wlanMode == ZM_MODE_AP) {
+        id = zfApFindSta(dev, dst);
+    }
+    else {
+        id = 0;
+    }
+    zmw_leave_critical_section(dev);
+
+#if ZM_AGG_FPGA_DEBUG
+    id = 0;
+#endif
+
+    return id;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetQueue             */
+/*      return Queue Pool index.                                        */
+/*      take aid as input, look for the queue index associated          */
+/*      with this aid.                                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Queue number                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxGetQueue(zdev_t* dev, u16_t aid, u16_t tid)
+{
+    //u16_t   i;
+    TID_TX  tid_tx;
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    /*
+     * not a STA aid
+     */
+    if (0xffff == aid)
+        return NULL;
+
+    //zmw_enter_critical_section(dev);
+
+    tid_tx = wd->aggSta[aid].tid_tx[tid];
+    if (!tid_tx) return NULL;
+    if (0 == tid_tx->aggQEnabled)
+        return NULL;
+
+    //zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxNewQueue             */
+/*      return Queue Pool index.                                        */
+/*      take aid as input, find a new queue for this aid.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Queue number                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxNewQueue(zdev_t* dev, u16_t aid, u16_t tid, zbuf_t* buf)
+{
+    u16_t   i;
+    TID_TX  tid_tx=NULL;
+    u16_t   ac = zcUpToAc[tid&0x7] & 0x3;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * not a STA aid
+     */
+    if (0xffff == aid)
+        return NULL;
+
+    zmw_enter_critical_section(dev);
+
+    /*
+     * find one new queue for sta
+     */
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled)
+        {
+                /*
+                 * this q is enabled
+                 */
+        }
+        else
+        {
+            tid_tx = wd->aggQPool[i];
+            tid_tx->aggQEnabled = 1;
+            tid_tx->aggQSTA = aid;
+            tid_tx->ac = ac;
+            tid_tx->tid = tid;
+            tid_tx->aggHead = tid_tx->aggTail = tid_tx->size = 0;
+            tid_tx->aggReady = 0;
+            wd->aggSta[aid].tid_tx[tid] = tid_tx;
+            tid_tx->dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+            tid_tx->dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+            tid_tx->dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+            break;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxEnqueue              */
+/*      return Status code ZM_SUCCESS or error code                     */
+/*      take (aid,ac,qnum,buf) as input                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*      ac  : access category                                           */
+/*      qnum: the queue number to which will be enqueued                */
+/*      buf : the packet to be queued                                   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      status code                                                     */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxEnqueue(zdev_t* dev, zbuf_t* buf, u16_t aid, TID_TX tid_tx)
+{
+    //u16_t   qlen, frameLen;
+    u32_t   time;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+
+    if (tid_tx->size < (ZM_AGGQ_SIZE - 2))
+    {
+        /* Queue not full */
+
+
+        /*
+         * buffer copy
+         * in zfwBufFree will return a ndismsendcomplete
+         * to resolve the synchronize problem in aggregate
+         */
+
+        u8_t    sendComplete = 0;
+
+        tid_tx->aggvtxq[tid_tx->aggHead].buf = buf;
+        time = zm_agg_GetTime();
+        tid_tx->aggvtxq[tid_tx->aggHead].arrivalTime = time;
+        tid_tx->aggvtxq[tid_tx->aggHead].baw_retransmit = 0;
+
+        tid_tx->aggHead = ((tid_tx->aggHead + 1) & ZM_AGGQ_SIZE_MASK);
+        tid_tx->lastArrival = time;
+        tid_tx->size++;
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        if (buf && (tid_tx->size < (ZM_AGGQ_SIZE - 10))) {
+            tid_tx->complete = tid_tx->aggHead;
+            sendComplete = 1;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (!DESTQ.exist(dev, 0, tid_tx->ac, tid_tx, NULL)) {
+            DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+        }
+
+        zm_msg1_agg(ZM_LV_0, "tid_tx->size=", tid_tx->size);
+        //zm_debug_msg1("tid_tx->size=", tid_tx->size);
+
+        if (buf && sendComplete && wd->zfcbSendCompleteIndication) {
+            //zmw_leave_critical_section(dev);
+            wd->zfcbSendCompleteIndication(dev, buf);
+        }
+
+        /*if (tid_tx->size >= 16 && zfHpGetFreeTxdCount(dev) > 20)
+            zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+        */
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zm_msg1_agg(ZM_LV_0, "can't enqueue, tid_tx->size=", tid_tx->size);
+        /*
+         * Queue Full
+         */
+
+        /*
+         * zm_msg1_agg(ZM_LV_0, "Queue full, qnum = ", qnum);
+         * wd->commTally.txQosDropCount[ac]++;
+         * zfwBufFree(dev, buf, ZM_SUCCESS);
+         * zm_msg1_agg(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+         *
+         * return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+         */
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (!DESTQ.exist(dev, 0, tid_tx->ac, tid_tx, NULL)) {
+            DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+    }
+
+    return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+}
+
+u16_t    zfAggDestExist(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq) {
+    struct dest* dest;
+    u16_t   exist = 0;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (!DESTQ.Head[ac]) {
+        exist = 0;
+    }
+    else {
+        dest = DESTQ.Head[ac];
+        if (dest->tid_tx == tid_tx) {
+            exist = 1;
+        }
+        else {
+            while (dest->next != DESTQ.Head[ac]) {
+                dest = dest->next;
+                if (dest->tid_tx == tid_tx){
+                    exist = 1;
+                    break;
+                }
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return exist;
+}
+
+void    zfAggDestInsert(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq)
+{
+    struct dest* new_dest;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    new_dest = zfwMemAllocate(dev, sizeof(struct dest));
+    if(!new_dest)
+    {
+        return;
+    }
+    new_dest->Qtype = Qtype;
+    new_dest->tid_tx = tid_tx;
+    if (0 == Qtype)
+        new_dest->tid_tx = tid_tx;
+    else
+        new_dest->vtxq = vtxq;
+    if (!DESTQ.Head[ac]) {
+
+        zmw_enter_critical_section(dev);
+        new_dest->next = new_dest;
+        DESTQ.Head[ac] = DESTQ.dest[ac] = new_dest;
+        zmw_leave_critical_section(dev);
+    }
+    else {
+
+        zmw_enter_critical_section(dev);
+        new_dest->next = DESTQ.dest[ac]->next;
+        DESTQ.dest[ac]->next = new_dest;
+        zmw_leave_critical_section(dev);
+    }
+
+
+    //DESTQ.size[ac]++;
+    return;
+}
+
+void    zfAggDestDelete(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq)
+{
+    struct dest* dest, *temp;
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->destLock) {
+        zmw_leave_critical_section(dev);
+        return;
+    }
+
+
+    //zmw_declare_for_critical_section();
+    for (i=0; i<4; i++) {
+        if (!DESTQ.Head[i]) continue;
+        dest = DESTQ.Head[i];
+        if (!dest) continue;
+
+
+        while (dest && (dest->next != DESTQ.Head[i])) {
+            if (Qtype == 0 && dest->next->tid_tx == tid_tx){
+                break;
+            }
+            if (Qtype == 1 && dest->next->vtxq == vtxq) {
+                break;
+            }
+            dest = dest->next;
+        }
+
+        if ((Qtype == 0 && dest->next->tid_tx == tid_tx) || (Qtype == 1 && dest->next->vtxq == vtxq)) {
+
+            tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            if (tid_tx->size) {
+                zmw_leave_critical_section(dev);
+                return;
+            }
+            if (!DESTQ.Head[i]) {
+                temp = NULL;
+            }
+            else {
+                temp = dest->next;
+                if (temp == dest) {
+                    DESTQ.Head[i] = DESTQ.dest[i] = NULL;
+                    //DESTQ.size[i] = 0;
+                }
+                else {
+                    dest->next = dest->next->next;
+                }
+            }
+
+            if (temp == NULL)
+                {/* do nothing */} //zfwMemFree(dev, temp, sizeof(struct dest));
+            else
+                zfwMemFree(dev, temp, sizeof(struct dest));
+
+            /*zmw_enter_critical_section(dev);
+            if (DESTQ.size[i] > 0)
+                DESTQ.size[i]--;
+            zmw_leave_critical_section(dev);
+            */
+        }
+
+    }
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void    zfAggDestInit(zdev_t* dev)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    for (i=0; i<4; i++) {
+        //wd->destQ.Head[i].next = wd->destQ.Head[i];
+        //wd->destQ.dest[i] = wd->destQ.Head[i];
+        //DESTQ.size[i] = 0;
+        DESTQ.Head[i] = NULL;
+    }
+    DESTQ.insert  = zfAggDestInsert;
+    DESTQ.delete  = zfAggDestDelete;
+    DESTQ.init    = zfAggDestInit;
+    DESTQ.getNext = zfAggDestGetNext;
+    DESTQ.exist   = zfAggDestExist;
+    DESTQ.ppri = 0;
+    return;
+}
+
+struct dest* zfAggDestGetNext(zdev_t* dev, u16_t ac)
+{
+    struct dest *dest = NULL;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (DESTQ.dest[ac]) {
+        dest = DESTQ.dest[ac];
+        DESTQ.dest[ac] = DESTQ.dest[ac]->next;
+    }
+    else {
+        dest = NULL;
+    }
+    zmw_leave_critical_section(dev);
+
+    return dest;
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+u16_t   zfAggTidTxInsertHead(zdev_t* dev, struct bufInfo *buf_info,TID_TX tid_tx)
+{
+    zbuf_t* buf;
+    u32_t time;
+    struct baw_header *baw_header;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+
+    buf = buf_info->buf;
+
+    zmw_enter_critical_section(dev);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    zmw_leave_critical_section(dev);
+
+    if (tid_tx->size >= (ZM_AGGQ_SIZE - 2)) {
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+    tid_tx->aggTail = (tid_tx->aggTail == 0)? ZM_AGGQ_SIZE_MASK: tid_tx->aggTail - 1;
+    tid_tx->aggvtxq[tid_tx->aggTail].buf = buf;
+    //time = zm_agg_GetTime();
+    tid_tx->aggvtxq[tid_tx->aggTail].arrivalTime = buf_info->timestamp;
+    tid_tx->aggvtxq[tid_tx->aggTail].baw_retransmit = buf_info->baw_retransmit;
+
+    baw_header = &tid_tx->aggvtxq[tid_tx->aggTail].baw_header;
+    baw_header->headerLen   = buf_info->baw_header->headerLen;
+    baw_header->micLen      = buf_info->baw_header->micLen;
+    baw_header->snapLen     = buf_info->baw_header->snapLen;
+    baw_header->removeLen   = buf_info->baw_header->removeLen;
+    baw_header->keyIdx      = buf_info->baw_header->keyIdx;
+    zfwMemoryCopy((u8_t *)baw_header->header, (u8_t *)buf_info->baw_header->header, 58);
+    zfwMemoryCopy((u8_t *)baw_header->mic   , (u8_t *)buf_info->baw_header->mic   , 8);
+    zfwMemoryCopy((u8_t *)baw_header->snap  , (u8_t *)buf_info->baw_header->snap  , 8);
+
+    tid_tx->size++;
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    zmw_leave_critical_section(dev);
+
+    //tid_tx->lastArrival = time;
+    if (1 == tid_tx->size) {
+        DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+    }
+
+
+    zm_msg1_agg(ZM_LV_0, "0xC2:insertHead, tid_tx->size=", tid_tx->size);
+
+    return TRUE;
+}
+#endif //disable BAW
+#endif
+
+void    zfiTxComplete(zdev_t* dev)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    if( (wd->wlanMode == ZM_MODE_AP) ||
+        (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+        (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+        zfAggTxScheduler(dev, 0);
+    }
+
+    return;
+}
+
+TID_TX  zfAggTxReady(zdev_t* dev) {
+    //struct dest* dest;
+    u16_t   i;
+    TID_TX  tid_tx = NULL;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled)
+        {
+            if (wd->aggQPool[i]->size >= 16) {
+                tid_tx = wd->aggQPool[i];
+                break;
+            }
+        }
+        else {
+        }
+    }
+    zmw_leave_critical_section(dev);
+    return tid_tx;
+}
+
+u16_t   zfAggValidTidTx(zdev_t* dev, TID_TX tid_tx) {
+    u16_t   i, valid = 0;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i] == tid_tx)
+        {
+            valid = 1;
+            break;
+        }
+        else {
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return valid;
+}
+
+void    zfAggTxScheduler(zdev_t* dev, u8_t ScanAndClear)
+{
+    TID_TX  tid_tx = NULL;
+    void*   vtxq;
+    struct dest* dest;
+    zbuf_t*  buf;
+    u32_t txql, min_txql;
+    //u16_t aggr_size = 1;
+    u16_t txq_threshold;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (!wd->aggInitiated)
+    {
+        return;
+    }
+
+    /* debug */
+    txql = TXQL;
+    min_txql = AGG_MIN_TXQL;
+
+    if(wd->txq_threshold)
+        txq_threshold = wd->txq_threshold;
+    else
+        txq_threshold = AGG_MIN_TXQL;
+
+    tid_tx = zfAggTxReady(dev);
+    if (tid_tx) ScanAndClear = 0;
+    while (zfHpGetFreeTxdCount(dev) > 20 && (TXQL < txq_threshold || tid_tx)) {
+    //while (zfHpGetFreeTxdCount(dev) > 20 && (ScanAndClear || tid_tx)) {
+    //while (TXQL < txq_threshold) {
+        u16_t i;
+        u8_t ac;
+        s8_t destQ_count = 0;
+    //while ((zfHpGetFreeTxdCount(dev)) > 32) {
+
+        //DbgPrint("zfAggTxScheduler: in while loop");
+        for (i=0; i<4; i++) {
+            if (DESTQ.Head[i]) destQ_count++;
+        }
+        if (0 >= destQ_count) break;
+
+        zmw_enter_critical_section(dev);
+        ac = pri[DESTQ.ppri]; DESTQ.ppri = (DESTQ.ppri + 1) % 10;
+        zmw_leave_critical_section(dev);
+
+        for (i=0; i<10; i++){
+            if(DESTQ.Head[ac]) break;
+
+            zmw_enter_critical_section(dev);
+            ac = pri[DESTQ.ppri]; DESTQ.ppri = (DESTQ.ppri + 1) % 10;
+            zmw_leave_critical_section(dev);
+        }
+        if (i == 10) break;
+        //DbgPrint("zfAggTxScheduler: have dest Q");
+        zmw_enter_critical_section(dev);
+        wd->destLock = 1;
+        zmw_leave_critical_section(dev);
+
+        dest = DESTQ.getNext(dev, ac);
+        if (!dest) {
+            zmw_enter_critical_section(dev);
+            wd->destLock = 0;
+            zmw_leave_critical_section(dev);
+
+            DbgPrint("bug report! DESTQ.getNext got nothing!");
+            break;
+        }
+        if (dest->Qtype == 0) {
+            tid_tx = dest->tid_tx;
+
+            //DbgPrint("zfAggTxScheduler: have tid_tx Q");
+
+            if(tid_tx && zfAggValidTidTx(dev, tid_tx))
+                tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            else {
+                zmw_enter_critical_section(dev);
+                wd->destLock = 0;
+                zmw_leave_critical_section(dev);
+
+                tid_tx = zfAggTxReady(dev);
+                continue;
+            }
+
+            zmw_enter_critical_section(dev);
+            wd->destLock = 0;
+            zmw_leave_critical_section(dev);
+            //zmw_enter_critical_section(dev);
+            if (tid_tx && !tid_tx->size) {
+
+                //zmw_leave_critical_section(dev);
+                //DESTQ.delete(dev, 0, tid_tx, NULL);
+            }
+            else if(wd->aggState == 0){
+                //wd->aggState = 1;
+                //zmw_leave_critical_section(dev);
+                zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+                //wd->aggState = 0;
+            }
+            else {
+                //zmw_leave_critical_section(dev);
+                break;
+            }
+        }
+        else {
+            vtxq = dest->vtxq;
+            buf = zfGetVtxq(dev, ac);
+            zm_assert( buf != 0 );
+
+            zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+
+        }
+        /*flush all but < 16 frames in tid_tx to TXQ*/
+        tid_tx = zfAggTxReady(dev);
+    }
+
+    /*while ((zfHpGetFreeTxdCount(dev)) > 32) {
+    //while ((zfHpGetFreeTxdCount(dev)) > 32) {
+
+        destQ_count = 0;
+        for (i=0; i<4; i++) destQ_count += wd->destQ.size[i];
+        if (0 >= destQ_count) break;
+
+        ac = pri[wd->destQ.ppri]; wd->destQ.ppri = (wd->destQ.ppri + 1) % 10;
+        for (i=0; i<10; i++){
+            if(wd->destQ.size[ac]!=0) break;
+            ac = pri[wd->destQ.ppri]; wd->destQ.ppri = (wd->destQ.ppri + 1) % 10;
+        }
+        if (i == 10) break;
+        dest = wd->destQ.getNext(dev, ac);
+        if (dest->Qtype == 0) {
+            tid_tx = dest->tid_tx;
+            tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            if (!tid_tx->size) {
+                wd->destQ.delete(dev, 0, tid_tx, NULL);
+                break;
+            }
+            else if((wd->aggState == 0) && (tid_tx->size >= 16)){
+                zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+            }
+            else {
+                break;
+            }
+        }
+
+    }
+    */
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTx                     */
+/*      return Status code ZM_SUCCESS or error code                     */
+/*      management A-MPDU aggregation function,                         */
+/*      management aggregation queue, calculate arrivalrate,            */
+/*      add/delete an aggregation queue of a stream,                    */
+/*      enqueue packets into responsible aggregate queue.               */
+/*      take (dev, buf, ac) as input                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : packet buff                                               */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      status code                                                     */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTx(zdev_t* dev, zbuf_t* buf, u16_t tid)
+{
+    u16_t aid;
+    //u16_t qnum;
+    //u16_t aggflag = 0;
+    //u16_t arrivalrate = 0;
+    TID_TX tid_tx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if(!wd->aggInitiated)
+    {
+        return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+    }
+
+    aid = zfAggGetSta(dev, buf);
+
+    //arrivalrate = zfAggTxArrivalRate(dev, aid, tid);
+
+    if (0xffff == aid)
+    {
+        /*
+         * STA not associated, this is a BC/MC or STA->AP packet
+         */
+
+        return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+    }
+
+    /*
+     * STA associated, a unicast packet
+     */
+
+    tid_tx = zfAggTxGetQueue(dev, aid, tid);
+
+    /*tid_q.tid_tx = tid_tx;
+    wd->destQ.insert = zfAggDestInsert;
+    wd->destQ.insert(dev, 0, tid_q);
+    */
+    if (tid_tx != NULL)
+    {
+        /*
+         * this (aid, ac) is aggregated
+         */
+
+        //if (arrivalrate < ZM_AGG_LOW_THRESHOLD)
+        if (0)
+        {
+            /*
+             * arrival rate too low
+             * delete this aggregate queue
+             */
+
+            zmw_enter_critical_section(dev);
+
+            //wd->aggQPool[qnum]->clearFlag = wd->aggQPool[qnum]->deleteFlag =1;
+
+            zmw_leave_critical_section(dev);
+
+        }
+
+        return zfAggTxEnqueue(dev, buf, aid, tid_tx);
+
+    }
+    else
+    {
+        /*
+         * this (aid, ac) not yet aggregated
+         * queue not found
+         */
+
+        //if (arrivalrate > ZM_AGG_HIGH_THRESHOLD)
+        if (1)
+        {
+            /*
+             * arrivalrate high enough to get a new agg queue
+             */
+
+            tid_tx = zfAggTxNewQueue(dev, aid, tid, buf);
+
+            //zm_msg1_agg(ZM_LV_0, "get new AggQueue qnum = ", tid_tx->);
+
+            if (tid_tx)
+            {
+                /*
+                 * got a new aggregate queue
+                 */
+
+                //zmw_enter_critical_section(dev);
+
+                //wd->aggSta[aid].aggFlag[ac] = 1;
+
+                //zmw_leave_critical_section(dev);
+
+                /*
+                 * add ADDBA functions here
+                 * return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+                 */
+
+
+                //zfAggSendAddbaRequest(dev, tid_tx->dst, tid_tx->ac, tid_tx->tid);
+                //zmw_enter_critical_section(dev);
+
+                //wd->aggSta[aid].aggFlag[ac] = 0;
+
+                //zmw_leave_critical_section(dev);
+
+                return zfAggTxEnqueue(dev, buf, aid, tid_tx);
+
+            }
+            else
+            {
+                /*
+                 * just can't get a new aggregate queue
+                 */
+
+                return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+            }
+        }
+        else
+        {
+            /*
+             * arrival rate is not high enough to get a new agg queue
+             */
+
+            return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+        }
+    }
+
+
+
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxReadyCount           */
+/*      return counter of ready to aggregate queues.                    */
+/*      take (dev, ac) as input, only calculate the ready to aggregate  */
+/*      queues of one particular ac.                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      counter of ready to aggregate queues                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxReadyCount(zdev_t* dev, u16_t ac)
+{
+    u16_t i;
+    u16_t readycount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i=0 ; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled && (wd->aggQPool[i]->aggReady || \
+                wd->aggQPool[i]->clearFlag) && ac == wd->aggQPool[i]->ac)
+            readycount++;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return readycount;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxPartial              */
+/*      return the number that Vtxq has to send.                        */
+/*      take (dev, ac, readycount) as input, calculate the ratio of     */
+/*      Vtxq length to (Vtxq length + readycount) of a particular ac,   */
+/*      and returns the Vtxq length * the ratio                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*      readycount: the number of ready to aggregate queues of this ac  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Vtxq length * ratio                                             */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxPartial(zdev_t* dev, u16_t ac, u16_t readycount)
+{
+    u16_t qlen;
+    u16_t partial;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    qlen = zm_agg_qlen(dev, wd->vtxqHead[ac], wd->vtxqTail[ac]);
+
+    if ((qlen + readycount) > 0)
+    {
+        partial = (u16_t)( zm_agg_weight(ac) * ((u16_t)qlen/(qlen + \
+                        readycount)) );
+    }
+    else
+    {
+        partial = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (partial > qlen)
+        partial = qlen;
+
+    return partial;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxSend                 */
+/*      return sentcount                                                */
+/*      take (dev, ac, n) as input, n is the number of scheduled agg    */
+/*      queues to be sent of the particular ac.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*      n   : the number of scheduled aggregation queues to be sent     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      sentcount                                                       */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxSend(zdev_t* dev, u32_t freeTxd, TID_TX tid_tx)
+{
+    //u16_t   qnum;
+    //u16_t   qlen;
+    u16_t   j;
+    //u16_t   sentcount = 0;
+    zbuf_t* buf;
+    struct  aggControl aggControl;
+    u16_t   aggLen;
+    //zbuf_t*  newBuf;
+    //u16_t   bufLen;
+    //TID_BAW tid_baw = NULL;
+    //struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //while (tid_tx->size > 0)
+
+    zmw_enter_critical_section(dev);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    aggLen = zm_agg_min(16, zm_agg_min(tid_tx->size, (u16_t)(freeTxd - 2)));
+    zmw_leave_critical_section(dev);
+
+            /*
+             * why there have to be 2 free Txd?
+             */
+    if (aggLen <=0 )
+        return 0;
+
+
+    if (aggLen == 1) {
+        buf = zfAggTxGetVtxq(dev, tid_tx);
+        if (buf)
+            zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+        if (tid_tx->size == 0) {
+            //DESTQ.delete(dev, 0, tid_tx, NULL);
+        }
+
+        return 1;
+    }
+                /*
+                 * Free Txd queue is big enough to put aggregation
+                 */
+    zmw_enter_critical_section(dev);
+    if (wd->aggState == 1) {
+        zmw_leave_critical_section(dev);
+        return 0;
+    }
+    wd->aggState = 1;
+    zmw_leave_critical_section(dev);
+
+
+    zm_msg1_agg(ZM_LV_0, "aggLen=", aggLen);
+    tid_tx->aggFrameSize = 0;
+    for (j=0; j < aggLen; j++) {
+        buf = zfAggTxGetVtxq(dev, tid_tx);
+
+        zmw_enter_critical_section(dev);
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        zmw_leave_critical_section(dev);
+
+        if ( buf ) {
+            //struct aggTally *agg_tal;
+            u16_t completeIndex;
+
+            if (0 == j) {
+                aggControl.ampduIndication = ZM_AGG_FIRST_MPDU;
+
+            }
+            else if ((j == (aggLen - 1)) || tid_tx->size == 0)
+            {
+                aggControl.ampduIndication = ZM_AGG_LAST_MPDU;
+                //wd->aggState = 0;
+
+            }
+            else
+            {
+                aggControl.ampduIndication = ZM_AGG_MIDDLE_MPDU;
+                /* the packet is delayed more than 500 ms, drop it */
+
+            }
+            tid_tx->aggFrameSize += zfwBufGetSize(dev, buf);
+            aggControl.addbaIndication = 0;
+            aggControl.aggEnabled = 1;
+
+#ifdef ZM_AGG_TALLY
+            agg_tal = &wd->agg_tal;
+            agg_tal->sent_packets_sum++;
+
+#endif
+
+            zfAggTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0, &aggControl, tid_tx);
+
+            zmw_enter_critical_section(dev);
+            completeIndex = tid_tx->complete;
+            if(zm_agg_inQ(tid_tx, tid_tx->complete))
+                zm_agg_plus(tid_tx->complete);
+            zmw_leave_critical_section(dev);
+
+            if(zm_agg_inQ(tid_tx, completeIndex) && wd->zfcbSendCompleteIndication
+                    && tid_tx->aggvtxq[completeIndex].buf) {
+                wd->zfcbSendCompleteIndication(dev, tid_tx->aggvtxq[completeIndex].buf);
+                zm_debug_msg0("in queue complete worked!");
+            }
+
+        }
+        else {
+            /*
+             * this aggregation queue is empty
+             */
+            zm_msg1_agg(ZM_LV_0, "aggLen not reached, but no more frame, j=", j);
+
+            break;
+        }
+    }
+    zmw_enter_critical_section(dev);
+    wd->aggState = 0;
+    zmw_leave_critical_section(dev);
+
+    //zm_acquire_agg_spin_lock(Adapter);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    //zm_release_agg_spin_lock(Adapter);
+
+    if (tid_tx->size == 0) {
+        //DESTQ.delete(dev, 0, tid_tx, NULL);
+    }
+
+
+
+    //zfAggInvokeBar(dev, tid_tx);
+    if(j>0) {
+        aggr_count++;
+        zm_msg1_agg(ZM_LV_0, "0xC2:sent 1 aggr, aggr_count=", aggr_count);
+        zm_msg1_agg(ZM_LV_0, "0xC2:sent 1 aggr, aggr_size=", j);
+    }
+    return j;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetReadyQueue        */
+/*      return the number of the aggregation queue                      */
+/*      take (dev, ac) as input, find the agg queue with smallest       */
+/*      arrival time (waited longest) among those ready or clearFlag    */
+/*      set queues.                                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      aggregation queue number                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxGetReadyQueue(zdev_t* dev, u16_t ac)
+{
+    //u16_t       qnum = ZM_AGG_POOL_SIZE;
+    u16_t       i;
+    u32_t       time = 0;
+    TID_TX      tid_tx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i=0 ;i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (1 == wd->aggQPool[i]->aggQEnabled && ac == wd->aggQPool[i]->ac &&
+                (wd->aggQPool[i]->size > 0))
+        {
+            if (0 == time || time > wd->aggQPool[i]->aggvtxq[ \
+                            wd->aggQPool[i]->aggHead ].arrivalTime)
+            {
+                tid_tx = wd->aggQPool[i];
+                time = tid_tx->aggvtxq[ tid_tx->aggHead ].arrivalTime;
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetVtxq              */
+/*      return an MSDU                                                  */
+/*      take (dev, qnum) as input, return an MSDU out of the agg queue. */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      qnum: queue number                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      a MSDU                                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfAggTxGetVtxq(zdev_t* dev, TID_TX tid_tx)
+{
+    zbuf_t* buf = NULL;
+
+    zmw_declare_for_critical_section();
+
+    if (tid_tx->aggHead != tid_tx->aggTail)
+    {
+        buf = tid_tx->aggvtxq[ tid_tx->aggTail ].buf;
+
+        tid_tx->aggvtxq[tid_tx->aggTail].buf = NULL;
+
+        zmw_enter_critical_section(dev);
+        tid_tx->aggTail = ((tid_tx->aggTail + 1) & ZM_AGGQ_SIZE_MASK);
+        if(tid_tx->size > 0) tid_tx->size--;
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        if (NULL == buf) {
+            //tid_tx->aggTail = tid_tx->aggHead = tid_tx->size = 0;
+            //zm_msg1_agg(ZM_LV_0, "GetVtxq buf == NULL, tid_tx->size=", tid_tx->size);
+        }
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        /*
+         * queue is empty
+         */
+        zm_msg1_agg(ZM_LV_0, "tid_tx->aggHead == tid_tx->aggTail, tid_tx->size=", tid_tx->size);
+
+    }
+
+    if (zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail) != tid_tx->size)
+        zm_msg1_agg(ZM_LV_0, "qlen!=tid_tx->size! tid_tx->size=", tid_tx->size);
+    return buf;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxDeleteQueue          */
+/*      return ZM_SUCCESS (can't fail)                                  */
+/*      take (dev, qnum) as input, reset (delete) this aggregate queue, */
+/*      this queue is virtually returned to the aggregate queue pool.   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      qnum: queue number                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxDeleteQueue(zdev_t* dev, u16_t qnum)
+{
+    u16_t ac, tid;
+    struct aggQueue *tx_tid;
+    struct aggSta   *agg_sta;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    tx_tid = wd->aggQPool[qnum];
+    agg_sta = &wd->aggSta[tx_tid->aggQSTA];
+    ac = tx_tid->ac;
+    tid = tx_tid->tid;
+
+    zmw_enter_critical_section(dev);
+
+    tx_tid->aggQEnabled = 0;
+    tx_tid->aggHead = tx_tid->aggTail = 0;
+    tx_tid->aggReady = 0;
+    tx_tid->clearFlag = tx_tid->deleteFlag = 0;
+    tx_tid->size = 0;
+    agg_sta->count[ac] = 0;
+
+    agg_sta->tid_tx[tid] = NULL;
+    agg_sta->aggFlag[ac] = 0;
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg1_agg(ZM_LV_0, "queue deleted! qnum=", qnum);
+
+    return ZM_SUCCESS;
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+void zfBawCore(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen) {
+    TID_BAW tid_baw;
+    s16_t i;
+    zbuf_t* buf;
+    struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    tid_baw = BAW->getQ(dev, baw_seq);
+    //tid_baw = NULL;
+    if (NULL == tid_baw)
+        return;
+
+    total_mpdu += aggLen;
+    for (i = aggLen - 1; i>=0; i--) {
+        if (((bitmap >> i) & 0x1) == 0) {
+            buf_info = BAW->pop(dev, i, tid_baw);
+            buf = buf_info->buf;
+            if (buf) {
+                //wd->zfcbSetBawQ(dev, buf, 0);
+                zfAggTidTxInsertHead(dev, buf_info, tid_baw->tid_tx);
+            }
+        }
+        else {
+            success_mpdu++;
+        }
+    }
+    BAW->disable(dev, tid_baw);
+    zfAggTxScheduler(dev);
+    zm_debug_msg1("success_mpdu = ", success_mpdu);
+    zm_debug_msg1("  total_mpdu = ", total_mpdu);
+}
+
+void    zfBawInit(zdev_t* dev) {
+    TID_BAW tid_baw;
+    u16_t i,j;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        for (j=0; j<ZM_VTXQ_SIZE; j++) {
+            tid_baw->frame[j].buf = NULL;
+        }
+        tid_baw->enabled = tid_baw->head = tid_baw->tail = tid_baw->size = 0;
+        tid_baw->start_seq = 0;
+    }
+    BAW->delPoint = 0;
+    BAW->core = zfBawCore;
+    BAW->getNewQ = zfBawGetNewQ;
+    BAW->insert = zfBawInsert;
+    BAW->pop = zfBawPop;
+    BAW->enable = zfBawEnable;
+    BAW->disable = zfBawDisable;
+    BAW->getQ = zfBawGetQ;
+}
+
+
+
+TID_BAW zfBawGetNewQ(zdev_t* dev, u16_t start_seq, TID_TX tid_tx) {
+    TID_BAW tid_baw=NULL;
+    TID_BAW next_baw=NULL;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    /*
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        if (FALSE == tid_baw->enabled)
+            break;
+    }
+    */
+
+    tid_baw = &BAW->tid_baw[BAW->delPoint];
+    i = BAW->delPoint;
+    //if (ZM_BAW_POOL_SIZE == i) {
+        //return NULL;
+    //    u8_t temp = BAW->delPoint;
+    //    tid_baw = &BAW->tid_baw[BAW->delPoint];
+    //    BAW->disable(dev, tid_baw);
+    //    BAW->delPoint = (BAW->delPoint < (ZM_BAW_POOL_SIZE - 1))? (BAW->delPoint + 1): 0;
+    //    temp = BAW->delPoint;
+    //}
+
+    zm_msg1_agg(ZM_LV_0, "get new tid_baw, index=", i);
+    BAW->delPoint = (i < (ZM_BAW_POOL_SIZE -1))? (i + 1): 0;
+    next_baw = &BAW->tid_baw[BAW->delPoint];
+    if (1 == next_baw->enabled) BAW->disable(dev, next_baw);
+
+    BAW->enable(dev, tid_baw, start_seq);
+    tid_baw->tid_tx = tid_tx;
+
+    return tid_baw;
+}
+
+u16_t   zfBawInsert(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r) {
+    //TID_BAW tid_baw;
+    //u16_t   bufLen;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    if(tid_baw->size < (ZM_VTXQ_SIZE - 1)) {
+        struct baw_header *baw_header = &tid_baw->frame[tid_baw->head].baw_header;
+
+        baw_header->headerLen   = header_r->headerLen;
+        baw_header->micLen      = header_r->micLen;
+        baw_header->snapLen     = header_r->snapLen;
+        baw_header->removeLen   = header_r->removeLen;
+        baw_header->keyIdx      = header_r->keyIdx;
+        zfwMemoryCopy((u8_t *)baw_header->header, (u8_t *)header_r->header, 58);
+        zfwMemoryCopy((u8_t *)baw_header->mic   , (u8_t *)header_r->mic   , 8);
+        zfwMemoryCopy((u8_t *)baw_header->snap  , (u8_t *)header_r->snap  , 8);
+        //wd->zfcbSetBawQ(dev, buf, 1);
+        tid_baw->frame[tid_baw->head].buf = buf;
+        tid_baw->frame[tid_baw->head].baw_seq = baw_seq;
+        tid_baw->frame[tid_baw->head].baw_retransmit = baw_retransmit + 1;
+
+        //tid_baw->frame[tid_baw->head].data = pBuf->data;
+        tid_baw->head++;
+        tid_baw->size++;
+    }
+    else {
+        //wd->zfcbSetBawQ(dev, buf, 0);
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        return FALSE;
+    }
+    return TRUE;
+}
+
+struct bufInfo* zfBawPop(zdev_t* dev, u16_t index, TID_BAW tid_baw) {
+    //TID_BAW tid_baw;
+    //zbuf_t* buf;
+    struct bufInfo *buf_info;
+    zmw_get_wlan_dev(dev);
+
+    buf_info = &wd->buf_info;
+    buf_info->baw_header = NULL;
+
+    if (NULL == (buf_info->buf = tid_baw->frame[index].buf))
+        return buf_info;
+
+    buf_info->baw_retransmit = tid_baw->frame[index].baw_retransmit;
+    buf_info->baw_header = &tid_baw->frame[index].baw_header;
+    buf_info->timestamp = tid_baw->frame[index].timestamp;
+    //pBuf->data = pBuf->buffer;
+    //wd->zfcbRestoreBufData(dev, buf);
+    tid_baw->frame[index].buf = NULL;
+
+    return buf_info;
+}
+
+void    zfBawEnable(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq) {
+    //TID_BAW tid_baw;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    tid_baw->enabled = TRUE;
+    tid_baw->head = tid_baw->tail = tid_baw->size = 0;
+    tid_baw->start_seq = start_seq;
+}
+
+void    zfBawDisable(zdev_t* dev, TID_BAW tid_baw) {
+    //TID_BAW tid_baw;
+    u16_t i;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    for (i=0; i<ZM_VTXQ_SIZE; i++) {
+        if (tid_baw->frame[i].buf) {
+
+            //wd->zfcbSetBawQ(dev, tid_baw->frame[i].buf, 0);
+            zfwBufFree(dev, tid_baw->frame[i].buf, ZM_SUCCESS);
+            tid_baw->frame[i].buf = NULL;
+        }
+    }
+
+    tid_baw->enabled = FALSE;
+}
+
+TID_BAW zfBawGetQ(zdev_t* dev, u16_t baw_seq) {
+    TID_BAW tid_baw=NULL;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        if (TRUE == tid_baw->enabled)
+        {
+            zm_msg1_agg(ZM_LV_0, "get an old tid_baw, baw_seq=", baw_seq);
+            zm_msg1_agg(ZM_LV_0, "check a  tid_baw->start_seq=", tid_baw->start_seq);
+            if(baw_seq == tid_baw->start_seq)
+                break;
+        }
+
+    }
+    if (ZM_BAW_POOL_SIZE == i)
+        return NULL;
+    return tid_baw;
+}
+#endif //disable BAW
+#endif
+
+u16_t zfAggTallyReset(zdev_t* dev)
+{
+    struct aggTally* agg_tal;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    agg_tal = &wd->agg_tal;
+    agg_tal->got_packets_sum = 0;
+    agg_tal->got_bytes_sum = 0;
+    agg_tal->sent_bytes_sum = 0;
+    agg_tal->sent_packets_sum = 0;
+    agg_tal->avg_got_packets = 0;
+    agg_tal->avg_got_bytes = 0;
+    agg_tal->avg_sent_packets = 0;
+    agg_tal->avg_sent_bytes = 0;
+    agg_tal->time = 0;
+    return 0;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggScanAndClear           */
+/*      If the packets in a queue have waited for too long, clear and   */
+/*      delete this aggregation queue.                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev     : device pointer                                        */
+/*      time    : current time                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t   zfAggScanAndClear(zdev_t* dev, u32_t time)
+{
+    u16_t i;
+    u16_t head;
+    u16_t tail;
+    u32_t tick;
+    u32_t arrivalTime;
+    //u16_t aid, ac;
+    TID_TX tid_tx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if(!(wd->state == ZM_WLAN_STATE_ENABLED)) return 0;
+    zfAggTxScheduler(dev, 1);
+    tick = zm_agg_GetTime();
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (!wd->aggQPool[i]) return 0;
+        if (1 == wd->aggQPool[i]->aggQEnabled)
+        {
+            tid_tx = wd->aggQPool[i];
+            zmw_enter_critical_section(dev);
+
+            head = tid_tx->aggHead;
+            tail = tid_tx->aggTail;
+
+            arrivalTime = (u32_t)tid_tx->aggvtxq[tid_tx->aggTail].arrivalTime;
+
+
+            if((tick - arrivalTime) <= ZM_AGG_CLEAR_TIME)
+            {
+
+            }
+            else if((tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail)) > 0)
+            {
+
+                tid_tx->clearFlag = 1;
+
+                //zm_msg1_agg(ZM_LV_0, "clear queue    tick =", tick);
+                //zm_msg1_agg(ZM_LV_0, "clear queue arrival =", arrivalTime);
+
+
+                //zmw_leave_critical_section(dev);
+                //zfAggTxScheduler(dev);
+                //zmw_enter_critical_section(dev);
+
+            }
+
+            if (tid_tx->size == 0)
+            {
+                /*
+                 * queue empty
+                 */
+                if (tick - tid_tx->lastArrival > ZM_AGG_DELETE_TIME)
+                {
+                    zm_msg1_agg(ZM_LV_0, "delete queue, idle for n sec. n = ", \
+                            ZM_AGG_DELETE_TIME/10);
+
+                    zmw_leave_critical_section(dev);
+                    zfAggTxDeleteQueue(dev, i);
+                    zmw_enter_critical_section(dev);
+                }
+            }
+
+            zmw_leave_critical_section(dev);
+        }
+    }
+
+        zfAggRxClear(dev, time);
+
+#ifdef ZM_AGG_TALLY
+    if((wd->tick % 100) == 0) {
+        zfAggPrintTally(dev);
+    }
+#endif
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggPrintTally(zdev_t* dev)
+{
+    struct aggTally* agg_tal;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    agg_tal = &wd->agg_tal;
+
+    if(agg_tal->got_packets_sum < 10)
+    {
+        zfAggTallyReset(dev);
+        return 0;
+    }
+
+    agg_tal->time++;
+    agg_tal->avg_got_packets = (agg_tal->avg_got_packets * (agg_tal->time - 1) +
+            agg_tal->got_packets_sum) / agg_tal->time;
+    agg_tal->avg_got_bytes = (agg_tal->avg_got_bytes * (agg_tal->time - 1) +
+            agg_tal->got_bytes_sum) / agg_tal->time;
+    agg_tal->avg_sent_packets = (agg_tal->avg_sent_packets * (agg_tal->time - 1)
+            + agg_tal->sent_packets_sum) / agg_tal->time;
+    agg_tal->avg_sent_bytes = (agg_tal->avg_sent_bytes * (agg_tal->time - 1) +
+            agg_tal->sent_bytes_sum) / agg_tal->time;
+    zm_msg1_agg(ZM_LV_0, "got_packets_sum =", agg_tal->got_packets_sum);
+    zm_msg1_agg(ZM_LV_0, "  got_bytes_sum =", agg_tal->got_bytes_sum);
+    zm_msg1_agg(ZM_LV_0, "sent_packets_sum=", agg_tal->sent_packets_sum);
+    zm_msg1_agg(ZM_LV_0, " sent_bytes_sum =", agg_tal->sent_bytes_sum);
+    agg_tal->got_packets_sum = agg_tal->got_bytes_sum =agg_tal->sent_packets_sum
+                = agg_tal->sent_bytes_sum = 0;
+    zm_msg1_agg(ZM_LV_0, "avg_got_packets =", agg_tal->avg_got_packets);
+    zm_msg1_agg(ZM_LV_0, "  avg_got_bytes =", agg_tal->avg_got_bytes);
+    zm_msg1_agg(ZM_LV_0, "avg_sent_packets=", agg_tal->avg_sent_packets);
+    zm_msg1_agg(ZM_LV_0, " avg_sent_bytes =", agg_tal->avg_sent_bytes);
+    if ((wd->commTally.BA_Fail == 0) || (wd->commTally.Hw_Tx_MPDU == 0))
+    {
+        zm_msg1_agg(ZM_LV_0, "Hardware Tx MPDU=", wd->commTally.Hw_Tx_MPDU);
+        zm_msg1_agg(ZM_LV_0, "  BA Fail number=", wd->commTally.BA_Fail);
+    }
+    else
+        zm_msg1_agg(ZM_LV_0, "1/(BA fail rate)=", wd->commTally.Hw_Tx_MPDU/wd->commTally.BA_Fail);
+
+    return 0;
+}
+
+u16_t zfAggRxClear(zdev_t* dev, u32_t time)
+{
+    u16_t   i;
+    struct agg_tid_rx *tid_rx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        zmw_enter_critical_section(dev);
+        tid_rx = wd->tid_rx[i];
+        if (tid_rx->baw_head != tid_rx->baw_tail)
+        {
+            u16_t j = tid_rx->baw_tail;
+            while ((j != tid_rx->baw_head) && !tid_rx->frame[j].buf) {
+            	j = (j + 1) & ZM_AGG_BAW_MASK;
+            }
+            if ((j != tid_rx->baw_head) && (time - tid_rx->frame[j].arrivalTime) >
+                    (ZM_AGG_CLEAR_TIME - 5))
+            {
+                zmw_leave_critical_section(dev);
+                zm_msg0_agg(ZM_LV_1, "queue RxFlush by RxClear");
+                zfAggRxFlush(dev, 0, tid_rx);
+                zmw_enter_critical_section(dev);
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    return ZM_SUCCESS;
+}
+
+struct agg_tid_rx* zfAggRxEnabled(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   dst0, src[3], ac, aid, fragOff;
+    u8_t    up;
+    u16_t   offset = 0;
+    u16_t   seq_no;
+    u16_t frameType;
+    u16_t frameCtrl;
+    u16_t frameSubtype;
+    u32_t tcp_seq;
+    //struct aggSta *agg_sta;
+#if ZM_AGG_FPGA_REORDERING
+    struct agg_tid_rx *tid_rx;
+#endif
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    seq_no = zmw_rx_buf_readh(dev, buf, 22) >> 4;
+    //DbgPrint("Rx seq=%d\n", seq_no);
+    if (wd->sta.EnableHT == 0)
+    {
+        return NULL;
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 0);
+    frameType = frameCtrl & 0xf;
+    frameSubtype = frameCtrl & 0xf0;
+
+
+    if (frameType != ZM_WLAN_DATA_FRAME) //non-Qos Data? (frameSubtype&0x80)
+    {
+        return NULL;
+    }
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+    tcp_seq = zmw_rx_buf_readb(dev, buf, 22+36) << 24;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+37) << 16;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+38) << 8;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+39);
+#endif
+
+    ZM_SEQ_DEBUG("In                   %5d, %12u\n", seq_no, tcp_seq);
+    dst0 = zmw_rx_buf_readh(dev, buf, offset+4);
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+
+#if ZM_AGG_FPGA_DEBUG
+    aid = 0;
+#else
+    aid = zfApFindSta(dev, src);
+#endif
+
+    //agg_sta = &wd->aggSta[aid];
+    //zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+    //ac = zcUpToAc[up&0x7] & 0x3;
+
+    /*
+     * Filter unicast frame only, aid == 0 is for debug only
+     */
+    if ((dst0 & 0x1) == 0 && aid == 0)
+    {
+#if ZM_AGG_FPGA_REORDERING
+        tid_rx = zfAggRxGetQueue(dev, buf) ;
+        if(!tid_rx)
+            return NULL;
+        else
+        {
+            //if (tid_rx->addBaExchangeStatusCode == ZM_AGG_ADDBA_RESPONSE)
+            return tid_rx;
+        }
+#else
+        return NULL;
+#endif
+    }
+
+    return NULL;
+}
+
+u16_t zfAggRx(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo *addInfo, struct agg_tid_rx *tid_rx)
+{
+    u16_t   seq_no;
+    s16_t   index;
+    u16_t   offset = 0;
+    zbuf_t* pbuf;
+    u8_t    frameSubType;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    ZM_PERFORMANCE_RX_REORDER(dev);
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+
+    index = seq_no - tid_rx->seq_start;
+    /*
+     * for debug
+     */
+
+    /* zm_msg2_agg(ZM_LV_0, "queue seq = ", seq_no);
+     * DbgPrint("%s:%s%lxh %s%lxh\n", __func__, "queue seq=", seq_no,
+     *   "; seq_start=", tid_rx->seq_start);
+     */
+
+    //DbgPrint("seq_no=%d, seq_start=%d\n", seq_no, tid_rx->seq_start);
+
+    /* In some APs, we found that it might transmit NULL data whose sequence number
+       is out or order. In order to avoid this problem, we ignore these NULL data.
+     */
+
+    frameSubType = (zmw_rx_buf_readh(dev, buf, 0) & 0xF0) >> 4;
+
+    /* If this is a NULL data instead of Qos NULL data */
+    if ((frameSubType & 0x0C) == 0x04)
+    {
+        s16_t seq_diff;
+
+        seq_diff = (seq_no > tid_rx->seq_start) ?
+                       seq_no - tid_rx->seq_start : tid_rx->seq_start - seq_no;
+
+        if (seq_diff > ZM_AGG_BAW_SIZE)
+        {
+            zm_debug_msg0("Free Rx NULL data in zfAggRx");
+
+            /* Free Rx buffer */
+            zfwBufFree(dev, buf, 0);
+            return ZM_ERR_OUT_OF_ORDER_NULL_DATA;
+        }
+    }
+
+    /*
+     * sequence number wrap at 4k
+     */
+    if (tid_rx->seq_start > seq_no)
+    {
+        //index += 4096;
+
+        zmw_enter_critical_section(dev);
+        if (tid_rx->seq_start >= 4096) {
+            tid_rx->seq_start = 0;
+        }
+        zmw_leave_critical_section(dev);
+
+    }
+
+    if (tid_rx->seq_start == seq_no) {
+    	zmw_enter_critical_section(dev);
+    	if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) > 0) {
+    	    //DbgPrint("head=%d, tail=%d", tid_rx->baw_head, tid_rx->baw_tail);
+            tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        }
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+    	zmw_leave_critical_section(dev);
+
+        ZM_PERFORMANCE_RX_SEQ(dev, buf);
+
+    	if (wd->zfcbRecv80211 != NULL) {
+            //seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            //DbgPrint("1. seq=%d\n", seq_no);
+
+            wd->zfcbRecv80211(dev, buf, addInfo);
+        }
+        else {
+            zfiRecv80211(dev, buf, addInfo);
+        }
+    }
+    else if (!zfAggRxEnqueue(dev, buf, tid_rx, addInfo))
+    {
+        /*
+         * duplicated packet
+         */
+        return 1;
+    }
+
+    while (tid_rx->baw_head != tid_rx->baw_tail) {// && tid_rx->frame[tid_rx->baw_tail].buf)
+        u16_t tailIndex;
+
+        zmw_enter_critical_section(dev);
+
+        tailIndex = tid_rx->baw_tail;
+        pbuf = tid_rx->frame[tailIndex].buf;
+        tid_rx->frame[tailIndex].buf = 0;
+        if (!pbuf)
+        {
+            zmw_leave_critical_section(dev);
+            break;
+        }
+
+        tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+
+
+        //if(pbuf && tid_rx->baw_size > 0)
+        //    tid_rx->baw_size--;
+
+        zmw_leave_critical_section(dev);
+
+        ZM_PERFORMANCE_RX_SEQ(dev, pbuf);
+
+        if (wd->zfcbRecv80211 != NULL)
+        {
+            //seq_no = zmw_rx_buf_readh(dev, pbuf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            //DbgPrint("1. seq=%d\n", seq_no);
+            wd->zfcbRecv80211(dev, pbuf, addInfo);
+        }
+        else
+        {
+            //seq_no = zmw_rx_buf_readh(dev, pbuf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            zfiRecv80211(dev, pbuf, addInfo);
+        }
+    }
+
+    return 1;
+}
+
+struct agg_tid_rx *zfAggRxGetQueue(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   src[3];
+    u16_t   aid, ac, i;
+    u16_t   offset = 0;
+    struct agg_tid_rx *tid_rx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+    aid = zfApFindSta(dev, src);
+
+    ac = (zmw_rx_buf_readh(dev, buf, 24) & 0xF);
+
+    // mark by spin lock debug
+    //zmw_enter_critical_section(dev);
+
+    for (i=0; i<ZM_AGG_POOL_SIZE ; i++)
+    {
+        if((wd->tid_rx[i]->aid == aid) && (wd->tid_rx[i]->ac == ac))
+        {
+            tid_rx = wd->tid_rx[i];
+            break;
+        }
+    }
+
+    // mark by spin lock debug
+    //zmw_leave_critical_section(dev);
+    return tid_rx;
+}
+
+
+u16_t   zfAggRxEnqueue(zdev_t* dev, zbuf_t* buf, struct agg_tid_rx *tid_rx, struct zsAdditionInfo *addInfo)
+{
+    u16_t seq_no, offset = 0;
+    u16_t q_index;
+    s16_t index;
+    u8_t  bdropframe = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+    index  = seq_no - tid_rx->seq_start;
+
+    /*
+     * sequence number wrap at 4k
+     * -1000: check for duplicate past packet
+     */
+    bdropframe = 0;
+    if (tid_rx->seq_start > seq_no) {
+        if ((tid_rx->seq_start > 3967) && (seq_no < 128)) {
+            index += 4096;
+        } else if (tid_rx->seq_start - seq_no > 70) {
+            zmw_enter_critical_section(dev);
+            tid_rx->sq_behind_count++;
+            if (tid_rx->sq_behind_count > 3) {
+                tid_rx->sq_behind_count = 0;
+            } else {
+                bdropframe = 1;
+            }
+            zmw_leave_critical_section(dev);
+        } else {
+            bdropframe = 1;
+        }
+    } else {
+        if (seq_no - tid_rx->seq_start > 70) {
+            zmw_enter_critical_section(dev);
+            tid_rx->sq_exceed_count++;
+            if (tid_rx->sq_exceed_count > 3) {
+                tid_rx->sq_exceed_count = 0;
+            } else {
+                bdropframe = 1;
+            }
+            zmw_leave_critical_section(dev);
+        }
+    }
+
+    if (bdropframe == 1) {
+        /*if (wd->zfcbRecv80211 != NULL) {
+            wd->zfcbRecv80211(dev, buf, addInfo);
+        }
+        else {
+            zfiRecv80211(dev, buf, addInfo);
+        }*/
+
+        ZM_PERFORMANCE_FREE(dev, buf);
+
+        zfwBufFree(dev, buf, 0);
+        /*zfAggRxFlush(dev, seq_no, tid_rx);
+        tid_rx->seq_start = seq_no;
+        index = seq_no - tid_rx->seq_start;
+        */
+
+        //DbgPrint("Free an old packet, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+
+        /*
+         * duplicate past packet
+         * happens only in simulated aggregation environment
+         */
+        return 0;
+    } else {
+        zmw_enter_critical_section(dev);
+        if (tid_rx->sq_exceed_count > 0){
+            tid_rx->sq_exceed_count--;
+        }
+
+        if (tid_rx->sq_behind_count > 0) {
+            tid_rx->sq_behind_count--;
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    if (index < 0) {
+        zfAggRxFlush(dev, seq_no, tid_rx);
+        tid_rx->seq_start = seq_no;
+        index = 0;
+    }
+
+    //if (index >= (ZM_AGG_BAW_SIZE - 1))
+    if (index >= (ZM_AGG_BAW_MASK))
+    {
+        /*
+         * queue full
+         */
+        //DbgPrint("index >= 64, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+        zfAggRxFlush(dev, seq_no, tid_rx);
+        //tid_rx->seq_start = seq_no;
+        index = seq_no - tid_rx->seq_start;
+        if ((tid_rx->seq_start > seq_no) && (tid_rx->seq_start > 1000) && (tid_rx->seq_start - 1000) > seq_no)
+        {
+        //index = seq_no - tid_rx->seq_start;
+            index += 4096;
+        }
+        //index = seq_no - tid_rx->seq_start;
+        while (index >= (ZM_AGG_BAW_MASK)) {
+            //DbgPrint("index >= 64, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+            tid_rx->seq_start = (tid_rx->seq_start + ZM_AGG_BAW_MASK) & (4096 - 1);
+            index = seq_no - tid_rx->seq_start;
+            if ((tid_rx->seq_start > seq_no) && (tid_rx->seq_start > 1000) && (tid_rx->seq_start - 1000) > seq_no)
+            {
+                index += 4096;
+            }
+        }
+    }
+
+
+    q_index = (tid_rx->baw_tail + index) & ZM_AGG_BAW_MASK;
+    if (tid_rx->frame[q_index].buf && (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) >
+                (((q_index) - tid_rx->baw_tail) & ZM_AGG_BAW_MASK)))
+    {
+
+        ZM_PERFORMANCE_DUP(dev, tid_rx->frame[q_index].buf, buf);
+        zfwBufFree(dev, buf, 0);
+        //DbgPrint("Free a duplicate packet, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+        //DbgPrint("head=%d, tail=%d", tid_rx->baw_head, tid_rx->baw_tail);
+        /*
+         * duplicate packet
+         */
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+    if(tid_rx->frame[q_index].buf) {
+        zfwBufFree(dev, tid_rx->frame[q_index].buf, 0);
+        tid_rx->frame[q_index].buf = 0;
+    }
+
+    tid_rx->frame[q_index].buf = buf;
+    tid_rx->frame[q_index].arrivalTime = zm_agg_GetTime();
+    zfwMemoryCopy((void*)&tid_rx->frame[q_index].addInfo, (void*)addInfo, sizeof(struct zsAdditionInfo));
+
+    /*
+     * for debug simulated aggregation only,
+     * should be done in rx of ADDBA Request
+     */
+    //tid_rx->addInfo = addInfo;
+
+
+    if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) <= index)
+    {
+        //tid_rx->baw_size = index + 1;
+        if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) <=
+                //((q_index + 1) & ZM_AGG_BAW_MASK))
+                (((q_index) - tid_rx->baw_tail) & ZM_AGG_BAW_MASK))//tid_rx->baw_size )
+            tid_rx->baw_head = (q_index + 1) & ZM_AGG_BAW_MASK;
+    }
+    zmw_leave_critical_section(dev);
+
+    /*
+     * success
+     */
+    //DbgPrint("head=%d, tail=%d, start=%d", tid_rx->baw_head, tid_rx->baw_tail, tid_rx->seq_start);
+    return 1;
+}
+
+u16_t zfAggRxFlush(zdev_t* dev, u16_t seq_no, struct agg_tid_rx *tid_rx)
+{
+    zbuf_t* pbuf;
+    u16_t   seq;
+    struct zsAdditionInfo addInfo;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    ZM_PERFORMANCE_RX_FLUSH(dev);
+
+    while (1)
+    {
+        zmw_enter_critical_section(dev);
+        if (tid_rx->baw_tail == tid_rx->baw_head) {
+            zmw_leave_critical_section(dev);
+            break;
+        }
+
+        pbuf = tid_rx->frame[tid_rx->baw_tail].buf;
+        zfwMemoryCopy((void*)&addInfo, (void*)&tid_rx->frame[tid_rx->baw_tail].addInfo, sizeof(struct zsAdditionInfo));
+        tid_rx->frame[tid_rx->baw_tail].buf = 0;
+        //if(pbuf && tid_rx->baw_size > 0) tid_rx->baw_size--;
+        tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+	    zmw_leave_critical_section(dev);
+
+        if (pbuf)
+        {
+
+            ZM_PERFORMANCE_RX_SEQ(dev, pbuf);
+
+            if (wd->zfcbRecv80211 != NULL)
+            {
+                seq = zmw_rx_buf_readh(dev, pbuf, 22) >> 4;
+                //DbgPrint("Recv indicate seq=%d\n", seq);
+                //DbgPrint("2. seq=%d\n", seq);
+                wd->zfcbRecv80211(dev, pbuf, &addInfo);
+            }
+            else
+            {
+                seq = zmw_rx_buf_readh(dev, pbuf, 22) >> 4;
+                //DbgPrint("Recv indicate seq=%d\n", seq);
+                zfiRecv80211(dev, pbuf, &addInfo);
+            }
+        }
+    }
+
+    zmw_enter_critical_section(dev);
+    tid_rx->baw_head = tid_rx->baw_tail = 0;
+    zmw_leave_critical_section(dev);
+    return 1;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggRxFreeBuf              */
+/*      Frees all queued packets in buffer when the driver is down.     */
+/*      The zfFreeResource() will check if the buffer is all freed.     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev     : device pointer                                        */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t   zfAggRxFreeBuf(zdev_t* dev, u16_t destroy)
+{
+    u16_t   i;
+    zbuf_t* buf;
+    struct agg_tid_rx *tid_rx;
+
+    TID_TX  tid_tx;
+    //struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        u16_t j;
+
+        tid_rx = wd->tid_rx[i];
+
+        for(j=0; j <= ZM_AGG_BAW_SIZE; j++)
+        {
+            zmw_enter_critical_section(dev);
+            buf = tid_rx->frame[j].buf;
+            tid_rx->frame[j].buf = 0;
+            zmw_leave_critical_section(dev);
+
+            if (buf)
+            {
+                zfwBufFree(dev, buf, 0);
+            }
+        }
+
+        #if 0
+        if ( tid_rx->baw_head != tid_rx->baw_tail )
+        {
+            while (tid_rx->baw_head != tid_rx->baw_tail)
+            {
+                buf = tid_rx->frame[tid_rx->baw_tail].buf;
+                tid_rx->frame[tid_rx->baw_tail].buf = 0;
+                if (buf)
+                {
+                    zfwBufFree(dev, buf, 0);
+
+                    zmw_enter_critical_section(dev);
+                    tid_rx->frame[tid_rx->baw_tail].buf = 0;
+                    zmw_leave_critical_section(dev);
+                }
+                zmw_enter_critical_section(dev);
+                //if (tid_rx->baw_size > 0)tid_rx->baw_size--;
+                tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+                tid_rx->seq_start++;
+                zmw_leave_critical_section(dev);
+            }
+        }
+        #endif
+
+        zmw_enter_critical_section(dev);
+        tid_rx->seq_start = 0;
+        tid_rx->baw_head = tid_rx->baw_tail = 0;
+        tid_rx->aid = ZM_MAX_STA_SUPPORT;
+        zmw_leave_critical_section(dev);
+
+        #ifdef ZM_ENABLE_AGGREGATION
+        #ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+        if (tid_baw->enabled) {
+            zm_msg1_agg(ZM_LV_0, "Device down, clear BAW queue:", i);
+            BAW->disable(dev, tid_baw);
+        }
+        #endif
+        #endif
+        if (1 == wd->aggQPool[i]->aggQEnabled) {
+            tid_tx = wd->aggQPool[i];
+            buf = zfAggTxGetVtxq(dev, tid_tx);
+            while (buf) {
+                zfwBufFree(dev, buf, 0);
+                buf = zfAggTxGetVtxq(dev, tid_tx);
+            }
+        }
+
+        if(destroy) {
+            zfwMemFree(dev, wd->aggQPool[i], sizeof(struct aggQueue));
+            zfwMemFree(dev, wd->tid_rx[i], sizeof(struct agg_tid_rx));
+        }
+    }
+    #ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+    if(destroy) zfwMemFree(dev, BAW, sizeof(struct baw_enabler));
+    #endif
+    #endif
+    return ZM_SUCCESS;
+}
+
+
+void zfAggRecvBAR(zdev_t* dev, zbuf_t *buf) {
+    u16_t start_seq, len;
+    u8_t i, bitmap[8];
+    len = zfwBufGetSize(dev, buf);
+    start_seq = zmw_rx_buf_readh(dev, buf, len-2);
+    DbgPrint("Received a BAR Control frame, start_seq=%d", start_seq>>4);
+    /* todo: set the bitmap by reordering buffer! */
+    for (i=0; i<8; i++) bitmap[i]=0;
+    zfSendBA(dev, start_seq, bitmap);
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+void zfAggTxRetransmit(zdev_t* dev, struct bufInfo *buf_info, struct aggControl *aggControl, TID_TX tid_tx) {
+    u16_t removeLen;
+    u16_t err;
+
+    zmw_get_wlan_dev(dev);
+    if (aggControl && (ZM_AGG_FIRST_MPDU == aggControl->ampduIndication) ) {
+        tid_tx->bar_ssn = buf_info->baw_header->header[15];
+        aggControl->tid_baw->start_seq = tid_tx->bar_ssn >> 4;
+        zm_msg1_agg(ZM_LV_0, "start seq=", tid_tx->bar_ssn >> 4);
+    }
+    buf_info->baw_header->header[4] |= (1 << 11);
+    if (aggControl && aggControl->aggEnabled) {
+        //if (wd->enableAggregation==0 && !(buf_info->baw_header->header[6]&0x1))
+        //{
+            //if (((buf_info->baw_header->header[2] & 0x3) == 2))
+            //{
+                /* Enable aggregation */
+                buf_info->baw_header->header[1] |= 0x20;
+                if (ZM_AGG_LAST_MPDU == aggControl->ampduIndication) {
+                    buf_info->baw_header->header[1] |= 0x4000;
+                }
+                else {
+                    buf_info->baw_header->header[1] &= ~0x4000;
+                    //zm_debug_msg0("ZM_AGG_LAST_MPDU");
+                }
+            //}
+            //else {
+            //    zm_debug_msg1("no aggr, header[2]&0x3 = ",buf_info->baw_header->header[2] & 0x3)
+            //    aggControl->aggEnabled = 0;
+            //}
+        //}
+        //else {
+        //    zm_debug_msg1("no aggr, wd->enableAggregation = ", wd->enableAggregation);
+        //    zm_debug_msg1("no aggr, !header[6]&0x1 = ",!(buf_info->baw_header->header[6]&0x1));
+        //    aggControl->aggEnabled = 0;
+        //}
+    }
+
+    /*if (aggControl->tid_baw) {
+        struct baw_header_r header_r;
+
+        header_r.header      = buf_info->baw_header->header;
+        header_r.mic         = buf_info->baw_header->mic;
+        header_r.snap        = buf_info->baw_header->snap;
+        header_r.headerLen   = buf_info->baw_header->headerLen;
+        header_r.micLen      = buf_info->baw_header->micLen;
+        header_r.snapLen     = buf_info->baw_header->snapLen;
+        header_r.removeLen   = buf_info->baw_header->removeLen;
+        header_r.keyIdx      = buf_info->baw_header->keyIdx;
+
+        BAW->insert(dev, buf_info->buf, tid_tx->bar_ssn >> 4, aggControl->tid_baw, buf_info->baw_retransmit, &header_r);
+    }*/
+
+    if ((err = zfHpSend(dev,
+                    buf_info->baw_header->header,
+                    buf_info->baw_header->headerLen,
+                    buf_info->baw_header->snap,
+                    buf_info->baw_header->snapLen,
+                    buf_info->baw_header->mic,
+                    buf_info->baw_header->micLen,
+                    buf_info->buf,
+                    buf_info->baw_header->removeLen,
+                    ZM_EXTERNAL_ALLOC_BUF,
+                    (u8_t)tid_tx->ac,
+                    buf_info->baw_header->keyIdx)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+    zfwBufFree(dev, buf_info->buf, 0);
+    return;
+
+}
+#endif //disable BAW
+#endif
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxSendEth              */
+/*      Called to transmit Ethernet frame from upper elayer.            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x10-0x17=>VAP, 0x20-0x25=>WDS   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen, Honda      Atheros Communications, Inc.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u8_t flag, struct aggControl *aggControl, TID_TX tid_tx)
+{
+    u16_t err;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t removeLen;
+    u16_t header[(8+30+2+18)/2];    /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t headerLen;
+    u16_t mic[8/2];
+    u16_t micLen;
+    u16_t snap[8/2];
+    u16_t snapLen;
+    u16_t fragLen;
+    u16_t frameLen;
+    u16_t fragNum;
+    struct zsFrag frag;
+    u16_t i, id;
+    u16_t da[3];
+    u16_t sa[3];
+    u8_t up;
+    u8_t qosType, keyIdx = 0;
+    u16_t fragOff;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_msg1_tx(ZM_LV_2, "zfTxSendEth(), port=", port);
+
+    /* Get IP TOS for QoS AC and IP frag offset */
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 16);
+        da[1] = zmw_tx_buf_readh(dev, buf, 18);
+        da[2] = zmw_tx_buf_readh(dev, buf, 20);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 16);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 18);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else
+    {
+        //
+    }
+#else
+    /* DA */
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+    /* SA */
+    sa[0] = zmw_tx_buf_readh(dev, buf, 6);
+    sa[1] = zmw_tx_buf_readh(dev, buf, 8);
+    sa[2] = zmw_tx_buf_readh(dev, buf, 10);
+#endif
+    //Decide Key Index in ATOM, No meaning in OTUS--CWYang(m)
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        keyIdx = wd->ap.bcHalKeyIdx[port];
+        id = zfApFindSta(dev, da);
+        if (id != 0xffff)
+        {
+            switch (wd->ap.staTable[id].encryMode)
+            {
+            case ZM_AES:
+            case ZM_TKIP:
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+                keyIdx = wd->ap.staTable[id].keyIdx;
+                break;
+            }
+        }
+    }
+    else
+    {
+        switch (wd->sta.encryMode)
+        {
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            keyIdx = wd->sta.keyId;
+            break;
+        case ZM_AES:
+        case ZM_TKIP:
+            if ((da[0]& 0x1))
+                keyIdx = 5;
+            else
+                keyIdx = 4;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            keyIdx = wd->sta.cencKeyId;
+            break;
+#endif //ZM_ENABLE_CENC
+        }
+    }
+
+    /* Create SNAP */
+    removeLen = zfTxGenWlanSnap(dev, buf, snap, &snapLen);
+    //zm_msg1_tx(ZM_LV_0, "fragOff=", fragOff);
+
+    fragLen = wd->fragThreshold;
+    frameLen = zfwBufGetSize(dev, buf);
+    frameLen -= removeLen;
+
+#if 0
+    /* Create MIC */
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+         (wd->sta.encryMode == ZM_TKIP) )
+    {
+        if ( frameLen > fragLen )
+        {
+            micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic);
+        }
+        else
+        {
+            /* append MIC by HMAC */
+            micLen = 8;
+        }
+    }
+    else
+    {
+        micLen = 0;
+    }
+#else
+    if ( frameLen > fragLen )
+    {
+        micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic);
+    }
+    else
+    {
+        /* append MIC by HMAC */
+        micLen = 0;
+    }
+#endif
+
+    /* Access Category */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaQosType(dev, da, &qosType);
+        if (qosType == 0)
+        {
+            up = 0;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if (wd->sta.wmeConnected == 0)
+        {
+            up = 0;
+        }
+    }
+    else
+    {
+        /* TODO : STA QoS control field */
+        up = 0;
+    }
+
+    /* Assign sequence number */
+    zmw_enter_critical_section(dev);
+    frag.seq[0] = ((wd->seq[zcUpToAc[up&0x7]]++) << 4);
+    if (aggControl && (ZM_AGG_FIRST_MPDU == aggControl->ampduIndication) ) {
+        tid_tx->bar_ssn = frag.seq[0];
+
+        zm_msg1_agg(ZM_LV_0, "start seq=", tid_tx->bar_ssn >> 4);
+    }
+    //tid_tx->baw_buf[tid_tx->baw_head-1].baw_seq=frag.seq[0];
+    zmw_leave_critical_section(dev);
+
+
+        frag.buf[0] = buf;
+        frag.bufType[0] = bufType;
+        frag.flag[0] = flag;
+        fragNum = 1;
+
+    for (i=0; i<fragNum; i++)
+    {
+        /* Create WLAN header(Control Setting + 802.11 header + IV) */
+        if (up !=0 ) zm_debug_msg1("up not 0, up=",up);
+        headerLen = zfTxGenWlanHeader(dev, frag.buf[i], header, frag.seq[i],
+                                      frag.flag[i], snapLen+micLen, removeLen,
+                                      port, da, sa, up, &micLen, snap, snapLen,
+                                      aggControl);
+
+        /* Get buffer DMA address */
+        //if ((addrTblSize = zfwBufMapDma(dev, frag.buf[i], &addrTbl)) == 0)
+        //if ((addrTblSize = zfwMapTxDma(dev, frag.buf[i], &addrTbl)) == 0)
+        //{
+        //    err = ZM_ERR_BUFFER_DMA_ADDR;
+        //    goto zlError;
+        //}
+
+        /* Flush buffer on cache */
+        //zfwBufFlush(dev, frag.buf[i]);
+
+#if 0
+        zm_msg1_tx(ZM_LV_0, "headerLen=", headerLen);
+        zm_msg1_tx(ZM_LV_0, "snapLen=", snapLen);
+        zm_msg1_tx(ZM_LV_0, "micLen=", micLen);
+        zm_msg1_tx(ZM_LV_0, "removeLen=", removeLen);
+        zm_msg1_tx(ZM_LV_0, "addrTblSize=", addrTblSize);
+        zm_msg1_tx(ZM_LV_0, "frag.bufType[0]=", frag.bufType[0]);
+#endif
+
+        fragLen = zfwBufGetSize(dev, frag.buf[i]);
+        if ((da[0]&0x1) == 0)
+        {
+            wd->commTally.txUnicastFrm++;
+            wd->commTally.txUnicastOctets += (fragLen+snapLen);
+        }
+        else if ((da[0]& 0x1))
+        {
+            wd->commTally.txBroadcastFrm++;
+            wd->commTally.txBroadcastOctets += (fragLen+snapLen);
+        }
+        else
+        {
+            wd->commTally.txMulticastFrm++;
+            wd->commTally.txMulticastOctets += (fragLen+snapLen);
+        }
+        wd->ledStruct.txTraffic++;
+
+#if 0 //Who care this?
+        if ( (i)&&(i == (fragNum-1)) )
+        {
+            wd->trafTally.txDataByteCount -= micLen;
+        }
+#endif
+
+        /*if (aggControl->tid_baw && aggControl->aggEnabled) {
+            struct baw_header_r header_r;
+
+            header_r.header      = header;
+            header_r.mic         = mic;
+            header_r.snap        = snap;
+            header_r.headerLen   = headerLen;
+            header_r.micLen      = micLen;
+            header_r.snapLen     = snapLen;
+            header_r.removeLen   = removeLen;
+            header_r.keyIdx      = keyIdx;
+
+            BAW->insert(dev, buf, tid_tx->bar_ssn >> 4, aggControl->tid_baw, 0, &header_r);
+        }*/
+
+        if ((err = zfHpSend(dev, header, headerLen, snap, snapLen,
+                             mic, micLen, frag.buf[i], removeLen,
+                             frag.bufType[i], zcUpToAc[up&0x7], keyIdx)) != ZM_SUCCESS)
+        {
+            goto zlError;
+        }
+
+
+        continue;
+
+zlError:
+        if (frag.bufType[i] == ZM_EXTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, frag.buf[i], err);
+        }
+        else if (frag.bufType[i] == ZM_INTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, frag.buf[i], 0);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    } /* for (i=0; i<fragNum; i++) */
+
+    return ZM_SUCCESS;
+}
+
+/*
+ * zfAggSendADDBA() refers zfSendMmFrame() in cmm.c
+ */
+u16_t   zfAggSendAddbaRequest(zdev_t* dev, u16_t *dst, u16_t ac, u16_t up)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetAddbaFrameBody(dev, buf, offset, ac, up);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+    zfAggGenAddbaHeader(dev, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetAddbaFrameBody(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t ac, u16_t up)
+{
+    u16_t ba_parameter, start_seq;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * ADDBA Request frame body
+     */
+
+    /*
+     * Category
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 3);
+    /*
+     * Action details = 0
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_ADDBA_REQUEST_FRAME);
+    /*
+     * Dialog Token = nonzero
+     * TBD: define how to get dialog token?
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+    /*
+     * Block Ack parameter set
+     * BA policy = 1 for immediate BA, 0 for delayed BA
+     * TID(4bits) & buffer size(4bits) (TID=up & buffer size=0x80)
+     * TBD: how to get buffer size?
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0    ¢x    B1     ¢x B2  B5 ¢x B6      B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Reserved ¢x BA policy ¢x  TID   ¢x Buffer size ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    ba_parameter = 1 << 12;     // buffer size = 0x40(64)
+    ba_parameter |= up << 2;    // tid = up
+    ba_parameter |= 2;          // ba policy = 1
+    zmw_tx_buf_writeh(dev, buf, offset, ba_parameter);
+    offset+=2;
+    /*
+     * BA timeout value
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    offset+=2;
+    /*
+     * BA starting sequence number
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x B0       B3 ¢x B4              B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Frag num(0) ¢x BA starting seq num ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    start_seq = ((wd->seq[ac]) << 4) & 0xFFF0;
+    zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+    offset+=2;
+
+    return offset;
+}
+
+u16_t zfAggGenAddbaHeader(zdev_t* dev, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    u8_t  hlen = 32;        // MAC ctrl + PHY ctrl + 802.11 MM header
+    //u8_t frameType = ZM_WLAN_FRAME_TYPE_ACTION;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * Generate control setting
+     */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 24+len+4;   //Length
+    header[1] = 0x8;        //MAC control, backoff + (ack)
+
+#if 0
+    /* CCK 1M */
+    header[2] = 0x0f00;          //PHY control L
+    header[3] = 0x0000;          //PHY control H
+#else
+    /* OFDM 6M */
+    header[2] = 0x0f01;          //PHY control L
+    header[3] = 0x000B;          //PHY control H
+#endif
+
+    /*
+     * Generate WLAN header
+     * Frame control frame type and subtype
+     */
+    header[4+0] = ZM_WLAN_FRAME_TYPE_ACTION;
+    /*
+     * Duration
+     */
+    header[4+1] = 0;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[4+8] = 0;
+        header[4+9] = 0;
+        header[4+10] = 0;
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Address 3 = BSSID */
+        header[4+8] = wd->macAddr[0];
+        header[4+9] = wd->macAddr[1];
+        header[4+10] = wd->macAddr[2] + (vap<<8);
+    }
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        header[4+7] = wd->macAddr[2] + (vap<<8);
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+
+    return hlen;
+}
+
+
+u16_t   zfAggProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t category;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+
+    switch (category)
+    {
+    case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+        zfAggBlockAckActionFrame(dev, buf);
+        break;
+
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+u16_t   zfAggBlockAckActionFrame(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t action;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    action = zmw_rx_buf_readb(dev, buf, 25);
+#ifdef ZM_ENABLE_AGGREGATION
+    switch (action)
+    {
+    case ZM_WLAN_ADDBA_REQUEST_FRAME:
+        zm_msg0_agg(ZM_LV_0, "Received BA Action frame is ADDBA request");
+        zfAggRecvAddbaRequest(dev, buf);
+        break;
+    case ZM_WLAN_ADDBA_RESPONSE_FRAME:
+        zm_msg0_agg(ZM_LV_0, "Received BA Action frame is ADDBA response");
+        zfAggRecvAddbaResponse(dev, buf);
+        break;
+    case ZM_WLAN_DELBA_FRAME:
+        zfAggRecvDelba(dev, buf);
+        break;
+    }
+#endif
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggRecvAddbaRequest(zdev_t* dev, zbuf_t* buf)
+{
+    //u16_t dialog;
+    struct aggBaFrameParameter bf;
+    u16_t i;
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    bf.buf = buf;
+    bf.dialog = zmw_rx_buf_readb(dev, buf, 26);
+    /*
+     * ba parameter set
+     */
+    bf.ba_parameter = zmw_rx_buf_readh(dev, buf, 27);
+    bf.ba_policy   = (bf.ba_parameter >> 1) & 1;
+    bf.tid         = (bf.ba_parameter >> 2) & 0xF;
+    bf.buffer_size = (bf.ba_parameter >> 6);
+    /*
+     * BA timeout value
+     */
+    bf.ba_timeout = zmw_rx_buf_readh(dev, buf, 29);
+    /*
+     * BA starting sequence number
+     */
+    bf.ba_start_seq = zmw_rx_buf_readh(dev, buf, 31) >> 4;
+
+    i=26;
+    while(i < 32) {
+        zm_debug_msg2("Recv ADDBA Req:", zmw_rx_buf_readb(dev,buf,i));
+        i++;
+    }
+
+    zfAggSendAddbaResponse(dev, &bf);
+
+    zfAggAddbaSetTidRx(dev, buf, &bf);
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggAddbaSetTidRx(zdev_t* dev, zbuf_t* buf, struct aggBaFrameParameter *bf)
+{
+    u16_t i, ac, aid, fragOff;
+    u16_t src[3];
+    u16_t offset = 0;
+    u8_t  up;
+    struct agg_tid_rx *tid_rx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+    aid = zfApFindSta(dev, src);
+
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+    ac = zcUpToAc[up&0x7] & 0x3;
+
+    ac = bf->tid;
+
+    for (i=0; i<ZM_AGG_POOL_SIZE ; i++)
+    {
+        if((wd->tid_rx[i]->aid == aid) && (wd->tid_rx[i]->ac == ac))
+        {
+            tid_rx = wd->tid_rx[i];
+            break;
+        }
+    }
+
+    if (!tid_rx)
+    {
+        for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+        {
+            if (wd->tid_rx[i]->aid == ZM_MAX_STA_SUPPORT)
+            {
+                tid_rx = wd->tid_rx[i];
+                break;
+            }
+        }
+        if (!tid_rx)
+            return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    tid_rx->aid = aid;
+    tid_rx->ac = ac;
+    tid_rx->addBaExchangeStatusCode = ZM_AGG_ADDBA_RESPONSE;
+    tid_rx->seq_start = bf->ba_start_seq;
+    tid_rx->baw_head = tid_rx->baw_tail = 0;
+    tid_rx->sq_exceed_count = tid_rx->sq_behind_count = 0;
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+u16_t   zfAggRecvAddbaResponse(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t i,ac, aid=0;
+    u16_t src[3];
+    struct aggBaFrameParameter bf;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, 10);
+    src[1] = zmw_rx_buf_readh(dev, buf, 12);
+    src[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+        aid = zfApFindSta(dev, src);
+
+
+    bf.buf = buf;
+    bf.dialog = zmw_rx_buf_readb(dev, buf, 26);
+    bf.status_code = zmw_rx_buf_readh(dev, buf, 27);
+    if (!bf.status_code)
+    {
+        wd->addbaComplete=1;
+    }
+
+    /*
+     * ba parameter set
+     */
+    bf.ba_parameter = zmw_rx_buf_readh(dev, buf, 29);
+    bf.ba_policy   = (bf.ba_parameter >> 1) & 1;
+    bf.tid         = (bf.ba_parameter >> 2) & 0xF;
+    bf.buffer_size = (bf.ba_parameter >> 6);
+    /*
+     * BA timeout value
+     */
+    bf.ba_timeout = zmw_rx_buf_readh(dev, buf, 31);
+
+    i=26;
+    while(i < 32) {
+        zm_debug_msg2("Recv ADDBA Rsp:", zmw_rx_buf_readb(dev,buf,i));
+        i++;
+    }
+
+    ac = zcUpToAc[bf.tid&0x7] & 0x3;
+
+    //zmw_enter_critical_section(dev);
+
+    //wd->aggSta[aid].aggFlag[ac] = 0;
+
+    //zmw_leave_critical_section(dev);
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggRecvDelba(zdev_t* dev, zbuf_t* buf)
+{
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggSendAddbaResponse(zdev_t* dev, struct aggBaFrameParameter *bf)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+    u16_t dst[3];
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetAddbaResponseFrameBody(dev, buf, bf, offset);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+
+    dst[0] = zmw_rx_buf_readh(dev, bf->buf, 10);
+    dst[1] = zmw_rx_buf_readh(dev, bf->buf, 12);
+    dst[2] = zmw_rx_buf_readh(dev, bf->buf, 14);
+    zfAggGenAddbaHeader(dev, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    //zfAggSendAddbaRequest(dev, dst, zcUpToAc[bf->tid&0x7] & 0x3, bf->tid);
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetAddbaResponseFrameBody(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf, u16_t offset)
+{
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * ADDBA Request frame body
+     */
+
+    /*
+     * Category
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 3);
+    /*
+     * Action details = 0
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_ADDBA_RESPONSE_FRAME);
+    /*
+     * Dialog Token = nonzero
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, bf->dialog);
+    /*
+     * Status code
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    offset+=2;
+    /*
+     * Block Ack parameter set
+     * BA policy = 1 for immediate BA, 0 for delayed BA
+     * TID(4bits) & buffer size(4bits) (TID=0x1 & buffer size=0x80)
+     * TBD: how to get TID number and buffer size?
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0    ¢x    B1     ¢x B2  B5 ¢x B6      B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Reserved ¢x BA policy ¢x  TID   ¢x Buffer size ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, bf->ba_parameter);
+    offset+=2;
+    /*
+     * BA timeout value
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, bf->ba_timeout);
+    offset+=2;
+
+    return offset;
+}
+
+void   zfAggInvokeBar(zdev_t* dev, TID_TX tid_tx)
+{
+    struct aggBarControl aggBarControl;
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    //bar_control = aggBarControl->tid_info << 12 | aggBarControl->compressed_bitmap << 2
+    //        | aggBarControl->multi_tid << 1 | aggBarControl->bar_ack_policy;
+    aggBarControl.bar_ack_policy = 0;
+    aggBarControl.multi_tid = 0;
+    aggBarControl.compressed_bitmap = 0;
+    aggBarControl.tid_info = tid_tx->tid;
+    zfAggSendBar(dev, tid_tx, &aggBarControl);
+
+    return;
+
+}
+/*
+ * zfAggSendBar() refers zfAggSendAddbaRequest()
+ */
+u16_t   zfAggSendBar(zdev_t* dev, TID_TX tid_tx, struct aggBarControl *aggBarControl)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 16+8;  /* mac header + control headers*/
+    u16_t header[(8+24+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetBarBody(dev, buf, offset, tid_tx, aggBarControl);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+    zfAggGenBarHeader(dev, tid_tx->dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetBarBody(zdev_t* dev, zbuf_t* buf, u16_t offset, TID_TX tid_tx, struct aggBarControl *aggBarControl)
+{
+    u16_t bar_control, start_seq;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * BAR Control frame body
+     */
+
+    /*
+     * BAR Control Field
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0   ¢x    B1     ¢x     B2     ¢x B3   B11 ¢x B12  B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x BAR Ack ¢x Multi-TID ¢x Compressed ¢x Reserved ¢x TID_INFO ¢x
+     * ¢x  Policy ¢x           ¢x   Bitmap   ¢x          ¢x          ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    bar_control = aggBarControl->tid_info << 12 | aggBarControl->compressed_bitmap << 2
+            | aggBarControl->multi_tid << 1 | aggBarControl->bar_ack_policy;
+
+    zmw_tx_buf_writeh(dev, buf, offset, bar_control);
+    offset+=2;
+    if (0 == aggBarControl->multi_tid) {
+        /*
+         * BA starting sequence number
+         * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+         * ¢x B0       B3 ¢x B4              B15 ¢x
+         * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+         * ¢x Frag num(0) ¢x BA starting seq num ¢x
+         * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+         */
+        start_seq = (tid_tx->bar_ssn << 4) & 0xFFF0;
+        zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+        offset+=2;
+    }
+    if (1 == aggBarControl->multi_tid && 1 == aggBarControl->compressed_bitmap) {
+        /* multi-tid BlockAckReq variant, not implemented*/
+    }
+
+    return offset;
+}
+
+u16_t zfAggGenBarHeader(zdev_t* dev, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    u8_t  hlen = 16+8;        // MAC ctrl + PHY ctrl + 802.11 MM header
+    //u8_t frameType = ZM_WLAN_FRAME_TYPE_ACTION;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * Generate control setting
+     */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 16+len+4;   //Length
+    header[1] = 0x8;        //MAC control, backoff + (ack)
+
+#if 1
+    /* CCK 1M */
+    header[2] = 0x0f00;          //PHY control L
+    header[3] = 0x0000;          //PHY control H
+#else
+    /* CCK 6M */
+    header[2] = 0x0f01;          //PHY control L
+    header[3] = 0x000B;          //PHY control H
+
+#endif
+    /*
+     * Generate WLAN header
+     * Frame control frame type and subtype
+     */
+    header[4+0] = ZM_WLAN_FRAME_TYPE_BAR;
+    /*
+     * Duration
+     */
+    header[4+1] = 0;
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+7] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+7] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+
+    return hlen;
+}
diff --git a/drivers/staging/otus/80211core/cagg.h b/drivers/staging/otus/80211core/cagg.h
new file mode 100644
index 000000000000..1d87a564162b
--- /dev/null
+++ b/drivers/staging/otus/80211core/cagg.h
@@ -0,0 +1,435 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                          */
+/*  Module Name : cagg.h                                                    */
+/*                                                                          */
+/*  Abstract                                                                */
+/*      This module contains A-MPDU aggregation relatived functions.        */
+/*                                                                          */
+/*  NOTES                                                                   */
+/*      None                                                                */
+/*                                                                          */
+/****************************************************************************/
+/*Revision History:                                                         */
+/*    Who         When        What                                          */
+/*    --------    --------    ----------------------------------------------*/
+/*                                                                          */
+/*    Honda       12-4-06     created                                       */
+/*                                                                          */
+/****************************************************************************/
+
+#ifndef _CAGG_H
+#define _CAGG_H
+
+
+/*
+ * the aggregation functions flag, 0 if don't do aggregate
+ */
+
+#define ZM_AGG_FPGA_DEBUG                   1
+#define ZM_AGG_FPGA_REORDERING              1
+
+#ifndef ZM_AGG_TALLY
+//#define ZM_AGG_TALLY
+#endif
+/*
+ * Aggregate control
+ */
+
+
+#define ZM_AGG_POOL_SIZE                    20
+#define ZM_BAW_POOL_SIZE                    32
+#define ZM_AGGQ_SIZE                        64
+#define ZM_AGGQ_SIZE_MASK                   (ZM_AGGQ_SIZE-1)
+#define ZM_AGG_LOW_THRESHOLD                1
+#define ZM_AGG_HIGH_THRESHOLD               5
+
+/*
+ * number of access categories (ac)
+ */
+#define ZM_AC                               4
+/*
+ * the timer to clear aggregation queue, unit: 1 tick
+ * if the packet is too old (current time - arrival time)
+ * the packet and the aggregate queue will be cleared
+ */
+#define ZM_AGG_CLEAR_TIME                   10
+/*
+ * delete the queue if idle for ZM_DELETE_TIME
+ * unit: 10ms
+ */
+#define ZM_AGG_DELETE_TIME                  10000
+
+/*
+ * block ack window size
+ */
+#define ZM_AGG_BAW_SIZE                     64
+#define ZM_AGG_BAW_MASK                     (ZM_AGG_BAW_SIZE-1)
+/*
+ * originator     ADDBA Resquest    receiver
+ *      |----------------------------->|
+ *     1|          ACK                 |1
+ *      |<-----------------------------|
+ *     2|         ADDBA Response       |2
+ *      |<-----------------------------|
+ *     3|          ACK                 |3
+ *      |----------------------------->|
+ *     4                                4
+ */
+#define ZM_AGG_ADDBA_REQUEST                1
+#define ZM_AGG_ADDBA_REQUEST_ACK            2
+#define ZM_AGG_ADDBA_RESPONSE               3
+#define ZM_AGG_ADDBA_RESPONSE_ACK           4
+
+#define ZM_AGG_SINGLE_MPDU                  00
+#define ZM_AGG_FIRST_MPDU                   01
+#define ZM_AGG_MIDDLE_MPDU                  11
+#define ZM_AGG_LAST_MPDU                    10
+/*
+ * end of Aggregate control
+ */
+
+#define TID_TX  struct aggQueue*
+#define TID_BAW struct baw_q*
+#define BAW wd->baw_enabler
+#define DESTQ wd->destQ
+
+/*
+ * Queue access
+ */
+#define zm_agg_qlen(dev, head, tail) ((head - tail) & ZM_AGGQ_SIZE_MASK)
+#define zm_agg_inQ(tid_tx, pt) ((((pt - tid_tx->aggTail) & ZM_AGGQ_SIZE_MASK) < \
+        ((tid_tx->aggHead - tid_tx->aggTail) & ZM_AGGQ_SIZE_MASK))? TRUE:FALSE)
+#define zm_agg_plus(pt) pt = (pt + 1) & ZM_AGGQ_SIZE_MASK
+#define zm_agg_min(A, B) ((A>B)? B:A)
+#define zm_agg_GetTime() wd->tick
+#define TXQL (zfHpGetMaxTxdCount(dev) - zfHpGetFreeTxdCount(dev))
+
+/* don't change AGG_MIN_TXQL easily, this might cause BAW BSOD */
+#define AGG_MIN_TXQL                        2
+/*
+ * consider tcp,udp,ac(1234)
+ */
+#define zm_agg_dynamic_threshold(dev, ar)   ((ar > 16)? 11: \
+                                             (ar > 12)? 8: \
+                                             (ar > 8)? 5: \
+                                             (ar > 4)? 2:1)
+#define zm_agg_weight(ac)   ((3 == ac)? 4: \
+                             (2 == ac)? 3: \
+                             (0 == ac)? 2:1)
+/*
+ * the required free queue ratio per ac
+ */
+
+#define zm_agg_ratio(ac)    ((3 == ac)? 3: \
+                             (2 == ac)? (zfHpGetMaxTxdCount(dev)*1/4): \
+                             (0 == ac)? (zfHpGetMaxTxdCount(dev)*2/4): \
+                                        (zfHpGetMaxTxdCount(dev)*3/4))
+
+//#define zm_agg_ratio(ac)    3
+/*
+ * end of Queue access
+ */
+
+#define ZM_AGGMSG_LEV    ZM_LV_3
+#define zm_msg0_agg(lv, msg) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_agg(lv, msg, val) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_agg(lv, msg, val) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+struct baw_header_r {
+    u16_t       *header;
+    u16_t       *mic;
+    u16_t       *snap;
+    u16_t       headerLen;
+    u16_t       micLen;
+    u16_t       snapLen;
+    u16_t       removeLen;
+    u8_t        keyIdx;
+};
+
+struct baw_header {
+    u16_t       header[29];//[(8+30+2+18)/2];  58 bytes  /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t       headerLen;
+    u16_t       mic[4]; //[8/2]; 8 bytes
+    u16_t       micLen;
+    u16_t       snap[4]; //[8/2]; 8 bytes
+    u16_t       snapLen;
+    u16_t       removeLen;
+    u8_t        keyIdx;
+};
+
+struct bufInfo {
+    zbuf_t*     buf;
+    u8_t        baw_retransmit;
+    u32_t       timestamp;
+    struct baw_header   *baw_header;
+};
+#endif
+struct aggElement
+{
+    zbuf_t*     buf;
+    u32_t       arrivalTime;
+    u8_t        baw_retransmit;
+    struct zsAdditionInfo addInfo;
+    //struct baw_header  baw_header;
+};
+
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+struct baw_buf
+{
+    zbuf_t*     buf;
+    u16_t       baw_seq;
+    u32_t       timestamp;
+    u8_t        baw_retransmit;
+    struct baw_header baw_header;
+};
+
+struct baw_q {
+    struct baw_buf  frame[ZM_VTXQ_SIZE];
+    u16_t       enabled;
+    u16_t       start_seq;
+    u16_t       head;
+    u16_t       tail;
+    u16_t       size;
+    TID_TX      tid_tx;
+
+    //struct baw_header *baw_header;
+};
+
+struct baw_enabler
+{
+    struct baw_q    tid_baw[ZM_BAW_POOL_SIZE];
+    u8_t    delPoint;
+    void    (*core)(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen);
+    //void    (*core);
+    void    (*init)(zdev_t* dev);
+    TID_BAW (*getNewQ)(zdev_t* dev, u16_t start_seq, TID_TX tid_tx);
+    TID_BAW (*getQ)(zdev_t* dev, u16_t baw_seq);
+    u16_t   (*insert)(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r);
+    struct bufInfo* (*pop)(zdev_t* dev, u16_t index, TID_BAW tid_baw);
+    void    (*enable)(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq);
+    void    (*disable)(zdev_t* dev, TID_BAW tid_baw);
+
+};
+#endif
+struct aggQueue
+{
+    struct      aggElement  aggvtxq[ZM_AGGQ_SIZE];
+    u16_t       aggHead;
+    u16_t       aggTail;
+    s16_t       size;
+    u16_t       aggQSTA;
+    u16_t       aggQEnabled;
+    u16_t       ac;
+    u16_t       tid;
+    u16_t       aggReady;
+    u16_t       clearFlag;
+    u16_t       deleteFlag;
+    u32_t       lastArrival;
+    u16_t       aggFrameSize;
+    u16_t       bar_ssn;    /* starting sequence number in BAR */
+    u16_t       dst[3];
+    u16_t       complete;     /* complete indication pointer */
+};
+
+struct aggSta
+{
+    u16_t       count[ZM_AC];
+    TID_TX      tid_tx[8];
+    u16_t       aggFlag[ZM_AC];
+};
+
+struct agg_tid_rx
+{
+    u16_t       aid;
+    u16_t       ac;
+    u16_t       addBaExchangeStatusCode;
+    //struct zsAdditionInfo *addInfo;
+    u16_t       seq_start;		/* first seq expected next */
+    u16_t       baw_head;		/* head of valid block ack window */
+    u16_t       baw_tail;		/* tail of valid block ack window */
+    //u16_t       free_count;		/* block ack window size	*/
+    u8_t        sq_exceed_count;
+    u8_t        sq_behind_count;
+    struct aggElement frame[ZM_AGG_BAW_SIZE + 1]; /* out-of-order rx frames */
+};
+
+struct aggControl
+{
+    u16_t       aggEnabled;
+    u16_t       ampduIndication;
+    u16_t       addbaIndication;
+    //TID_BAW     tid_baw;
+    u32_t       timestamp;
+};
+
+struct aggBaFrameParameter
+{
+    zbuf_t*     buf;
+    u16_t       ba_parameter;
+    u8_t        dialog;
+    u16_t       ba_policy;
+    u16_t       tid;
+    u16_t       buffer_size;
+    u16_t       ba_timeout;
+    u16_t       ba_start_seq;
+    u16_t       status_code;
+};
+
+struct aggBarControl
+{
+    u16_t       bar_ack_policy      ;
+    u16_t       multi_tid           ;
+    u16_t       compressed_bitmap   ;
+    u16_t       tid_info            ;
+};
+
+struct aggTally
+{
+    u32_t       got_packets_sum;
+    u32_t       got_bytes_sum;
+    u32_t       sent_packets_sum;
+    u32_t       sent_bytes_sum;
+    u32_t       avg_got_packets;
+    u32_t       avg_got_bytes;
+    u32_t       avg_sent_packets;
+    u32_t       avg_sent_bytes;
+    u16_t       time;
+};
+
+
+struct destQ {
+    struct dest{
+        u16_t   Qtype : 1; /* 0 aggr, 1 vtxq */
+        TID_TX  tid_tx;
+        void*   vtxq;
+
+        struct dest* next;
+    } *dest[4];
+    struct dest* Head[4];
+    //s16_t   size[4];
+    u16_t   ppri;
+    void    (*insert)(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+    void    (*delete)(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq);
+    void    (*init)(zdev_t* dev);
+    struct dest* (*getNext)(zdev_t* dev, u16_t ac);
+    u16_t   (*exist)(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+    //void    (*scan)(zdev_t* dev);
+};
+/*
+ * aggregation tx
+ */
+void    zfAggInit(zdev_t* dev);
+u16_t   zfApFindSta(zdev_t* dev, u16_t* addr);
+u16_t   zfAggGetSta(zdev_t* dev, zbuf_t* buf);
+TID_TX  zfAggTxGetQueue(zdev_t* dev, u16_t aid, u16_t tid);
+TID_TX  zfAggTxNewQueue(zdev_t* dev, u16_t aid, u16_t tid, zbuf_t* buf);
+u16_t   zfAggTxEnqueue(zdev_t* dev, zbuf_t* buf, u16_t aid, TID_TX tid_tx);
+u16_t   zfAggTx(zdev_t* dev, zbuf_t* buf, u16_t tid);
+u16_t   zfAggTxReadyCount(zdev_t* dev, u16_t ac);
+u16_t   zfAggTxPartial(zdev_t* dev, u16_t ac, u16_t readycount);
+u16_t   zfAggTxSend(zdev_t* dev, u32_t freeTxd, TID_TX tid_tx);
+TID_TX  zfAggTxGetReadyQueue(zdev_t* dev, u16_t ac);
+zbuf_t* zfAggTxGetVtxq(zdev_t* dev, TID_TX tid_tx);
+u16_t   zfAggTxDeleteQueue(zdev_t* dev, u16_t qnum);
+u16_t   zfAggScanAndClear(zdev_t* dev, u32_t time);
+u16_t   zfAggClearQueue(zdev_t* dev);
+void    zfAggTxScheduler(zdev_t* dev, u8_t ScanAndClear);
+
+/* tid_tx manipulation */
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+u16_t   zfAggTidTxInsertHead(zdev_t* dev, struct bufInfo* buf_info, TID_TX tid_tx);
+#endif
+void    zfAggDestInsert(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+void    zfAggDestDelete(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq);
+void    zfAggDestInit(zdev_t* dev);
+struct dest* zfAggDestGetNext(zdev_t* dev, u16_t ac);
+u16_t   zfAggDestExist(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+/*
+ * aggregation rx
+ */
+struct agg_tid_rx *zfAggRxEnabled(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRx(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo *addInfo, struct agg_tid_rx *tid_rx);
+struct agg_tid_rx *zfAggRxGetQueue(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRxEnqueue(zdev_t* dev, zbuf_t* buf, struct agg_tid_rx *tid_rx, struct zsAdditionInfo *addInfo);
+u16_t   zfAggRxFlush(zdev_t* dev, u16_t seq_no, struct agg_tid_rx *tid_rx);
+u16_t   zfAggRxFreeBuf(zdev_t* dev, u16_t destroy);
+u16_t   zfAggRxClear(zdev_t* dev, u32_t time);
+void    zfAggRecvBAR(zdev_t* dev, zbuf_t* buf);
+/*
+ * end of aggregation rx
+ */
+
+/*
+ * ADDBA
+ */
+u16_t   zfAggSendAddbaRequest(zdev_t* dev, u16_t *dst, u16_t ac, u16_t up);
+u16_t   zfAggSetAddbaFrameBody(zdev_t* dev,zbuf_t* buf, u16_t offset, u16_t ac, u16_t up);
+u16_t   zfAggGenAddbaHeader(zdev_t* dev, u16_t* dst,
+                u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+u16_t   zfAggProcessAction(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggBlockAckActionFrame(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvAddbaRequest(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvAddbaResponse(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvDelba(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggSendAddbaResponse(zdev_t* dev, struct aggBaFrameParameter *bf);
+u16_t   zfAggSetAddbaResponseFrameBody(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf, u16_t offset);
+u16_t   zfAggAddbaSetTidRx(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf);
+/*
+ * zfAggTxSendEth
+ */
+u16_t zfAggTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u8_t flag, struct aggControl *aggControl, TID_TX tid_tx);
+
+/*
+ * statistics functions
+ */
+u16_t zfAggTallyReset(zdev_t* dev);
+
+u16_t   zfAggPrintTally(zdev_t* dev);
+
+/*
+ * BAR
+ */
+void    zfAggInvokeBar(zdev_t* dev, TID_TX tid_tx);
+u16_t   zfAggSendBar(zdev_t* dev, TID_TX tid_tx, struct aggBarControl *aggBarControl);
+u16_t   zfAggSetBarBody(zdev_t* dev, zbuf_t* buf, u16_t offset, TID_TX tid_tx, struct aggBarControl *aggBarControl);
+u16_t   zfAggGenBarHeader(zdev_t* dev, u16_t* dst,
+                u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+/* BAW BA retransmission */
+void    zfBawCore(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen);
+void    zfBawInit(zdev_t* dev);
+TID_BAW zfBawGetNewQ(zdev_t* dev, u16_t start_seq, TID_TX tid_tx);
+u16_t   zfBawInsert(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r);
+struct bufInfo* zfBawPop(zdev_t* dev, u16_t index, TID_BAW tid_baw);
+void    zfBawEnable(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq);
+void    zfBawDisable(zdev_t* dev, TID_BAW tid_baw);
+TID_BAW zfBawGetQ(zdev_t* dev, u16_t baw_seq);
+void zfAggTxRetransmit(zdev_t* dev, struct bufInfo *buf_info, struct aggControl *aggControl, TID_TX tid_tx);
+#endif
+/* extern functions */
+extern zbuf_t* zfGetVtxq(zdev_t* dev, u8_t ac);
+
+#endif /* #ifndef _CAGG_H */
+
diff --git a/drivers/staging/otus/80211core/ccmd.c b/drivers/staging/otus/80211core/ccmd.c
new file mode 100644
index 000000000000..479977973671
--- /dev/null
+++ b/drivers/staging/otus/80211core/ccmd.c
@@ -0,0 +1,1861 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cmd.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains command interface functions.               */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+
+u16_t zfWlanReset(zdev_t* dev);
+u32_t zfUpdateRxRate(zdev_t* dev);
+
+
+extern void zfiUsbRecv(zdev_t *dev, zbuf_t *buf);
+extern void zfiUsbRegIn(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern void zfiUsbOutComplete(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr);
+extern void zfiUsbRegOutComplete(zdev_t* dev);
+extern u16_t zfHpReinit(zdev_t* dev, u32_t frequency);
+
+/* Get size (byte) of driver core global data structure.    */
+/* This size will be used by driver wrapper to allocate     */
+/* a memory space for driver core to store global variables */
+u16_t zfiGlobalDataSize(zdev_t* dev)
+{
+    u32_t ret;
+    ret = (sizeof(struct zsWlanDev));
+    zm_assert((ret>>16) == 0);
+    return (u16_t)ret;
+}
+
+
+/* Initialize WLAN hardware and software, resource will be allocated */
+/* for WLAN operation, must be called first before other function.   */
+extern u16_t zfiWlanOpen(zdev_t* dev, struct zsCbFuncTbl* cbFuncTbl)
+{
+    //u16_t ret;
+    //u32_t i;
+    //u8_t* ch;
+    //u8_t  bPassive;
+    u32_t devSize;
+    struct zfCbUsbFuncTbl cbUsbFuncTbl;
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("start");
+
+    devSize = sizeof(struct zsWlanDev);
+    /* Zeroize zsWlanDev struct */
+    zfZeroMemory((u8_t*)wd, (u16_t)devSize);
+
+#ifdef ZM_ENABLE_AGGREGATION
+    zfAggInit(dev);
+#endif
+
+    zfCwmInit(dev);
+
+    wd->commTally.RateCtrlTxMPDU = 0;
+    wd->commTally.RateCtrlBAFail = 0;
+    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+
+    if (cbFuncTbl == NULL)
+    {
+        /* zfcbRecvEth() is mandatory */
+        zm_assert(0);
+    }
+    else
+    {
+        if (cbFuncTbl->zfcbRecvEth == NULL)
+        {
+            /* zfcbRecvEth() is mandatory */
+            zm_assert(0);
+        }
+        wd->zfcbAuthNotify = cbFuncTbl->zfcbAuthNotify;
+        wd->zfcbAuthNotify = cbFuncTbl->zfcbAuthNotify;
+        wd->zfcbAsocNotify = cbFuncTbl->zfcbAsocNotify;
+        wd->zfcbDisAsocNotify = cbFuncTbl->zfcbDisAsocNotify;
+        wd->zfcbApConnectNotify = cbFuncTbl->zfcbApConnectNotify;
+        wd->zfcbConnectNotify = cbFuncTbl->zfcbConnectNotify;
+        wd->zfcbScanNotify = cbFuncTbl->zfcbScanNotify;
+        wd->zfcbMicFailureNotify = cbFuncTbl->zfcbMicFailureNotify;
+        wd->zfcbApMicFailureNotify = cbFuncTbl->zfcbApMicFailureNotify;
+        wd->zfcbIbssPartnerNotify = cbFuncTbl->zfcbIbssPartnerNotify;
+        wd->zfcbMacAddressNotify = cbFuncTbl->zfcbMacAddressNotify;
+        wd->zfcbSendCompleteIndication = cbFuncTbl->zfcbSendCompleteIndication;
+        wd->zfcbRecvEth = cbFuncTbl->zfcbRecvEth;
+        wd->zfcbRestoreBufData = cbFuncTbl->zfcbRestoreBufData;
+        wd->zfcbRecv80211 = cbFuncTbl->zfcbRecv80211;
+#ifdef ZM_ENABLE_CENC
+        wd->zfcbCencAsocNotify = cbFuncTbl->zfcbCencAsocNotify;
+#endif //ZM_ENABLE_CENC
+        wd->zfcbClassifyTxPacket = cbFuncTbl->zfcbClassifyTxPacket;
+        wd->zfcbHwWatchDogNotify = cbFuncTbl->zfcbHwWatchDogNotify;
+    }
+
+    //add by honda 0330
+    cbUsbFuncTbl.zfcbUsbRecv = zfiUsbRecv;
+    cbUsbFuncTbl.zfcbUsbRegIn = zfiUsbRegIn;
+    cbUsbFuncTbl.zfcbUsbOutComplete = zfiUsbOutComplete;
+    cbUsbFuncTbl.zfcbUsbRegOutComplete = zfiUsbRegOutComplete;
+    zfwUsbRegisterCallBack(dev, &cbUsbFuncTbl);
+    /* Init OWN MAC address */
+    wd->macAddr[0] = 0x8000;
+    wd->macAddr[1] = 0x0000;
+    wd->macAddr[2] = 0x0000;
+
+    wd->regulationTable.regionCode = 0xffff;
+
+    zfHpInit(dev, wd->frequency);
+
+    /* init region code */
+    //wd->regulationTable.regionCode = NULL1_WORLD; //Only 2.4g RegCode
+    //zfHpGetRegulationTablefromRegionCode(dev, NULL1_WORLD);
+    //zfiWlanSetDot11DMode(dev , 1); // Enable 802.11d
+    /* Get the first channel */
+    //wd->frequency = zfChGetFirstChannel(dev, &bPassive);
+#ifdef ZM_AP_DEBUG
+    //wd->frequency = 2437;
+#endif
+
+    //STA mode
+    wd->sta.mTxRate = 0x0;
+    wd->sta.uTxRate = 0x3;
+    wd->sta.mmTxRate = 0x0;
+    wd->sta.adapterState = ZM_STA_STATE_DISCONNECT;
+    wd->sta.capability[0] = 0x01;
+    wd->sta.capability[1] = 0x00;
+
+    wd->sta.preambleTypeHT = 0;
+    wd->sta.htCtrlBandwidth = 0;
+    wd->sta.htCtrlSTBC = 0;
+    wd->sta.htCtrlSG = 0;
+    wd->sta.defaultTA = 0;
+    //wd->sta.activescanTickPerChannel = ZM_TIME_ACTIVE_SCAN/ZM_MS_PER_TICK;
+	{
+		u8_t Dur = ZM_TIME_ACTIVE_SCAN;
+		zfwGetActiveScanDur(dev, &Dur);
+		wd->sta.activescanTickPerChannel = Dur/ZM_MS_PER_TICK;
+
+	}
+    wd->sta.passiveScanTickPerChannel = ZM_TIME_PASSIVE_SCAN/ZM_MS_PER_TICK;
+    wd->sta.bAutoReconnect = TRUE;
+    wd->sta.dropUnencryptedPkts = FALSE;
+
+    /* set default to bypass all multicast packet for linux, window XP would set 0 by wrapper initialization */
+	wd->sta.bAllMulticast = 1;
+
+    /* Initial the RIFS Status / RIFS-like frame count / RIFS count */
+    wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+    wd->sta.rifsLikeFrameCnt = 0;
+    wd->sta.rifsCount = 0;
+
+    wd->sta.osRxFilter = 0;
+    wd->sta.bSafeMode = 0;
+
+    //Common
+    zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_DISCONNECT);
+    wd->beaconInterval = 100;
+    wd->rtsThreshold = 2346;
+    wd->fragThreshold = 32767;
+    wd->wlanMode = ZM_MODE_INFRASTRUCTURE;
+    wd->txMCS = 0xff;    //AUTO
+    wd->dtim = 1;
+    //wd->txMT = 1;       //OFDM
+    wd->tick = 1;
+    wd->maxTxPower2 = 0xff;
+    wd->maxTxPower5 = 0xff;
+    wd->supportMode = 0xffffffff;
+    wd->ws.adhocMode = ZM_ADHOCBAND_G;
+    wd->ws.autoSetFrequency = 0xff;
+
+    //AP mode
+    //wd->bgMode = wd->ws.bgMode;
+    wd->ap.ssidLen[0] = 6;
+    wd->ap.ssid[0][0] = 'Z';
+    wd->ap.ssid[0][1] = 'D';
+    wd->ap.ssid[0][2] = '1';
+    wd->ap.ssid[0][3] = '2';
+    wd->ap.ssid[0][4] = '2';
+    wd->ap.ssid[0][5] = '1';
+
+    // Init the country iso name as NA
+    wd->ws.countryIsoName[0] = 0;
+    wd->ws.countryIsoName[1] = 0;
+    wd->ws.countryIsoName[2] = '\0';
+
+	/* init fragmentation is disabled */
+	//zfiWlanSetFragThreshold(dev, 0);
+
+	/* airopeek : swSniffer 1=>on  0=>off */
+	wd->swSniffer = 0;
+    wd->XLinkMode = 0;
+
+// jhlee HT 0
+#if 1
+    /* AP Mode*/
+    /* Init HT Capability Info */
+    wd->ap.HTCap.Data.ElementID = ZM_WLAN_EID_HT_CAPABILITY;
+    wd->ap.HTCap.Data.Length = 26;
+    //wd->ap.HTCap.Data.SupChannelWidthSet = 0;
+    //wd->ap.HTCap.Data.MIMOPowerSave = 3;
+    //wd->ap.HTCap.Data.ShortGIfor40MHz = 0;
+    //wd->ap.HTCap.Data.ShortGIfor20MHz = 0;
+    //wd->ap.HTCap.Data.DSSSandCCKin40MHz = 0;
+    wd->ap.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+    wd->ap.HTCap.Data.MCSSet[0] = 0xFF; // MCS 0 ~  7
+    wd->ap.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+
+    /* Init Extended HT Capability Info */
+    wd->ap.ExtHTCap.Data.ElementID = ZM_WLAN_EID_EXTENDED_HT_CAPABILITY;
+    wd->ap.ExtHTCap.Data.Length = 22;
+    wd->ap.ExtHTCap.Data.ControlChannel = 6;
+    //wd->ap.ExtHTCap.Data.ExtChannelOffset = 3;
+    wd->ap.ExtHTCap.Data.ChannelInfo |= ExtHtCap_RecomTxWidthSet;
+    //wd->ap.ExtHTCap.Data.RIFSMode = 1;
+    wd->ap.ExtHTCap.Data.OperatingInfo |= 1;
+
+    /* STA Mode*/
+    /* Init HT Capability Info */
+    wd->sta.HTCap.Data.ElementID = ZM_WLAN_EID_HT_CAPABILITY;
+    wd->sta.HTCap.Data.Length = 26;
+
+    /* Test with 5G-AP : 7603 */
+    //wd->sta.HTCap.Data.SupChannelWidthSet = 1;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SMEnabled;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_ShortGIfor40MHz;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_DSSSandCCKin40MHz;
+#ifndef ZM_DISABLE_AMSDU8K_SUPPORT
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_MaxAMSDULength;
+#endif
+    //wd->sta.HTCap.Data.MIMOPowerSave = 0;
+    //wd->sta.HTCap.Data.ShortGIfor40MHz = 0;
+    //wd->sta.HTCap.Data.ShortGIfor20MHz = 0;
+    //wd->sta.HTCap.Data.DSSSandCCKin40MHz = 0;
+    wd->sta.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+    wd->sta.HTCap.Data.MCSSet[0] = 0xFF; // MCS 0 ~  7
+    wd->sta.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+    wd->sta.HTCap.Data.PCO |= HTCAP_TransmissionTime3;
+    //wd->sta.HTCap.Data.TransmissionTime = 0;
+    /* Init Extended HT Capability Info */
+    wd->sta.ExtHTCap.Data.ElementID = ZM_WLAN_EID_EXTENDED_HT_CAPABILITY;
+    wd->sta.ExtHTCap.Data.Length = 22;
+    wd->sta.ExtHTCap.Data.ControlChannel = 6;
+
+    //wd->sta.ExtHTCap.Data.ExtChannelOffset |= 3;
+    wd->sta.ExtHTCap.Data.ChannelInfo |= ExtHtCap_ExtChannelOffsetBelow;
+
+    //wd->sta.ExtHTCap.Data.RecomTxWidthSet = 1;
+    //wd->sta.ExtHTCap.Data.RIFSMode = 1;
+    wd->sta.ExtHTCap.Data.OperatingInfo |= 1;
+#endif
+
+#if 0
+    /* WME test code */
+    wd->ap.qosMode[0] = 1;
+#endif
+
+    wd->ledStruct.ledMode[0] = 0x2221;
+    wd->ledStruct.ledMode[1] = 0x2221;
+
+    zfTimerInit(dev);
+
+    ZM_PERFORMANCE_INIT(dev);
+
+    zfBssInfoCreate(dev);
+    zfScanMgrInit(dev);
+    zfPowerSavingMgrInit(dev);
+
+#if 0
+    /* Test code */
+    {
+        u32_t key[4] = {0xffffffff, 0xff, 0, 0};
+        u16_t addr[3] = {0x8000, 0x01ab, 0x0000};
+        //zfSetKey(dev, 0, 0, ZM_WEP64, addr, key);
+        //zfSetKey(dev, 0, 0, ZM_AES, addr, key);
+        //zfSetKey(dev, 64, 0, 1, wd->macAddr, key);
+    }
+#endif
+
+    // WME settings
+    wd->ws.staWmeEnabled = 1;           // Enable WME by default
+    #define ZM_UAPSD_Q_SIZE 32 //2^N
+    wd->ap.uapsdQ = zfQueueCreate(dev, ZM_UAPSD_Q_SIZE);
+    zm_assert(wd->ap.uapsdQ != NULL);
+    wd->sta.uapsdQ = zfQueueCreate(dev, ZM_UAPSD_Q_SIZE);
+    zm_assert(wd->sta.uapsdQ != NULL);
+
+    //zfHpInit(dev, wd->frequency);
+
+    /* MAC address */
+    //zfHpSetMacAddress(dev, wd->macAddr, 0);
+    zfHpGetMacAddress(dev);
+
+    zfCoreSetFrequency(dev, wd->frequency);
+
+#if ZM_PCI_LOOP_BACK == 1
+    zfwWriteReg(dev, ZM_REG_PCI_CONTROL, 6);
+#endif /* #if ZM_PCI_LOOP_BACK == 1 */
+
+    //zfiWlanSetDot11DMode(dev , 1); // Enable 802.11d
+    //zfiWlanSetDot11HDFSMode(dev , 1); // Enable 802.11h DFS
+    wd->sta.DFSEnable = 1;
+    wd->sta.capability[1] |= ZM_BIT_0;
+
+    //zfiWlanSetFrequency(dev, 5260000, TRUE);
+    //zfiWlanSetAniMode(dev , 1); // Enable ANI
+
+    /* Trgger Rx DMA */
+    zfHpStartRecv(dev);
+
+    zm_debug_msg0("end");
+
+    return 0;
+}
+
+/* WLAN hardware will be shutdown and all resource will be release */
+u16_t zfiWlanClose(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_init(ZM_LV_0, "enter");
+
+    wd->state = ZM_WLAN_STATE_CLOSEDED;
+
+    //zfiWlanDisable(dev, 1);
+    zfWlanReset(dev);
+
+    zfHpStopRecv(dev);
+
+    /* Disable MAC */
+    /* Disable PHY */
+    /* Disable RF */
+
+    zfHpRelease(dev);
+
+    zfQueueDestroy(dev, wd->ap.uapsdQ);
+    zfQueueDestroy(dev, wd->sta.uapsdQ);
+
+    zfBssInfoDestroy(dev);
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 1);  //1 for release structure memory
+    /* end of add by honda */
+#endif
+
+    zm_msg0_init(ZM_LV_0, "exit");
+
+    return 0;
+}
+
+void zfGetWrapperSetting(zdev_t* dev)
+{
+    u8_t   bPassive;
+    u16_t vapId = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+#if 0
+    if ( (wd->ws.countryIsoName[0] != 0)
+         || (wd->ws.countryIsoName[1] != 0)
+         || (wd->ws.countryIsoName[2] != '\0') )
+    {
+        zfHpGetRegulationTablefromRegionCode(
+            dev,
+            zfHpGetRegionCodeFromIsoName(dev, wd->ws.countryIsoName) );
+    }
+#endif
+    zmw_enter_critical_section(dev);
+
+    wd->wlanMode = wd->ws.wlanMode;
+
+    /* set channel */
+    if ( wd->ws.frequency )
+    {
+        wd->frequency = wd->ws.frequency;
+        wd->ws.frequency = 0;
+    }
+    else
+    {
+        wd->frequency = zfChGetFirstChannel(dev, &bPassive);
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if (wd->ws.adhocMode == ZM_ADHOCBAND_A)
+            {
+                wd->frequency = ZM_CH_A_36;
+            }
+            else
+            {
+            wd->frequency = ZM_CH_G_6;
+            }
+        }
+    }
+#ifdef ZM_AP_DEBUG
+    /* honda add for debug, 2437 channel 6, 2452 channel 9 */
+    wd->frequency = 2437;
+    /* end of add by honda */
+#endif
+
+    /* set preamble type */
+    switch (wd->ws.preambleType)
+    {
+    case ZM_PREAMBLE_TYPE_AUTO:
+    case ZM_PREAMBLE_TYPE_SHORT:
+    case ZM_PREAMBLE_TYPE_LONG:
+        wd->preambleType = wd->ws.preambleType;
+        break;
+    default:
+        wd->preambleType = ZM_PREAMBLE_TYPE_SHORT;
+        break;
+    }
+    wd->ws.preambleType = 0;
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            wd->ap.authAlgo[0] = wd->ws.authMode;
+            wd->ap.encryMode[0] = wd->ws.encryMode;
+        }
+        else
+        {
+            wd->ap.authAlgo[vapId + 1] = wd->ws.authMode;
+            wd->ap.encryMode[vapId + 1] = wd->ws.encryMode;
+        }
+        wd->ws.authMode = 0;
+        wd->ws.encryMode = ZM_NO_WEP;
+
+        /* Get beaconInterval from WrapperSetting */
+        if ((wd->ws.beaconInterval >= 20) && (wd->ws.beaconInterval <= 1000))
+        {
+            wd->beaconInterval = wd->ws.beaconInterval;
+        }
+        else
+        {
+            wd->beaconInterval = 100; //100ms
+        }
+
+        if (wd->ws.dtim > 0)
+        {
+            wd->dtim = wd->ws.dtim;
+        }
+        else
+        {
+            wd->dtim = 1;
+        }
+
+        wd->ap.qosMode = wd->ws.apWmeEnabled & 0x1;
+        wd->ap.uapsdEnabled = (wd->ws.apWmeEnabled & 0x2) >> 1;
+    }
+    else
+    {
+        wd->sta.authMode = wd->ws.authMode;
+        wd->sta.currentAuthMode = wd->ws.authMode;
+        wd->sta.wepStatus = wd->ws.wepStatus;
+
+        if ( wd->ws.beaconInterval )
+        {
+            wd->beaconInterval = wd->ws.beaconInterval;
+        }
+        else
+        {
+            wd->beaconInterval = 0x64;
+        }
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            /* 1. Set default channel 6 (2437MHz) */
+//            wd->frequency = 2437;
+
+            /* 2. Otus support 802.11g Mode */
+            if ((wd->ws.adhocMode == ZM_ADHOCBAND_G) ||
+                (wd->ws.adhocMode == ZM_ADHOCBAND_BG) ||
+                (wd->ws.adhocMode == ZM_ADHOCBAND_ABG) ) {
+                wd->wfc.bIbssGMode = 1;
+            } else {
+                wd->wfc.bIbssGMode = 0;
+            }
+
+            /* 3. set short preamble  */
+            //wd->sta.preambleType = ZM_PREAMBLE_TYPE_SHORT ;
+        }
+
+        /* set ATIM window */
+        if ( wd->ws.atimWindow )
+        {
+            wd->sta.atimWindow = wd->ws.atimWindow;
+        }
+        else
+        {
+            //wd->sta.atimWindow = 0x0a;
+            wd->sta.atimWindow = 0;
+        }
+
+        //wd->sta.connectingHiddenAP = 1;//wd->ws.connectingHiddenAP;
+        wd->sta.dropUnencryptedPkts = wd->ws.dropUnencryptedPkts;
+        wd->sta.ibssJoinOnly = wd->ws.ibssJoinOnly;
+
+        if ( wd->ws.bDesiredBssid )
+        {
+            zfMemoryCopy(wd->sta.desiredBssid, wd->ws.desiredBssid, 6);
+            wd->sta.bDesiredBssid = TRUE;
+            wd->ws.bDesiredBssid = FALSE;
+        }
+        else
+        {
+            wd->sta.bDesiredBssid = FALSE;
+        }
+
+        /* check ssid */
+        if ( wd->ws.ssidLen != 0 )
+        {
+            if ( (!zfMemoryIsEqual(wd->ws.ssid, wd->sta.ssid,
+                                   wd->sta.ssidLen))||
+                 (wd->ws.ssidLen != wd->sta.ssidLen)||
+                 (wd->sta.authMode == ZM_AUTH_MODE_WPA)||
+                 (wd->sta.authMode == ZM_AUTH_MODE_WPAPSK) ||
+                 (wd->ws.staWmeQosInfo!= 0) )
+            {
+                /*if u-APSD test(set QosInfo), clear connectByReasso to do association (not reassociation)*/
+                wd->sta.connectByReasso = FALSE;
+                wd->sta.failCntOfReasso = 0;
+                wd->sta.pmkidInfo.bssidCount = 0;
+
+                wd->sta.ssidLen = wd->ws.ssidLen;
+                zfMemoryCopy(wd->sta.ssid, wd->ws.ssid, wd->sta.ssidLen);
+
+                if ( wd->sta.ssidLen < 32 )
+                {
+                    wd->sta.ssid[wd->sta.ssidLen] = 0;
+                }
+            }
+        }
+        else
+        {   /* ANY BSS */
+            wd->sta.ssid[0] = 0;
+            wd->sta.ssidLen = 0;
+        }
+
+        wd->sta.wmeEnabled = wd->ws.staWmeEnabled;
+        wd->sta.wmeQosInfo = wd->ws.staWmeQosInfo;
+
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+u16_t zfWlanEnable(zdev_t* dev)
+{
+    u8_t     bssid[6] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode == ZM_MODE_UNKNOWN )
+    {
+        zm_debug_msg0("Unknown Mode...Skip...");
+        return 0;
+    }
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        u16_t vapId;
+
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            /* AP mode */
+            zfApInitStaTbl(dev);
+
+            /* AP default parameters */
+            wd->bRate = 0xf;
+            wd->gRate = 0xff;
+            wd->bRateBasic = 0xf;
+            wd->gRateBasic = 0x0;
+            //wd->beaconInterval = 100;
+            wd->ap.apBitmap = 1;
+            wd->ap.beaconCounter = 0;
+            //wd->ap.vapNumber = 1;    //mark by ygwei for Vap
+
+            wd->ap.hideSsid[0] = 0;
+            wd->ap.staAgingTimeSec = 10*60;
+            wd->ap.staProbingTimeSec = 60;
+
+            for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+            {
+                wd->ap.bcmcHead[i] = wd->ap.bcmcTail[i] = 0;
+            }
+
+            //wd->ap.uniHead = wd->ap.uniTail = 0;
+
+            /* load AP parameters */
+            wd->bRateBasic = wd->ws.bRateBasic;
+            wd->gRateBasic = wd->ws.gRateBasic;
+            wd->bgMode = wd->ws.bgMode;
+            if ((wd->ws.ssidLen <= 32) && (wd->ws.ssidLen != 0))
+            {
+                wd->ap.ssidLen[0] = wd->ws.ssidLen;
+                for(i=0; i<wd->ws.ssidLen; i++)
+                {
+                    wd->ap.ssid[0][i] = wd->ws.ssid[i];
+                }
+                wd->ws.ssidLen = 0; // Reset Wrapper Variable
+            }
+
+            if (wd->ap.encryMode[0] == 0)
+            {
+                wd->ap.capab[0] = 0x001;
+            }
+            else
+            {
+                wd->ap.capab[0] = 0x011;
+            }
+            /* set Short Slot Time bit if not 11b */
+            if (wd->ap.wlanType[0] != ZM_WLAN_TYPE_PURE_B)
+            {
+              wd->ap.capab[0] |= 0x400;
+            }
+
+            // wd->ap.vapNumber = 1;    // mark by ygwei for Vap Test
+        }
+        else
+        {
+#if 0
+            /* VAP Test Code */
+            wd->ap.apBitmap = 0x3;
+            wd->ap.capab[1] = 0x401;
+            wd->ap.ssidLen[1] = 4;
+            wd->ap.ssid[1][0] = 'v';
+            wd->ap.ssid[1][1] = 'a';
+            wd->ap.ssid[1][2] = 'p';
+            wd->ap.ssid[1][3] = '1';
+            wd->ap.authAlgo[1] = wd->ws.authMode;
+            wd->ap.encryMode[1] = wd->ws.encryMode;
+            wd->ap.vapNumber = 2;
+#else
+            /* VAP Test Code */
+            wd->ap.apBitmap = 0x1 | (0x01 << (vapId+1));
+
+            if ((wd->ws.ssidLen <= 32) && (wd->ws.ssidLen != 0))
+            {
+                wd->ap.ssidLen[vapId+1] = wd->ws.ssidLen;
+                for(i=0; i<wd->ws.ssidLen; i++)
+                {
+                    wd->ap.ssid[vapId+1][i] = wd->ws.ssid[i];
+                }
+                wd->ws.ssidLen = 0; // Reset Wrapper Variable
+            }
+
+            if (wd->ap.encryMode[vapId+1] == 0)
+            {
+                wd->ap.capab[vapId+1] = 0x401;
+            }
+            else
+            {
+                wd->ap.capab[vapId+1] = 0x411;
+            }
+
+            wd->ap.authAlgo[vapId+1] = wd->ws.authMode;
+            wd->ap.encryMode[vapId+1] = wd->ws.encryMode;
+
+            /* Need to be modified when VAP is used */
+            //wd->ap.vapNumber = 2;
+#endif
+        }
+
+        wd->ap.vapNumber++;
+
+        zfCoreSetFrequency(dev, wd->frequency);
+
+        zfInitMacApMode(dev);
+
+        /* Disable protection mode */
+        zfApSetProtectionMode(dev, 0);
+
+        zfApSendBeacon(dev);
+    } /*if (wd->wlanMode == ZM_MODE_AP) */
+    else
+    {
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+        zmw_enter_critical_section(dev);
+        wd->sta.oppositeCount = 0;    /* reset opposite count */
+        //wd->sta.bAutoReconnect = wd->sta.bAutoReconnectEnabled;
+        //wd->sta.scanWithSSID = 0;
+        zfStaInitOppositeInfo(dev);
+        zmw_leave_critical_section(dev);
+
+        zfStaResetStatus(dev, 0);
+
+        if ( (wd->sta.cmDisallowSsidLength != 0)&&
+             (wd->sta.ssidLen == wd->sta.cmDisallowSsidLength)&&
+             (zfMemoryIsEqual(wd->sta.ssid, wd->sta.cmDisallowSsid,
+                              wd->sta.ssidLen)) &&
+             (wd->sta.wepStatus == ZM_ENCRYPTION_TKIP))
+        {   /* countermeasures */
+            zm_debug_msg0("countermeasures disallow association");
+
+        }
+        else
+        {
+            switch( wd->wlanMode )
+            {
+                case ZM_MODE_IBSS:
+                    /* some registers may be set here */
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+                    {
+                        zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_IBSS_WPA2PSK);
+                    }
+                    else
+                    {
+                        zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_IBSS_GENERAL);
+                    }
+
+                    zm_msg0_mm(ZM_LV_0, "ZM_MODE_IBSS");
+                    zfIbssConnectNetwork(dev);
+                    break;
+
+                case ZM_MODE_INFRASTRUCTURE:
+                    /* some registers may be set here */
+                    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_STA);
+
+                    zfInfraConnectNetwork(dev);
+                    break;
+
+                case ZM_MODE_PSEUDO:
+                    /* some registers may be set here */
+                    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_STA);
+
+                    zfUpdateBssid(dev, bssid);
+                    zfCoreSetFrequency(dev, wd->frequency);
+                    break;
+
+                default:
+                    break;
+            }
+        }
+
+    }
+
+
+    //if ( (wd->wlanMode != ZM_MODE_INFRASTRUCTURE)&&
+    //     (wd->wlanMode != ZM_MODE_AP) )
+    if ( wd->wlanMode == ZM_MODE_PSEUDO )
+    {
+        /* Reset Wlan status */
+        zfWlanReset(dev);
+
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+        }
+        zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    }
+
+
+    if(wd->wlanMode == ZM_MODE_AP)
+    {
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+        }
+        //zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    }
+
+    // Assign default Tx Rate
+    if ( wd->sta.EnableHT )
+    {
+		u32_t oneTxStreamCap;
+		oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+		if(oneTxStreamCap)
+			wd->CurrentTxRateKbps = 135000;
+		else
+			wd->CurrentTxRateKbps = 270000;
+        wd->CurrentRxRateKbps = 270000;
+    }
+    else
+    {
+        wd->CurrentTxRateKbps = 54000;
+        wd->CurrentRxRateKbps = 54000;
+    }
+
+    wd->state = ZM_WLAN_STATE_ENABLED;
+
+    return 0;
+}
+
+/* Enable/disable Wlan operation */
+u16_t zfiWlanEnable(zdev_t* dev)
+{
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_1, "Enable Wlan");
+
+    zfGetWrapperSetting(dev);
+
+    zfZeroMemory((u8_t*) &wd->trafTally, sizeof(struct zsTrafTally));
+
+    // Reset cmMicFailureCount to 0 for new association request
+    if ( wd->sta.cmMicFailureCount == 1 )
+    {
+        zfTimerCancel(dev, ZM_EVENT_CM_TIMER);
+        wd->sta.cmMicFailureCount = 0;
+    }
+
+    zfFlushVtxq(dev);
+    if ((wd->queueFlushed & 0x10) != 0)
+    {
+        zfHpUsbReset(dev);
+    }
+    ret = zfWlanEnable(dev);
+
+    return ret;
+}
+/* Add a flag named ResetKeyCache to show if KeyCache should be cleared.
+   for hostapd in AP mode, if driver receives iwconfig ioctl
+   after setting group key, it shouldn't clear KeyCache.                 */
+u16_t zfiWlanDisable(zdev_t* dev, u8_t ResetKeyCache)
+{
+    u16_t  i;
+    u8_t isConnected;
+
+    zmw_get_wlan_dev(dev);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    zmw_declare_for_critical_section();
+#endif
+    wd->state = ZM_WLAN_STATE_DISABLED;
+
+    zm_msg0_mm(ZM_LV_1, "Disable Wlan");
+
+    if ( wd->wlanMode != ZM_MODE_AP )
+    {
+        isConnected = zfStaIsConnected(dev);
+
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+             (wd->sta.currentAuthMode != ZM_AUTH_MODE_WPA2) )
+        {
+            /* send deauthentication frame */
+            if (isConnected)
+            {
+                //zfiWlanDeauth(dev, NULL, 0);
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                //zmw_debug_msg0("send a Deauth frame!");
+            }
+        }
+
+        // Remove all the connected peer stations
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            wd->sta.ibssBssIsCreator = 0;
+            zfTimerCancel(dev, ZM_EVENT_IBSS_MONITOR);
+            zfStaIbssMonitoring(dev, 1);
+        }
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssWpa2Psk = 0;
+        zmw_leave_critical_section(dev);
+#endif
+
+        wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+
+        /* reset connect timeout counter */
+        wd->sta.connectTimeoutCount = 0;
+
+        /* reset connectState to None */
+        wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+
+        /* reset leap enable variable */
+        wd->sta.leapEnabled = 0;
+
+        /* Disable the RIFS Status / RIFS-like frame count / RIFS count */
+        if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+            zfHpDisableRifs(dev);
+        wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+        wd->sta.rifsLikeFrameCnt = 0;
+        wd->sta.rifsCount = 0;
+
+        wd->sta.osRxFilter = 0;
+        wd->sta.bSafeMode = 0;
+
+        zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+        if (ResetKeyCache)
+            zfHpResetKeyCache(dev);
+
+        if (isConnected)
+        {
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECTION_DISABLED, wd->sta.bssid);
+            }
+        }
+        else
+        {
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_DISABLED, wd->sta.bssid);
+            }
+        }
+    }
+    else //if (wd->wlanMode == ZM_MODE_AP)
+    {
+        for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+        {
+            /* send deauthentication frame */
+            if (wd->ap.staTable[i].valid == 1)
+            {
+                /* Reason : Sending station is leaving */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH,
+                        wd->ap.staTable[i].addr, 3, 0, 0);
+            }
+        }
+
+        if (ResetKeyCache)
+            zfHpResetKeyCache(dev);
+
+        wd->ap.vapNumber--;
+    }
+
+    /* stop beacon */
+    zfHpDisableBeacon(dev);
+
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+    /* Flush AP PS queues */
+    zfApFlushBufferedPsFrame(dev);
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+
+    #ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 0);  //1 for release structure memory
+    /* end of add by honda */
+    #endif
+
+    // Clear the information for the peer stations of IBSS or AP of Station mode
+    zfZeroMemory((u8_t*)wd->sta.oppositeInfo, sizeof(struct zsOppositeInfo) * ZM_MAX_OPPOSITE_COUNT);
+
+    /* Turn off Software WEP/TKIP */
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zm_debug_msg0("Disable software encryption");
+        zfStaDisableSWEncryption(dev);
+    }
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    return 0;
+}
+
+u16_t zfiWlanSuspend(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // Change the HAL state to init so that any packet can't be transmitted between
+    // resume & HAL reinit. This would cause the chip hang issue in OTUS.
+    zmw_enter_critical_section(dev);
+    wd->halState = ZM_HAL_STATE_INIT;
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+u16_t zfiWlanResume(zdev_t* dev, u8_t doReconn)
+{
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* Redownload firmware, Reinit MAC,PHY,RF */
+    zfHpReinit(dev, wd->frequency);
+
+    //Set channel according to AP's configuration
+    zfCoreSetFrequencyExV2(dev, wd->frequency, wd->BandWidth40,
+            wd->ExtOffset, NULL, 1);
+
+    zfHpSetMacAddress(dev, wd->macAddr, 0);
+
+    /* Start Rx */
+    zfHpStartRecv(dev);
+
+    zfFlushVtxq(dev);
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE &&
+         wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return 1;
+    }
+
+    zm_msg0_mm(ZM_LV_1, "Resume Wlan");
+    if ( (zfStaIsConnected(dev)) || (zfStaIsConnecting(dev)) )
+    {
+        if (doReconn == 1)
+        {
+            zm_msg0_mm(ZM_LV_1, "Re-connect...");
+            zmw_enter_critical_section(dev);
+            wd->sta.connectByReasso = FALSE;
+            zmw_leave_critical_section(dev);
+
+            zfWlanEnable(dev);
+        }
+        else if (doReconn == 0)
+        {
+            zfHpSetRollCallTable(dev);
+        }
+    }
+
+    ret = 0;
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                 zfiWlanFlushAllQueuedBuffers */
+/*      Flush Virtual TxQ, MmQ, PS frames and defragment list           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanFlushAllQueuedBuffers(zdev_t* dev)
+{
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+    /* Flush AP PS queues */
+    zfApFlushBufferedPsFrame(dev);
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+}
+
+/* Do WLAN site survey */
+u16_t zfiWlanScan(zdev_t* dev)
+{
+    u16_t ret = 1;
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("");
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        wd->heartBeatNotification |= ZM_BSSID_LIST_SCAN;
+        wd->sta.scanFrequency = 0;
+        //wd->sta.pUpdateBssList->bssCount = 0;
+        ret = 0;
+    }
+    else
+    {
+        #if 0
+        if ( !zfStaBlockWlanScan(dev) )
+        {
+            zm_debug_msg0("scan request");
+            //zfTimerSchedule(dev, ZM_EVENT_SCAN, ZM_TICK_ZERO);
+            ret = 0;
+            goto start_scan;
+        }
+        #else
+            goto start_scan;
+        #endif
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return ret;
+
+start_scan:
+    zmw_leave_critical_section(dev);
+
+    if(wd->ledStruct.LEDCtrlFlagFromReg & ZM_LED_CTRL_FLAG_ALPHA)   // flag for Alpha
+        wd->ledStruct.LEDCtrlFlag |= ZM_LED_CTRL_FLAG_ALPHA;
+
+    ret = zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+    zm_debug_msg1("ret = ", ret);
+
+    return ret;
+}
+
+
+/* rate         */
+/*    0 : AUTO  */
+/*    1 : CCK 1M */
+/*    2 : CCK 2M */
+/*    3 : CCK 5.5M */
+/*    4 : CCK 11M */
+/*    5 : OFDM 6M */
+/*    6 : OFDM 9M */
+/*    7 : OFDM 12M */
+/*    8 : OFDM 18M */
+/*    9 : OFDM 24M */
+/*    10 : OFDM 36M */
+/*    11 : OFDM 48M */
+/*    12 : OFDM 54M */
+/*    13 : MCS 0 */
+/*    28 : MCS 15 */
+u16_t zcRateToMCS[] =
+    {0xff, 0, 1, 2, 3, 0xb, 0xf, 0xa, 0xe, 0x9, 0xd, 0x8, 0xc};
+u16_t zcRateToMT[] = {0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1};
+
+u16_t zfiWlanSetTxRate(zdev_t* dev, u16_t rate)
+{   // jhlee HT 0
+    zmw_get_wlan_dev(dev);
+
+    if (rate <=12)
+    {
+        wd->txMCS = zcRateToMCS[rate];
+        wd->txMT = zcRateToMT[rate];
+        return ZM_SUCCESS;
+    }
+    else if ((rate<=28)||(rate==13+32))
+    {
+        wd->txMCS = rate - 12 - 1;
+        wd->txMT = 2;
+        return ZM_SUCCESS;
+    }
+
+    return ZM_ERR_INVALID_TX_RATE;
+}
+
+const u32_t zcRateIdToKbps40M[] =
+    {
+        1000, 2000, 5500, 11000,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6000, 9000, 12000, 18000,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24000, 36000, 48000, 54000,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        13500, 27000, 40500, 54000,               /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        81000, 108000, 121500, 135000,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        27000, 54000, 81000, 108000,              /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        162000, 216000, 243000, 270000,           /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        270000, 300000, 150000                    /* MCS14SG, MCS15SG, MCS7SG , 28 29 30*/
+    };
+
+const u32_t zcRateIdToKbps20M[] =
+    {
+        1000, 2000, 5500, 11000,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6000, 9000, 12000, 18000,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24000, 36000, 48000, 54000,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        6500, 13000, 19500, 26000,                /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        39000, 52000, 58500, 65000,               /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        13000, 26000, 39000, 52000,               /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        78000, 104000, 117000, 130000,            /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        130000, 144400, 72200                     /* MCS14SG, MCS15SG, MSG7SG , 28 29 30*/
+    };
+
+u32_t zfiWlanQueryTxRate(zdev_t* dev)
+{
+    u8_t rateId = 0xff;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* If Tx rate had not been trained, return maximum Tx rate instead */
+    if ((wd->wlanMode == ZM_MODE_INFRASTRUCTURE) && (zfStaIsConnected(dev)))
+    {
+        zmw_enter_critical_section(dev);
+        //Not in fixed rate mode
+        if (wd->txMCS == 0xff)
+        {
+            if ((wd->sta.oppositeInfo[0].rcCell.flag & ZM_RC_TRAINED_BIT) == 0)
+            {
+                rateId = wd->sta.oppositeInfo[0].rcCell.operationRateSet[wd->sta.oppositeInfo[0].rcCell.operationRateCount-1];
+            }
+            else
+            {
+                rateId = wd->sta.oppositeInfo[0].rcCell.operationRateSet[wd->sta.oppositeInfo[0].rcCell.currentRateIndex];
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+    if (rateId != 0xff)
+    {
+        if (wd->sta.htCtrlBandwidth)
+        {
+            return zcRateIdToKbps40M[rateId];
+        }
+        else
+        {
+            return zcRateIdToKbps20M[rateId];
+        }
+    }
+    else
+    {
+        return wd->CurrentTxRateKbps;
+    }
+}
+
+void zfWlanUpdateRxRate(zdev_t* dev, struct zsAdditionInfo* addInfo)
+{
+    u32_t rxRateKbps;
+    zmw_get_wlan_dev(dev);
+    //zm_msg1_mm(ZM_LV_0, "addInfo->Tail.Data.RxMacStatus =", addInfo->Tail.Data.RxMacStatus & 0x03);
+
+    /* b5~b4: MPDU indication.                */
+    /*        00: Single MPDU.                */
+    /*        10: First MPDU of A-MPDU.       */
+    /*        11: Middle MPDU of A-MPDU.      */
+    /*        01: Last MPDU of A-MPDU.        */
+    /* Only First MPDU and Single MPDU have PLCP header */
+    /* First MPDU  : (mpduInd & 0x30) == 0x00 */
+    /* Single MPDU : (mpduInd & 0x30) == 0x20 */
+    if ((addInfo->Tail.Data.RxMacStatus & 0x10) == 0)
+    {
+        /* Modulation type */
+        wd->modulationType = addInfo->Tail.Data.RxMacStatus & 0x03;
+        switch(wd->modulationType)
+        {
+    		case 0x0: wd->rateField = addInfo->PlcpHeader[0] & 0xff; //CCK mode
+                      wd->rxInfo = 0;
+                      break;
+    		case 0x1: wd->rateField = addInfo->PlcpHeader[0] & 0x0f; //Legacy-OFDM mode
+                      wd->rxInfo = 0;
+                      break;
+    		case 0x2: wd->rateField = addInfo->PlcpHeader[3]; //HT-OFDM mode
+    		          wd->rxInfo = addInfo->PlcpHeader[6];
+                      break;
+            default: break;
+         }
+
+        rxRateKbps = zfUpdateRxRate(dev);
+        if (wd->CurrentRxRateUpdated == 1)
+        {
+            if (rxRateKbps > wd->CurrentRxRateKbps)
+            {
+                wd->CurrentRxRateKbps = rxRateKbps;
+            }
+        }
+        else
+        {
+            wd->CurrentRxRateKbps = rxRateKbps;
+            wd->CurrentRxRateUpdated = 1;
+        }
+    }
+}
+#if 0
+u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
+                               24000, 12000, 6000, 54000, 36000, 18000, 9000};
+u32_t zcIndextoRateN20L[16] = {6500, 13000, 19500, 26000, 39000, 52000, 58500,
+                              65000, 13000, 26000, 39000, 52000, 78000, 104000,
+                              117000, 130000};
+u32_t zcIndextoRateN20S[16] = {7200, 14400, 21700, 28900, 43300, 57800, 65000,
+                              72200, 14400, 28900, 43300, 57800, 86700, 115600,
+                              130000, 144400};
+u32_t zcIndextoRateN40L[16] = {13500, 27000, 40500, 54000, 81000, 108000, 121500,
+                              135000, 27000, 54000, 81000, 108000, 162000, 216000,
+                              243000, 270000};
+u32_t zcIndextoRateN40S[16] = {15000, 30000, 45000, 60000, 90000, 120000, 135000,
+                              150000, 30000, 60000, 90000, 120000, 180000, 240000,
+                              270000, 300000};
+#endif
+
+extern u16_t zcIndextoRateBG[16];
+extern u32_t zcIndextoRateN20L[16];
+extern u32_t zcIndextoRateN20S[16];
+extern u32_t zcIndextoRateN40L[16];
+extern u32_t zcIndextoRateN40S[16];
+
+u32_t zfiWlanQueryRxRate(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->CurrentRxRateUpdated = 0;
+    return wd->CurrentRxRateKbps;
+}
+
+u32_t zfUpdateRxRate(zdev_t* dev)
+{
+    u8_t mcs, bandwidth;
+    u32_t rxRateKbps = 130000;
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->modulationType)
+    {
+		case 0x0: //CCK mode
+                  switch (wd->rateField)
+                  {
+                      case 0x0a: rxRateKbps = 1000;
+                            break;
+                      case 0x14: rxRateKbps = 2000;
+
+                      case 0x37: rxRateKbps = 5500;
+                            break;
+                      case 0x6e: rxRateKbps = 11000;
+                            break;
+                      default:
+                            break;
+                  }
+                  break;
+		case 0x1: //Legacy-OFDM mode
+                  if (wd->rateField <= 15)
+                  {
+                      rxRateKbps = zcIndextoRateBG[wd->rateField];
+                  }
+                  break;
+		case 0x2: //HT-OFDM mode
+		          mcs = wd->rateField & 0x7F;
+		          bandwidth = wd->rateField & 0x80;
+                  if (mcs <= 15)
+                  {
+                      if (bandwidth != 0)
+                      {
+                          if((wd->rxInfo & 0x80) != 0)
+                          {
+                              /* Short GI 40 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN40S[mcs];
+                          }
+                          else
+                          {
+                              /* Long GI 40 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN40L[mcs];
+                          }
+                      }
+                      else
+                      {
+                          if((wd->rxInfo & 0x80) != 0)
+                          {
+                              /* Short GI 20 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN20S[mcs];
+                          }
+                          else
+                          {
+                              /* Long GI 20 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN20L[mcs];
+                          }
+                      }
+                  }
+                  break;
+        default:
+                  break;
+    }
+    //zm_msg1_mm(ZM_LV_0, "wd->CurrentRxRateKbps=", wd->CurrentRxRateKbps);
+
+     // ToDo: use bandwith field to define 40MB
+    return rxRateKbps;
+}
+
+/* Get WLAN stastics */
+u16_t zfiWlanGetStatistics(zdev_t* dev)
+{
+    /* Return link statistics */
+    return 0;
+}
+
+u16_t zfiWlanReset(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->state = ZM_WLAN_STATE_DISABLED;
+
+    return zfWlanReset(dev);
+}
+
+/* Reset WLAN */
+u16_t zfWlanReset(zdev_t* dev)
+{
+    u8_t isConnected;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg0("zfWlanReset");
+
+    isConnected = zfStaIsConnected(dev);
+
+    //if ( wd->wlanMode != ZM_MODE_AP )
+    {
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+             (wd->sta.currentAuthMode != ZM_AUTH_MODE_WPA2) )
+        {
+            /* send deauthentication frame */
+            if (isConnected)
+            {
+                //zfiWlanDeauth(dev, NULL, 0);
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                //zmw_debug_msg0("send a Deauth frame!");
+            }
+        }
+    }
+
+    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+    zfHpResetKeyCache(dev);
+
+    if (isConnected)
+    {
+        //zfiWlanDisable(dev);
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECTION_RESET, wd->sta.bssid);
+        }
+    }
+    else
+    {
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_RESET, wd->sta.bssid);
+        }
+    }
+
+    /* stop beacon */
+    zfHpDisableBeacon(dev);
+
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+
+    #ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 0);  //1 for release structure memory
+    /* end of add by honda */
+    #endif
+
+    zfStaRefreshBlockList(dev, 1);
+
+    zmw_enter_critical_section(dev);
+
+    zfTimerCancel(dev, ZM_EVENT_IBSS_MONITOR);
+    zfTimerCancel(dev, ZM_EVENT_CM_BLOCK_TIMER);
+    zfTimerCancel(dev, ZM_EVENT_CM_DISCONNECT);
+
+    wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+    wd->sta.connectByReasso = FALSE;
+    wd->sta.cmDisallowSsidLength = 0;
+    wd->sta.bAutoReconnect = 0;
+    wd->sta.InternalScanReq = 0;
+    wd->sta.encryMode = ZM_NO_WEP;
+    wd->sta.wepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+    wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+    wd->sta.cmMicFailureCount = 0;
+    wd->sta.ibssBssIsCreator = 0;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    wd->sta.ibssWpa2Psk = 0;
+#endif
+    /* reset connect timeout counter */
+    wd->sta.connectTimeoutCount = 0;
+
+    /* reset leap enable variable */
+    wd->sta.leapEnabled = 0;
+
+    /* Reset the RIFS Status / RIFS-like frame count / RIFS count */
+    if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+        zfHpDisableRifs(dev);
+    wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+    wd->sta.rifsLikeFrameCnt = 0;
+    wd->sta.rifsCount = 0;
+
+    wd->sta.osRxFilter = 0;
+    wd->sta.bSafeMode = 0;
+
+    // Clear the information for the peer stations of IBSS or AP of Station mode
+    zfZeroMemory((u8_t*)wd->sta.oppositeInfo, sizeof(struct zsOppositeInfo) * ZM_MAX_OPPOSITE_COUNT);
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+    zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+    /* Turn off Software WEP/TKIP */
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zm_debug_msg0("Disable software encryption");
+        zfStaDisableSWEncryption(dev);
+    }
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    /* Keep Pseudo mode */
+    if ( wd->wlanMode != ZM_MODE_PSEUDO )
+    {
+        wd->wlanMode = ZM_MODE_INFRASTRUCTURE;
+    }
+    return 0;
+}
+
+/* Deauthenticate a STA */
+u16_t zfiWlanDeauth(zdev_t* dev, u16_t* macAddr, u16_t reason)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        //u16_t id;
+
+        /*
+         * we will reset all key in zfHpResetKeyCache() when call
+         * zfiWlanDisable(), if we want to reset PairwiseKey for each sta,
+         * need to use a nullAddr to let keyindex not match.
+         * otherwise hardware will still find PairwiseKey when AP change
+         * encryption mode from WPA to WEP
+         */
+
+        /*
+        if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+        {
+            u32_t key[8];
+            u16_t nullAddr[3] = { 0x0, 0x0, 0x0 };
+
+            if (wd->ap.staTable[i].encryMode != ZM_NO_WEP)
+            {
+                zfHpSetApPairwiseKey(dev, nullAddr,
+                        ZM_NO_WEP, &key[0], &key[4], i+1);
+            }
+            //zfHpSetApPairwiseKey(dev, (u16_t *)macAddr,
+            //        ZM_NO_WEP, &key[0], &key[4], id+1);
+            wd->ap.staTable[id].encryMode = ZM_NO_WEP;
+            wd->ap.staTable[id].keyIdx = 0xff;
+        }
+        */
+
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, macAddr, reason, 0, 0);
+    }
+    else
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+    }
+
+    /* Issue DEAUTH command to FW */
+    return 0;
+}
+
+
+/* XP packet filter feature : */
+/* 1=>enable: All multicast address packets, not just the ones enumerated in the multicast address list. */
+/* 0=>disable */
+void zfiWlanSetAllMulticast(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+    zm_msg1_mm(ZM_LV_0, "sta.bAllMulticast = ", setting);
+    wd->sta.bAllMulticast = (u8_t)setting;
+}
+
+
+/* HT configure API */
+void zfiWlanSetHTCtrl(zdev_t* dev, u32_t *setting, u32_t forceTxTPC)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->preambleType        = (u8_t)setting[0];
+    wd->sta.preambleTypeHT  = (u8_t)setting[1];
+    wd->sta.htCtrlBandwidth = (u8_t)setting[2];
+    wd->sta.htCtrlSTBC      = (u8_t)setting[3];
+    wd->sta.htCtrlSG        = (u8_t)setting[4];
+    wd->sta.defaultTA       = (u8_t)setting[5];
+    wd->enableAggregation   = (u8_t)setting[6];
+    wd->enableWDS           = (u8_t)setting[7];
+
+    wd->forceTxTPC          = forceTxTPC;
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanQueryHTCtrl(zdev_t* dev, u32_t *setting, u32_t *forceTxTPC)
+{
+    zmw_get_wlan_dev(dev);
+
+    setting[0] = wd->preambleType;
+    setting[1] = wd->sta.preambleTypeHT;
+    setting[2] = wd->sta.htCtrlBandwidth;
+    setting[3] = wd->sta.htCtrlSTBC;
+    setting[4] = wd->sta.htCtrlSG;
+    setting[5] = wd->sta.defaultTA;
+    setting[6] = wd->enableAggregation;
+    setting[7] = wd->enableWDS;
+
+    *forceTxTPC = wd->forceTxTPC;
+}
+
+void zfiWlanDbg(zdev_t* dev, u8_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->enableHALDbgInfo = setting;
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanSetRxPacketDump(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+    if (setting)
+    {
+        wd->rxPacketDump = 1;   /* enable */
+    }
+    else
+    {
+        wd->rxPacketDump = 0;   /* disable */
+    }
+}
+
+
+/* FB50 in OS XP, RD private test code */
+/* Tally */
+void zfiWlanResetTally(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+	   wd->commTally.txUnicastFrm = 0;		//txUnicastFrames
+	   wd->commTally.txMulticastFrm = 0;		//txMulticastFrames
+	   wd->commTally.txUnicastOctets = 0;	//txUniOctets  byte size
+	   wd->commTally.txMulticastOctets = 0;	//txMultiOctets  byte size
+	   wd->commTally.txFrmUpperNDIS = 0;
+	   wd->commTally.txFrmDrvMgt = 0;
+	   wd->commTally.RetryFailCnt = 0;
+	   wd->commTally.Hw_TotalTxFrm = 0;		//Hardware total Tx Frame
+	   wd->commTally.Hw_RetryCnt = 0;		//txMultipleRetriesFrames
+	   wd->commTally.Hw_UnderrunCnt = 0;//
+	   wd->commTally.DriverRxFrmCnt = 0;//
+	   wd->commTally.rxUnicastFrm = 0;		//rxUnicastFrames
+	   wd->commTally.rxMulticastFrm = 0;	//rxMulticastFrames
+	   wd->commTally.NotifyNDISRxFrmCnt = 0;//
+	   wd->commTally.rxUnicastOctets = 0;	//rxUniOctets  byte size
+	   wd->commTally.rxMulticastOctets = 0;	//rxMultiOctets  byte size
+	   wd->commTally.DriverDiscardedFrm = 0;// Discard by ValidateFrame
+	   wd->commTally.LessThanDataMinLen = 0;//
+	   wd->commTally.GreaterThanMaxLen = 0;//
+	   wd->commTally.DriverDiscardedFrmCauseByMulticastList = 0;
+	   wd->commTally.DriverDiscardedFrmCauseByFrmCtrl = 0;
+	   wd->commTally.rxNeedFrgFrm = 0;		// need more frg frm
+	   wd->commTally.DriverRxMgtFrmCnt = 0;
+	   wd->commTally.rxBroadcastFrm = 0;	//Receive broadcast frame count
+	   wd->commTally.rxBroadcastOctets = 0;	//Receive broadcast frame byte size
+	   wd->commTally.Hw_TotalRxFrm = 0;//
+	   wd->commTally.Hw_CRC16Cnt = 0;		//rxPLCPCRCErrCnt
+	   wd->commTally.Hw_CRC32Cnt = 0;		//rxCRC32ErrCnt
+	   wd->commTally.Hw_DecrypErr_UNI = 0;//
+	   wd->commTally.Hw_DecrypErr_Mul = 0;//
+	   wd->commTally.Hw_RxFIFOOverrun = 0;//
+	   wd->commTally.Hw_RxTimeOut = 0;
+	   wd->commTally.LossAP = 0;//
+
+	   wd->commTally.Tx_MPDU = 0;
+	   wd->commTally.BA_Fail = 0;
+	   wd->commTally.Hw_Tx_AMPDU = 0;
+	   wd->commTally.Hw_Tx_MPDU = 0;
+
+    wd->commTally.txQosDropCount[0] = 0;
+    wd->commTally.txQosDropCount[1] = 0;
+    wd->commTally.txQosDropCount[2] = 0;
+    wd->commTally.txQosDropCount[3] = 0;
+    wd->commTally.txQosDropCount[4] = 0;
+
+    wd->commTally.Hw_RxMPDU = 0;
+    wd->commTally.Hw_RxDropMPDU = 0;
+    wd->commTally.Hw_RxDelMPDU = 0;
+
+    wd->commTally.Hw_RxPhyMiscError = 0;
+    wd->commTally.Hw_RxPhyXRError = 0;
+    wd->commTally.Hw_RxPhyOFDMError = 0;
+    wd->commTally.Hw_RxPhyCCKError = 0;
+    wd->commTally.Hw_RxPhyHTError = 0;
+    wd->commTally.Hw_RxPhyTotalCount = 0;
+
+#if	(defined(GCCK) && defined(OFDM))
+	   wd->commTally.rx11bDataFrame = 0;
+	   wd->commTally.rxOFDMDataFrame = 0;
+#endif
+
+    zmw_leave_critical_section(dev);
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanQueryTally(zdev_t* dev, struct zsCommTally *tally)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    zfMemoryCopy((u8_t*)tally, (u8_t*)&wd->commTally, sizeof(struct zsCommTally));
+    zmw_leave_critical_section(dev);
+}
+void zfiWlanQueryTrafTally(zdev_t* dev, struct zsTrafTally *tally)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    zfMemoryCopy((u8_t*)tally, (u8_t*)&wd->trafTally, sizeof(struct zsTrafTally));
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanQueryMonHalRxInfo(zdev_t* dev, struct zsMonHalRxInfo *monHalRxInfo)
+{
+    zfHpQueryMonHalRxInfo(dev, (u8_t *)monHalRxInfo);
+}
+
+/* parse the modeMDKEnable to DrvCore */
+void zfiDKEnable(zdev_t* dev, u32_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->modeMDKEnable = enable;
+    zm_debug_msg1("modeMDKEnable = ", wd->modeMDKEnable);
+}
+
+/* airoPeek */
+u32_t zfiWlanQueryPacketTypePromiscuous(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->swSniffer;
+}
+
+/* airoPeek */
+void zfiWlanSetPacketTypePromiscuous(zdev_t* dev, u32_t setValue)
+{
+    zmw_get_wlan_dev(dev);
+
+	   wd->swSniffer = setValue;
+	   zm_msg1_mm(ZM_LV_0, "wd->swSniffer ", wd->swSniffer);
+	   if (setValue)
+	   {
+	       	/* write register for sniffer mode */
+            zfHpSetSnifferMode(dev, 1);
+            zm_msg0_mm(ZM_LV_1, "enalbe sniffer mode");
+	   }
+	   else
+	   {
+            zfHpSetSnifferMode(dev, 0);
+            zm_msg0_mm(ZM_LV_0, "disalbe sniffer mode");
+	   }
+}
+
+void zfiWlanSetXLinkMode(zdev_t* dev, u32_t setValue)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->XLinkMode = setValue;
+    if (setValue)
+    {
+        /* write register for sniffer mode */
+        zfHpSetSnifferMode(dev, 1);
+    }
+    else
+    {
+        zfHpSetSnifferMode(dev, 0);
+    }
+}
+
+extern void zfStaChannelManagement(zdev_t* dev, u8_t scan);
+void zfiSetChannelManagement(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch (setting)
+    {
+        case 1:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 1;
+            wd->ExtOffset   = 1;
+            break;
+        case 3:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 1;
+            wd->ExtOffset   = 3;
+            break;
+        case 0:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 0;
+            wd->ExtOffset   = 0;
+            break;
+        default:
+            wd->BandWidth40 = 0;
+            wd->ExtOffset   = 0;
+            break;
+
+    }
+    zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, NULL);
+}
+
+void zfiSetRifs(zdev_t* dev, u16_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.ie.HtInfo.ChannelInfo |= ExtHtCap_RIFSMode;
+    wd->sta.EnableHT = 1;
+    switch (setting)
+    {
+        case 0:
+            wd->sta.HT2040 = 0;
+//            zfHpSetRifs(dev, 1, 0, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+        case 1:
+            wd->sta.HT2040 = 1;
+//            zfHpSetRifs(dev, 1, 1, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+        default:
+            wd->sta.HT2040 = 0;
+//            zfHpSetRifs(dev, 1, 0, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+    }
+}
+
+void zfiCheckRifs(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if(wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_RIFSMode)
+    {
+//        zfHpSetRifs(dev, wd->sta.EnableHT, wd->sta.HT2040, (wd->sta.currentFrequency < 3000)? 1:0);
+    }
+}
+
+void zfiSetReorder(zdev_t* dev, u16_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->reorder = value;
+}
+
+void zfiSetSeqDebug(zdev_t* dev, u16_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->seq_debug = value;
+}
diff --git a/drivers/staging/otus/80211core/cfunc.c b/drivers/staging/otus/80211core/cfunc.c
new file mode 100644
index 000000000000..d7c49d7523df
--- /dev/null
+++ b/drivers/staging/otus/80211core/cfunc.c
@@ -0,0 +1,1227 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+u8_t zfQueryOppositeRate(zdev_t* dev, u8_t dst_mac[6], u8_t frameType)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* For AP's rate adaption */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        return 0;
+    }
+
+    /* For STA's rate adaption */
+    if ( (frameType & 0x0c) == ZM_WLAN_DATA_FRAME )
+    {
+        if ( ZM_IS_MULTICAST(dst_mac) )
+        {
+            return wd->sta.mTxRate;
+        }
+        else
+        {
+            return wd->sta.uTxRate;
+        }
+    }
+
+    return wd->sta.mmTxRate;
+}
+
+void zfCopyToIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                         u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length;i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset+i, src[i]);
+    }
+}
+
+void zfCopyToRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                      u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length;i++)
+    {
+        zmw_rx_buf_writeb(dev, buf, offset+i, src[i]);
+    }
+}
+
+void zfCopyFromIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                           u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        dst[i] = zmw_tx_buf_readb(dev, buf, offset+i);
+    }
+}
+
+void zfCopyFromRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                        u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        dst[i] = zmw_rx_buf_readb(dev, buf, offset+i);
+    }
+}
+
+#if 1
+void zfMemoryCopy(u8_t* dst, u8_t* src, u16_t length)
+{
+    zfwMemoryCopy(dst, src, length);
+}
+
+void zfMemoryMove(u8_t* dst, u8_t* src, u16_t length)
+{
+    zfwMemoryMove(dst, src, length);
+}
+
+void zfZeroMemory(u8_t* va, u16_t length)
+{
+    zfwZeroMemory(va, length);
+}
+
+u8_t zfMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length)
+{
+    return zfwMemoryIsEqual(m1, m2, length);
+}
+#endif
+
+u8_t zfRxBufferEqualToStr(zdev_t* dev, zbuf_t* buf,
+                          const u8_t* str, u16_t offset, u16_t length)
+{
+    u16_t i;
+    u8_t ch;
+
+    for(i=0; i<length; i++)
+    {
+        ch = zmw_rx_buf_readb(dev, buf, offset+i);
+        if ( ch != str[i] )
+        {
+            return FALSE;
+        }
+    }
+
+    return TRUE;
+}
+
+void zfTxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        zmw_tx_buf_writeb(dev, dst, dstOffset+i,
+                          zmw_tx_buf_readb(dev, src, srcOffset+i));
+    }
+}
+
+void zfRxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        zmw_rx_buf_writeb(dev, dst, dstOffset+i,
+                             zmw_rx_buf_readb(dev, src, srcOffset+i));
+    }
+}
+
+
+void zfCollectHWTally(zdev_t*dev, u32_t* rsp, u8_t id)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (id == 0)
+    {
+        wd->commTally.Hw_UnderrunCnt += (0xFFFF & rsp[1]);
+        wd->commTally.Hw_TotalRxFrm += rsp[2];
+        wd->commTally.Hw_CRC32Cnt += rsp[3];
+        wd->commTally.Hw_CRC16Cnt += rsp[4];
+        #ifdef ZM_ENABLE_NATIVE_WIFI
+        /* These code are here to satisfy Vista DTM */
+        wd->commTally.Hw_DecrypErr_UNI += ((rsp[5]>50) && (rsp[5]<60))?50:rsp[5];
+        #else
+        wd->commTally.Hw_DecrypErr_UNI += rsp[5];
+        #endif
+        wd->commTally.Hw_RxFIFOOverrun += rsp[6];
+        wd->commTally.Hw_DecrypErr_Mul += rsp[7];
+        wd->commTally.Hw_RetryCnt += rsp[8];
+        wd->commTally.Hw_TotalTxFrm += rsp[9];
+        wd->commTally.Hw_RxTimeOut +=rsp[10];
+
+        wd->commTally.Tx_MPDU += rsp[11];
+        wd->commTally.BA_Fail += rsp[12];
+        wd->commTally.Hw_Tx_AMPDU += rsp[13];
+        wd->commTally.Hw_Tx_MPDU += rsp[14];
+        wd->commTally.RateCtrlTxMPDU += rsp[11];
+        wd->commTally.RateCtrlBAFail += rsp[12];
+    }
+    else
+    {
+        wd->commTally.Hw_RxMPDU += rsp[1];
+        wd->commTally.Hw_RxDropMPDU += rsp[2];
+        wd->commTally.Hw_RxDelMPDU += rsp[3];
+
+        wd->commTally.Hw_RxPhyMiscError += rsp[4];
+        wd->commTally.Hw_RxPhyXRError += rsp[5];
+        wd->commTally.Hw_RxPhyOFDMError += rsp[6];
+        wd->commTally.Hw_RxPhyCCKError += rsp[7];
+        wd->commTally.Hw_RxPhyHTError += rsp[8];
+        wd->commTally.Hw_RxPhyTotalCount += rsp[9];
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (id == 0)
+    {
+        zm_msg1_mm(ZM_LV_1, "rsplen =", rsp[0]);
+        zm_msg1_mm(ZM_LV_1, "Hw_UnderrunCnt    = ", (0xFFFF & rsp[1]));
+        zm_msg1_mm(ZM_LV_1, "Hw_TotalRxFrm     = ", rsp[2]);
+        zm_msg1_mm(ZM_LV_1, "Hw_CRC32Cnt       = ", rsp[3]);
+        zm_msg1_mm(ZM_LV_1, "Hw_CRC16Cnt       = ", rsp[4]);
+        zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_UNI  = ", rsp[5]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxFIFOOverrun  = ", rsp[6]);
+        zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_Mul  = ", rsp[7]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RetryCnt       = ", rsp[8]);
+        zm_msg1_mm(ZM_LV_1, "Hw_TotalTxFrm     = ", rsp[9]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxTimeOut      = ", rsp[10]);
+        zm_msg1_mm(ZM_LV_1, "Tx_MPDU           = ", rsp[11]);
+        zm_msg1_mm(ZM_LV_1, "BA_Fail           = ", rsp[12]);
+        zm_msg1_mm(ZM_LV_1, "Hw_Tx_AMPDU       = ", rsp[13]);
+        zm_msg1_mm(ZM_LV_1, "Hw_Tx_MPDU        = ", rsp[14]);
+    }
+    else
+    {
+        zm_msg1_mm(ZM_LV_1, "rsplen             = ", rsp[0]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxMPDU          = ", (0xFFFF & rsp[1]));
+        zm_msg1_mm(ZM_LV_1, "Hw_RxDropMPDU      = ", rsp[2]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxDelMPDU       = ", rsp[3]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyMiscError  = ", rsp[4]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyXRError    = ", rsp[5]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyOFDMError  = ", rsp[6]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyCCKError   = ", rsp[7]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyHTError    = ", rsp[8]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyTotalCount = ", rsp[9]);
+    }
+
+}
+
+/* Timer related functions */
+void zfTimerInit(zdev_t* dev)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("");
+
+    wd->timerList.freeCount = ZM_MAX_TIMER_COUNT;
+    wd->timerList.head = &(wd->timerList.list[0]);
+    wd->timerList.tail = &(wd->timerList.list[ZM_MAX_TIMER_COUNT-1]);
+    wd->timerList.head->pre = NULL;
+    wd->timerList.head->next = &(wd->timerList.list[1]);
+    wd->timerList.tail->pre = &(wd->timerList.list[ZM_MAX_TIMER_COUNT-2]);
+    wd->timerList.tail->next = NULL;
+
+    for( i=1; i<(ZM_MAX_TIMER_COUNT-1); i++ )
+    {
+        wd->timerList.list[i].pre = &(wd->timerList.list[i-1]);
+        wd->timerList.list[i].next = &(wd->timerList.list[i+1]);
+    }
+
+    wd->bTimerReady = TRUE;
+}
+
+
+u16_t zfTimerSchedule(zdev_t* dev, u16_t event, u32_t tick)
+{
+    struct zsTimerEntry *pFreeEntry;
+    struct zsTimerEntry *pEntry;
+    u8_t   i, count;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->timerList.freeCount == 0 )
+    {
+        zm_debug_msg0("no more timer");
+        return 1;
+    }
+
+    //zm_debug_msg2("event = ", event);
+    //zm_debug_msg1("target tick = ", wd->tick + tick);
+
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    if ( count == 0 )
+    {
+        wd->timerList.freeCount--;
+        wd->timerList.head->event = event;
+        wd->timerList.head->timer = wd->tick + tick;
+        //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+
+        return 0;
+    }
+
+    pFreeEntry = wd->timerList.tail;
+    pFreeEntry->timer = wd->tick + tick;
+    pFreeEntry->event = event;
+    wd->timerList.tail = pFreeEntry->pre;
+    pEntry = wd->timerList.head;
+
+    for( i=0; i<count; i++ )
+    {
+        // prevent from the case of tick overflow
+        if ( ( pEntry->timer > pFreeEntry->timer )&&
+             ((pEntry->timer - pFreeEntry->timer) < 1000000000) )
+        {
+            if ( i != 0 )
+            {
+                pFreeEntry->pre = pEntry->pre;
+                pFreeEntry->pre->next = pFreeEntry;
+            }
+            else
+            {
+                pFreeEntry->pre = NULL;
+            }
+
+            pEntry->pre = pFreeEntry;
+            pFreeEntry->next = pEntry;
+            break;
+        }
+
+        pEntry = pEntry->next;
+    }
+
+    if ( i == 0 )
+    {
+        wd->timerList.head = pFreeEntry;
+    }
+
+    if ( i == count )
+    {
+        pFreeEntry->pre = pEntry->pre;
+        pFreeEntry->pre->next = pFreeEntry;
+        pEntry->pre = pFreeEntry;
+        pFreeEntry->next = pEntry;
+    }
+
+    wd->timerList.freeCount--;
+    //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+
+    return 0;
+}
+
+u16_t zfTimerCancel(zdev_t* dev, u16_t event)
+{
+    struct zsTimerEntry *pEntry;
+    u8_t   i, count;
+
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg2("event = ", event);
+    //zm_debug_msg1("free timer count(b) = ", wd->timerList.freeCount);
+
+    pEntry = wd->timerList.head;
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    for( i=0; i<count; i++ )
+    {
+        if ( pEntry->event == event )
+        {
+            if ( pEntry == wd->timerList.head )
+            {   /* remove head entry */
+                wd->timerList.head = pEntry->next;
+                wd->timerList.tail->next = pEntry;
+                pEntry->pre = wd->timerList.tail;
+                wd->timerList.tail = pEntry;
+                pEntry = wd->timerList.head;
+            }
+            else
+            {   /* remove non-head entry */
+                pEntry->pre->next = pEntry->next;
+                pEntry->next->pre = pEntry->pre;
+                wd->timerList.tail->next = pEntry;
+                pEntry->pre = wd->timerList.tail;
+                wd->timerList.tail = pEntry;
+                pEntry = pEntry->next;
+            }
+
+            wd->timerList.freeCount++;
+        }
+        else
+        {
+            pEntry = pEntry->next;
+        }
+    }
+
+    //zm_debug_msg1("free timer count(a) = ", wd->timerList.freeCount);
+
+    return 0;
+}
+
+void zfTimerClear(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->timerList.freeCount = ZM_MAX_TIMER_COUNT;
+}
+
+u16_t zfTimerCheckAndHandle(zdev_t* dev)
+{
+    struct zsTimerEntry *pEntry;
+    struct zsTimerEntry *pTheLastEntry = NULL;
+    u16_t  event[ZM_MAX_TIMER_COUNT];
+    u8_t   i, j=0, count;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( !wd->bTimerReady )
+    {
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    pEntry = wd->timerList.head;
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    for( i=0; i<count; i++ )
+    {
+        // prevent from the case of tick overflow
+        if ( ( pEntry->timer > wd->tick )&&
+             ((pEntry->timer - wd->tick) < 1000000000) )
+        {
+            break;
+        }
+
+        event[j++] = pEntry->event;
+        pTheLastEntry = pEntry;
+        pEntry = pEntry->next;
+    }
+
+    if ( j > 0 )
+    {
+        wd->timerList.tail->next = wd->timerList.head;
+        wd->timerList.head->pre = wd->timerList.tail;
+        wd->timerList.head = pEntry;
+        wd->timerList.tail = pTheLastEntry;
+        wd->timerList.freeCount += j;
+        //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfProcessEvent(dev, event, j);
+
+    return 0;
+}
+
+u32_t zfCoreSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key)
+{
+    u32_t ret;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->sta.flagKeyChanging++;
+    zm_debug_msg1("   zfCoreSetKey++++ ", wd->sta.flagKeyChanging);
+    zmw_leave_critical_section(dev);
+
+    ret = zfHpSetKey(dev, user, keyId, type, mac, key);
+    return ret;
+}
+
+void zfCoreSetKeyComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+#if 0
+    wd->sta.flagKeyChanging = 0;
+#else
+    if(wd->sta.flagKeyChanging)
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.flagKeyChanging--;
+        zmw_leave_critical_section(dev);
+    }
+#endif
+    zm_debug_msg1("  zfCoreSetKeyComplete--- ", wd->sta.flagKeyChanging);
+
+    zfPushVtxq(dev);
+}
+
+void zfCoreHalInitComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->halState = ZM_HAL_STATE_RUNNING;
+    zmw_leave_critical_section(dev);
+
+    zfPushVtxq(dev);
+}
+
+void zfCoreMacAddressNotify(zdev_t* dev, u8_t* addr)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->macAddr[0] = addr[0] | ((u16_t)addr[1]<<8);
+    wd->macAddr[1] = addr[2] | ((u16_t)addr[3]<<8);
+    wd->macAddr[2] = addr[4] | ((u16_t)addr[5]<<8);
+
+
+    //zfHpSetMacAddress(dev, wd->macAddr, 0);
+    if (wd->zfcbMacAddressNotify != NULL)
+    {
+        wd->zfcbMacAddressNotify(dev, addr);
+    }
+}
+
+void zfCoreSetIsoName(zdev_t* dev, u8_t* isoName)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.countryIsoName[0] = isoName[0];
+    wd->ws.countryIsoName[1] = isoName[1];
+    wd->ws.countryIsoName[2] = '\0';
+ }
+
+
+extern void zfScanMgrScanEventStart(zdev_t* dev);
+extern u8_t zfScanMgrScanEventTimeout(zdev_t* dev);
+extern void zfScanMgrScanEventRetry(zdev_t* dev);
+
+void zfProcessEvent(zdev_t* dev, u16_t* eventArray, u8_t eventCount)
+{
+    u8_t i, j, bypass = FALSE;
+    u16_t eventBypass[32];
+    u8_t eventBypassCount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfZeroMemory((u8_t*) eventBypass, 64);
+
+    for( i=0; i<eventCount; i++ )
+    {
+        for( j=0; j<eventBypassCount; j++ )
+        {
+            if ( eventBypass[j] == eventArray[i] )
+            {
+                bypass = TRUE;
+                break;
+            }
+        }
+
+        if ( bypass )
+        {
+            continue;
+        }
+
+        switch( eventArray[i] )
+        {
+            case ZM_EVENT_SCAN:
+                {
+                    zfScanMgrScanEventStart(dev);
+                    eventBypass[eventBypassCount++] = ZM_EVENT_IN_SCAN;
+                    eventBypass[eventBypassCount++] = ZM_EVENT_TIMEOUT_SCAN;
+                }
+                break;
+
+            case ZM_EVENT_TIMEOUT_SCAN:
+                {
+                    u8_t res;
+
+                    res = zfScanMgrScanEventTimeout(dev);
+                    if ( res == 0 )
+                    {
+                        eventBypass[eventBypassCount++] = ZM_EVENT_TIMEOUT_SCAN;
+                    }
+                    else if ( res == 1 )
+                    {
+                        eventBypass[eventBypassCount++] = ZM_EVENT_IN_SCAN;
+                    }
+                }
+                break;
+
+            case ZM_EVENT_IBSS_MONITOR:
+                {
+                    zfStaIbssMonitoring(dev, 0);
+                }
+                break;
+
+            case ZM_EVENT_IN_SCAN:
+                {
+                    zfScanMgrScanEventRetry(dev);
+                }
+                break;
+
+            case ZM_EVENT_CM_TIMER:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_TIMER");
+
+                    wd->sta.cmMicFailureCount = 0;
+                }
+                break;
+
+            case ZM_EVENT_CM_DISCONNECT:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_DISCONNECT");
+
+                    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+
+                    zmw_enter_critical_section(dev);
+                    //zfTimerSchedule(dev, ZM_EVENT_CM_BLOCK_TIMER,
+                    //                ZM_TICK_CM_BLOCK_TIMEOUT);
+
+                    /* Timer Resolution on WinXP is 15/16 ms  */
+                    /* Decrease Time offset for <XP> Counter Measure */
+                    zfTimerSchedule(dev, ZM_EVENT_CM_BLOCK_TIMER,
+                                         ZM_TICK_CM_BLOCK_TIMEOUT - ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET);
+
+                    zmw_leave_critical_section(dev);
+                    wd->sta.cmMicFailureCount = 0;
+                    //zfiWlanDisable(dev);
+                    zfHpResetKeyCache(dev);
+                    if (wd->zfcbConnectNotify != NULL)
+                    {
+                        wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_DISCONNECT_MIC_FAIL,
+                             wd->sta.bssid);
+                    }
+                }
+                break;
+
+            case ZM_EVENT_CM_BLOCK_TIMER:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_BLOCK_TIMER");
+
+                    //zmw_enter_critical_section(dev);
+                    wd->sta.cmDisallowSsidLength = 0;
+                    if ( wd->sta.bAutoReconnect )
+                    {
+                        zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_BLOCK_TIMER:bAutoReconnect!=0");
+                        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+                        zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+                    }
+                    //zmw_leave_critical_section(dev);
+                }
+                break;
+
+            case ZM_EVENT_TIMEOUT_ADDBA:
+                {
+                    if (!wd->addbaComplete && (wd->addbaCount < 5))
+                    {
+                        zfAggSendAddbaRequest(dev, wd->sta.bssid, 0, 0);
+                        wd->addbaCount++;
+                        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_ADDBA, 100);
+                    }
+                    else
+                    {
+                        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_ADDBA);
+                    }
+                }
+                break;
+
+            #ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+            case ZM_EVENT_TIMEOUT_PERFORMANCE:
+                {
+                    zfiPerformanceRefresh(dev);
+                }
+                break;
+            #endif
+            case ZM_EVENT_SKIP_COUNTERMEASURE:
+				//enable the Countermeasure
+				{
+					zm_debug_msg0("Countermeasure : Enable MIC Check ");
+					wd->TKIP_Group_KeyChanging = 0x0;
+				}
+				break;
+
+            default:
+                break;
+        }
+    }
+}
+
+void zfBssInfoCreate(zdev_t* dev)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.bssList.bssCount = 0;
+    wd->sta.bssList.head = NULL;
+    wd->sta.bssList.tail = NULL;
+    wd->sta.bssInfoArrayHead = 0;
+    wd->sta.bssInfoArrayTail = 0;
+    wd->sta.bssInfoFreeCount = ZM_MAX_BSS;
+
+    for( i=0; i< ZM_MAX_BSS; i++ )
+    {
+        //wd->sta.bssInfoArray[i] = &(wd->sta.bssInfoPool[i]);
+        wd->sta.bssInfoArray[i] = zfwMemAllocate(dev, sizeof(struct zsBssInfo));
+
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfBssInfoDestroy(zdev_t* dev)
+{
+    u8_t   i;
+    zmw_get_wlan_dev(dev);
+
+    zfBssInfoRefresh(dev, 1);
+
+    for( i=0; i< ZM_MAX_BSS; i++ )
+    {
+        if (wd->sta.bssInfoArray[i] != NULL)
+        {
+            zfwMemFree(dev, wd->sta.bssInfoArray[i], sizeof(struct zsBssInfo));
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    }
+    return;
+}
+
+struct zsBssInfo* zfBssInfoAllocate(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.bssInfoFreeCount == 0)
+        return NULL;
+
+    pBssInfo = wd->sta.bssInfoArray[wd->sta.bssInfoArrayHead];
+    wd->sta.bssInfoArray[wd->sta.bssInfoArrayHead] = NULL;
+    wd->sta.bssInfoArrayHead = (wd->sta.bssInfoArrayHead + 1) & (ZM_MAX_BSS - 1);
+    wd->sta.bssInfoFreeCount--;
+
+    zfZeroMemory((u8_t*)pBssInfo, sizeof(struct zsBssInfo));
+
+    return pBssInfo;
+}
+
+void zfBssInfoFree(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(wd->sta.bssInfoArray[wd->sta.bssInfoArrayTail] == NULL);
+
+    pBssInfo->signalStrength = pBssInfo->signalQuality = 0;
+    pBssInfo->sortValue = 0;
+
+    wd->sta.bssInfoArray[wd->sta.bssInfoArrayTail] = pBssInfo;
+    wd->sta.bssInfoArrayTail = (wd->sta.bssInfoArrayTail + 1) & (ZM_MAX_BSS - 1);
+    wd->sta.bssInfoFreeCount++;
+}
+
+void zfBssInfoReorderList(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo = NULL;
+    struct zsBssInfo* pInsBssInfo = NULL;
+    struct zsBssInfo* pNextBssInfo = NULL;
+    struct zsBssInfo* pPreBssInfo = NULL;
+    u8_t i = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.bssList.bssCount > 1)
+    {
+        pInsBssInfo = wd->sta.bssList.head;
+        wd->sta.bssList.tail = pInsBssInfo;
+        pBssInfo = pInsBssInfo->next;
+        pInsBssInfo->next = NULL;
+        while (pBssInfo != NULL)
+        {
+            i = 0;
+            while (1)
+            {
+//                if (pBssInfo->signalStrength >= pInsBssInfo->signalStrength)
+                if( pBssInfo->sortValue >= pInsBssInfo->sortValue)
+                {
+                    if (i==0)
+                    {
+                        //Insert BssInfo to head
+                        wd->sta.bssList.head = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        pBssInfo->next = pInsBssInfo;
+                        break;
+                    }
+                    else
+                    {
+                        //Insert BssInfo to neither head nor tail
+                        pPreBssInfo->next = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        pBssInfo->next = pInsBssInfo;
+                        break;
+                    }
+                }
+                else
+                {
+                    if (pInsBssInfo->next != NULL)
+                    {
+                        //Signal strength smaller than current BssInfo, check next
+                        pPreBssInfo = pInsBssInfo;
+                        pInsBssInfo = pInsBssInfo->next;
+                    }
+                    else
+                    {
+                        //Insert BssInfo to tail
+                        pInsBssInfo->next = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        wd->sta.bssList.tail = pBssInfo;
+                        pBssInfo->next = NULL;
+                        break;
+                    }
+                }
+                i++;
+            }
+            pBssInfo = pNextBssInfo;
+            pInsBssInfo = wd->sta.bssList.head;
+        }
+    } //if (wd->sta.bssList.bssCount > 1)
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfBssInfoInsertToList(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(pBssInfo);
+
+    //zm_debug_msg2("pBssInfo = ", pBssInfo);
+
+    if ( wd->sta.bssList.bssCount == 0 )
+    {
+        wd->sta.bssList.head = pBssInfo;
+        wd->sta.bssList.tail = pBssInfo;
+    }
+    else
+    {
+        wd->sta.bssList.tail->next = pBssInfo;
+        wd->sta.bssList.tail = pBssInfo;
+    }
+
+    pBssInfo->next = NULL;
+    wd->sta.bssList.bssCount++;
+
+    //zm_debug_msg2("bss count = ", wd->sta.bssList.bssCount);
+}
+
+void zfBssInfoRemoveFromList(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    struct zsBssInfo* pNowBssInfo;
+    struct zsBssInfo* pPreBssInfo = NULL;
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(pBssInfo);
+    zm_assert(wd->sta.bssList.bssCount);
+
+    //zm_debug_msg2("pBssInfo = ", pBssInfo);
+
+    pNowBssInfo = wd->sta.bssList.head;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        if ( pNowBssInfo == pBssInfo )
+        {
+            if ( i == 0 )
+            {   /* remove head */
+                wd->sta.bssList.head = pBssInfo->next;
+            }
+            else
+            {
+                pPreBssInfo->next = pBssInfo->next;
+            }
+
+            if ( i == (wd->sta.bssList.bssCount - 1) )
+            {   /* remove tail */
+                wd->sta.bssList.tail = pPreBssInfo;
+            }
+
+            break;
+        }
+
+        pPreBssInfo = pNowBssInfo;
+        pNowBssInfo = pNowBssInfo->next;
+    }
+
+    zm_assert(i != wd->sta.bssList.bssCount);
+    wd->sta.bssList.bssCount--;
+
+    //zm_debug_msg2("bss count = ", wd->sta.bssList.bssCount);
+}
+
+void zfBssInfoRefresh(zdev_t* dev, u16_t mode)
+{
+    struct zsBssInfo*   pBssInfo;
+    struct zsBssInfo*   pNextBssInfo;
+    u8_t   i, bssCount;
+
+    zmw_get_wlan_dev(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+    bssCount = wd->sta.bssList.bssCount;
+
+    for( i=0; i<bssCount; i++ )
+    {
+        if (mode == 1)
+        {
+            pNextBssInfo = pBssInfo->next;
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+            pBssInfo = pNextBssInfo;
+        }
+        else
+        {
+            if ( pBssInfo->flag & ZM_BSS_INFO_VALID_BIT )
+            {   /* this one must be kept */
+                pBssInfo->flag &= ~ZM_BSS_INFO_VALID_BIT;
+                pBssInfo = pBssInfo->next;
+            }
+            else
+            {
+                #define ZM_BSS_CACHE_TIME_IN_MS   20000
+                if ((wd->tick - pBssInfo->tick) > (ZM_BSS_CACHE_TIME_IN_MS/ZM_MS_PER_TICK))
+                {
+                    pNextBssInfo = pBssInfo->next;
+                    zfBssInfoRemoveFromList(dev, pBssInfo);
+                    zfBssInfoFree(dev, pBssInfo);
+                    pBssInfo = pNextBssInfo;
+                }
+                else
+                {
+                    pBssInfo = pBssInfo->next;
+                }
+            }
+        }
+    } //for( i=0; i<bssCount; i++ )
+    return;
+}
+
+void zfDumpSSID(u8_t length, u8_t *value)
+{
+    u8_t buf[50];
+    u8_t tmpLength = length;
+
+    if ( tmpLength > 49 )
+    {
+        tmpLength = 49;
+    }
+
+    zfMemoryCopy(buf, value, tmpLength);
+    buf[tmpLength] = '\0';
+    //printk("SSID: %s\n", buf);
+    //zm_debug_msg_s("ssid = ", value);
+}
+
+void zfCoreReinit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.flagKeyChanging = 0;
+    wd->sta.flagFreqChanging = 0;
+}
+
+void zfGenerateRandomBSSID(zdev_t* dev, u8_t *MACAddr, u8_t *BSSID)
+{
+    //ULONGLONG   time;
+    u32_t time;
+
+    zmw_get_wlan_dev(dev);
+
+    time = wd->tick;
+
+    //
+    // Initialize the random BSSID to be the same as MAC address.
+    //
+
+    // RtlCopyMemory(BSSID, MACAddr, sizeof(DOT11_MAC_ADDRESS));
+    zfMemoryCopy(BSSID, MACAddr, 6);
+
+    //
+    // Get the system time in 10 millisecond.
+    //
+
+    // NdisGetCurrentSystemTime((PLARGE_INTEGER)&time);
+    // time /= 100000;
+
+    //
+    // Randomize the first 4 bytes of BSSID.
+    //
+
+    BSSID[0] ^= (u8_t)(time & 0xff);
+    BSSID[0] &= ~0x01;              // Turn off multicast bit
+    BSSID[0] |= 0x02;               // Turn on local bit
+
+    time >>= 8;
+    BSSID[1] ^= (u8_t)(time & 0xff);
+
+    time >>= 8;
+    BSSID[2] ^= (u8_t)(time & 0xff);
+
+    time >>= 8;
+    BSSID[3] ^= (u8_t)(time & 0xff);
+}
+
+u8_t zfiWlanGetDestAddrFromBuf(zdev_t *dev, zbuf_t *buf, u16_t *macAddr)
+{
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 16);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 18);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 4);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 6);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 8);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 4);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 6);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 8);
+    }
+    else
+    {
+        return 1;
+    }
+#else
+    /* DA */
+    macAddr[0] = zmw_tx_buf_readh(dev, buf, 0);
+    macAddr[1] = zmw_tx_buf_readh(dev, buf, 2);
+    macAddr[2] = zmw_tx_buf_readh(dev, buf, 4);
+#endif
+
+    return 0;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
+
+u16_t zfFindCleanFrequency(zdev_t* dev, u32_t adhocMode)
+{
+    u8_t   i, j;
+    u16_t  returnChannel;
+    u16_t  count_24G = 0, min24GIndex = 0;
+    u16_t  count_5G = 0,  min5GIndex = 0;
+    u16_t  CombinationBssNumberIn24G[15] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  BssNumberIn24G[17]  = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  Array_24G[15]       = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  BssNumberIn5G[31]   = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  Array_5G[31]        = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((pBssInfo = wd->sta.bssList.head) == NULL)
+    {
+        if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+            adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            returnChannel = zfChGetFirst2GhzChannel(dev);
+        }
+        else
+        {
+            returnChannel = zfChGetFirst5GhzChannel(dev);
+        }
+
+        return returnChannel;
+    }
+
+    /* #1 Get Allowed Channel following Country Code ! */
+    zmw_declare_for_critical_section();
+    zmw_enter_critical_section(dev);
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel < 3000)
+        { // 2.4GHz
+            Array_24G[count_24G] = wd->regulationTable.allowChannel[i].channel;
+            count_24G++;
+        }
+        else
+        { // 5GHz
+            count_5G++;
+            Array_5G[i] = wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    while( pBssInfo != NULL )
+    {
+        /* #2_1 Count BSS number in some specificed frequency in 2.4GHz band ! */
+        if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+            adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            for( i=0; i<=(count_24G+3); i++ )
+            {
+                if( pBssInfo->frequency == Array_24G[i] )
+                { // Array_24G[0] correspond to BssNumberIn24G[2]
+                    BssNumberIn24G[pBssInfo->channel+1]++;
+                }
+            }
+        }
+
+        /* #2_2 Count BSS number in some specificed frequency in 5GHz band ! */
+        if( adhocMode == ZM_ADHOCBAND_A || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            for( i=0; i<count_5G; i++ )
+            { // 5GHz channel is not equal to array index
+                if( pBssInfo->frequency == Array_5G[i] )
+                { // Array_5G[0] correspond to BssNumberIn5G[0]
+                    BssNumberIn5G[i]++;
+                }
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+#if 0
+    for(i=0; i<=(count_24G+3); i++)
+    {
+        printk("2.4GHz Before combin, %d BSS network : %d", i, BssNumberIn24G[i]);
+    }
+
+    for(i=0; i<count_5G; i++)
+    {
+        printk("5GHz Before combin, %d BSS network : %d", i, BssNumberIn5G[i]);
+    }
+#endif
+
+    if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+        adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        /* #3_1 Count BSS number that influence the specificed frequency in 2.4GHz ! */
+        for( j=0; j<count_24G; j++ )
+        {
+            CombinationBssNumberIn24G[j] = BssNumberIn24G[j]   + BssNumberIn24G[j+1] +
+                                           BssNumberIn24G[j+2] + BssNumberIn24G[j+3] +
+                                           BssNumberIn24G[j+4];
+            //printk("After combine, the number of BSS network channel %d is %d",
+            //                                   j , CombinationBssNumberIn24G[j]);
+        }
+
+        /* #4_1 Find the less utilized frequency in 2.4GHz band ! */
+        min24GIndex = zfFindMinimumUtilizationChannelIndex(dev, CombinationBssNumberIn24G, count_24G);
+    }
+
+    /* #4_2 Find the less utilized frequency in 5GHz band ! */
+    if( adhocMode == ZM_ADHOCBAND_A || adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        min5GIndex = zfFindMinimumUtilizationChannelIndex(dev, BssNumberIn5G, count_5G);
+    }
+
+    if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G || adhocMode == ZM_ADHOCBAND_BG )
+    {
+        return Array_24G[min24GIndex];
+    }
+    else if( adhocMode == ZM_ADHOCBAND_A )
+    {
+        return Array_5G[min5GIndex];
+    }
+    else if( adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        if ( CombinationBssNumberIn24G[min24GIndex] <= BssNumberIn5G[min5GIndex] )
+            return Array_24G[min24GIndex];
+        else
+            return Array_5G[min5GIndex];
+    }
+    else
+        return 2412;
+}
+
+u16_t zfFindMinimumUtilizationChannelIndex(zdev_t* dev, u16_t* array, u16_t count)
+{
+    u8_t   i;
+    u16_t  tempMinIndex, tempMinValue;
+
+    zmw_get_wlan_dev(dev);
+
+    i = 1;
+    tempMinIndex = 0;
+    tempMinValue = array[tempMinIndex];
+    while( i< count )
+    {
+        if( array[i] < tempMinValue )
+        {
+            tempMinValue = array[i];
+            tempMinIndex = i;
+        }
+        i++;
+    }
+
+    return tempMinIndex;
+}
+
+u8_t zfCompareWithBssid(zdev_t* dev, u16_t* bssid)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( zfMemoryIsEqual((u8_t*)bssid, (u8_t*)wd->sta.bssid, 6) )
+    {
+        return 1;
+    }
+    else
+    {
+        return 0;
+    }
+}
diff --git a/drivers/staging/otus/80211core/cfunc.h b/drivers/staging/otus/80211core/cfunc.h
new file mode 100644
index 000000000000..fc7548c39d1b
--- /dev/null
+++ b/drivers/staging/otus/80211core/cfunc.h
@@ -0,0 +1,449 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : func_extr.c                                           */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains function prototype.                        */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _CFUNC_H
+#define _CFUNC_H
+
+#include "queue.h"
+
+/* amsdu.c */
+void zfDeAmsdu(zdev_t* dev, zbuf_t* buf, u16_t vap, u8_t encryMode);
+
+/* cscanmgr.c */
+void zfScanMgrInit(zdev_t* dev);
+u8_t zfScanMgrScanStart(zdev_t* dev, u8_t scanType);
+void zfScanMgrScanStop(zdev_t* dev, u8_t scanType);
+void zfScanMgrScanAck(zdev_t* dev);
+
+/* cpsmgr.c */
+void zfPowerSavingMgrInit(zdev_t* dev);
+void zfPowerSavingMgrSetMode(zdev_t* dev, u8_t mode);
+void zfPowerSavingMgrMain(zdev_t* dev);
+void zfPowerSavingMgrWakeup(zdev_t* dev);
+u8_t zfPowerSavingMgrIsSleeping(zdev_t *dev);
+void zfPowerSavingMgrProcessBeacon(zdev_t* dev, zbuf_t* buf);
+void zfPowerSavingMgrAtimWinExpired(zdev_t* dev);
+void zfPowerSavingMgrConnectNotify(zdev_t *dev);
+void zfPowerSavingMgrPreTBTTInterrupt(zdev_t *dev);
+
+/* ccmd.c */
+u16_t zfWlanEnable(zdev_t* dev);
+
+/* cfunc.c */
+u8_t zfQueryOppositeRate(zdev_t* dev, u8_t dst_mac[6], u8_t frameType);
+void zfCopyToIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                         u16_t offset, u16_t length);
+void zfCopyToRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                      u16_t offset, u16_t length);
+void zfCopyFromIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                           u16_t offset, u16_t length);
+void zfCopyFromRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                        u16_t offset, u16_t length);
+void zfMemoryCopy(u8_t* dst, u8_t* src, u16_t length);
+void zfMemoryMove(u8_t* dst, u8_t* src, u16_t length);
+void zfZeroMemory(u8_t* va, u16_t length);
+u8_t zfMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length);
+u8_t zfRxBufferEqualToStr(zdev_t* dev, zbuf_t* buf, const u8_t* str,
+                          u16_t offset, u16_t length);
+void zfTxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length);
+void zfRxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length);
+
+void zfCollectHWTally(zdev_t*dev, u32_t* rsp, u8_t id);
+void zfTimerInit(zdev_t* dev);
+u16_t zfTimerSchedule(zdev_t* dev, u16_t event, u32_t tick);
+u16_t zfTimerCancel(zdev_t* dev, u16_t event);
+void zfTimerClear(zdev_t* dev);
+u16_t zfTimerCheckAndHandle(zdev_t* dev);
+void zfProcessEvent(zdev_t* dev, u16_t* eventArray, u8_t eventCount);
+
+void zfBssInfoCreate(zdev_t* dev);
+void zfBssInfoDestroy(zdev_t* dev);
+
+struct zsBssInfo* zfBssInfoAllocate(zdev_t* dev);
+void zfBssInfoFree(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoReorderList(zdev_t* dev);
+void zfBssInfoInsertToList(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoRemoveFromList(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoRefresh(zdev_t* dev, u16_t mode);
+void zfCoreSetFrequencyComplete(zdev_t* dev);
+void zfCoreSetFrequency(zdev_t* dev, u16_t frequency);
+void zfCoreSetFrequencyV2(zdev_t* dev, u16_t frequency,
+        zfpFreqChangeCompleteCb cb);
+void zfCoreSetFrequencyEx(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb);
+void zfCoreSetFrequencyExV2(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb, u8_t forceSetFreq);
+void zfReSetCurrentFrequency(zdev_t* dev);
+u32_t zfCoreSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key);
+void zfCoreSetKeyComplete(zdev_t* dev);
+void zfCoreReinit(zdev_t* dev);
+void zfCoreMacAddressNotify(zdev_t* dev, u8_t *addr);
+void zfCoreSetIsoName(zdev_t* dev, u8_t* isoName);
+void zfGenerateRandomBSSID(zdev_t* dev, u8_t *MACAddr, u8_t *BSSID);
+void zfCoreHalInitComplete(zdev_t* dev);
+
+u16_t zfFindCleanFrequency(zdev_t* dev, u32_t adhocMode);
+u16_t zfFindMinimumUtilizationChannelIndex(zdev_t* dev, u16_t* array, u16_t count);
+u8_t zfCompareWithBssid(zdev_t* dev, u16_t* bssid);
+
+/* chb.c */
+void zfDumpBssList(zdev_t* dev);
+
+
+u16_t zfIssueCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen, u16_t src, u8_t* buf);
+
+
+/* cic.c */
+void zfUpdateBssid(zdev_t* dev, u8_t* bssid);
+void zfResetSupportRate(zdev_t* dev, u8_t type);
+void zfUpdateSupportRate(zdev_t* dev, u8_t* rateArray);
+u8_t zfIsGOnlyMode(zdev_t* dev, u16_t  frequency, u8_t* rateArray);
+void zfGatherBMode(zdev_t* dev, u8_t* rateArray, u8_t* extrateArray);
+u8_t zfPSDeviceSleep(zdev_t* dev);
+u16_t zfGetRandomNumber(zdev_t* dev, u16_t initValue);
+void zfCoreEvent(zdev_t* dev, u16_t event, u8_t* rsp);
+void zfBeaconCfgInterrupt(zdev_t* dev, u8_t* rsp);
+void zfEndOfAtimWindowInterrupt(zdev_t* dev);
+
+/* cinit.c */
+u16_t zfTxGenWlanHeader(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t seq,
+                        u8_t flag, u16_t plusLen, u16_t minusLen, u16_t port,
+                        u16_t* da, u16_t* sa, u8_t up, u16_t *micLen,
+                        u16_t* snap, u16_t snapLen, struct aggControl *aggControl);
+u16_t zfTxGenMmHeader(zdev_t* dev, u8_t frameType, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+void zfInitMacApMode(zdev_t* dev);
+u16_t zfChGetNextChannel(zdev_t* dev, u16_t frequency, u8_t* pbPassive);
+u16_t zfChGetFirstChannel(zdev_t* dev, u8_t* pbPassive);
+u16_t zfChGetFirst2GhzChannel(zdev_t* dev);
+u16_t zfChGetFirst5GhzChannel(zdev_t* dev);
+u16_t zfChGetLastChannel(zdev_t* dev, u8_t* pbPassive);
+u16_t zfChGetLast5GhzChannel(zdev_t* dev);
+u16_t zfChNumToFreq(zdev_t* dev, u8_t ch, u8_t freqBand);
+u8_t zfChFreqToNum(u16_t freq, u8_t* bIs5GBand);
+
+/* cmm.c */
+void zfProcessManagement(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo); //CWYang(m)
+void zfSendMmFrame(zdev_t* dev, u8_t frameType, u16_t* dst,
+                   u32_t p1, u32_t p2, u32_t p3);
+u16_t zfFindElement(zdev_t* dev, zbuf_t* buf, u8_t eid);
+u16_t zfFindWifiElement(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype);
+u16_t zfFindSuperGElement(zdev_t* dev, zbuf_t* buf, u8_t type);
+u16_t zfFindXRElement(zdev_t* dev, zbuf_t* buf, u8_t type);
+u16_t zfRemoveElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t eid);
+u16_t zfUpdateElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t* updateeid);
+void zfProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src);
+void zfProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+u16_t zfSendProbeReq(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t bWithSSID);
+u16_t zfMmAddIeSupportRate(zdev_t* dev, zbuf_t* buf,
+                           u16_t offset, u8_t eid, u8_t rateSet);
+u16_t zfMmAddIeDs(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfMmAddIeErp(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfUpdateDefaultQosParameter(zdev_t* dev, u8_t mode);
+u16_t zfMmAddIeWpa(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t apId);
+u16_t zfMmAddHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset); //CWYang(+)
+u16_t zfMmAddPreNHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfMmAddExtendedHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset); //CWYang(+)
+u16_t zfFindATHExtCap(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype);
+u16_t zfFindBrdcmMrvlRlnkExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindMarvelExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindBroadcomExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindRlnkExtCap(zdev_t* dev, zbuf_t* buf);
+
+/* cmmap.c */
+void zfMmApTimeTick(zdev_t* dev);
+void zfApAgingSta(zdev_t* dev);
+u16_t zfApAddSta(zdev_t* dev, u16_t* addr, u16_t state, u16_t apId, u8_t type,
+                 u8_t qosType, u8_t qosInfo);
+void zfApProtctionMonitor(zdev_t* dev);
+void zfApProcessBeacon(zdev_t* dev, zbuf_t* buf);
+void zfApProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessAsocRsp(zdev_t* dev, zbuf_t* buf);
+void zfApProcessDeauth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessDisasoc(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+void zfApStoreAsocReqIe(zdev_t* dev, zbuf_t* buf, u16_t aid);
+u16_t zfApAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap);
+void zfApSendBeacon(zdev_t* dev);
+u16_t zfApGetSTAInfo(zdev_t* dev, u16_t* addr, u16_t* state, u8_t* vap);
+u16_t zfIntrabssForward(zdev_t* dev, zbuf_t* buf, u8_t srcVap);
+u16_t zfApBufferPsFrame(zdev_t* dev, zbuf_t* buf, u16_t port);
+void zfApInitStaTbl(zdev_t* dev);
+void zfApGetStaTxRateAndQosType(zdev_t* dev, u16_t* addr, u32_t* phyCtrl,
+                                u8_t* qosType, u16_t* rcProbingFlag);
+void zfApGetStaQosType(zdev_t* dev, u16_t* addr, u8_t* qosType);
+void zfApSetStaTxRate(zdev_t* dev, u16_t* addr, u32_t phyCtrl);
+struct zsMicVar* zfApGetRxMicKey(zdev_t* dev, zbuf_t* buf);
+struct zsMicVar* zfApGetTxMicKey(zdev_t* dev, zbuf_t* buf, u8_t* qosType);
+u16_t zfApAddIeWmePara(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap);
+u16_t zfApUpdatePsBit(zdev_t* dev, zbuf_t* buf, u8_t* vap, u8_t* uapsdTrig);
+void zfApProcessPsPoll(zdev_t* dev, zbuf_t* buf);
+u16_t zfApFindSta(zdev_t* dev, u16_t* addr);
+void zfApGetStaEncryType(zdev_t* dev, u16_t* addr, u8_t* encryType);
+void zfApGetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t* iv16, u32_t* iv32);
+void zfApSetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t iv16, u32_t iv32);
+void zfApClearStaKey(zdev_t* dev, u16_t* addr);
+#ifdef ZM_ENABLE_CENC
+void zfApGetStaCencIvAndKeyIdx(zdev_t* dev, u16_t* addr, u32_t *iv,
+        u8_t *keyIdx);
+void zfApSetStaCencIv(zdev_t* dev, u16_t* addr, u32_t *iv);
+#endif //ZM_ENABLE_CENC
+void zfApSetProtectionMode(zdev_t* dev, u16_t mode);
+void zfApFlushBufferedPsFrame(zdev_t* dev);
+void zfApSendFailure(zdev_t* dev, u8_t* addr);
+u8_t zfApRemoveFromPsQueue(zdev_t* dev, u16_t id, u16_t* src);
+void zfApProcessAction(zdev_t* dev, zbuf_t* buf);
+/* cmmsta.c */
+void zfMmStaTimeTick(zdev_t* dev);
+void zfReWriteBeaconStartAddress(zdev_t* dev);  // Mxzeng
+void zfStaProcessBeacon(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo); //CWYang(m)
+void zfStaProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfStaProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfStaProcessAsocRsp(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessDeauth(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessDisasoc(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+void zfStaProcessAtim(zdev_t* dev, zbuf_t* buf);
+void zfStaStoreAsocRspIe(zdev_t* dev, zbuf_t* buf);
+void zfStaChannelManagement(zdev_t* dev, u8_t scan);
+void zfIbssConnectNetwork(zdev_t* dev);
+void zfInfraConnectNetwork(zdev_t* dev);
+u8_t zfCheckAuthentication(zdev_t* dev, struct zsBssInfo* pBssInfo);
+u8_t zfChangeAdapterState(zdev_t* dev, u8_t newState);
+u16_t zfStaAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+u16_t zfStaAddIeIbss(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfStaStartConnect(zdev_t* dev, u8_t bIsSharedKey);
+u8_t zfStaIsConnected(zdev_t* dev);
+u8_t zfStaIsConnecting(zdev_t* dev);
+u8_t zfStaIsDisconnect(zdev_t* dev);
+void zfStaSendBeacon(zdev_t* dev);
+void zfSendNullData(zdev_t* dev, u8_t type);
+void zfSendPSPoll(zdev_t* dev);
+void zfSendBA(zdev_t* dev, u16_t start_seq, u8_t *bitmap);
+void zdRateInfoCountTx(zdev_t* dev, u16_t* macAddr);
+struct zsMicVar* zfStaGetRxMicKey(zdev_t* dev, zbuf_t* buf);
+struct zsMicVar* zfStaGetTxMicKey(zdev_t* dev, zbuf_t* buf);
+u16_t zfStaRxValidateFrame(zdev_t* dev, zbuf_t* buf);
+void zfStaMicFailureHandling(zdev_t* dev, zbuf_t* buf);
+u8_t zfStaBlockWlanScan(zdev_t* dev);
+void zfStaIbssPSCheckState(zdev_t* dev, zbuf_t* buf);
+u8_t zfStaIbssPSQueueData(zdev_t* dev, zbuf_t* buf);
+void zfStaIbssPSSend(zdev_t* dev);
+void zfStaResetStatus(zdev_t* dev, u8_t bInit);
+u16_t zfStaAddIeWmeInfo(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t qosInfo);
+void zfInitPartnerNotifyEvent(zdev_t* dev, zbuf_t* buf, struct zsPartnerNotifyEvent *event);
+void zfStaInitOppositeInfo(zdev_t* dev);
+void zfStaIbssMonitoring(zdev_t* dev, u8_t reset);
+struct zsBssInfo* zfStaFindBssInfo(zdev_t* dev, zbuf_t* buf, struct zsWlanProbeRspFrameHeader *pProbeRspHeader);
+u8_t zfStaInitBssInfo(zdev_t* dev, zbuf_t* buf,
+        struct zsWlanProbeRspFrameHeader *pProbeRspHeader,
+        struct zsBssInfo* pBssInfo, struct zsAdditionInfo* AddInfo, u8_t type);
+s8_t zfStaFindFreeOpposite(zdev_t* dev, u16_t *sa, int *pFoundIdx);
+s8_t zfStaFindOppositeByMACAddr(zdev_t* dev, u16_t *sa, u8_t *pFoundIdx);
+void zfStaRefreshBlockList(zdev_t* dev, u16_t flushFlag);
+void zfStaConnectFail(zdev_t* dev, u16_t reason, u16_t* bssid, u8_t weight);
+void zfStaGetTxRate(zdev_t* dev, u16_t* macAddr, u32_t* phyCtrl,
+        u16_t* rcProbingFlag);
+u16_t zfStaProcessAction(zdev_t* dev, zbuf_t* buf);
+struct zsTkipSeed* zfStaGetRxSeed(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+u16_t zfStaAddIeCenc(zdev_t* dev, zbuf_t* buf, u16_t offset);
+#endif //ZM_ENABLE_CENC
+void zfStaEnableSWEncryption(zdev_t *dev, u8_t value);
+void zfStaDisableSWEncryption(zdev_t *dev);
+u16_t zfComputeBssInfoWeightValue(zdev_t *dev, u8_t isBMode, u8_t isHT, u8_t isHT40, u8_t signalStrength);
+u16_t zfStaAddIbssAdditionalIE(zdev_t* dev, zbuf_t* buf, u16_t offset);
+
+/* ctkip.c */
+void zfTkipInit(u8_t* key, u8_t* ta, struct zsTkipSeed* pSeed, u8_t* initIv);
+void zfMicSetKey(u8_t* key, struct zsMicVar* pMic);
+void zfMicAppendByte(u8_t b, struct zsMicVar* pMic);
+void zfMicClear(struct zsMicVar* pMic);
+void zfMicAppendTxBuf(zdev_t* dev, zbuf_t* buf, u8_t* da, u8_t* sa,
+                      u16_t removeLen, u8_t* mic);
+u8_t zfMicRxVerify(zdev_t* dev, zbuf_t* buf);
+void zfMicGetMic(u8_t* dst, struct zsMicVar* pMic);
+void zfCalTxMic(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u16_t *da, u16_t *sa, u8_t up, u8_t *mic);
+void zfTKIPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* key, u32_t* icv);
+u16_t zfTKIPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* key);
+void zfTkipGetseeds(u16_t iv16, u8_t *RC4Key, struct zsTkipSeed *Seed);
+u8_t zfTkipPhase1KeyMix(u32_t iv32, struct zsTkipSeed* pSeed);
+u8_t zfTkipPhase2KeyMix(u16_t iv16, struct zsTkipSeed* pSeed);
+void zfWEPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv);
+u16_t zfWEPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv);
+
+/* ctxrx.c */
+u16_t zfSend80211Frame(zdev_t* dev, zbuf_t* buf);
+void zfIsrPciTxComp(zdev_t* dev);
+void zfTxPciDmaStart(zdev_t* dev);
+u16_t zfTxPortControl(zdev_t* dev, zbuf_t* buf, u16_t port);
+u16_t zfTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port,
+                  u16_t bufType, u16_t flag);
+u16_t zfTxGenWlanTail(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t snaplen,
+                      u16_t* mic);
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen);
+void zfTxGetIpTosAndFrag(zdev_t* dev, zbuf_t* buf, u8_t* up, u16_t* fragOff);
+u16_t zfPutVtxq(zdev_t* dev, zbuf_t* buf);
+void zfPushVtxq(zdev_t* dev);
+u8_t zfIsVtxqEmpty(zdev_t* dev);
+u16_t zfGetSeqCtrl(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u8_t zfGetFragNo(zdev_t* dev, zbuf_t* buf);
+void zfShowRxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfShowTxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+u16_t zfPutVmmq(zdev_t* dev, zbuf_t* buf);
+void zfFlushVtxq(zdev_t* dev);
+void zfAgingDefragList(zdev_t* dev, u16_t flushFlag);
+
+void zfLed100msCtrl(zdev_t* dev);
+void zf80211FrameSend(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t snapLen,
+                           u16_t* da, u16_t* sa, u8_t up, u16_t headerLen, u16_t* snap,
+                           u16_t* tail, u16_t tailLen, u16_t offset, u16_t bufType,
+                           u8_t ac, u8_t keyIdx);
+void zfCheckIsRIFSFrame(zdev_t* dev, zbuf_t* buf, u16_t frameSubType);
+
+/* queue.c */
+struct zsQueue* zfQueueCreate(zdev_t* dev, u16_t size);
+void zfQueueDestroy(zdev_t* dev, struct zsQueue* q);
+u16_t zfQueuePutNcs(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick);
+u16_t zfQueuePut(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick);
+zbuf_t* zfQueueGet(zdev_t* dev, struct zsQueue* q);
+zbuf_t* zfQueueGetWithMac(zdev_t* dev, struct zsQueue* q, u8_t* addr, u8_t* mb);
+void zfQueueFlush(zdev_t* dev, struct zsQueue* q);
+void zfQueueAge(zdev_t* dev, struct zsQueue* q, u32_t tick, u32_t msAge);
+void zfQueueGenerateUapsdTim(zdev_t* dev, struct zsQueue* q,
+        u8_t* uniBitMap, u16_t* highestByte);
+
+/* hpmain.c */
+u16_t zfHpInit(zdev_t* dev, u32_t frequency);
+u16_t zfHpRelease(zdev_t* dev);
+void zfHpSetFrequencyEx(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset, u8_t initRF);
+u16_t zfHpStartRecv(zdev_t* dev);
+u16_t zfHpStopRecv(zdev_t* dev);
+u16_t zfHpResetKeyCache(zdev_t* dev);
+u16_t zfHpSetApStaMode(zdev_t* dev, u8_t mode);
+u16_t zfHpSetBssid(zdev_t* dev, u8_t* bssid);
+u16_t zfHpSetSnifferMode(zdev_t* dev, u16_t on);
+u8_t zfHpUpdateQosParameter(zdev_t* dev, u16_t* cwminTbl, u16_t* cwmaxTbl,
+        u16_t* aifsTbl, u16_t* txopTbl);
+void zfHpSetAtimWindow(zdev_t* dev, u16_t atimWin);
+void zfHpEnableBeacon(zdev_t* dev, u16_t mode, u16_t bcnInterval, u16_t dtim, u8_t enableAtim);
+void zfHpDisableBeacon(zdev_t* dev);
+void zfHpSetBasicRateSet(zdev_t* dev, u16_t bRateBasic, u16_t gRateBasic);
+void zfHpSetRTSCTSRate(zdev_t* dev, u32_t rate);
+void zfHpSetMacAddress(zdev_t* dev, u16_t* macAddr, u16_t macAddrId);
+u32_t zfHpGetMacAddress(zdev_t* dev);
+u32_t zfHpGetTransmitPower(zdev_t* dev);
+void zfHpSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList, u8_t bAllMulticast);
+
+u16_t zfHpRemoveKey(zdev_t* dev, u16_t user);
+u32_t zfHpSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key);
+//u32_t zfHpSetStaPairwiseKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+//        u32_t* key, u32_t* micKey);
+//u32_t zfHpSetStaGroupKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+//        u32_t* key, u32_t* micKey);
+u32_t zfHpSetApPairwiseKey(zdev_t* dev, u16_t* staMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t staAid);
+u32_t zfHpSetApGroupKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t vapId);
+u32_t zfHpSetDefaultKey(zdev_t* dev, u8_t keyId, u8_t type, u32_t* key, u32_t* micKey);
+u32_t zfHpSetPerUserKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t* mac, u8_t type, u32_t* key, u32_t* micKey);
+
+void zfHpSendBeacon(zdev_t* dev, zbuf_t* buf, u16_t len);
+u16_t zfHpGetPayloadLen(zdev_t* dev,
+                        zbuf_t* buf,
+                        u16_t len,
+                        u16_t plcpHdrLen,
+                        u32_t *rxMT,
+                        u32_t *rxMCS,
+                        u32_t *rxBW,
+                        u32_t *rxSG
+                        );
+u32_t zfHpGetFreeTxdCount(zdev_t* dev);
+u32_t zfHpGetMaxTxdCount(zdev_t* dev);
+u16_t zfHpSend(zdev_t* dev, u16_t* header, u16_t headerLen,
+        u16_t* snap, u16_t snapLen, u16_t* tail, u16_t tailLen, zbuf_t* buf,
+        u16_t offset, u16_t bufType, u8_t ac, u8_t keyIdx);
+void zfHpGetRegulationTablefromRegionCode(zdev_t* dev, u16_t regionCode);
+void zfHpGetRegulationTablefromCountry(zdev_t* dev, u16_t CountryCode);
+u8_t zfHpGetRegulationTablefromISO(zdev_t* dev, u8_t *countryInfo, u8_t length);
+const char* zfHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode);
+u16_t zfHpGetRegionCodeFromIsoName(zdev_t* dev, u8_t *countryIsoName);
+u8_t zfHpGetRegulatoryDomain(zdev_t* dev);
+void zfHpLedCtrl(zdev_t* dev, u16_t ledId, u8_t mode);
+u16_t zfHpResetTxRx(zdev_t* dev);
+u16_t zfHpDeleteAllowChannel(zdev_t* dev, u16_t freq);
+u16_t zfHpAddAllowChannel(zdev_t* dev, u16_t freq);
+u32_t zfHpCwmUpdate(zdev_t* dev);
+u32_t zfHpAniUpdate(zdev_t* dev);
+u32_t zfHpAniUpdateRssi(zdev_t* dev, u8_t rssi);
+void zfHpAniAttach(zdev_t* dev);
+void zfHpAniArPoll(zdev_t* dev, u32_t listenTime, u32_t phyCnt1, u32_t phyCnt2);
+void zfHpHeartBeat(zdev_t* dev);
+void zfHpPowerSaveSetState(zdev_t* dev, u8_t psState);
+void zfHpPowerSaveSetMode(zdev_t* dev, u8_t staMode, u8_t psMode, u16_t bcnInterval);
+u16_t zfHpIsDfsChannel(zdev_t* dev, u16_t freq);
+u16_t zfHpIsDfsChannelNCS(zdev_t* dev, u16_t freq);
+u16_t zfHpFindFirstNonDfsChannel(zdev_t* dev, u16_t aBand);
+u16_t zfHpIsAllowedChannel(zdev_t* dev, u16_t freq);
+void zfHpDisableDfsChannel(zdev_t* dev, u8_t disableFlag);
+void zfHpSetTTSIFSTime(zdev_t* dev, u8_t sifs_time);
+
+void zfHpQueryMonHalRxInfo(zdev_t* dev, u8_t *monHalRxInfo);
+
+void zfDumpSSID(u8_t length, u8_t *value);
+void zfHpSetAggPktNum(zdev_t* dev, u32_t num);
+void zfHpSetMPDUDensity(zdev_t* dev, u8_t density);
+void zfHpSetSlotTime(zdev_t* dev, u8_t type);
+void zfHpSetSlotTimeRegister(zdev_t* dev, u8_t type);
+void zfHpSetRifs(zdev_t* dev, u8_t ht_enable, u8_t ht2040, u8_t g_mode);
+void zfHpBeginSiteSurvey(zdev_t* dev, u8_t status);
+void zfHpFinishSiteSurvey(zdev_t* dev, u8_t status);
+u16_t zfHpEnableHwRetry(zdev_t* dev);
+u16_t zfHpDisableHwRetry(zdev_t* dev);
+void zfHpSWDecrypt(zdev_t* dev, u8_t enable);
+void zfHpSWEncrypt(zdev_t* dev, u8_t enable);
+u32_t zfHpCapability(zdev_t* dev);
+void zfHpSetRollCallTable(zdev_t* dev);
+u8_t zfHpregulatoryDomain(zdev_t* dev);
+u16_t zfStaAddIePowerCap(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u8_t zfHpGetMaxTxPower(zdev_t* dev);
+u8_t zfHpGetMinTxPower(zdev_t* dev);
+u16_t zfStaAddIeSupportCh(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfHpEnableRifs(zdev_t* dev, u8_t mode24g, u8_t modeHt, u8_t modeHt2040);
+void zfHpDisableRifs(zdev_t* dev);
+u16_t zfHpUsbReset(zdev_t* dev);
+
+
+#endif /* #ifndef _CFUNC_H */
diff --git a/drivers/staging/otus/80211core/chb.c b/drivers/staging/otus/80211core/chb.c
new file mode 100644
index 000000000000..7fac15011256
--- /dev/null
+++ b/drivers/staging/otus/80211core/chb.c
@@ -0,0 +1,200 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : hb.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains house keeping and timer functions.         */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+/* Called by wrapper every 10 msec */
+void zfiHeartBeat(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->tick++;
+
+#if 0
+    /* => every 1.28 seconds */
+    if (wd->cwm.cw_enable && ((wd->tick & 0x7f) == 0x3f))
+    {
+        zfHpCwmUpdate(dev);
+    }
+#endif
+    /* => every 2.56 seconds */
+    if ((wd->tick & 0xff) == 0)
+    {
+        zfAgingDefragList(dev, 1);
+    }
+
+    /* Watch Dog */
+    //zfWatchDog();
+
+    /* LED Control (per 100ms) */
+    if ((wd->tick % 10) == 9)
+    {
+        zfLed100msCtrl(dev);
+#ifdef ZM_ENABLE_BA_RATECTRL
+        if (!wd->modeMDKEnable)
+        {
+            zfiDbgReadTally(dev);
+        }
+#endif
+    }
+
+#ifdef ZM_ENABLE_REWRITE_BEACON_START_ADDRESS
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIsConnected(dev) )
+        {
+            zfReWriteBeaconStartAddress(dev);
+        }
+    }
+#endif
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIsConnected(dev) )
+        {
+            wd->tickIbssReceiveBeacon++;  // add 10ms
+
+            if ( (wd->sta.ibssSiteSurveyStatus == 2) &&
+                 (wd->tickIbssReceiveBeacon == 300) &&
+                 (wd->sta.ibssReceiveBeaconCount < 3) )
+            {
+                zm_debug_msg0("It is happen!!! No error message");
+                zfReSetCurrentFrequency(dev);
+            }
+        }
+    }
+
+    if(wd->sta.ReceivedPacketRateCounter <= 0)
+    {
+        wd->sta.ReceivedPktRatePerSecond = wd->sta.TotalNumberOfReceivePackets;
+	//zm_debug_msg1("Receive Packet Per Second  = ", wd->sta.ReceivedPktRatePerSecond);
+	    if (wd->sta.TotalNumberOfReceivePackets != 0)
+	    {
+	        wd->sta.avgSizeOfReceivePackets = wd->sta.TotalNumberOfReceiveBytes/wd->sta.TotalNumberOfReceivePackets;
+	    }
+	    else
+	    {
+	        wd->sta.avgSizeOfReceivePackets = 640;
+	    }
+        wd->sta.TotalNumberOfReceivePackets = 0;
+        wd->sta.TotalNumberOfReceiveBytes = 0;
+        wd->sta.ReceivedPacketRateCounter = 100; /*for another 1s*/
+    }
+    else
+    {
+        wd->sta.ReceivedPacketRateCounter--;
+    }
+
+	/* => every 1.28 seconds */
+	if((wd->tick & 0x7f) == 0x3f)
+	{
+		if( wd->sta.NonNAPcount > 0)
+		{
+			wd->sta.RTSInAGGMode = TRUE;
+			wd->sta.NonNAPcount = 0;
+		}
+		else
+		{
+			wd->sta.RTSInAGGMode = FALSE;
+		}
+	}
+
+
+
+    /* Maintain management time tick */
+    zfMmApTimeTick(dev);
+    zfMmStaTimeTick(dev);
+
+    //zfPhyCrTuning(dev);
+
+    //zfTxPowerControl(dev);
+    zfHpHeartBeat(dev);
+
+}
+
+
+void zfDumpBssList(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    u8_t   str[33];
+    u8_t   i, j;
+    u32_t  addr1, addr2;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg0("***** Bss scan result *****");
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        if ( i )
+        {
+            zm_debug_msg0("---------------------------");
+        }
+
+        zm_debug_msg1("BSS #", i);
+        for(j=0; j<pBssInfo->ssid[1]; j++)
+        {
+            str[j] = pBssInfo->ssid[2+j];
+        }
+        str[pBssInfo->ssid[1]] = 0;
+        zm_debug_msg0("SSID = ");
+        zm_debug_msg0(str);
+
+        addr1 = (pBssInfo->bssid[0] << 16) + (pBssInfo->bssid[1] << 8 )
+                + pBssInfo->bssid[2];
+        addr2 = (pBssInfo->bssid[3] << 16) + (pBssInfo->bssid[4] << 8 )
+                + pBssInfo->bssid[5];
+        zm_debug_msg2("Bssid = ", addr1);
+        zm_debug_msg2("        ", addr2);
+        zm_debug_msg1("frequency = ", pBssInfo->frequency);
+        zm_debug_msg1("security type = ", pBssInfo->securityType);
+        zm_debug_msg1("WME = ", pBssInfo->wmeSupport);
+        zm_debug_msg1("beacon interval = ", pBssInfo->beaconInterval[0]
+                      + (pBssInfo->beaconInterval[1] << 8));
+        zm_debug_msg1("capability = ", pBssInfo->capability[0]
+                      + (pBssInfo->capability[1] << 8));
+        if ( pBssInfo->supportedRates[1] > 0 )
+        {
+            for( j=0; j<pBssInfo->supportedRates[1]; j++ )
+            {
+                zm_debug_msg2("supported rates = ", pBssInfo->supportedRates[2+j]);
+            }
+        }
+
+        for( j=0; j<pBssInfo->extSupportedRates[1]; j++ )
+        {
+            zm_debug_msg2("ext supported rates = ", pBssInfo->extSupportedRates[2+j]);
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+    zmw_leave_critical_section(dev);
+
+    zm_debug_msg0("***************************");
+}
+
diff --git a/drivers/staging/otus/80211core/cic.c b/drivers/staging/otus/80211core/cic.c
new file mode 100644
index 000000000000..c84f079e3d84
--- /dev/null
+++ b/drivers/staging/otus/80211core/cic.c
@@ -0,0 +1,496 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+
+void zfUpdateBssid(zdev_t* dev, u8_t* bssid)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    //zmw_enter_critical_section(dev);
+    wd->sta.bssid[0] = bssid[0] + (((u16_t) bssid[1]) << 8);
+    wd->sta.bssid[1] = bssid[2] + (((u16_t) bssid[3]) << 8);
+    wd->sta.bssid[2] = bssid[4] + (((u16_t) bssid[5]) << 8);
+    //zmw_leave_critical_section(dev);
+
+    zfHpSetBssid(dev, bssid);
+
+}
+
+/************************************************************************************/
+/*                                                                                  */
+/*    FUNCTION DESCRIPTION                  zfResetSupportRate                      */
+/*      Reset support rate to default value.                                        */
+/*                                                                                  */
+/*    INPUTS                                                                        */
+/*      dev : device pointer                                                        */
+/*      type: ZM_DEFAULT_SUPPORT_RATE_ZERO       => reset to zero                   */
+/*            ZM_DEFAULT_SUPPORT_RATE_DISCONNECT => reset to disconnect status      */
+/*            ZM_DEFAULT_SUPPORT_RATE_IBSS_B     => reset to IBSS creator(b mode)   */
+/*            ZM_DEFAULT_SUPPORT_RATE_IBSS_AG    => reset to IBSS creator(a/g mode) */
+/*                                                                                  */
+/************************************************************************************/
+void zfResetSupportRate(zdev_t* dev, u8_t type)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(type)
+    {
+    case ZM_DEFAULT_SUPPORT_RATE_ZERO:
+        wd->bRate = 0;
+        wd->bRateBasic = 0;
+        wd->gRate = 0;
+        wd->gRateBasic = 0;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_DISCONNECT:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0xff;
+        wd->gRateBasic = 0x15;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_IBSS_B:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0;
+        wd->gRateBasic = 0;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_IBSS_AG:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0xff;
+        wd->gRateBasic = 0;
+        break;
+    }
+}
+
+void zfUpdateSupportRate(zdev_t* dev, u8_t* rateArray)
+{
+    u8_t bRate=0, bRateBasic=0, gRate=0, gRateBasic=0;
+    u8_t length = rateArray[1];
+    u8_t i, j;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=2; i<length+2; i++)
+    {
+        for(j=0; j<4; j++)
+        {
+            if ( (rateArray[i] & 0x7f) == zg11bRateTbl[j] )
+            {
+                bRate |= (1 << j);
+                if ( rateArray[i] & 0x80 )
+                {
+                    bRateBasic |= (1 << j);
+                }
+            }
+        }
+
+        if ( j == 4 )
+        {
+            for(j=0; j<8; j++)
+            {
+                if ( (rateArray[i] & 0x7f) == zg11gRateTbl[j] )
+                {
+                    gRate |= (1 << j);
+                    if ( rateArray[i] & 0x80 )
+                    {
+                        gRateBasic |= (1 << j);
+                    }
+                }
+            }
+        }
+    }
+
+
+    wd->bRate |= bRate;
+    wd->bRateBasic |= bRateBasic;
+    wd->gRate |= gRate;
+    wd->gRateBasic |= gRateBasic;
+}
+
+u8_t zfIsGOnlyMode(zdev_t* dev, u16_t  frequency, u8_t* rateArray)
+{
+    u8_t length = rateArray[1];
+    u8_t i, j;
+
+    if (frequency < 3000) {
+        for (i = 2; i < length+2; i++) {
+            for (j = 0; j < 8; j++) {
+                if ( ((rateArray[i] & 0x7f) == zg11gRateTbl[j])
+                     && (rateArray[i] & 0x80) ) {
+                    return 1;
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+void zfGatherBMode(zdev_t* dev, u8_t* rateArray, u8_t* extrateArray)
+{
+    u8_t gatherBMode[ZM_MAX_SUPP_RATES_IE_SIZE + 2];
+    u8_t i, j, k = 0;
+    u8_t length;
+
+    gatherBMode[0] = ZM_WLAN_EID_SUPPORT_RATE;
+    gatherBMode[1] = 0;
+
+    length = rateArray[1];
+    for (i = 2; i < length+2; i++) {
+        for (j = 0; j < 4; j++) {
+            if ( (rateArray[i] & 0x7f) == zg11bRateTbl[j] ) {
+                gatherBMode[2+k] = rateArray[i];
+
+                gatherBMode[1]++;
+                k++;
+            }
+        }
+    }
+
+    length = extrateArray[1];
+    for (i = 2; i < length+2; i++) {
+        for (j = 0; j < 4; j++) {
+            if ( (extrateArray[i] & 0x7f) == zg11bRateTbl[j] ) {
+                gatherBMode[2+k] = extrateArray[i];
+
+                gatherBMode[1]++;
+                k++;
+            }
+        }
+    }
+
+    extrateArray[0] = extrateArray[1] = 0;
+    zfMemoryCopy(rateArray, gatherBMode, gatherBMode[1]+2);
+}
+
+u16_t zfGetRandomNumber(zdev_t* dev, u16_t initValue)
+{
+#if 0
+    /* Compiler/Linker error on Linux */
+    if ( initValue )
+    {
+        srand(initValue);
+    }
+
+    return ((u16_t)rand());
+#endif
+    return 0;
+}
+
+u8_t zfPSDeviceSleep(zdev_t* dev)
+{
+    //zmw_get_wlan_dev(dev);
+
+    /* enter PS mode */
+
+    return 0;
+}
+
+u8_t zcOfdmPhyCrtlToRate[] =
+{
+    /* 0x8=48M, 0x9=24M, 0xa=12M, 0xb=6M, 0xc=54M, 0xd=36M, 0xe=18M, 0xf=9M */
+            10,       8,       6,      4,      11,       9,       7,      5
+};
+
+u8_t zfPhyCtrlToRate(u32_t phyCtrl)
+{
+    u32_t mt, mcs, sg;
+    u8_t rate = 0;
+
+    mt = phyCtrl & 0x3;
+    mcs = (phyCtrl>>18) & 0x3f;
+    sg = (phyCtrl>>31) & 0x1;
+
+    if ((mt == 0) && (mcs <=3))
+    {
+        rate = (u8_t)mcs;
+    }
+    else if ((mt == 1) && (mcs >= 0x8) && (mcs <= 0xf))
+    {
+        rate = zcOfdmPhyCrtlToRate[mcs-8];
+    }
+    else if ((mt == 2) && (mcs <= 15))
+    {
+        rate = (u8_t)mcs + 12;
+        if(sg) {
+            if (mcs != 7)
+            {
+                rate = (u8_t)mcs + 12 + 2;
+            }
+            else //MCS7-SG
+            {
+                rate = (u8_t)30;
+            }
+        }
+    }
+
+    return rate;
+}
+
+
+void zfCoreEvent(zdev_t* dev, u16_t event, u8_t* rsp)
+{
+    u16_t i;
+    zbuf_t* psBuf;
+    u8_t moreData;
+    u8_t vap = 0;
+    u8_t peerIdx;
+    s8_t res;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    if (event == 0) //Beacon Event
+    {
+        if ( wd->wlanMode == ZM_MODE_AP )
+        {
+            zfApSendBeacon(dev);
+
+            if (wd->CurrentDtimCount == 0)
+            {
+                /* TODO : Send queued broadcast frames at BC/MC event */
+                do
+                {
+                    psBuf = NULL;
+                    moreData = 0;
+                    zmw_enter_critical_section(dev);
+                    if (wd->ap.bcmcTail[vap] != wd->ap.bcmcHead[vap])
+                    {
+                        //zm_msg0_mm(ZM_LV_0, "Send BCMC frames");
+                        psBuf = wd->ap.bcmcArray[vap][wd->ap.bcmcHead[vap]];
+                        wd->ap.bcmcHead[vap] = (wd->ap.bcmcHead[vap] + 1)
+                                & (ZM_BCMC_ARRAY_SIZE - 1);
+                        if (wd->ap.bcmcTail[vap] != wd->ap.bcmcHead[vap])
+                        {
+                            moreData = 0x20;
+                        }
+                    }
+                    zmw_leave_critical_section(dev);
+                    if (psBuf != NULL)
+                    {
+                        /* TODO : config moreData bit */
+                        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF,
+                                moreData);
+                    }
+                } while(psBuf != NULL);
+
+            }
+        }
+        else
+        {
+            /* STA mode */
+            if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+            {
+                /* send queued packets */
+                for(i=0; i<wd->sta.staPSDataCount; i++)
+                {
+                    zfTxSendEth(dev, wd->sta.staPSDataQueue[i], 0,
+                                ZM_EXTERNAL_ALLOC_BUF, 0);
+                }
+
+                wd->sta.staPSDataCount = 0;
+            }
+
+            if ( wd->wlanMode == ZM_MODE_IBSS )
+            {
+                zfStaSendBeacon(dev);
+                wd->sta.ibssAtimTimer = ZM_BIT_15 | wd->sta.atimWindow;
+            }
+
+            zfPowerSavingMgrPreTBTTInterrupt(dev);
+        }
+    } //if (event == 0) //Beacon Event
+    else if (event == 1) //Retry completed event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+        /* Degrade Tx Rate */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->ap.staTable[i].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 1) //Retry completed event
+    else if (event == 2) //Tx Fail event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+
+        /* Degrade Tx Rate */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->ap.staTable[i].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+
+            zfApSendFailure(dev, rsp);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 2) //Tx Fail event
+    else if (event == 3) //Tx Comp event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+
+        /* TODO : Tx completed, used for rate control probing */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxSuccessEvent(dev, &wd->ap.staTable[i].rcCell, zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxSuccessEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 3) //Tx Comp event
+    else if (event == 4) //BA failed count
+    {
+        u32_t fail;
+        u32_t rate;
+        peerIdx = 0;
+
+        fail=((u32_t*)rsp)[0] & 0xFFFF;
+        rate=((u32_t*)rsp)[0] >> 16;
+
+        if (rate > 15) {
+            rate = (rate & 0xF) + 12 + 2;
+        }
+        else {
+            rate = rate + 12;
+        }
+
+        zmw_enter_critical_section(dev);
+        zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, (u8_t)rate, fail);
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfBeaconCfgInterrupt(zdev_t* dev, u8_t* rsp)
+{
+    u32_t txBeaconCounter;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        txBeaconCounter = *((u32_t *)rsp);
+        if ( wd->sta.beaconTxCnt != txBeaconCounter )
+        {
+            wd->sta.txBeaconInd = 1;
+
+            zmw_enter_critical_section(dev);
+            wd->tickIbssSendBeacon = 0;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            wd->sta.txBeaconInd = 0;
+        }
+
+#ifdef ZM_ENABLE_IBSS_DELAYED_JOIN_INDICATION
+        if ( wd->sta.txBeaconInd && wd->sta.ibssDelayedInd )
+        {
+            if (wd->zfcbIbssPartnerNotify != NULL)
+            {
+                wd->zfcbIbssPartnerNotify(dev, 1, &wd->sta.ibssDelayedIndEvent);
+            }
+
+            wd->sta.ibssDelayedInd = 0;
+        }
+#endif
+
+        wd->sta.beaconTxCnt = txBeaconCounter;
+
+        // Need to check if the time is expired after ATIM window??
+
+        // Check if we have buffered any data for those stations that are sleeping
+        // If it's true, then transmitting ATIM pkt to notify them
+
+#ifdef ZM_ENABLE_IBSS_PS
+        // TODO: Need to check if the station receive our ATIM pkt???
+        zfStaIbssPSSend(dev);
+
+        if ( wd->sta.atimWindow == 0 )
+        {
+            // We won't receive the end of ATIM isr so we fake it
+            zfPowerSavingMgrAtimWinExpired(dev);
+        }
+#endif
+    }
+}
+
+void zfEndOfAtimWindowInterrupt(zdev_t* dev)
+{
+#ifdef ZM_ENABLE_IBSS_PS
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        // Transmit any queued pkt for the stations!!
+        zfPowerSavingMgrAtimWinExpired(dev);
+    }
+#endif
+}
diff --git a/drivers/staging/otus/80211core/cinit.c b/drivers/staging/otus/80211core/cinit.c
new file mode 100644
index 000000000000..5f853ce79309
--- /dev/null
+++ b/drivers/staging/otus/80211core/cinit.c
@@ -0,0 +1,1911 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : init.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains init functions.                            */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+extern const u8_t zcUpToAc[8];
+
+u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
+                               24000, 12000, 6000, 54000, 36000, 18000, 9000};
+u32_t zcIndextoRateN20L[16] = {6500, 13000, 19500, 26000, 39000, 52000, 58500,
+                              65000, 13000, 26000, 39000, 52000, 78000, 104000,
+                              117000, 130000};
+u32_t zcIndextoRateN20S[16] = {7200, 14400, 21700, 28900, 43300, 57800, 65000,
+                              72200, 14400, 28900, 43300, 57800, 86700, 115600,
+                              130000, 144400};
+u32_t zcIndextoRateN40L[16] = {13500, 27000, 40500, 54000, 81000, 108000, 121500,
+                              135000, 27000, 54000, 81000, 108000, 162000, 216000,
+                              243000, 270000};
+u32_t zcIndextoRateN40S[16] = {15000, 30000, 45000, 60000, 90000, 120000, 135000,
+                              150000, 30000, 60000, 90000, 120000, 180000, 240000,
+                              270000, 300000};
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxGenWlanHeader           */
+/*      Generate WLAN MAC header and LLC header.                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      id : Index of TxD                                               */
+/*      port : WLAN port                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      length of removed Ethernet header                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxGenWlanHeader(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t seq,
+                        u8_t flag, u16_t plusLen, u16_t minusLen, u16_t port,
+                        u16_t* da, u16_t* sa, u8_t up, u16_t *micLen,
+                        u16_t* snap, u16_t snapLen, struct aggControl *aggControl)
+{
+
+    u16_t len;
+    u16_t macCtrl;
+    u32_t phyCtrl;
+    u16_t hlen = 16;
+    u16_t icvLen = 0;
+    u16_t wdsPortId;
+    u16_t vap = 0;
+    u16_t mcs = 0;
+    u16_t mt = 0;
+    u8_t  qosType;
+    u8_t  b1, b2;
+    u16_t wdsPort;
+    u8_t  encExemptionActionType;
+    u16_t rateProbingFlag = 0;
+    u8_t  tkipFrameOffset = 0;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t    res, peerIdx;
+    u8_t    userIdx=0;
+    u16_t   *iv16;
+    u32_t   *iv32;
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+   /* Generate WLAN header */
+    /* Frame control */
+    header[4] = 0x0008 | (flag<<8);
+    /* Duration */
+    header[5] = 0x0000;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        /* ToDS bit */
+        header[4] |= 0x0100;
+
+        /*Sometimes we wake up to tx/rx but AP still think we are sleeping, so still need to set this bit*/
+        if ( zfPowerSavingMgrIsSleeping(dev) || wd->sta.psMgr.tempWakeUp == 1 )
+        {
+            header[4] |= 0x1000;
+        }
+
+        /* Address 1 = BSSID */
+        header[6] = wd->sta.bssid[0];
+        header[7] = wd->sta.bssid[1];
+        header[8] = wd->sta.bssid[2];
+        /* Address 3 = DA */
+        header[12] = da[0];
+        header[13] = da[1];
+        header[14] = da[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 1 = DA */
+        header[6] = da[0];
+        header[7] = da[1];
+        header[8] = da[2];
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[12] = 0;
+        header[13] = 0;
+        header[14] = 0;
+
+        /* PSEUDO test : WDS */
+        if (wd->enableWDS)
+        {
+            /* ToDS and FromDS bit */
+            header[4] |= 0x0300;
+
+            /* Address 4 = SA */
+            header[16] = 0;
+            header[17] = 0;
+            header[18] = 0;
+
+            hlen = 19;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        /* Address 1 = DA */
+        header[6] = da[0];
+        header[7] = da[1];
+        header[8] = da[2];
+        /* Address 3 = BSSID */
+        header[12] = wd->sta.bssid[0];
+        header[13] = wd->sta.bssid[1];
+        header[14] = wd->sta.bssid[2];
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        zmw_enter_critical_section(dev);
+        res = zfStaFindOppositeByMACAddr(dev, da, &peerIdx);
+        if(res == 0) // Find opposite in our OppositeInfo Structure !
+        {
+            userIdx = peerIdx;
+        }
+        zmw_leave_critical_section(dev);
+#endif
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        if (port < 0x20)
+        /* AP mode */
+        {
+            /* FromDS bit */
+            header[4] |= 0x0200;
+
+            /* Address 1 = DA */
+            header[6] = da[0];
+            header[7] = da[1];
+            header[8] = da[2];
+            /* Address 3 = SA */
+            header[12] = sa[0];
+            header[13] = sa[1];
+            header[14] = sa[2];
+
+            if (port < ZM_MAX_AP_SUPPORT)
+            {
+                vap = port;
+                header[14] += (vap<<8);
+            }
+        }
+        else
+        /* WDS port */
+        {
+            /* ToDS and FromDS bit */
+            header[4] |= 0x0300;
+
+            wdsPortId = port - 0x20;
+
+            /* Address 1 = RA */
+            header[6] = wd->ap.wds.macAddr[wdsPortId][0];
+            header[7] = wd->ap.wds.macAddr[wdsPortId][1];
+            header[8] = wd->ap.wds.macAddr[wdsPortId][2];
+            /* Address 3 = DA */
+            header[12] = da[0];
+            header[13] = da[1];
+            header[14] = da[2];
+            /* Address 4 = SA */
+            header[16] = sa[0];
+            header[17] = sa[1];
+            header[18] = sa[2];
+
+            hlen = 19;
+        }
+    } /* else if (wd->wlanMode == ZM_MODE_AP) */
+
+    /* Address 2 = TA */
+    header[9] = wd->macAddr[0];
+    header[10] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    header[11] = wd->macAddr[2]; //Multiple SSID
+#else
+    header[11] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+
+    if ( (wd->wlanMode == ZM_MODE_IBSS) && (wd->XLinkMode) )
+    {
+        header[9]  = sa[0];
+        header[10] = sa[1];
+        header[11] = sa[2];
+    }
+
+    /* Sequence Control */
+    header[15] = seq;
+
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaTxRateAndQosType(dev, da, &phyCtrl, &qosType, &rateProbingFlag);
+        mt = (u16_t)(phyCtrl & 0x3);
+        mcs = (u16_t)((phyCtrl >> 16) & 0x3f);
+#if 1
+        //zfApGetStaQosType(dev, da, &qosType);
+
+        /* if DA == WME STA */
+        if (qosType == 1)
+        {
+            /* QoS data */
+            header[4] |= 0x0080;
+
+            /* QoS Control */
+            header[hlen] = up;
+            hlen += 1;
+        }
+#endif
+    }
+
+#if 0
+    //AGG Test Code
+    if (header[6] == 0x8000)
+    {
+        /* QoS data */
+        header[4] |= 0x0080;
+
+        /* QoS Control */
+        header[hlen] = 0;
+        hlen += 1;
+    }
+#endif
+
+    if (wd->wlanMode == ZM_MODE_AP) {
+        /* Todo: rate control here for qos field */
+    }
+    else {
+        /* Rate control */
+        zfStaGetTxRate(dev, da, &phyCtrl, &rateProbingFlag);
+        mt = (u16_t)(phyCtrl & 0x3);
+        mcs = (u16_t)((phyCtrl >> 16) & 0x3f);
+    }
+
+    if (wd->txMCS != 0xff)
+    {
+        /* fixed rate */
+	    phyCtrl = ((u32_t)wd->txMCS<<16) + wd->txMT;
+        mcs = wd->txMCS;
+        mt = wd->txMT;
+    }
+
+    if (wd->enableAggregation)
+    {
+        /* force enable aggregation */
+        if (wd->enableAggregation==2 && !(header[6]&0x1))
+        {
+            /* QoS data */
+            header[4] |= 0x0080;
+
+            /* QoS Control */
+            header[hlen] = 0;
+            hlen += 1;
+        }
+        /* if wd->enableAggregation=1 => force disable */
+        /* if wd->enableAggregation=0 => auto */
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /*
+     * aggregation control
+     */
+
+    /*
+     * QoS data
+     */
+    if (wd->wlanMode == ZM_MODE_AP) {
+        if (aggControl && mt == 2) {
+            if (wd->enableAggregation==0 && !(header[6]&0x1))
+            {
+                header[4] |= 0x0080;
+
+                /*
+                 * QoS Control
+                 */
+                header[hlen] = 0;
+                hlen += 1;
+            }
+        }
+    }
+#endif
+
+    // MSDU Length
+    len = zfwBufGetSize(dev, buf);
+
+    /* Generate control setting */
+    /* Backoff, Non-Burst and hardware duration */
+    macCtrl = 0x208;
+
+    /* ACK */
+    if ((header[6] & 0x1) == 0x1)
+    {
+        /* multicast frame : Set NO-ACK bit */
+        macCtrl |= 0x4;
+    }
+    else
+    {
+        /* unicast frame */
+    #if 0
+        // Enable RTS according to MPDU Lengths ( not MSDU Lengths )
+        if (len >= wd->rtsThreshold)
+        {
+            /* Enable RTS */
+            macCtrl |= 1;
+        }
+    #endif
+    }
+    /* VAP test code */
+    //macCtrl |= 0x4;
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        u8_t encryType;
+        u16_t iv16;
+        u32_t iv32;
+
+        /* Check whether this is a multicast frame */
+        if ((header[6] & 0x1) == 0x1)
+        {
+            /* multicast frame */
+            if (wd->ap.encryMode[vap] == ZM_TKIP)
+            {
+                wd->ap.iv16[vap]++;
+
+                if(wd->ap.iv16[vap] == 0)
+                {
+                    wd->ap.iv32[vap]++;
+                }
+
+                b1 = (u8_t) (wd->ap.iv16[vap] >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) wd->ap.iv16[vap];
+                b2 = 0x20 | (wd->ap.bcKeyIndex[vap] << 6);
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) wd->ap.iv32[vap];
+                header[hlen+3] = (u16_t) (wd->ap.iv32[vap] >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }
+            else if (wd->ap.encryMode[vap] == ZM_AES)
+            {
+                wd->ap.iv16[vap]++;
+
+                if(wd->ap.iv16[vap] == 0)
+                {
+                    wd->ap.iv32[vap]++;
+                }
+
+                b1 = (u8_t) wd->ap.iv16[vap];
+                b2 = (u8_t) (wd->ap.iv16[vap] >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000 | (wd->ap.bcKeyIndex[vap] << 14);
+                header[hlen+2] = (u16_t) (wd->ap.iv32[vap]);
+                header[hlen+3] = (u16_t) (wd->ap.iv32[vap] >> 16);
+
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }
+            #ifdef ZM_ENABLE_CENC
+            else if (wd->ap.encryMode[vap] == ZM_CENC)
+            {
+                //u32_t txiv[4];
+
+                wd->ap.txiv[vap][0]++;
+
+                if (wd->ap.txiv[vap][0] == 0)
+                {
+                    wd->ap.txiv[vap][1]++;
+                }
+
+                if (wd->ap.txiv[vap][1] == 0)
+                {
+                    wd->ap.txiv[vap][2]++;
+                }
+
+                if (wd->ap.txiv[vap][2] == 0)
+                {
+                    wd->ap.txiv[vap][3]++;
+                }
+
+                if (wd->ap.txiv[vap][3] == 0)
+                {
+                    wd->ap.txiv[vap][0] = 0;
+                    wd->ap.txiv[vap][1] = 0;
+                    wd->ap.txiv[vap][2] = 0;
+                }
+
+                header[hlen] = (wd->ap.bcKeyIndex[vap] & 0x0001);    /* For Key Id and reserved field */
+                header[hlen+1] = (u16_t)wd->ap.txiv[vap][0];
+                header[hlen+2] = (u16_t)(wd->ap.txiv[vap][0] >> 16);
+                header[hlen+3] = (u16_t)wd->ap.txiv[vap][1];
+                header[hlen+4] = (u16_t)(wd->ap.txiv[vap][1] >> 16);
+                header[hlen+5] = (u16_t)wd->ap.txiv[vap][2];
+                header[hlen+6] = (u16_t)(wd->ap.txiv[vap][2] >> 16);
+                header[hlen+7] = (u16_t)wd->ap.txiv[vap][3];
+                header[hlen+8] = (u16_t)(wd->ap.txiv[vap][3] >> 16);
+
+                macCtrl |= 0x80;
+                icvLen = 16;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 9;
+            }
+            #endif //ZM_ENABLE_CENC
+        }
+        else
+        {
+            /* Get STA's encryption type */
+            zfApGetStaEncryType(dev, da, &encryType);
+
+            if (encryType == ZM_TKIP)
+            {
+                /* Get iv16 and iv32 */
+                zfApGetStaWpaIv(dev, da, &iv16, &iv32);
+
+                iv16++;
+                if (iv16 == 0)
+                {
+                    iv32++;
+                }
+
+                b1 = (u8_t) (iv16 >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) iv16;
+                b2 = 0x20;
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) iv32;
+                header[hlen+3] = (u16_t) (iv32 >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+
+                /* Set iv16 and iv32 */
+                zfApSetStaWpaIv(dev, da, iv16, iv32);
+            }
+            else if (encryType == ZM_AES)
+            {
+                /* Get iv16 and iv32 */
+                zfApGetStaWpaIv(dev, da, &iv16, &iv32);
+
+                iv16++;
+                if (iv16 == 0)
+                {
+                    iv32++;
+                }
+
+                b1 = (u8_t) iv16;
+                b2 = (u8_t) (iv16 >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000;
+                header[hlen+2] = (u16_t) (iv32);
+                header[hlen+3] = (u16_t) (iv32 >> 16);
+
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 4;
+
+                /* Set iv16 and iv32 */
+                zfApSetStaWpaIv(dev, da, iv16, iv32);
+            }
+            #ifdef ZM_ENABLE_CENC
+            else if (encryType == ZM_CENC)
+            {
+                u32_t txiv[4];
+                u8_t keyIdx;
+
+                /* Get CENC TxIV */
+                zfApGetStaCencIvAndKeyIdx(dev, da, txiv, &keyIdx);
+
+                txiv[0] += 2;
+
+                if (txiv[0] == 0 || txiv[0] == 1)
+                {
+                    txiv[1]++;
+                }
+
+                if (txiv[1] == 0)
+                {
+                    txiv[2]++;
+                }
+
+                if (txiv[2] == 0)
+                {
+                    txiv[3]++;
+                }
+
+                if (txiv[3] == 0)
+                {
+                    txiv[0] = 0;
+                    txiv[1] = 0;
+                    txiv[2] = 0;
+                }
+
+                header[hlen] = (keyIdx & 0x0001);    /* For Key Id and reserved field */
+                header[hlen+1] = (u16_t)txiv[0];
+                header[hlen+2] = (u16_t)(txiv[0] >> 16);
+                header[hlen+3] = (u16_t)txiv[1];
+                header[hlen+4] = (u16_t)(txiv[1] >> 16);
+                header[hlen+5] = (u16_t)txiv[2];
+                header[hlen+6] = (u16_t)(txiv[2] >> 16);
+                header[hlen+7] = (u16_t)txiv[3];
+                header[hlen+8] = (u16_t)(txiv[3] >> 16);
+
+                macCtrl |= 0x80;
+                icvLen = 16;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 9;
+
+                /* Set CENC IV */
+                zfApSetStaCencIv(dev, da, txiv);
+            }
+            #endif //ZM_ENABLE_CENC
+        }
+
+        /* protection mode */
+        if (wd->ap.protectionMode == 1)
+        {
+            /* Enable Self-CTS */
+            macCtrl &= 0xFFFC;
+            macCtrl |= 2;
+        }
+
+        /* Rate Control */
+        if (port < 0x20)
+        {
+            /* AP */
+            /* IV */
+            if ((wd->ap.encryMode[vap] == ZM_WEP64) ||
+                    (wd->ap.encryMode[vap] == ZM_WEP128) ||
+                    (wd->ap.encryMode[vap] == ZM_WEP256))
+            {
+                header[4] |= 0x4000;
+                header[hlen] = 0x0;   //IV
+                header[hlen+1] = wd->ap.bcKeyIndex[vap] << 14; //IV with Keyid--CWYang(m)
+                hlen += 2;
+                icvLen = 4;
+                macCtrl |= 0x40;
+            }
+        }
+        else
+        {
+            /* WDS */
+
+            /* TODO : Fixed rate to 54M */
+            phyCtrl = 0xc0001;   //PHY control L
+
+            /* WDS port checking */
+            if ((wdsPort = (port - 0x20)) >= ZM_MAX_WDS_SUPPORT)
+            {
+                wdsPort = 0;
+            }
+
+            #if 1
+            /* IV */
+            switch (wd->ap.wds.encryMode[wdsPort])
+            {
+            case ZM_WEP64:
+            case ZM_WEP128:
+            case ZM_WEP256:
+                    header[4] |= 0x4000;
+                    header[hlen] = 0x0;   //IV
+                    header[hlen+1] = wd->ap.bcKeyIndex[vap] << 14; //IV with Keyid
+                    hlen += 2;
+                    icvLen = 4;
+                    macCtrl |= 0x40;
+                    break;
+
+            case ZM_TKIP:
+                    wd->sta.iv16++;
+
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    b1 = (u8_t) (wd->sta.iv16 >> 8);
+                    b2 = (b1 | 0x20) & 0x7f;
+                    header[hlen] = ((u16_t)b2 << 8) + b1;
+                    b1 = (u8_t) wd->sta.iv16;
+                    b2 = 0x20;
+                    header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                    header[hlen+2] = (u16_t) wd->sta.iv32;
+                    header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                    //macCtrl |= 0x80;
+                    macCtrl |= 0x40;
+                    icvLen = 4;
+
+                    /* set hardware MIC */
+                    if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                    {
+                        macCtrl |= 0x100;
+                        plusLen += 8;
+                        *micLen = 8;
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                    break;
+
+            case ZM_AES:
+                    wd->sta.iv16++;
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    b1 = (u8_t) wd->sta.iv16;
+                    b2 = (u8_t) (wd->sta.iv16 >> 8);
+                    header[hlen] = ((u16_t)b2 << 8) + b1;
+                    header[hlen+1] = 0x2000;
+                    header[hlen+2] = (u16_t) (wd->sta.iv32);
+                    header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                    macCtrl |= 0xc0; /* Set to AES in control setting */
+                    icvLen = 8;  /* MIC */
+
+                    header[4] |= 0x4000; /* Set WEP bit in wlan header */
+                    hlen += 4; /* plus IV length */
+                    break;
+            }/* end of switch */
+            #endif
+        }
+    }
+    else   /* wd->wlanMode != ZM_MODE_AP */
+    {
+        encExemptionActionType = zfwGetPktEncExemptionActionType(dev, buf);
+
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            #if 1
+            /* if WME AP */
+            if (wd->sta.wmeConnected != 0)
+            {
+                /* QoS data */
+                header[4] |= 0x0080;
+
+                /* QoS Control */
+                header[hlen] = up;
+                hlen += 1;
+            }
+            #endif
+
+            if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+            {
+                if ( wd->sta.authMode < ZM_AUTH_MODE_WPA )
+                {   /* non-WPA */
+                    if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+                    {
+                        if ( (wd->sta.encryMode == ZM_WEP64)||
+                             (wd->sta.encryMode == ZM_WEP128)||
+                             (wd->sta.encryMode == ZM_WEP256) )
+                        {
+                            header[4] |= 0x4000;
+                            header[hlen] = 0x0;   //IV
+                            header[hlen+1] = 0x0; //IV
+                            header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                            hlen += 2;
+                            icvLen = 4;
+
+                            /* For Software WEP */
+                            if ((wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) != 0)
+                            {
+                                u8_t keyLen = 5;
+                                u8_t iv[3];
+
+                                iv[0] = 0x0;
+                                iv[1] = 0x0;
+                                iv[2] = 0x0;
+
+                                if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP64)
+                                {
+                                    keyLen = 5;
+                                }
+                                else if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP128)
+                                {
+                                    keyLen = 13;
+                                }
+                                else if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP256)
+                                {
+                                    keyLen = 29;
+                                }
+
+                                zfWEPEncrypt(dev, buf, (u8_t*) snap, snapLen, minusLen, keyLen,
+                                        wd->sta.wepKey[wd->sta.keyId], iv);
+                            }
+                            else
+                            {
+                                macCtrl |= 0x40;
+                            }
+                        }
+                    }
+                }
+                else
+                {   /* WPA */
+                    if ( wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK )
+                    {
+                        wd->sta.iv16++;
+                        if ( wd->sta.iv16 == 0 )
+                        {
+                            wd->sta.iv32++;
+                        }
+
+                        /* set encryption mode */
+                        if ( wd->sta.encryMode == ZM_TKIP )
+                        {
+                            b1 = (u8_t) (wd->sta.iv16 >> 8);
+                            b2 = (b1 | 0x20) & 0x7f;
+                            header[hlen] = ((u16_t)b2 << 8) + b1;
+                            b1 = (u8_t) wd->sta.iv16;
+                            b2 = 0x20;
+
+                            // header[hlen+1] = (((u16_t) wd->sta.keyId) << 14) | (((u16_t)b2 << 8) + b1);
+                            // STA in infrastructure mode should use keyId = 0 to transmit unicast !
+                            header[hlen+1] = (((u16_t)b2 << 8) + b1);
+                            header[hlen+2] = (u16_t) wd->sta.iv32;
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                            /* If software encryption enable */
+                            if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0)
+                            {
+                                //macCtrl |= 0x80;
+                                /* TKIP same to WEP */
+                                macCtrl |= 0x40;
+                                icvLen = 4;
+
+                                /* set hardware MIC */
+                                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                                {
+                                    macCtrl |= 0x100;
+                                    plusLen += 8;
+                                    *micLen = 8;
+                                }
+                            }
+                            else
+                            {
+                                u8_t mic[8];
+                                u16_t offset;
+                                u32_t icv;
+                                u8_t RC4Key[16];
+
+                                /* TODO: Remove the criticial section here. */
+                                zmw_declare_for_critical_section();
+
+                                zmw_enter_critical_section(dev);
+                                /* Calculate MIC */
+                                zfCalTxMic(dev, buf, (u8_t *)snap, snapLen, minusLen, da, sa, up, mic);
+
+                                offset = zfwBufGetSize(dev, buf);
+
+                                /* Append MIC to the buffer */
+                                zfCopyToIntTxBuffer(dev, buf, mic, offset, 8);
+                                zfwBufSetSize(dev, buf, offset+8);
+                                zmw_leave_critical_section(dev);
+
+                                /* TKIP Key Mixing */
+                                zfTkipPhase1KeyMix(wd->sta.iv32, &wd->sta.txSeed);
+                                zfTkipPhase2KeyMix(wd->sta.iv16, &wd->sta.txSeed);
+                                zfTkipGetseeds(wd->sta.iv16, RC4Key, &wd->sta.txSeed);
+
+                                /* Encrypt Data */
+                                zfTKIPEncrypt(dev, buf, (u8_t *)snap, snapLen, minusLen, 16, RC4Key, &icv);
+
+                                icvLen = 4;
+                                len += 8;
+                            }
+
+                            header[4] |= 0x4000;
+                            hlen += 4;
+                        }
+                        else if ( wd->sta.encryMode == ZM_AES )
+                        {
+                            b1 = (u8_t) wd->sta.iv16;
+                            b2 = (u8_t) (wd->sta.iv16 >> 8);
+                            header[hlen] = ((u16_t)b2 << 8) + b1;
+                            // header[hlen+1] = (((u16_t) wd->sta.keyId) << 14) | (0x2000);
+                            // STA in infrastructure mode should use keyId = 0 to transmit unicast !
+                            header[hlen+1] = 0x2000;
+                            header[hlen+2] = (u16_t) (wd->sta.iv32);
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                            macCtrl |= 0xc0;
+                            icvLen = 8;  /* MIC */
+
+                            header[4] |= 0x4000;
+                            hlen += 4;
+                        }
+                        #ifdef ZM_ENABLE_CENC
+                        else if ( wd->sta.encryMode == ZM_CENC )
+                        {
+                            /* Accumlate the PN sequence */
+                            wd->sta.txiv[0] += 2;
+
+                            if (wd->sta.txiv[0] == 0 || wd->sta.txiv[0] == 1)
+                            {
+                                wd->sta.txiv[1]++;
+                            }
+
+                            if (wd->sta.txiv[1] == 0)
+                            {
+                                wd->sta.txiv[2]++;
+                            }
+
+                            if (wd->sta.txiv[2] == 0)
+                            {
+                                wd->sta.txiv[3]++;
+                            }
+
+                            if (wd->sta.txiv[3] == 0)
+                            {
+                                wd->sta.txiv[0] = 0;
+                                wd->sta.txiv[1] = 0;
+                                wd->sta.txiv[2] = 0;
+                            }
+
+                            header[hlen] = (wd->sta.cencKeyId & 0x0001);    /* For Key Id and reserved field */
+                            header[hlen+1] = (u16_t) wd->sta.txiv[0];
+                            header[hlen+2] = (u16_t) (wd->sta.txiv[0] >> 16);
+                            header[hlen+3] = (u16_t) wd->sta.txiv[1];
+                            header[hlen+4] = (u16_t) (wd->sta.txiv[1] >> 16);
+                            header[hlen+5] = (u16_t) wd->sta.txiv[2];
+                            header[hlen+6] = (u16_t) (wd->sta.txiv[2] >> 16);
+                            header[hlen+7] = (u16_t) wd->sta.txiv[3];
+                            header[hlen+8] = (u16_t) (wd->sta.txiv[3] >> 16);
+
+                            macCtrl |= 0x80;
+                            icvLen = 16;  /* MIC */
+
+                            header[4] |= 0x4000;
+                            hlen += 9;
+                        }
+                        #endif //ZM_ENABLE_CENC
+                    }
+                }
+            } // if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+        } /* if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE ) */
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+            {
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                if( wd->sta.oppositeInfo[userIdx].wpaState >= ZM_STA_WPA_STATE_PK_OK || wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK)
+                {
+                    int isUnicast = 1 ;
+
+                    if((da[0]& 0x1))
+                    {
+                        isUnicast = 0 ; // Not unicast , is broadcast
+                    }
+
+                    if( wd->sta.ibssWpa2Psk == 1 )
+                    { /* The IV order is not the same between unicast and broadcast ! */
+                        if ( isUnicast )
+                        {
+                            iv16 = &wd->sta.oppositeInfo[userIdx].iv16;
+                            iv32 = &wd->sta.oppositeInfo[userIdx].iv32;
+                        }
+                        else
+                        {
+                            iv16 = &wd->sta.iv16;
+                            iv32 = &wd->sta.iv32;
+                        }
+                    }
+                    else
+                    {
+                        iv16 = &wd->sta.iv16;
+                        iv32 = &wd->sta.iv32;
+                    }
+
+                    (*iv16)++;
+                    if ( *iv16 == 0 )
+                    {
+                        *iv32++;
+                    }
+
+                    if ( wd->sta.oppositeInfo[userIdx].encryMode == ZM_AES || wd->sta.encryMode == ZM_AES)
+                    {
+                        //printk("Station encryption mode is AES-CCMP\n") ;
+                        b1 = (u8_t) (*iv16);
+                        b2 = (u8_t) ((*iv16) >> 8);
+                        header[hlen] = ((u16_t)b2 << 8) + b1;
+
+                        if ( isUnicast )
+                        {
+                            header[hlen+1] = 0x2000;
+                        }
+                        else
+                        {
+                            header[hlen+1] = 0x2000 | (((u16_t) wd->sta.keyId) << 14);
+                        }
+
+                        header[hlen+2] = (u16_t) (*iv32);
+                        header[hlen+3] = (u16_t) ((*iv32) >> 16);
+                        macCtrl |= 0xc0;
+                        icvLen = 8;  /* MIC */
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                }
+                else if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED)
+                {
+                    if ( (wd->sta.encryMode == ZM_WEP64)||
+                        (wd->sta.encryMode == ZM_WEP128)||
+                        (wd->sta.encryMode == ZM_WEP256) )
+                    {
+                        header[4] |= 0x4000;
+                        header[hlen] = 0x0;   //IV
+                        header[hlen+1] = 0x0; //IV
+                        header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                        hlen += 2;
+                        icvLen = 4;
+                        macCtrl |= 0x40;
+                    }
+                }
+#else
+                /* ----- 20070405 add by Mxzeng ----- */
+                if( wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK )
+                {
+                    int isUnicast = 1 ;
+
+                    if((da[0]& 0x1))
+                    {
+                        isUnicast = 0 ; // Not unicast , is broadcast
+                    }
+
+                    wd->sta.iv16++;
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    if ( wd->sta.encryMode == ZM_AES )
+                    {
+                        //printk("Station encryption mode is AES-CCMP\n") ;
+                        b1 = (u8_t) wd->sta.iv16;
+                        b2 = (u8_t) (wd->sta.iv16 >> 8);
+                        header[hlen] = ((u16_t)b2 << 8) + b1;
+
+                        if ( isUnicast )
+                        {
+                            header[hlen+1] = 0x2000;
+                        }
+                        else
+                        {
+                            header[hlen+1] = 0x2000 | (((u16_t) wd->sta.keyId) << 14);
+                        }
+
+                            header[hlen+2] = (u16_t) (wd->sta.iv32);
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+                            macCtrl |= 0xc0;
+                            icvLen = 8;  /* MIC */
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                }
+                else if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED)
+                {
+                    if ( (wd->sta.encryMode == ZM_WEP64)||
+                         (wd->sta.encryMode == ZM_WEP128)||
+                         (wd->sta.encryMode == ZM_WEP256) )
+                    {
+                        header[4] |= 0x4000;
+                        header[hlen] = 0x0;   //IV
+                        header[hlen+1] = 0x0; //IV
+                        header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                        hlen += 2;
+                        icvLen = 4;
+                        macCtrl |= 0x40;
+                    }
+                }
+#endif
+            }   // End if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+        }   // End if ( wd->wlanMode == ZM_MODE_IBSS )
+        else if ( wd->wlanMode == ZM_MODE_PSEUDO )
+        {
+            switch (wd->sta.encryMode)
+	        {
+            case ZM_WEP64:
+            case ZM_WEP128:
+            case ZM_WEP256:
+                header[4] |= 0x4000;
+                header[hlen] = 0x0;   //IV
+                header[hlen+1] = 0x0; //IV
+                hlen += 2;
+                icvLen = 4;
+                macCtrl |= 0x40;
+                break;
+
+            case ZM_TKIP:
+            {
+                wd->sta.iv16++;
+                if ( wd->sta.iv16 == 0 )
+                {
+                    wd->sta.iv32++;
+                }
+
+                b1 = (u8_t) (wd->sta.iv16 >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) wd->sta.iv16;
+                b2 = 0x20;
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) wd->sta.iv32;
+                header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }/* end of PSEUDO TKIP */
+                break;
+
+            case ZM_AES:
+            {
+                wd->sta.iv16++;
+                if ( wd->sta.iv16 == 0 )
+                {
+                    wd->sta.iv32++;
+                }
+
+                b1 = (u8_t) wd->sta.iv16;
+                b2 = (u8_t) (wd->sta.iv16 >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000;
+                header[hlen+2] = (u16_t) (wd->sta.iv32);
+                header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+                header[4] |= 0x4000;
+                hlen += 4;
+            }/* end of PSEUDO AES */
+                    break;
+
+              #ifdef ZM_ENABLE_CENC
+              case ZM_CENC:
+                    /* Accumlate the PN sequence */
+                    wd->sta.txiv[0] += 2;
+
+                    if (wd->sta.txiv[0] == 0 || wd->sta.txiv[0] == 1)
+                    {
+                        wd->sta.txiv[1]++;
+                    }
+
+                    if (wd->sta.txiv[1] == 0)
+                    {
+                        wd->sta.txiv[2]++;
+                    }
+
+                    if (wd->sta.txiv[2] == 0)
+                    {
+                        wd->sta.txiv[3]++;
+                    }
+
+                    if (wd->sta.txiv[3] == 0)
+                    {
+                        wd->sta.txiv[0] = 0;
+                        wd->sta.txiv[1] = 0;
+                        wd->sta.txiv[2] = 0;
+                    }
+
+                    header[hlen] = 0;
+                    header[hlen+1] = (u16_t) wd->sta.txiv[0];
+                    header[hlen+2] = (u16_t) (wd->sta.txiv[0] >> 16);
+                    header[hlen+3] = (u16_t) wd->sta.txiv[1];
+                    header[hlen+4] = (u16_t) (wd->sta.txiv[1] >> 16);
+                    header[hlen+5] = (u16_t) wd->sta.txiv[2];
+                    header[hlen+6] = (u16_t) (wd->sta.txiv[2] >> 16);
+                    header[hlen+7] = (u16_t) wd->sta.txiv[3];
+                    header[hlen+8] = (u16_t) (wd->sta.txiv[3] >> 16);
+
+                    macCtrl |= 0x80;
+                    icvLen = 16;  /* MIC */
+
+                    header[4] |= 0x4000;
+                    hlen += 9;
+				break;
+		    #endif //ZM_ENABLE_CENC
+			}/* end of switch */
+		}
+
+        /* Generate control setting */
+
+        /* protection mode */
+        if (wd->enableProtectionMode)
+        {
+            if (wd->enableProtectionMode==2)
+            {
+                /* Force enable protection: self cts  */
+                macCtrl &= 0xFFFC;
+                macCtrl |= 2;
+            }
+            /* if wd->enableProtectionMode=1 => force disable */
+            /* if wd->enableProtectionMode=0 => auto */
+        }
+        else
+        {
+
+            /* protection mode */
+            if (wd->sta.bProtectionMode == TRUE)
+            {
+                /* Enable Self-CTS */
+                macCtrl &= 0xFFFC;
+                macCtrl |= 2;
+            }
+        }
+
+    }
+
+    if (wd->txMCS != 0xff)
+    {
+        /* fixed rate */
+	    phyCtrl = ((u32_t)wd->txMCS<<16) + wd->txMT;
+        mcs = wd->txMCS;
+        mt = wd->txMT;
+    }
+
+    if (mt == 2)
+    {
+#if 0
+        /* HT PT: 0 Mixed mode    1 Green field */
+	    if (wd->sta.preambleTypeHT == ZM_PREAMBLE_TYPE_GREEN_FIELD)
+	    {
+            phyCtrl |= 0x4;     /* Bit 2 */
+        }
+#endif
+        /* Bandwidth */
+        if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+        {
+            phyCtrl |= (0x80<<16);  /* BIT 23 */
+        }
+#if 0
+        /* STBC */
+        if (wd->sta.htCtrlSTBC<=0x3)
+        {
+            phyCtrl |= (wd->sta.htCtrlSTBC<<28);   /* BIT 23 */
+        }
+#endif
+        /* Short GI */
+        if(wd->sta.htCtrlSG)
+        {
+            phyCtrl |= (0x8000<<16);         /* BIT 31 */
+        }
+
+        /* TA */
+        if ( ((mcs >=0x8) && (mcs<=0xf))  || (wd->sta.htCtrlSTBC) )
+        {
+       	    phyCtrl |= 0x1800;               /* BIT 11 12 */
+    	}
+    }
+    else if(mt == 1)
+    {
+        #if 0
+        //bug that cause OFDM rate become duplicate legacy rate
+        /* Bandwidth */
+        if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+        {
+            phyCtrl |= (0x80<<16);  /* BIT 23 */
+            mt = 3;                 /* duplicate legacy */
+            phyCtrl |= mt;
+        }
+        #endif
+    }
+    else if(mt == 0)
+    {
+	/* CCK PT: Legcy Preamble: 1 long preamble    2 short preamble */
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_SHORT)
+        {
+    	       //phyCtrl |= 0x4;    /* BIT 2 */
+    	}
+    }
+
+    /* TA */
+    if (wd->sta.defaultTA)
+    {
+        phyCtrl |= 0x1000;
+    }
+    else
+    {
+        phyCtrl |= 0x0800;
+    }
+
+    //Get CurrentTxRate -- CWYang(+)
+    if ((mt == 0) || (mt == 1)) //B,G Rate
+    {
+        if (mcs < 16)
+        {
+            wd->CurrentTxRateKbps = zcIndextoRateBG[mcs];
+        }
+    }
+    else if (mt == 2)
+    {
+        if (mcs < 16)
+        {
+            if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+            {
+                if((phyCtrl & 0x80000000) != 0)
+                {
+                    /* Short GI 40 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN40S[mcs];
+                }
+                else
+                {
+                    /* Long GI 40 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN40L[mcs];
+                }
+            }
+            else
+            {
+                if((phyCtrl & 0x80000000) != 0)
+                {
+                    /* Short GI 20 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN20S[mcs];
+                }
+                else
+                {
+                    /* Long GI 20 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN20L[mcs];
+                }
+            }
+        }
+    }
+
+    //802.11 header(include IV) = (hlen<<1)-8
+    //ethernet frame = len
+    //snap + mic = plusLen
+    //ethernet header = minusLen
+    //icv = icvLen
+    //crc32 = 4
+    //length=802.11 header+snap+(ethernet frame-ethernet header)+mic+icv+crc32
+    header[0] = ((hlen<<1)-8)+plusLen+(len-minusLen)+icvLen+4;  //Length
+
+    // header[0] : MPDU Lengths
+    if ((header[6] & 0x1) != 0x1) // Unicast Frame
+    {
+        if (header[0] >= wd->rtsThreshold)
+        {
+            /* Enable RTS */
+            macCtrl |= 1;
+        }
+    }
+
+    if ( wd->sta.encryMode == ZM_TKIP )
+        tkipFrameOffset = 8;
+
+    if( wd->sta.EnableHT != 1 )
+    { // Aggregation should not be fragmented !
+        if ( header[0] > ( wd->fragThreshold + tkipFrameOffset ) )
+        {
+            return 0; // Need to be fragmented ! !
+        }
+    }
+
+    //if ( wd->sta.encryMode == ZM_TKIP )
+    //{
+    //    zm_debug_msg1("ctrl length = ", header[0]);
+    //}
+
+    //MAC control
+    if (rateProbingFlag != 0)
+    {
+        macCtrl |= 0x8000;
+    }
+    header[1] = macCtrl;
+    //PHY control L
+    header[2] = (u16_t) ((phyCtrl&0xffff) | 0x700 | (zcUpToAc[up&0x7]<<13));
+    //PHY control H
+    header[3] = (u16_t) ((phyCtrl>>16) | 0x700);
+
+    if (wd->enableAggregation)
+    {
+        /* force enable aggregation */
+        if (wd->enableAggregation==2 && !(header[6]&0x1))
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Enable aggregation */
+                header[1] |= 0x20;
+            }
+        }
+        /* if wd->enableAggregation=1 => force disable */
+        /* if wd->enableAggregation=0 => auto */
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    if (wd->addbaComplete) {
+        #ifdef ZM_BYPASS_AGGR_SCHEDULING
+        if (!(header[6]&0x1) && !rateProbingFlag && (wd->enableAggregation != 1))
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Unicast frame with HT rate => Enable aggregation */
+                /* We only support software encryption in single packet mode */
+                if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0 &&
+                    (wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) == 0)
+                {
+                    /* Set aggregation group bits per AC */
+                    header[1] |= (0x20 | (zcUpToAc[up&0x7]<<10));
+
+                    //if (wd->sta.currentFrequency < 3000)
+                    {
+                        /* issue: -PB42 Enable RTS/CTS to prevent OWL Tx hang up */
+                        /* If this is Owl Ap, enable RTS/CTS protect */
+                        if ( (wd->sta.athOwlAp == 1) || (wd->sta.RTSInAGGMode == TRUE) )
+                        {
+                            header[1] &= 0xfffc;
+                            header[1] |= 0x1;
+                        }
+
+                        /* Enable RIFS : workaround 854T RTS/CTS */
+                        /* Bit13 : TI enable RIFS */
+                        //header[1] |= 0x2000;
+                    }
+                }
+            }
+        }
+        #else
+        /*
+         * aggregation ampduIndication control
+         */
+        if (aggControl && aggControl->aggEnabled) {
+            if (wd->enableAggregation==0 && !(header[6]&0x1))
+            {
+                if (((header[2] & 0x3) == 2))
+                {
+                    /* Enable aggregation */
+                    header[1] |= 0x20;
+                    if (ZM_AGG_LAST_MPDU == aggControl->ampduIndication)
+                        header[1] |= 0x4000;
+                }
+                else {
+                    zm_debug_msg1("no aggr, header[2]&0x3 = ",header[2] & 0x3)
+                    aggControl->aggEnabled = 0;
+                }
+            }
+            else {
+                zm_debug_msg1("no aggr, wd->enableAggregation = ", wd->enableAggregation);
+                zm_debug_msg1("no aggr, !header[6]&0x1 = ",!(header[6]&0x1));
+                aggControl->aggEnabled = 0;
+            }
+        }
+        #endif
+
+        #ifdef ZM_AGGR_BIT_ON
+        if (!(header[6]&0x1) && !rateProbingFlag)
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Unicast frame with HT rate => Enable aggregation */
+                /* Set aggregation group bits per AC */
+                header[1] |= (0x20 | (zcUpToAc[up&0x7]<<10));
+
+                //if (wd->sta.currentFrequency < 3000)
+                {
+                    /* Enable RTS/CTS to prevent OWL Tx hang up */
+                    header[1] &= 0xfffc;
+                    header[1] |= 0x1;
+                }
+            }
+        }
+        #endif
+    }
+#endif
+
+    return (hlen<<1);
+}
+
+
+u16_t zfTxGenMmHeader(zdev_t* dev, u8_t frameType, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    //u16_t bodyLen;
+    u8_t  hlen = 32;        // MAC ctrl + PHY ctrl + 802.11 MM header
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* Generate control setting */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 24+len+4;   //Length
+    if ((dst[0] & 0x1) != 0) //Broadcast, multicast frames
+    {
+        header[1] = 0xc;            //MAC control, backoff + noack
+    }
+    else
+    {
+        header[1] = 0x8;            //MAC control, backoff + (ack)
+    }
+    /* Dualband Management frame tx Rate */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        if (wd->frequency < 3000)
+        {
+            /* CCK 1M */
+            header[2] = 0x0f00;          //PHY control L
+            header[3] = 0x0000;          //PHY control H
+        }
+        else
+        {
+            /* CCK 6M */
+            header[2] = 0x0f01;          //PHY control L
+            header[3] = 0x000B;          //PHY control H
+        }
+    }
+    else
+    {
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* CCK 2M */
+            header[2] = 0x0f00;          //PHY control L
+            header[3] = 0x0001;          //PHY control H
+        }
+        else
+        {
+            /* CCK 6M */
+            header[2] = 0x0f01;          //PHY control L
+            header[3] = 0x000B;          //PHY control H
+        }
+    }
+    /* Generate WLAN header */
+    /* Frame control */
+    header[4+0] = frameType;
+    /* Duration */
+    header[4+1] = 0;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if ( frameType == ZM_WLAN_FRAME_TYPE_PROBEREQ )
+        {
+            header[4+8] = 0xFFFF;
+            header[4+9] = 0xFFFF;
+            header[4+10] = 0xFFFF;
+        }
+        else if ( frameType == ZM_WLAN_FRAME_TYPE_BA ) {
+            /* do nothing */
+        }
+        else
+        {
+            header[4+8] = wd->sta.bssid[0];
+            header[4+9] = wd->sta.bssid[1];
+            header[4+10] = wd->sta.bssid[2];
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[4+8] = 0;
+        header[4+9] = 0;
+        header[4+10] = 0;
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+
+        if ( frameType == ZM_WLAN_FRAME_TYPE_ATIM )
+        {
+            /* put ATIM to queue 5th */
+            //header[2] |= (ZM_BIT_13|ZM_BIT_14);
+            header[2] |= ZM_BIT_15;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Address 3 = BSSID */
+        header[4+8] = wd->macAddr[0];
+        header[4+9] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+10] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+10] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+        //if in scan, must set address 3 to broadcast because of some ap would care this
+        //if ((wd->heartBeatNotification & ZM_BSSID_LIST_SCAN)
+        //        == ZM_BSSID_LIST_SCAN)
+        //if FrameType is Probe Request, Address3 should be boradcast
+        if (frameType == ZM_WLAN_FRAME_TYPE_PROBEREQ)
+        {
+            header[4+8] = 0xFFFF;
+            header[4+9] = 0xFFFF;
+            header[4+10] = 0xFFFF;
+        }
+    }
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+7] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+7] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+    if( frameType == ZM_WLAN_FRAME_TYPE_QOS_NULL )
+    {
+        /*Qos Control*/
+        header[4+12] = 0x0;
+        hlen+=2;
+        header[0]+=2;
+    }
+
+    if ( encrypt )
+    {
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+        {
+            if ( (wd->sta.encryMode == ZM_WEP64)||
+                 (wd->sta.encryMode == ZM_WEP128)||
+                 (wd->sta.encryMode == ZM_WEP256) )
+            {
+                header[4] |= 0x4000;
+                header[16] = 0x0;   //IV
+                header[17] = 0x0; //IV
+                header[17] |= (((u16_t) wd->sta.keyId) << 14);
+                hlen += 4;
+
+                header[0] += 8;         // icvLen = 4;
+                header[1] |= 0x40;      // enable encryption on macCtrl
+             }
+        }
+    }
+
+    // Enable HW duration
+    if ( frameType != ZM_WLAN_FRAME_TYPE_PSPOLL )
+    {
+        header[1] |= 0x200;
+    }
+
+    return hlen;
+}
+
+void zfInitMacApMode(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfHpEnableBeacon(dev, ZM_MODE_AP, (wd->beaconInterval/wd->ap.vapNumber), 1, 0);
+
+    /* AP mode */
+    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_AP);
+
+    /* VAP test code */
+    /* AP + VAP mode */
+    if (wd->ap.vapNumber >= 2)
+    {
+        for (i=1; i<ZM_MAX_AP_SUPPORT; i++)
+        {
+            if (((wd->ap.apBitmap >> i) & 0x1) != 0)
+            {
+                u16_t mac[3];
+                mac[0] = wd->macAddr[0];
+                mac[1] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+                mac[2] = wd->macAddr[2]; //Multiple SSID
+#else
+                mac[2] = wd->macAddr[2] + (i<<8); //VAP
+#endif
+                zfHpSetMacAddress(dev, mac, i);
+
+            }
+        }
+    }
+
+    /* basic rate setting */
+    zfHpSetBasicRateSet(dev, wd->bRateBasic, wd->gRateBasic);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME AP default. */
+    zfUpdateDefaultQosParameter(dev, 1);
+
+    return;
+}
+
+u16_t zfChGetNextChannel(zdev_t* dev, u16_t frequency, u8_t* pbPassive)
+{
+    u8_t   i;
+    u8_t   bPassive;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel == frequency )
+        {
+            if ( i == (wd->regulationTable.allowChannelCnt-1) )
+            {
+                i = 0;
+            }
+            else
+            {
+                i++;
+            }
+
+            if ( wd->regulationTable.allowChannel[i].channelFlags
+                    & ZM_REG_FLAG_CHANNEL_PASSIVE )
+            {
+                *pbPassive = TRUE;
+            }
+            else
+            {
+                *pbPassive = FALSE;
+            }
+
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    return 0xffff;
+}
+
+u16_t zfChGetFirstChannel(zdev_t* dev, u8_t* pbPassive)
+{
+    u8_t   bPassive;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+   if ( wd->regulationTable.allowChannel[0].channelFlags & ZM_REG_FLAG_CHANNEL_PASSIVE )
+    {
+        *pbPassive = TRUE;
+    }
+    else
+    {
+        *pbPassive = FALSE;
+    }
+
+    return wd->regulationTable.allowChannel[0].channel;
+}
+
+u16_t zfChGetFirst2GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel < 3000 )
+        {
+            /* find the first 2Ghz channel */
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    /* Can not find any 2Ghz channel */
+    return 0;
+}
+
+u16_t zfChGetFirst5GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel > 3000 )
+        {
+            /* find the first 5Ghz channel */
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    /* Can not find any 5Ghz channel */
+    return 0;
+}
+
+u16_t zfChGetLastChannel(zdev_t* dev, u8_t* pbPassive)
+{
+    u8_t   bPassive;
+    u8_t   ChannelIndex;
+
+    zmw_get_wlan_dev(dev);
+
+    ChannelIndex = wd->regulationTable.allowChannelCnt-1;
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+    if ( wd->regulationTable.allowChannel[ChannelIndex].channelFlags
+            & ZM_REG_FLAG_CHANNEL_PASSIVE )
+    {
+        *pbPassive = TRUE;
+    }
+    else
+    {
+        *pbPassive = FALSE;
+    }
+
+    return wd->regulationTable.allowChannel[ChannelIndex].channel;
+}
+
+u16_t zfChGetLast5GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+    u16_t   last5Ghzfrequency;
+
+    zmw_get_wlan_dev(dev);
+
+    last5Ghzfrequency = 0;
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel > 3000 )
+        {
+            last5Ghzfrequency = wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    return last5Ghzfrequency;
+}
+
+/* freqBand = 0 => auto check   */
+/*          = 1 => 2.4 GHz band */
+/*          = 2 => 5 GHz band   */
+u16_t zfChNumToFreq(zdev_t* dev, u8_t ch, u8_t freqBand)
+{
+    u16_t freq = 0xffff;
+
+    if ( freqBand == 0 )
+    {
+        if (ch > 14)
+        {   /* adapter is at 5 GHz band */
+            freqBand = 2;
+        }
+        else
+        {
+            freqBand = 1;
+        }
+    }
+
+    if ( freqBand == 2 )
+    {   /* the channel belongs to 5 GHz band */
+        if ( (ch >= 184)&&(ch <= 196) )
+        {
+            freq = 4000 + ch*5;
+        }
+        else
+        {
+            freq = 5000 + ch*5;
+        }
+    }
+    else
+    {   /* the channel belongs to 2.4 GHz band */
+        if ( ch == 14 )
+        {
+            freq = ZM_CH_G_14;
+        }
+        else
+        {
+            freq = ZM_CH_G_1 + (ch-1)*5;
+        }
+    }
+
+    return freq;
+}
+
+u8_t zfChFreqToNum(u16_t freq, u8_t* pbIs5GBand)
+{
+    u8_t   ch;
+    u8_t   Is5GBand;
+
+    /* to avoid NULL value */
+    if ( pbIs5GBand == NULL )
+    {
+        pbIs5GBand = &Is5GBand;
+    }
+
+    *pbIs5GBand = FALSE;
+
+    if ( freq == ZM_CH_G_14 )
+    {
+        ch = 14;
+    }
+    else if ( freq < 4000 )
+    {
+        ch = (freq - ZM_CH_G_1) / 5 + 1;
+    }
+    else if ( freq < 5000 )
+    {
+        ch = (freq - 4000) / 5;
+        *pbIs5GBand = TRUE;
+    }
+    else
+    {
+        ch = (freq - 5000) / 5;
+        *pbIs5GBand = TRUE;
+    }
+
+    return ch;
+}
diff --git a/drivers/staging/otus/80211core/cmm.c b/drivers/staging/otus/80211core/cmm.c
new file mode 100644
index 000000000000..b74379d928f6
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmm.c
@@ -0,0 +1,2141 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : mm.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains common functions for handle management     */
+/*      frame.                                                          */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+/* TODO : put all constant tables to a file */
+const u8_t zg11bRateTbl[4] = {2, 4, 11, 22};
+const u8_t zg11gRateTbl[8] = {12, 18, 24, 36, 48, 72, 96, 108};
+
+/* 0xff => element does not exist */
+const u8_t zgElementOffsetTable[] =
+{
+    4,      /*  0 : asoc req */
+    6,      /*  1 : asoc rsp */
+    10,     /*  2 : reasoc req*/
+    6,      /*  3 : reasoc rsp */
+    0,      /*  4 : probe req */
+    12,     /*  5 : probe rsp */
+    0xff,   /*  6 : reserved */
+    0xff,   /*  7 : reserved */
+    12,     /*  8 : beacon */
+    4,      /*  9 : ATIM */
+    0xff,   /* 10 : disasoc */
+    6,      /* 11 : auth */
+    0xff,   /* 12 : deauth */
+    4,      /* 13 : action */
+    0xff,   /* 14 : reserved */
+    0xff,   /* 15 : reserved */
+};
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFindElement               */
+/*      Find a specific element in management frame                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : management frame buffer                                   */
+/*      eid : target element id                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      byte offset of target element                                   */
+/*      or 0xffff if not found                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfFindElement(zdev_t* dev, zbuf_t* buf, u8_t eid)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id, HTEid=0;
+    u8_t oui[4] = {0x00, 0x50, 0xf2, 0x01};
+    u8_t oui11n[3] = {0x00,0x90,0x4C};
+    u8_t HTType = 0;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    // jhlee HT 0
+
+    if ((eid == ZM_WLAN_EID_HT_CAPABILITY) ||
+        (eid == ZM_WLAN_EID_EXTENDED_HT_CAPABILITY))
+    {
+        HTEid = eid;
+        eid = ZM_WLAN_EID_WPA_IE;
+        HTType = 1;
+    }
+
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == eid)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 && eid != ZM_WLAN_EID_SSID)
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( eid == ZM_WLAN_EID_WPA_IE )
+            {
+                /* avoid sta to be thought use 11n when find a WPA_IE */
+                if ( (HTType == 0) && zfRxBufferEqualToStr(dev, buf, oui, offset+2, 4) )
+                {
+                    return offset;
+                }
+
+                // jhlee HT 0
+                // CWYang(+)
+
+                if ((HTType == 1) && ( zfRxBufferEqualToStr(dev, buf, oui11n, offset+2, 3) ))
+                {
+                    if ( zmw_rx_buf_readb(dev, buf, offset+5) == HTEid )
+                    {
+                        return offset + 5;
+                    }
+                }
+
+            }
+            else
+            {
+                return offset;
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFindWifiElement           */
+/*      Find a specific Wifi element in management frame                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : management frame buffer                                   */
+/*      type : OUI type                                                 */
+/*      subType : OUI subtype                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      byte offset of target element                                   */
+/*      or 0xffff if not found                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfFindWifiElement(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0xF2)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+5)) == type))
+
+            {
+                if ( subtype != 0xff )
+                {
+                    if ( (tmp = zmw_rx_buf_readb(dev, buf, offset+6)) == subtype  )
+                    {
+                        return offset;
+                    }
+                }
+                else
+                {
+                    return offset;
+                }
+            }
+        }
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfRemoveElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t eid)
+{
+    u16_t offset = 0;
+    u16_t elen;
+    u8_t  HTEid = 0;
+    u8_t  oui[4] = {0x00, 0x50, 0xf2, 0x01};
+    u8_t  oui11n[3] = {0x00,0x90,0x4C};
+    u8_t  HTType = 0;
+
+    if ((eid == ZM_WLAN_EID_HT_CAPABILITY) ||
+        (eid == ZM_WLAN_EID_EXTENDED_HT_CAPABILITY))
+    {
+        HTEid = eid;
+        eid = ZM_WLAN_EID_WPA_IE;
+        HTType = 1;
+    }
+
+    while (offset < size)
+    {
+        elen = *(buf+offset+1);
+
+        if (*(buf+offset) == eid)
+        {
+            if ( eid == ZM_WLAN_EID_WPA_IE )
+            {
+                if ( (HTType == 0)
+                     && (*(buf+offset+2) == oui[0])
+                     && (*(buf+offset+3) == oui[1])
+                     && (*(buf+offset+4) == oui[2])
+                     && (*(buf+offset+5) == oui[3]) )
+                {
+                    zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                    return (size-elen-2);
+                }
+
+                if ( (HTType == 1)
+                    && (*(buf+offset+2) == oui11n[0])
+                    && (*(buf+offset+3) == oui11n[1])
+                    && (*(buf+offset+4) == oui11n[2])
+                    && (*(buf+offset+5) == HTEid) )
+                {
+                    zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                    return (size-elen-2);
+                }
+            }
+            else
+            {
+                zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                return (size-elen-2);
+            }
+        }
+
+        offset += (elen+2);
+    }
+
+    return size;
+}
+
+u16_t zfUpdateElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t* updateeid)
+{
+    u16_t offset = 0;
+    u16_t elen;
+
+    while (offset < size) {
+        elen = *(buf+offset+1);
+
+        if (*(buf+offset) == updateeid[0]) {
+            if (updateeid[1] <= elen) {
+                zfMemoryMove(buf+offset, updateeid, updateeid[1]+2);
+                zfMemoryMove(buf+offset+updateeid[1]+2, buf+offset+elen+2, size-offset-elen-2);
+
+                return size-(elen-updateeid[1]);
+            } else {
+                zfMemoryMove(buf+offset+updateeid[1]+2, buf+offset+elen+2, size-offset-elen-2);
+                zfMemoryMove(buf+offset, updateeid, updateeid[1]+2);
+
+                return size+(updateeid[1]-elen);
+            }
+        }
+
+        offset += (elen+2);
+    }
+
+    return size;
+}
+
+u16_t zfFindSuperGElement(zdev_t* dev, zbuf_t* buf, u8_t type)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t super_feature;
+    u8_t ouiSuperG[6] = {0x00,0x03,0x7f,0x01, 0x01, 0x00};
+
+    zmw_get_wlan_dev(dev);
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_VENDOR_PRIVATE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (zfRxBufferEqualToStr(dev, buf, ouiSuperG, offset+2, 6) && ( zmw_rx_buf_readb(dev, buf, offset+1) >= 6))
+            {
+                /* super_feature 0:useFastFrame, 1:useCompression, 2:useTurboPrime */
+                super_feature= zmw_rx_buf_readb(dev, buf, offset+8);
+                if ((super_feature & 0x01) || (super_feature & 0x02) || (super_feature & 0x04))
+                {
+                    return offset;
+                }
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindXRElement(zdev_t* dev, zbuf_t* buf, u8_t type)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t ouixr[6] = {0x00,0x03,0x7f,0x03, 0x01, 0x00};
+
+    zmw_get_wlan_dev(dev);
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_VENDOR_PRIVATE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (zfRxBufferEqualToStr(dev, buf, ouixr, offset+2, 6) && ( zmw_rx_buf_readb(dev, buf, offset+1) >= 6))
+            {
+                return offset;
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeSupportRate        */
+/*      Add information element Support Rate to buffer.                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      eid : element ID                                                */
+/*      rateSet :  CCK or OFDM                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeSupportRate(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t eid, u8_t rateSet)
+{
+    u8_t len = 0;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    //if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+    //{
+    //    return offset;
+    //}
+
+    /* Information : Support Rate */
+    if ( rateSet == ZM_RATE_SET_CCK )
+    {
+        for (i=0; i<4; i++)
+        {
+            if ((wd->bRate & (0x1<<i)) == (0x1<<i))
+            //if ((0xf & (0x1<<i)) == (0x1<<i))
+            {
+                zmw_tx_buf_writeb(dev, buf, offset+len+2,
+                                     zg11bRateTbl[i]+((wd->bRateBasic & (0x1<<i))<<(7-i)));
+                len++;
+            }
+        }
+    }
+    else if ( rateSet == ZM_RATE_SET_OFDM )
+    {
+        for (i=0; i<8; i++)
+        {
+            if ((wd->gRate & (0x1<<i)) == (0x1<<i))
+            //if ((0xff & (0x1<<i)) == (0x1<<i))
+            {
+                zmw_tx_buf_writeb(dev, buf, offset+len+2,
+                                     zg11gRateTbl[i]+((wd->gRateBasic & (0x1<<i))<<(7-i)));
+                len++;
+            }
+        }
+    }
+
+    if (len > 0)
+    {
+        /* Element ID */
+        zmw_tx_buf_writeb(dev, buf, offset, eid);
+
+        /* Element Length */
+        zmw_tx_buf_writeb(dev, buf, offset+1, len);
+
+        /* Return value */
+        offset += (2+len);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeDs                 */
+/*      Add information element DS to buffer.                           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeDs(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_DS);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 1);
+
+    /* Information : DS */
+    zmw_tx_buf_writeb(dev, buf, offset++,
+                         zfChFreqToNum(wd->frequency, NULL));
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeErp                */
+/*      Add information element ERP to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeErp(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_ERP);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 1);
+
+    /* Information : ERP */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->erpElement);
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeWpa                */
+/*      Add information element WPA to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         ZyDAS Technology Corporation    2006.2      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeWpa(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t apId)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    //zmw_inttx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    /* Element Length */
+    //zmw_inttx_buf_writeb(dev, buf, offset++, wd->ap.wpaLen);
+    for(i = 0; i < wd->ap.wpaLen[apId]; i++)
+    {
+        /* Information : WPA */
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.wpaIe[apId][i]);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddHTCapability         */
+/*      Add HT Capability Infomation to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang     ZyDAS Technology Corporation       2006.06    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+
+u16_t zfMmAddPreNHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    //u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.Length);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.Length);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                zfMmAddExtendedHTCapability   */
+/*      Add Extended HT Capability Infomation to buffer.                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang     ZyDAS Technology Corporation       2006.06    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddExtendedHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 22; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 22; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfSendMmFrame               */
+/*      Send management frame.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      frameType : management frame type                               */
+/*      dst : destination MAC address                                   */
+/*      p1 : parameter 1                                                */
+/*      p2 : parameter 2                                                */
+/*      p3 : parameter 3                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* probe req : p1=> bWithSSID, p2=>R, p3=>R                                      */
+/* probe rsp : p1=>R, p2=>R, p3=>VAP ID(AP)                             */
+/* deauth : p1=>Reason Code, p2=>R, p3=>VAP ID(AP)                      */
+/* Disasoc : p1=>Reason Code, p2=>R, p3=>VAP ID(AP)                     */
+/* ATIM : p1=>R, p2=>R, p3=>R                                           */
+/* (re)asoc rsp : p1=>Status Code, p2=>AID, p3=>VAP ID(AP)              */
+/* asoc req : p1=>R, p2=>R, p3=>R                                       */
+/* reasoc req : p1=>AP MAC[0], p2=>AP MAC[1], p3=>AP MAC[2]             */
+/* auth : p1=>low=Algorithm, high=Transaction, p2=>Status, p3=>VAP ID   */
+void zfSendMmFrame(zdev_t* dev, u8_t frameType, u16_t* dst,
+                   u32_t p1, u32_t p2, u32_t p3)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+    u16_t aid;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg2_mm(ZM_LV_2, "Send mm frame, type=", frameType);
+    /* TBD : Maximum size of managment frame */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    //Reserve room for wlan header
+    offset = hlen;
+
+    switch (frameType)
+    {
+        case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+            offset = zfSendProbeReq(dev, buf, offset, (u8_t) p1);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_PROBERSP :
+            zm_msg0_mm(ZM_LV_3, "probe rsp");
+            /* 24-31 Time Stamp : hardware WON'T fill this field */
+            zmw_tx_buf_writeh(dev, buf, offset, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+2, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+4, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+6, 0);
+            offset+=8;
+
+            /* Beacon Interval */
+            zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+            offset+=2;
+
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                vap = (u16_t) p3;
+                /* Capability */
+                zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+                offset+=2;
+                /* SSID */
+                offset = zfApAddIeSsid(dev, buf, offset, vap);
+            }
+            else
+            {
+                /* Capability */
+                zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+                zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+                /* SSID */
+                offset = zfStaAddIeSsid(dev, buf, offset);
+            }
+
+            /* Support Rate */
+            if ( wd->frequency < 3000 )
+            {
+            offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                          ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+            }
+            else
+            {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                          ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+            /* DS parameter set */
+            offset = zfMmAddIeDs(dev, buf, offset);
+
+            /* TODO ¡G IBSS */
+            if ( wd->wlanMode == ZM_MODE_IBSS )
+            {
+                offset = zfStaAddIeIbss(dev, buf, offset);
+
+                if (wd->frequency < 3000)
+                {
+                    if( wd->wfc.bIbssGMode
+                        && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+                    {
+                        /* ERP Information */
+                        wd->erpElement = 0;
+                        offset = zfMmAddIeErp(dev, buf, offset);
+
+                        /* Enable G Mode */
+                        /* Extended Supported Rates */
+                   	    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            	    }
+        	    }
+            }
+
+
+            if ((wd->wlanMode == ZM_MODE_AP)
+                 && (wd->ap.wlanType[vap] != ZM_WLAN_TYPE_PURE_B))
+            {
+                /* ERP Information */
+                offset = zfMmAddIeErp(dev, buf, offset);
+
+                /* Extended Supported Rates */
+		if ( wd->frequency < 3000 )
+                {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                   ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+            }
+
+            /* ERP Information */
+            //offset = zfMmAddIeErp(dev, buf, offset);
+
+            /* Extended Supported Rates */
+            //offset = zfMmAddIeSupportRate(dev, buf, offset,
+            //                              ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+
+            /* TODO : RSN */
+            if (wd->wlanMode == ZM_MODE_AP && wd->ap.wpaSupport[vap] == 1)
+            {
+                offset = zfMmAddIeWpa(dev, buf, offset, vap);
+            }
+            else if ( wd->wlanMode == ZM_MODE_IBSS && wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK)
+            {
+                offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+            }
+
+            /* WME Parameters */
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                if (wd->ap.qosMode == 1)
+                {
+                    offset = zfApAddIeWmePara(dev, buf, offset, vap);
+                }
+            }
+
+            if ( wd->wlanMode != ZM_MODE_IBSS )
+            {
+            // jhlee HT 0
+            //CWYang(+)
+                /* TODO : Need to check if it is ok */
+            /* HT Capabilities Info */
+            offset = zfMmAddHTCapability(dev, buf, offset);
+            //CWYang(+)
+            /* Extended HT Capabilities Info */
+            offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            }
+
+            if ( wd->sta.ibssAdditionalIESize )
+                offset = zfStaAddIbssAdditionalIE(dev, buf, offset);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_AUTH :
+            if (p1 == 0x30001)
+            {
+                hlen += 4;
+                offset += 4;        // for reserving wep header
+                encrypt = 1;
+            }
+
+            /* Algotrithm Number */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p1&0xffff));
+            offset+=2;
+
+            /* Transaction Number */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p1>>16));
+            offset+=2;
+
+            /* Status Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p2);
+            offset+=2;
+
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                vap = (u16_t) p3;
+            }
+
+            /* Challenge Text => share-2 or share-3 */
+            if (p1 == 0x20001)
+            {
+                if (p2 == 0) //Status == success
+                {
+                    zmw_buf_writeh(dev, buf, offset, 0x8010);
+                    offset+=2;
+                    /* share-2 : AP generate challenge text */
+                    for (i=0; i<128; i++)
+                    {
+                        wd->ap.challengeText[i] = (u8_t)zfGetRandomNumber(dev, 0);
+                    }
+                    zfCopyToIntTxBuffer(dev, buf, wd->ap.challengeText, offset, 128);
+                    offset += 128;
+                }
+            }
+            else if (p1 == 0x30001)
+            {
+                /* share-3 : STA return challenge Text */
+                zfCopyToIntTxBuffer(dev, buf, wd->sta.challengeText, offset, wd->sta.challengeText[1]+2);
+                offset += (wd->sta.challengeText[1]+2);
+            }
+
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+        case ZM_WLAN_FRAME_TYPE_REASOCREQ :
+            /* Capability */
+            zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+            zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+
+            /* Listen Interval */
+            zmw_tx_buf_writeh(dev, buf, offset, 0x0005);
+            offset+=2;
+
+            /* Reassocaited Request : Current AP address */
+            if (frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ)
+            {
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[0]);
+                offset+=2;
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[1]);
+                offset+=2;
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[2]);
+                offset+=2;
+            }
+
+            /* SSID */
+            offset = zfStaAddIeSsid(dev, buf, offset);
+
+
+            if ( wd->sta.currentFrequency < 3000 )
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+            }
+            else
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+            if ((wd->sta.capability[1] & ZM_BIT_0) == 1)
+            {   //spectrum managment flag enable
+                offset = zfStaAddIePowerCap(dev, buf, offset);
+                offset = zfStaAddIeSupportCh(dev, buf, offset);
+            }
+
+            if (wd->sta.currentFrequency < 3000)
+            {
+                /* Extended Supported Rates */
+                if (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N))
+                {
+                    offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+                }
+            }
+
+
+            //offset = zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+            //Move to wrapper function, for OS difference--CWYang(m)
+            //for windows wrapper, zfwStaAddIeWpaRsn() should be below:
+            //u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+            //{
+            //    return zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+            //}
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, frameType);
+
+#ifdef ZM_ENABLE_CENC
+            /* CENC */
+            //if (wd->sta.encryMode == ZM_CENC)
+            offset = zfStaAddIeCenc(dev, buf, offset);
+#endif //ZM_ENABLE_CENC
+            if (((wd->sta.wmeEnabled & ZM_STA_WME_ENABLE_BIT) != 0) //WME enabled
+              && ((wd->sta.apWmeCapability & 0x1) != 0)) //WME AP
+            {
+                if (((wd->sta.apWmeCapability & 0x80) != 0) //UAPSD AP
+                 && ((wd->sta.wmeEnabled & ZM_STA_UAPSD_ENABLE_BIT) != 0)) //UAPSD enabled
+                {
+                    offset = zfStaAddIeWmeInfo(dev, buf, offset, wd->sta.wmeQosInfo);
+                }
+                else
+                {
+                    offset = zfStaAddIeWmeInfo(dev, buf, offset, 0);
+                }
+            }
+            // jhlee HT 0
+            //CWYang(+)
+            if (wd->sta.EnableHT != 0)
+            {
+                #ifndef ZM_DISABLE_AMSDU8K_SUPPORT
+                    //Support 8K A-MSDU
+                    if (wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED)
+                    {
+                        wd->sta.HTCap.Data.HtCapInfo |= HTCAP_MaxAMSDULength;
+                    }
+                    else
+                    {
+                        wd->sta.HTCap.Data.HtCapInfo &= (~HTCAP_MaxAMSDULength);
+                    }
+                #else
+                    //Support 4K A-MSDU
+                    wd->sta.HTCap.Data.HtCapInfo &= (~HTCAP_MaxAMSDULength);
+                #endif
+
+                /* HT Capabilities Info */
+                if (wd->BandWidth40 == 1) {
+                    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+                }
+                else {
+                    wd->sta.HTCap.Data.HtCapInfo &= ~HTCAP_SupChannelWidthSet;
+                    //wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+                }
+
+                wd->sta.HTCap.Data.AMPDUParam &= ~HTCAP_MaxRxAMPDU3;
+                wd->sta.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+                wd->sta.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+                offset = zfMmAddHTCapability(dev, buf, offset);
+                offset = zfMmAddPreNHTCapability(dev, buf, offset);
+                //CWYang(+)
+                /* Extended HT Capabilities Info */
+                //offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            }
+
+
+            //Store asoc request frame body, for VISTA only
+            wd->sta.asocReqFrameBodySize = ((offset - hlen) >
+                    ZM_CACHED_FRAMEBODY_SIZE)?
+                    ZM_CACHED_FRAMEBODY_SIZE:(offset - hlen);
+            for (i=0; i<wd->sta.asocReqFrameBodySize; i++)
+            {
+                wd->sta.asocReqFrameBody[i] = zmw_tx_buf_readb(dev, buf, i + hlen);
+            }
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+        case ZM_WLAN_FRAME_TYPE_REASOCRSP :
+            vap = (u16_t) p3;
+
+            /* Capability */
+            zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+            offset+=2;
+
+            /* Status Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p1);
+            offset+=2;
+
+            /* AID */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p2|0xc000));
+            offset+=2;
+
+
+            if ( wd->frequency < 3000 )
+            {
+            /* Support Rate */
+            offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+            /* Extended Supported Rates */
+            offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+            else
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+
+
+            /* WME Parameters */
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                /* TODO : if WME STA then send WME parameter element */
+                if (wd->ap.qosMode == 1)
+                {
+                    offset = zfApAddIeWmePara(dev, buf, offset, vap);
+                }
+            }
+            // jhlee HT 0
+            //CWYang(+)
+            /* HT Capabilities Info */
+            offset = zfMmAddHTCapability(dev, buf, offset);
+            //CWYang(+)
+            /* Extended HT Capabilities Info */
+            offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ATIM :
+            /* NULL frame */
+            /* TODO : add two dumb bytes temporarily */
+            offset += 2;
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_QOS_NULL :
+            zmw_buf_writeh(dev, buf, offset, 0x0010);
+            offset += 2;
+            break;
+
+        case ZM_WLAN_DATA_FRAME :
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_DISASOC :
+        case ZM_WLAN_FRAME_TYPE_DEAUTH :
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+              vap = (u16_t) p3;
+
+              if ((aid = zfApFindSta(dev, dst)) != 0xffff)
+              {
+                  zmw_enter_critical_section(dev);
+                  /* Clear STA table */
+                  wd->ap.staTable[aid].valid = 0;
+
+                  zmw_leave_critical_section(dev);
+
+                  if (wd->zfcbDisAsocNotify != NULL)
+                  {
+                      wd->zfcbDisAsocNotify(dev, (u8_t*)dst, vap);
+                  }
+              }
+            }
+            /* Reason Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p1);
+            offset+=2;
+            break;
+    }
+
+    zfwBufSetSize(dev, buf, offset);
+
+    zm_msg2_mm(ZM_LV_2, "management frame body size=", offset-hlen);
+
+    //Copy wlan header
+    zfTxGenMmHeader(dev, frameType, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return;
+#if 0
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+#endif
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessManagement         */
+/*      Process received management frame.                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received management frame buffer                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessManagement(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo) //CWYang(m)
+{
+    u8_t frameType;
+    u16_t ta[3];
+    u16_t ra[3];
+    u16_t vap = 0, index = 0;
+    //u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    ra[0] = zmw_rx_buf_readh(dev, buf, 4);
+    ra[1] = zmw_rx_buf_readh(dev, buf, 6);
+    ra[2] = zmw_rx_buf_readh(dev, buf, 8);
+
+    ta[0] = zmw_rx_buf_readh(dev, buf, 10);
+    ta[1] = zmw_rx_buf_readh(dev, buf, 12);
+    ta[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    frameType = zmw_rx_buf_readb(dev, buf, 0);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#if 1
+        vap = 0;
+        if ((ra[0] & 0x1) != 1)
+        {
+            /* AP : Find virtual AP */
+            if ((index = zfApFindSta(dev, ta)) != 0xffff)
+            {
+                vap = wd->ap.staTable[index].vap;
+            }
+        }
+        zm_msg2_mm(ZM_LV_2, "vap=", vap);
+#endif
+
+        /* Dispatch by frame type */
+        switch (frameType)
+        {
+                /* Beacon */
+            case ZM_WLAN_FRAME_TYPE_BEACON :
+                zfApProcessBeacon(dev, buf);
+                break;
+                /* Authentication */
+            case ZM_WLAN_FRAME_TYPE_AUTH :
+                zfApProcessAuth(dev, buf, ta, vap);
+                break;
+                /* Association request */
+            case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+                /* Reassociation request */
+            case ZM_WLAN_FRAME_TYPE_REASOCREQ :
+                zfApProcessAsocReq(dev, buf, ta, vap);
+                break;
+                /* Association response */
+            case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+                //zfApProcessAsocRsp(dev, buf);
+                break;
+                /* Deauthentication */
+            case ZM_WLAN_FRAME_TYPE_DEAUTH :
+                zfApProcessDeauth(dev, buf, ta, vap);
+                break;
+                /* Disassociation */
+            case ZM_WLAN_FRAME_TYPE_DISASOC :
+                zfApProcessDisasoc(dev, buf, ta, vap);
+                break;
+                /* Probe request */
+            case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+                zfProcessProbeReq(dev, buf, ta);
+                break;
+                /* Probe response */
+            case ZM_WLAN_FRAME_TYPE_PROBERSP :
+                zfApProcessProbeRsp(dev, buf, AddInfo);
+                break;
+                /* Action */
+            case ZM_WLAN_FRAME_TYPE_ACTION :
+                zfApProcessAction(dev, buf);
+                break;
+        }
+    }
+    else //if ((wd->wlanMode == ZM_MODE_INFRASTRUCTURE) || (wd->wlanMode == ZM_MODE_IBSS))
+    {
+        /* Dispatch by frame type */
+        switch (frameType)
+        {
+                /* Beacon */
+            case ZM_WLAN_FRAME_TYPE_BEACON :
+                /* if enable 802.11h and current chanel is silent but receive beacon from other AP */
+                if (((wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                        & ZM_REG_FLAG_CHANNEL_CSA) != 0) && wd->sta.DFSEnable)
+                {
+                    wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                            &= ~(ZM_REG_FLAG_CHANNEL_CSA & ZM_REG_FLAG_CHANNEL_PASSIVE);
+                }
+                zfStaProcessBeacon(dev, buf, AddInfo); //CWYang(m)
+                break;
+                /* Authentication */
+            case ZM_WLAN_FRAME_TYPE_AUTH :
+                /* TODO : vap parameter is useless in STA mode, get rid of it */
+                zfStaProcessAuth(dev, buf, ta, 0);
+                break;
+                /* Association request */
+            case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+                /* TODO : vap parameter is useless in STA mode, get rid of it */
+                zfStaProcessAsocReq(dev, buf, ta, 0);
+                break;
+                /* Association response */
+            case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+                /* Reassociation request */
+            case ZM_WLAN_FRAME_TYPE_REASOCRSP :
+                zfStaProcessAsocRsp(dev, buf);
+                break;
+                /* Deauthentication */
+            case ZM_WLAN_FRAME_TYPE_DEAUTH :
+                zm_debug_msg0("Deauthentication received");
+                zfStaProcessDeauth(dev, buf);
+                break;
+                /* Disassociation */
+            case ZM_WLAN_FRAME_TYPE_DISASOC :
+                zm_debug_msg0("Disassociation received");
+                zfStaProcessDisasoc(dev, buf);
+                break;
+                /* Probe request */
+            case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+                zfProcessProbeReq(dev, buf, ta);
+                break;
+                /* Probe response */
+            case ZM_WLAN_FRAME_TYPE_PROBERSP :
+                /* if enable 802.11h and current chanel is silent but receive probe response from other AP */
+                if (((wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                        & ZM_REG_FLAG_CHANNEL_CSA) != 0) && wd->sta.DFSEnable)
+                {
+                    wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                            &= ~(ZM_REG_FLAG_CHANNEL_CSA & ZM_REG_FLAG_CHANNEL_PASSIVE);
+                }
+                zfStaProcessProbeRsp(dev, buf, AddInfo);
+                break;
+
+            case ZM_WLAN_FRAME_TYPE_ATIM:
+                zfStaProcessAtim(dev, buf);
+                break;
+                /* Action */
+            case ZM_WLAN_FRAME_TYPE_ACTION :
+                zm_msg0_mm(ZM_LV_2, "ProcessActionMgtFrame");
+                zfStaProcessAction(dev, buf);
+                break;
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeReq           */
+/*      Process probe request management frame.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src)
+{
+    u16_t offset;
+    u8_t len;
+    u16_t i, j;
+    u8_t ch;
+    u16_t sendFlag;
+
+    zmw_get_wlan_dev(dev);
+
+    /* check mode : AP/IBSS */
+    if ((wd->wlanMode != ZM_MODE_AP) && (wd->wlanMode != ZM_MODE_IBSS))
+    {
+        zm_msg0_mm(ZM_LV_3, "Ignore probe req");
+        return;
+    }
+
+    if ((wd->wlanMode != ZM_MODE_AP) && (wd->sta.adapterState == ZM_STA_STATE_DISCONNECT))
+    {
+        zm_msg0_mm(ZM_LV_3, "Packets dropped due to disconnect state");
+        return;
+    }
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, 0, 0, 0);
+
+        return;
+    }
+
+    /* check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_3, "probe req SSID not found");
+        return;
+    }
+
+    len = zmw_rx_buf_readb(dev, buf, offset+1);
+
+    for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+    {
+        if ((wd->ap.apBitmap & (1<<i)) != 0)
+        {
+            zm_msg1_mm(ZM_LV_3, "len=", len);
+            sendFlag = 0;
+            /* boardcast SSID */
+            if (len == 0)
+            {
+                if (wd->ap.hideSsid[i] == 0)
+                {
+                    sendFlag = 1;
+                }
+            }
+            /* Not broadcast SSID */
+            else if (wd->ap.ssidLen[i] == len)
+            {
+                for (j=0; j<len; j++)
+                {
+                    if ((ch = zmw_rx_buf_readb(dev, buf, offset+2+j))
+                            != wd->ap.ssid[i][j])
+                    {
+                        break;
+                    }
+                }
+                if (j == len)
+                {
+                    sendFlag = 1;
+                }
+            }
+            if (sendFlag == 1)
+            {
+                /* Send probe response */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, i, 0, i);
+            }
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeRsp           */
+/*      Process probe response management frame.                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*  AddInfo : Rx Header and Rx Mac Status                               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Aress Yang          ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+    /* Gather scan result */
+    /* Parse TIM and send PS-POLL in power saving mode */
+    struct zsWlanProbeRspFrameHeader*  pProbeRspHeader;
+    struct zsBssInfo* pBssInfo;
+    u8_t   pBuf[sizeof(struct zsWlanProbeRspFrameHeader)];
+    int    res;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0,
+                       sizeof(struct zsWlanProbeRspFrameHeader));
+    pProbeRspHeader = (struct zsWlanProbeRspFrameHeader*) pBuf;
+
+    zmw_enter_critical_section(dev);
+
+    //zm_debug_msg1("bss count = ", wd->sta.bssList.bssCount);
+
+    pBssInfo = zfStaFindBssInfo(dev, buf, pProbeRspHeader);
+
+    //if ( i == wd->sta.bssList.bssCount )
+    if ( pBssInfo == NULL )
+    {
+        /* Allocate a new entry if BSS not in the scan list */
+        pBssInfo = zfBssInfoAllocate(dev);
+        if (pBssInfo != NULL)
+        {
+            res = zfStaInitBssInfo(dev, buf, pProbeRspHeader, pBssInfo, AddInfo, 0);
+            //zfDumpSSID(pBssInfo->ssid[1], &(pBssInfo->ssid[2]));
+            if ( res != 0 )
+            {
+                zfBssInfoFree(dev, pBssInfo);
+            }
+            else
+            {
+                zfBssInfoInsertToList(dev, pBssInfo);
+            }
+        }
+    }
+    else
+    {
+        res = zfStaInitBssInfo(dev, buf, pProbeRspHeader, pBssInfo, AddInfo, 1);
+        if (res == 2)
+        {
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            int idx;
+
+            // It would reset the alive counter if the peer station is found!
+            zfStaFindFreeOpposite(dev, (u16_t *)pBssInfo->macaddr, &idx);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfSendProbeReq              */
+/*      Send probe request management frame.                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+
+u16_t zfSendProbeReq(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t bWithSSID)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    /* SSID */
+    if (bWithSSID == 0)  /* broadcast ssid */
+    {
+        //zmw_leave_critical_section(dev);
+        zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+        zmw_tx_buf_writeb(dev, buf, offset++, 0);   /* length = 0 */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        if (wd->ws.probingSsidList[bWithSSID-1].ssidLen == 0)
+        {
+            zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+            zmw_tx_buf_writeb(dev, buf, offset++, 0);   /* length = 0 */
+        }
+        else
+        {
+            zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+            zmw_tx_buf_writeb(dev, buf, offset++,
+                    wd->ws.probingSsidList[bWithSSID-1].ssidLen);
+            zfCopyToIntTxBuffer(dev, buf,
+                    wd->ws.probingSsidList[bWithSSID-1].ssid,
+                    offset,
+                    wd->ws.probingSsidList[bWithSSID-1].ssidLen); /* ssid */
+            offset += wd->ws.probingSsidList[bWithSSID-1].ssidLen;
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    /* Supported rates */
+    if ( wd->sta.currentFrequency < 3000 )
+    {   /* 802.11b+g */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+        if (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) {
+            if (wd->wlanMode == ZM_MODE_IBSS) {
+                if (wd->wfc.bIbssGMode) {
+                    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+                }
+            } else {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+        }
+    }
+    else
+    {  /* 802.11a */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+    }
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfUpdateDefaultQosParameter */
+/*      Update TxQs CWMIN, CWMAX, AIFS and TXO to WME default value.    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      mode : 0=>STA, 1=>AP                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfUpdateDefaultQosParameter(zdev_t* dev, u8_t mode)
+{
+    u16_t cwmin[5];
+    u16_t cwmax[5];
+    u16_t aifs[5];
+    u16_t txop[5];
+
+    /* WMM parameter for STA */
+    /* Best Effor */
+    cwmin[0] = 15;
+    cwmax[0] = 1023;
+    aifs[0] = 3 * 9 + 10;
+    txop[0] = 0;
+    /* Back Ground */
+    cwmin[1] = 15;
+    cwmax[1] = 1023;
+    aifs[1] = 7 * 9 + 10;
+    txop[1] = 0;
+    /* VIDEO */
+    cwmin[2] = 7;
+    cwmax[2] = 15;
+    aifs[2] = 2 * 9 + 10;
+    txop[2] = 94;
+    /* VOICE */
+    cwmin[3] = 3;
+    cwmax[3] = 7;
+    aifs[3] = 2 * 9 + 10;
+    txop[3] = 47;
+    /* Special TxQ */
+    cwmin[4] = 3;
+    cwmax[4] = 7;
+    aifs[4] = 2 * 9 + 10;
+    txop[4] = 0;
+
+    /* WMM parameter for AP */
+    if (mode == 1)
+    {
+        cwmax[0] = 63;
+        aifs[3] = 1 * 9 + 10;
+        aifs[4] = 1 * 9 + 10;
+    }
+    zfHpUpdateQosParameter(dev, cwmin, cwmax, aifs, txop);
+}
+
+u16_t zfFindATHExtCap(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x03)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x7f)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+5)) == type))
+
+            {
+                if ( subtype != 0xff )
+                {
+                    if ( (tmp = zmw_rx_buf_readb(dev, buf, offset+6)) == subtype  )
+                    {
+                        return offset;
+                    }
+                }
+                else
+                {
+                    return offset;
+                }
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindBrdcmMrvlRlnkExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x10)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x18))
+
+            {
+                return offset;
+            }
+            else if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x43))
+
+            {
+                return offset;
+            }
+        }
+        else if ((id = zmw_rx_buf_readb(dev, buf, offset)) == 0x7F)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x01)
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindMarvelExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                  && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                  && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x43))
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindBroadcomExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while((offset+2) < bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) > (bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if (elen == 0)
+            {
+                return 0xffff;
+            }
+
+            if ( ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                 && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x10)
+                 && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x18) )
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+
+        offset += (elen+2);
+    }
+
+    return 0xffff;
+}
+
+u16_t zfFindRlnkExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while((offset+2) < bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == 0x7F)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) > (bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x01)
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+
+        offset += (elen+2);
+    }
+
+    return 0xffff;
+}
diff --git a/drivers/staging/otus/80211core/cmmap.c b/drivers/staging/otus/80211core/cmmap.c
new file mode 100644
index 000000000000..7f09fded459e
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmmap.c
@@ -0,0 +1,2402 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : mm.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains common functions for handle AP             */
+/*      management frame.                                               */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+extern const u8_t zcUpToAc[];
+
+void zfMmApTimeTick(zdev_t* dev)
+{
+    u32_t now;
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg1("wd->wlanMode : ", wd->wlanMode);
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* => every 1.28 seconds */
+        /* AP : aging STA that does not active for wd->ap.staAgingTime    */
+        now = wd->tick & 0x7f;
+        if (now == 0x0)
+        {
+            zfApAgingSta(dev);
+        }
+        else if (now == 0x1f)
+        {
+            zfQueueAge(dev, wd->ap.uapsdQ, wd->tick, 10000);
+        }
+        /* AP : check (wd->ap.protectedObss) and (wd->ap.bStaAssociated)  */
+        /*      to enable NonErp and Protection mode                      */
+        else if (now == 0x3f)
+        {
+            //zfApProtctionMonitor(dev);
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApInitStaTbl              */
+/*      Init AP's station table.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfApInitStaTbl(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        wd->ap.staTable[i].valid = 0;
+        wd->ap.staTable[i].state = 0;
+        wd->ap.staTable[i].addr[0] = 0;
+        wd->ap.staTable[i].addr[1] = 0;
+        wd->ap.staTable[i].addr[2] = 0;
+        wd->ap.staTable[i].time = 0;
+        wd->ap.staTable[i].vap = 0;
+        wd->ap.staTable[i].encryMode = ZM_NO_WEP;
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApFindSta                 */
+/*      Find a STA in station table.                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : Target STA address                                       */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      other : STA table index                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApFindSta(zdev_t* dev, u16_t* addr)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        if (wd->ap.staTable[i].valid == 1)
+        {
+            if ((wd->ap.staTable[i].addr[0] == addr[0])
+                    && (wd->ap.staTable[i].addr[1] == addr[1])
+                    && (wd->ap.staTable[i].addr[2] == addr[2]))
+            {
+                return i;
+            }
+        }
+    }
+    return 0xffff;
+}
+
+u16_t zfApGetSTAInfo(zdev_t* dev, u16_t* addr, u16_t* state, u8_t* vap)
+{
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *vap = wd->ap.staTable[id].vap;
+        *state = wd->ap.staTable[id++].state;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return id;
+}
+
+
+void zfApGetStaQosType(zdev_t* dev, u16_t* addr, u8_t* qosType)
+{
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *qosType = wd->ap.staTable[id].qosType;
+    }
+    else
+    {
+        *qosType = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+void zfApGetStaTxRateAndQosType(zdev_t* dev, u16_t* addr, u32_t* phyCtrl,
+                                u8_t* qosType, u16_t* rcProbingFlag)
+{
+    u16_t id;
+    u8_t rate;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->ap.staTable[id].rcCell, rcProbingFlag);
+#ifdef ZM_AP_DEBUG
+        //rate = 15;
+#endif
+        *phyCtrl = zcRateToPhyCtrl[rate];
+        *qosType = wd->ap.staTable[id].qosType;
+    }
+    else
+    {
+        if (wd->frequency < 3000)
+        {
+            /* CCK 1M */
+            //header[2] = 0x0f00;          //PHY control L
+            //header[3] = 0x0000;          //PHY control H
+            *phyCtrl = 0x00000F00;
+        }
+        else
+        {
+            /* CCK 6M */
+            //header[2] = 0x0f01;          //PHY control L
+            //header[3] = 0x000B;          //PHY control H
+            *phyCtrl = 0x000B0F01;
+        }
+        *qosType = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "PhyCtrl=", *phyCtrl);
+    return;
+}
+
+void zfApGetStaEncryType(zdev_t* dev, u16_t* addr, u8_t* encryType)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *encryType = wd->ap.staTable[id].encryMode;
+    }
+    else
+    {
+        *encryType = ZM_NO_WEP;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "encyrType=", *encryType);
+    return;
+}
+
+void zfApGetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t* iv16, u32_t* iv32)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *iv16 = wd->ap.staTable[id].iv16;
+        *iv32 = wd->ap.staTable[id].iv32;
+    }
+    else
+    {
+        *iv16 = 0;
+        *iv32 = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "iv16=", *iv16);
+    zm_msg2_mm(ZM_LV_3, "iv32=", *iv32);
+    return;
+}
+
+void zfApSetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t iv16, u32_t iv32)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        wd->ap.staTable[id].iv16 = iv16;
+        wd->ap.staTable[id].iv32 = iv32;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "iv16=", iv16);
+    zm_msg2_mm(ZM_LV_3, "iv32=", iv32);
+    return;
+}
+
+void zfApClearStaKey(zdev_t* dev, u16_t* addr)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t bcAddr[3] = { 0xffff, 0xffff, 0xffff };
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    if (zfMemoryIsEqual((u8_t*)bcAddr, (u8_t*)addr, sizeof(bcAddr)) == TRUE)
+    {
+        /* Turn off group key information */
+    //    zfClearKey(dev, 0);
+    }
+    else
+    {
+        zmw_declare_for_critical_section();
+
+        zmw_enter_critical_section(dev);
+
+        if ((id = zfApFindSta(dev, addr)) != 0xffff)
+        {
+            /* Turn off STA's key information */
+            zfHpRemoveKey(dev, id+1);
+
+            /* Update STA's Encryption Type */
+            wd->ap.staTable[id].encryMode = ZM_NO_WEP;
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_3, "Can't find STA address\n");
+        }
+        zmw_leave_critical_section(dev);
+    }
+}
+
+#ifdef ZM_ENABLE_CENC
+void zfApGetStaCencIvAndKeyIdx(zdev_t* dev, u16_t* addr, u32_t *iv, u8_t *keyIdx)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *iv++ = wd->ap.staTable[id].txiv[0];
+        *iv++ = wd->ap.staTable[id].txiv[1];
+        *iv++ = wd->ap.staTable[id].txiv[2];
+        *iv = wd->ap.staTable[id].txiv[3];
+        *keyIdx = wd->ap.staTable[id].cencKeyIdx;
+    }
+    else
+    {
+        *iv++ = 0x5c365c37;
+        *iv++ = 0x5c365c36;
+        *iv++ = 0x5c365c36;
+        *iv = 0x5c365c36;
+        *keyIdx = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void zfApSetStaCencIv(zdev_t* dev, u16_t* addr, u32_t *iv)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        wd->ap.staTable[id].txiv[0] = *iv++;
+        wd->ap.staTable[id].txiv[1] = *iv++;
+        wd->ap.staTable[id].txiv[2] = *iv++;
+        wd->ap.staTable[id].txiv[3] = *iv;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+#endif //ZM_ENABLE_CENC
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApFlushBufferedPsFrame    */
+/*      Free buffered PS frames.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfApFlushBufferedPsFrame(zdev_t* dev)
+{
+    u16_t emptyFlag;
+    u16_t freeCount;
+    u16_t vap;
+    zbuf_t* psBuf = NULL;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    freeCount = 0;
+    emptyFlag = 0;
+    while (1)
+    {
+        psBuf = NULL;
+        zmw_enter_critical_section(dev);
+        if (wd->ap.uniHead != wd->ap.uniTail)
+        {
+            psBuf = wd->ap.uniArray[wd->ap.uniHead];
+            wd->ap.uniHead = (wd->ap.uniHead + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+        }
+        else
+        {
+            emptyFlag = 1;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (psBuf != NULL)
+        {
+            zfwBufFree(dev, psBuf, ZM_ERR_FLUSH_PS_QUEUE);
+        }
+        zm_assert(freeCount++ < (ZM_UNI_ARRAY_SIZE*2));
+
+        if (emptyFlag != 0)
+        {
+            break;
+        }
+    }
+
+    for (vap=0; vap<ZM_MAX_AP_SUPPORT; vap++)
+    {
+        freeCount = 0;
+        emptyFlag = 0;
+        while (1)
+        {
+            psBuf = NULL;
+            zmw_enter_critical_section(dev);
+            if (wd->ap.bcmcHead[vap] != wd->ap.bcmcTail[vap])
+            {
+                psBuf = wd->ap.bcmcArray[vap][wd->ap.bcmcHead[vap]];
+                wd->ap.bcmcHead[vap] = (wd->ap.bcmcHead[vap] + 1)
+                        & (ZM_BCMC_ARRAY_SIZE - 1);
+            }
+            else
+            {
+                emptyFlag = 1;
+            }
+            zmw_leave_critical_section(dev);
+
+            if (psBuf != NULL)
+            {
+                zfwBufFree(dev, psBuf, ZM_ERR_FLUSH_PS_QUEUE);
+            }
+            zm_assert(freeCount++ < (ZM_BCMC_ARRAY_SIZE*2));
+
+            if (emptyFlag != 0)
+            {
+                break;
+            }
+        }
+    }
+    return;
+}
+
+
+u16_t zfApBufferPsFrame(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t id;
+    u16_t addr[3];
+    u16_t vap = 0;
+    u8_t up;
+    u16_t fragOff;
+    u8_t ac;
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (port < ZM_MAX_AP_SUPPORT)
+    {
+        vap = port;
+    }
+
+    addr[0] = zmw_rx_buf_readh(dev, buf, 0);
+    addr[1] = zmw_rx_buf_readh(dev, buf, 2);
+    addr[2] = zmw_rx_buf_readh(dev, buf, 4);
+
+    if ((addr[0] & 0x1) == 0x1)
+    {
+        if (wd->ap.staPowerSaving > 0)
+        {
+            zmw_enter_critical_section(dev);
+
+            /* Buffer this BC or MC frame */
+            if (((wd->ap.bcmcTail[vap]+1)&(ZM_BCMC_ARRAY_SIZE-1))
+                    != wd->ap.bcmcHead[vap])
+            {
+                wd->ap.bcmcArray[vap][wd->ap.bcmcTail[vap]++] = buf;
+                wd->ap.bcmcTail[vap] &= (ZM_BCMC_ARRAY_SIZE-1);
+                zmw_leave_critical_section(dev);
+
+                zm_msg0_tx(ZM_LV_0, "Buffer BCMC");
+            }
+            else
+            {
+                /* bcmcArray full */
+                zmw_leave_critical_section(dev);
+
+                zm_msg0_tx(ZM_LV_0, "BCMC buffer full");
+
+                /* free buffer according to buffer type */
+                zfwBufFree(dev, buf, ZM_ERR_BCMC_PS_BUFFER_UNAVAILABLE);
+            }
+            return 1;
+        }
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+
+        if ((id = zfApFindSta(dev, addr)) != 0xffff)
+        {
+            if (wd->ap.staTable[id].psMode == 1)
+            {
+
+                zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+                ac = zcUpToAc[up&0x7] & 0x3;
+
+                if ((wd->ap.staTable[id].qosType == 1) &&
+                        ((wd->ap.staTable[id].qosInfo & (0x8>>ac)) != 0))
+                {
+                    ret = zfQueuePutNcs(dev, wd->ap.uapsdQ, buf, wd->tick);
+                    zmw_leave_critical_section(dev);
+                    if (ret != ZM_SUCCESS)
+                    {
+                        zfwBufFree(dev, buf, ZM_ERR_AP_UAPSD_QUEUE_FULL);
+                    }
+                }
+                else
+                {
+                /* Buffer this unicast frame */
+                if (((wd->ap.uniTail+1)&(ZM_UNI_ARRAY_SIZE-1))
+                        != wd->ap.uniHead)
+                {
+                    wd->ap.uniArray[wd->ap.uniTail++] = buf;
+                    wd->ap.uniTail &= (ZM_UNI_ARRAY_SIZE-1);
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_tx(ZM_LV_0, "Buffer UNI");
+
+                }
+                else
+                {
+                    /* uniArray full */
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_tx(ZM_LV_0, "UNI buffer full");
+                    /* free buffer according to buffer type */
+                    zfwBufFree(dev, buf, ZM_ERR_UNI_PS_BUFFER_UNAVAILABLE);
+                }
+                }
+                return 1;
+            } /* if (wd->ap.staTable[id++].psMode == 1) */
+        } /* if ((id = zfApFindSta(dev, addr)) != 0xffff) */
+        zmw_leave_critical_section(dev);
+    }
+
+    return 0;
+}
+
+u16_t zfApGetSTAInfoAndUpdatePs(zdev_t* dev, u16_t* addr, u16_t* state,
+                                u8_t* vap, u16_t psMode, u8_t* uapsdTrig)
+{
+    u16_t id;
+    u8_t uapsdStaAwake = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+#ifdef ZM_AP_DEBUG
+    //psMode=0;
+#endif
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        if (psMode != 0)
+        {
+            zm_msg0_mm(ZM_LV_0, "psMode = 1");
+            if (wd->ap.staTable[id].psMode == 0)
+            {
+                wd->ap.staPowerSaving++;
+            }
+            else
+            {
+                if (wd->ap.staTable[id].qosType == 1)
+                {
+                    zm_msg0_mm(ZM_LV_0, "UAPSD trigger");
+                    *uapsdTrig = wd->ap.staTable[id].qosInfo;
+                }
+            }
+        }
+        else
+        {
+            if (wd->ap.staTable[id].psMode != 0)
+            {
+                wd->ap.staPowerSaving--;
+                if ((wd->ap.staTable[id].qosType == 1) && ((wd->ap.staTable[id].qosInfo&0xf)!=0))
+                {
+                    uapsdStaAwake = 1;
+                }
+            }
+        }
+
+        wd->ap.staTable[id].psMode = (u8_t) psMode;
+        wd->ap.staTable[id].time = wd->tick;
+        *vap = wd->ap.staTable[id].vap;
+        *state = wd->ap.staTable[id++].state;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (uapsdStaAwake == 1)
+    {
+        zbuf_t* psBuf;
+        u8_t mb;
+
+        while (1)
+        {
+            if ((psBuf = zfQueueGetWithMac(dev, wd->ap.uapsdQ, (u8_t*)addr, &mb)) != NULL)
+            {
+                zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+            }
+            else
+            {
+                break;
+            }
+        }
+    }
+
+    return id;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApGetNewSta               */
+/*      Get a new STA from station table.                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      other : STA table index                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApGetNewSta(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        if (wd->ap.staTable[i].valid == 0)
+        {
+            zm_msg2_mm(ZM_LV_0, "zfApGetNewSta=", i);
+            return i;
+        }
+    }
+    return 0xffff;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddSta                  */
+/*      Add a STA to station table.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : STA MAC address                                          */
+/*      state : STA state                                               */
+/*      apId : Virtual AP ID                                            */
+/*      type : 0=>11b, 1=>11g                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      Other : index                                                   */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddSta(zdev_t* dev, u16_t* addr, u16_t state, u16_t apId, u8_t type,
+                 u8_t qosType, u8_t qosInfo)
+{
+    u16_t index;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_msg1_mm(ZM_LV_0, "STA type=", type);
+
+    zmw_enter_critical_section(dev);
+
+    if ((index = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_2, "found");
+        /* Update STA state */
+        if ((state == ZM_STATE_AUTH) || (state == ZM_STATE_PREAUTH))
+        {
+            wd->ap.staTable[index].state = state;
+            wd->ap.staTable[index].time = wd->tick;
+            wd->ap.staTable[index].vap = (u8_t)apId;
+        }
+        else if (state == ZM_STATE_ASOC)
+        {
+            if ((wd->ap.staTable[index].state == ZM_STATE_AUTH))
+                    //&& (wd->ap.staTable[index].vap == apId))
+            {
+                wd->ap.staTable[index].state = state;
+                wd->ap.staTable[index].time = wd->tick;
+                wd->ap.staTable[index].qosType = qosType;
+                wd->ap.staTable[index].vap = (u8_t)apId;
+                wd->ap.staTable[index].staType = type;
+                wd->ap.staTable[index].qosInfo = qosInfo;
+
+                if (wd->frequency < 3000)
+                {
+                    /* Init 11b/g */
+                    zfRateCtrlInitCell(dev, &wd->ap.staTable[index].rcCell, type, 1, 1);
+                }
+                else
+                {
+                    /* Init 11a */
+                    zfRateCtrlInitCell(dev, &wd->ap.staTable[index].rcCell, type, 0, 1);
+                }
+
+                if (wd->zfcbApConnectNotify != NULL)
+                {
+                    wd->zfcbApConnectNotify(dev, (u8_t*)addr, apId);
+                }
+            }
+            else
+            {
+                index = 0xffff;
+            }
+        }
+    }
+    else
+    {
+        zm_msg0_mm(ZM_LV_2, "Not found");
+        if ((state == ZM_STATE_AUTH) || (state == ZM_STATE_PREAUTH))
+        {
+            /* Get a new STA and update state */
+            index = zfApGetNewSta(dev);
+            zm_msg2_mm(ZM_LV_1, "new STA index=", index);
+
+            if (index != 0xffff)
+            {
+                for (i=0; i<3; i++)
+                {
+                    wd->ap.staTable[index].addr[i] = addr[i];
+                }
+                wd->ap.staTable[index].state = state;
+                wd->ap.staTable[index].valid = 1;
+                wd->ap.staTable[index].time = wd->tick;
+                wd->ap.staTable[index].vap = (u8_t)apId;
+                wd->ap.staTable[index].encryMode = ZM_NO_WEP;
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return index;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAgingSta                */
+/*      Aging STA in station table.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      number of 11b STA in STA table                                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfApAgingSta(zdev_t* dev)
+{
+    u16_t i;
+    u32_t deltaMs;
+    u16_t addr[3];
+    u16_t txFlag;
+    u16_t psStaCount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    wd->ap.gStaAssociated = wd->ap.bStaAssociated = 0;
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        txFlag = 0;
+        zmw_enter_critical_section(dev);
+        if (wd->ap.staTable[i].valid == 1)
+        {
+            addr[0] = wd->ap.staTable[i].addr[0];
+            addr[1] = wd->ap.staTable[i].addr[1];
+            addr[2] = wd->ap.staTable[i].addr[2];
+            /* millisecond */
+            deltaMs = (u32_t)((u32_t)wd->tick-(u32_t)wd->ap.staTable[i].time)
+                      * ZM_MS_PER_TICK;
+
+            /* preauth */
+            if ((wd->ap.staTable[i].state == ZM_STATE_PREAUTH)
+                    && (deltaMs > ZM_PREAUTH_TIMEOUT_MS))
+            {
+                /* Aging STA */
+                wd->ap.staTable[i].valid = 0;
+                wd->ap.authSharing = 0;
+                txFlag = 1;
+            }
+
+            /* auth */
+            if ((wd->ap.staTable[i].state == ZM_STATE_AUTH)
+                    && (deltaMs > ZM_AUTH_TIMEOUT_MS))
+            {
+                /* Aging STA */
+                wd->ap.staTable[i].valid = 0;
+                txFlag = 1;
+            }
+
+            /* asoc */
+            if (wd->ap.staTable[i].state == ZM_STATE_ASOC)
+            {
+                if (wd->ap.staTable[i].psMode != 0)
+                {
+                    psStaCount++;
+                }
+
+                if (deltaMs > ((u32_t)wd->ap.staAgingTimeSec<<10))
+                {
+                    /* Aging STA */
+                    zm_msg1_mm(ZM_LV_0, "Age STA index=", i);
+                    wd->ap.staTable[i].valid = 0;
+                    txFlag = 1;
+                }
+                else if (deltaMs > ((u32_t)wd->ap.staProbingTimeSec<<10))
+                {
+                    if (wd->ap.staTable[i].psMode == 0)
+                    {
+                        /* Probing non-PS STA */
+                        zm_msg1_mm(ZM_LV_0, "Probing STA index=", i);
+                        wd->ap.staTable[i].time +=
+                                (wd->ap.staProbingTimeSec * ZM_TICK_PER_SECOND);
+                        txFlag = 2;
+                    }
+                }
+            }
+
+
+        }
+        zmw_leave_critical_section(dev);
+
+        if (txFlag == 1)
+        {
+            /* Send deauthentication management frame */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, addr, 4, 0, 0);
+        }
+        else if (txFlag == 2)
+        {
+            zfSendMmFrame(dev, ZM_WLAN_DATA_FRAME, addr, 0, 0, 0);
+        }
+
+    }
+
+    wd->ap.staPowerSaving = psStaCount;
+
+    return;
+}
+
+void zfApProtctionMonitor(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* 11b STA associated => nonErp, Protect */
+    if (wd->ap.bStaAssociated > 0)
+    {
+        /* Enable NonErp bit in information element */
+        wd->erpElement = ZM_WLAN_NON_ERP_PRESENT_BIT
+                         | ZM_WLAN_USE_PROTECTION_BIT;
+
+        /* Enable protection mode */
+        zfApSetProtectionMode(dev, 1);
+
+    }
+    /* 11b STA not associated, protection OBSS present => Protect */
+    else if (wd->ap.protectedObss > 2) //Threshold
+    {
+        if (wd->disableSelfCts == 0)
+        {
+            /* Disable NonErp bit in information element */
+            wd->erpElement = ZM_WLAN_USE_PROTECTION_BIT;
+
+            /* Enable protection mode */
+            zfApSetProtectionMode(dev, 1);
+        }
+    }
+    else
+    {
+        /* Disable NonErp bit in information element */
+        wd->erpElement = 0;
+
+        /* Disable protection mode */
+        zfApSetProtectionMode(dev, 0);
+    }
+    wd->ap.protectedObss = 0;
+}
+
+
+void zfApProcessBeacon(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t offset;
+    u8_t ch;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_3, "Rx beacon");
+
+    /* update Non-ERP flag(wd->ap.nonErpObss) */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) == 0xffff)
+    {
+        /* 11b OBSS */
+        wd->ap.protectedObss++;
+        return;
+    }
+
+    ch = zmw_rx_buf_readb(dev, buf, offset+2);
+    if ((ch & ZM_WLAN_USE_PROTECTION_BIT) == ZM_WLAN_USE_PROTECTION_BIT)
+    {
+        /* Protected OBSS */
+        wd->ap.protectedObss = 1;
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessAuth               */
+/*      Process authenticate management frame.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* Note : AP allows one authenticating STA at a time, does not          */
+/*        support multiple authentication process. Make sure            */
+/*        authentication state machine will not be blocked due          */
+/*        to incompleted authentication handshake.                      */
+void zfApProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t algo, seq, status;
+    u8_t authSharing;
+    u16_t ret;
+    u16_t i;
+    u8_t challengePassed = 0;
+    u8_t frameCtrl;
+    u32_t retAlgoSeq;
+    u32_t retStatus;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+    /* AP : Auth share 3 */
+    /* shift for WEP IV */
+    if ((frameCtrl & 0x40) != 0)
+    {
+        algo = zmw_rx_buf_readh(dev, buf, 28);
+        seq = zmw_rx_buf_readh(dev, buf, 30);
+        status = zmw_rx_buf_readh(dev, buf, 32);
+    }
+    else
+    {
+        algo = zmw_rx_buf_readh(dev, buf, 24);
+        seq = zmw_rx_buf_readh(dev, buf, 26);
+        status = zmw_rx_buf_readh(dev, buf, 28);
+    }
+
+    zm_msg2_mm(ZM_LV_0, "Rx Auth, seq=", seq);
+
+    /* Set default to authentication algorithm not support */
+    retAlgoSeq = 0x20000 | algo;
+    retStatus = 13; /* authentication algorithm not support */
+
+    /* AP : Auth open 1 */
+    if (algo == 0)
+    {
+        if (wd->ap.authAlgo[apId] == 0)
+        {
+            retAlgoSeq = 0x20000;
+            if (seq == 1)
+            {
+                /* AP : update STA to auth */
+                if ((ret = zfApAddSta(dev, src, ZM_STATE_AUTH, apId, 0, 0, 0)) != 0xffff)
+                {
+                    /* AP : call zfwAuthNotify() for host to judge */
+                    //zfwAuthNotify(dev, src);
+
+                    /* AP : response Auth seq=2, success */
+                    retStatus = 0;
+
+                }
+                else
+                {
+                    /* AP : response Auth seq=2, unspecific error */
+                    retStatus = 1;
+                }
+            }
+            else
+            {
+                /* AP : response Auth seq=2, sequence number out of expected */
+                retStatus = 14;
+            }
+        }
+    }
+    /* AP : Auth share 1 */
+    else if (algo == 1)
+    {
+        if (wd->ap.authAlgo[apId] == 1)
+        {
+            if (seq == 1)
+            {
+                retAlgoSeq = 0x20001;
+
+                /* critical section */
+                zmw_enter_critical_section(dev);
+                if (wd->ap.authSharing == 1)
+                {
+                    authSharing = 1;
+                }
+                else
+                {
+                    authSharing = 0;
+                    wd->ap.authSharing = 1;
+                }
+                /* end of critical section */
+                zmw_leave_critical_section(dev);
+
+                if (authSharing == 1)
+                {
+                    /* AP : response Auth seq=2, status = fail */
+                    retStatus = 1;
+                }
+                else
+                {
+                    /* AP : update STA to preauth */
+                    zfApAddSta(dev, src, ZM_STATE_PREAUTH, apId, 0, 0, 0);
+
+                    /* AP : call zfwAuthNotify() for host to judge */
+                    //zfwAuthNotify(dev, src);
+
+                    /* AP : response Auth seq=2 */
+                    retStatus = 0;
+                }
+            }
+            else if (seq == 3)
+            {
+                retAlgoSeq = 0x40001;
+
+                if (wd->ap.authSharing == 1)
+                {
+                    /* check challenge text */
+                    if (zmw_buf_readh(dev, buf, 30+4) == 0x8010)
+                    {
+                        for (i=0; i<128; i++)
+                        {
+                            if (wd->ap.challengeText[i]
+                                        != zmw_buf_readb(dev, buf, 32+i+4))
+                            {
+                                break;
+                            }
+                        }
+                        if (i == 128)
+                        {
+                            challengePassed = 1;
+                        }
+                    }
+
+                    if (challengePassed == 1)
+                    {
+                        /* AP : update STA to auth */
+                        zfApAddSta(dev, src, ZM_STATE_AUTH, apId, 0, 0, 0);
+
+                        /* AP : response Auth seq=2 */
+                        retStatus = 0;
+                    }
+                    else
+                    {
+                        /* AP : response Auth seq=2, challenge failure */
+                        retStatus = 15;
+
+                        /* TODO : delete STA */
+                    }
+
+                    wd->ap.authSharing = 0;
+                }
+            }
+            else
+            {
+                retAlgoSeq = 0x40001;
+                retStatus = 14;
+            }
+        }
+    }
+
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH, src, retAlgoSeq,
+            retStatus, apId);
+    return;
+}
+
+void zfApProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid = 0xffff;
+    u8_t frameType;
+    u16_t offset;
+    u8_t staType = 0;
+    u8_t qosType = 0;
+    u8_t qosInfo = 0;
+    u8_t tmp;
+    u16_t i, j, k;
+    u16_t encMode = 0;
+
+    zmw_get_wlan_dev(dev);
+    /* AP : check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) != 0xffff)
+    {
+        k = 0;
+        for (j = 0; j < wd->ap.vapNumber; j++)
+        {
+            if ((tmp = zmw_buf_readb(dev, buf, offset+1))
+                        != wd->ap.ssidLen[j])
+            {
+                k++;
+            }
+        }
+        if (k == wd->ap.vapNumber)
+        {
+            goto zlDeauth;
+        }
+
+        k = 0;
+        for (j = 0; j < wd->ap.vapNumber; j++)
+        {
+            for (i=0; i<wd->ap.ssidLen[j]; i++)
+            {
+                if ((tmp = zmw_buf_readb(dev, buf, offset+2+i))
+                        != wd->ap.ssid[j][i])
+                {
+                    break;
+                }
+            }
+            if (i == wd->ap.ssidLen[j])
+            {
+                apId = j;
+            }
+            else
+            {
+                k++;
+            }
+        }
+        if (k == wd->ap.vapNumber)
+        {
+            goto zlDeauth;
+        }
+    }
+
+    /* TODO : check capability */
+
+    /* AP : check support rate */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff)
+    {
+        /* 11g STA */
+        staType = 1;
+    }
+    //CWYang(+)
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* 11n STA */
+        staType = 2;
+    }
+
+    /* TODO : do not allow 11b STA to associated in Pure G mode */
+    if (wd->ap.wlanType[apId] == ZM_WLAN_TYPE_PURE_G && staType == 0)
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 3, 0, 0);
+        return;
+    }
+
+    /* In pure B mode, we set G STA into B mode */
+    if (wd->ap.wlanType[apId] == ZM_WLAN_TYPE_PURE_B && staType == 1)
+    {
+        staType = 0;
+    }
+
+    /* AP : check 11i and WPA */
+    /* AP : check 11h */
+
+    /* AP : check WME */
+    if ((offset = zfFindWifiElement(dev, buf, 2, 0)) != 0xffff)
+    {
+        /* WME STA */
+        qosType = 1;
+        zm_msg0_mm(ZM_LV_0, "WME STA");
+
+        if (wd->ap.uapsdEnabled != 0)
+        {
+            qosInfo = zmw_rx_buf_readb(dev, buf, offset+8);
+        }
+    }
+
+    if (wd->ap.wpaSupport[apId] == 1)
+    {
+        if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_WPA_IE)) != 0xffff )
+        {
+            /* get WPA IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+
+                zm_msg1_mm(ZM_LV_0, "WPA Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbAsocNotify != NULL)
+                {
+                    wd->zfcbAsocNotify(dev, src, wd->ap.stawpaIe[apId], wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+        else if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_RSN_IE)) != 0xffff )
+        {
+            /* get RSN IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+                zm_msg1_mm(ZM_LV_0, "RSN Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbAsocNotify != NULL)
+                {
+                    wd->zfcbAsocNotify(dev, src, wd->ap.stawpaIe[apId], wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+#ifdef ZM_ENABLE_CENC
+        else if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CENC_IE)) != 0xffff )
+        {
+            /* get CENC IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+                zm_msg1_mm(ZM_LV_0, "CENC Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbCencAsocNotify != NULL)
+                {
+                    wd->zfcbCencAsocNotify(dev, src, wd->ap.stawpaIe[apId],
+                            wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+#endif //ZM_ENABLE_CENC
+        else
+        {   /* ap is encryption but sta has no wpa/rsn ie */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+            return;
+        }
+    }
+    /* sta has wpa/rsn ie but ap is no encryption */
+    if ((wd->ap.wpaSupport[apId] == 0) && (encMode == 1))
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+        return;
+    }
+
+    /* AP : update STA to asoc */
+    aid = zfApAddSta(dev, src, ZM_STATE_ASOC, apId, staType, qosType, qosInfo);
+
+    zfApStoreAsocReqIe(dev, buf, aid);
+
+zlDeauth:
+    /* AP : send asoc rsp2 */
+    if (aid != 0xffff)
+    {
+        frameType = zmw_rx_buf_readb(dev, buf, 0);
+
+        if (frameType == ZM_WLAN_FRAME_TYPE_ASOCREQ)
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCRSP, src, 0, aid+1, apId);
+        }
+        else
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_REASOCRSP, src, 0, aid+1, apId);
+        }
+    }
+    else
+    {
+        /* TODO : send deauthentication */
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+    }
+
+    return;
+}
+
+void zfApStoreAsocReqIe(zdev_t* dev, zbuf_t* buf, u16_t aid)
+{
+    //struct zsWlanAssoFrameHeader* pAssoFrame;
+    //u8_t  pBuf[sizeof(struct zsWlanAssoFrameHeader)];
+    u16_t offset;
+    u32_t i;
+    u16_t length;
+    u8_t  *htcap;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+    {
+        wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+    }
+    /* capability: 2 octets */
+    offset = 24;
+
+    /* Listen interval: 2 octets */
+    offset = 26;
+
+    /* SSID */
+    offset = 28;
+
+    /* supported rates */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SUPPORT_RATE)) == 0xffff)
+        return;
+    length = zmw_rx_buf_readb(dev, buf, offset + 1);
+
+    /* extended supported rates */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) == 0xffff)
+        return;
+    length = zmw_rx_buf_readb(dev, buf, offset + 1);
+
+    /* power capability:4 octets */
+    offset = offset + 2 + length;
+
+    /* supported channels: 4 octets */
+    offset = offset + 2 + 4;
+
+    /* RSN */
+
+    /* QoS */
+
+    /* HT capabilities: 28 octets */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff) {
+        /* atheros pre n */
+        htcap = (u8_t *)&wd->ap.ie[aid].HtCap;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 26;
+        for (i=1; i<=26; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_debug_msg2("ASOC:  HT Capabilities, htcap=", htcap[i+1]);
+        }
+        return;
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff) {
+        /* pre n 2.0 standard */
+        htcap = (u8_t *)&wd->ap.ie[aid].HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_debug_msg2("ASOC:  HT Capabilities, htcap=", htcap[i]);
+        }
+    }
+    else {
+        /* not 11n AP */
+        return;
+    }
+
+
+    /* supported regulatory classes */
+    offset = offset + length;
+    //length = zmw_rx_buf_readb(dev, buf, offset + 1);
+    {
+    u8_t *htcap;
+    htcap = (u8_t *)&wd->sta.ie.HtInfo;
+    //zm_debug_msg2("ASOC:  HT Capabilities info=", ((u16_t *)htcap)[1]);
+    //zm_debug_msg2("ASOC:  A-MPDU parameters=", htcap[4]);
+    //zm_debug_msg2("ASOC:  Supported MCS set=", ((u32_t *)htcap)[1]>>8);
+    }
+
+}
+
+void zfApProcessAsocRsp(zdev_t* dev, zbuf_t* buf)
+{
+
+}
+
+void zfApProcessDeauth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* AP : if SA=associated STA then deauthenticate STA */
+    if ((aid = zfApFindSta(dev, src)) != 0xffff)
+    {
+        /* Clear STA table */
+        wd->ap.staTable[aid].valid = 0;
+        if (wd->zfcbDisAsocNotify != NULL)
+        {
+            wd->zfcbDisAsocNotify(dev, (u8_t*)src, apId);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfApProcessDisasoc(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* AP : if SA=associated STA then deauthenticate STA */
+    if ((aid = zfApFindSta(dev, src)) != 0xffff)
+    {
+        /* Clear STA table */
+        wd->ap.staTable[aid].valid = 0;
+        zmw_leave_critical_section(dev);
+        if (wd->zfcbDisAsocNotify != NULL)
+        {
+            wd->zfcbDisAsocNotify(dev, (u8_t*)src, apId);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+}
+
+
+void zfApProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+#if 0
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_0, "Rx probersp");
+
+    /* Gather scan result */
+
+    //zm_debug_msg1("bssList Count = ", wd->sta.bssList.bssCount);
+    /* return if not in scanning */
+    if ((wd->heartBeatNotification & ZM_BSSID_LIST_SCAN)
+            != ZM_BSSID_LIST_SCAN)
+    {
+        return;
+    }
+
+    //if ( wd->sta.pUpdateBssList->bssCount == ZM_MAX_BSS )
+    if ( wd->sta.bssList.bssCount == ZM_MAX_BSS )
+    {
+        return;
+    }
+
+    zfProcessProbeRsp(dev, buf, AddInfo);
+
+#endif
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeSsid               */
+/*      Add AP information element SSID to buffer.                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.ssidLen[vap]);
+
+    /* Information : SSID */
+    for (i=0; i<wd->ap.ssidLen[vap]; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.ssid[vap][i]);
+    }
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeTim                */
+/*      Add AP information element TIM to buffer.                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeTim(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    u8_t uniBitMap[9];
+    u16_t highestByte;
+    u16_t i;
+    u16_t lenOffset;
+    u16_t id;
+    u16_t dst[3];
+    u16_t aid;
+    u16_t bitPosition;
+    u16_t bytePosition;
+    zbuf_t* psBuf;
+    zbuf_t* tmpBufArray[ZM_UNI_ARRAY_SIZE];
+    u16_t tmpBufArraySize = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_TIM);
+
+    /* offset of Element Length */
+    lenOffset = offset++;
+
+    /* Information : TIM */
+    /* DTIM count */
+    /* TODO : Doesn't work for Virtual AP's case */
+    wd->CurrentDtimCount++;
+    if (wd->CurrentDtimCount >= wd->dtim)
+    {
+        wd->CurrentDtimCount = 0;
+    }
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->CurrentDtimCount);
+    /* DTIM period */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->dtim);
+    /* bitmap offset */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0);
+
+    /* Update BCMC bit */
+    if (wd->CurrentDtimCount == 0)
+    {
+        zmw_enter_critical_section(dev);
+        wd->ap.timBcmcBit[vap] = (wd->ap.bcmcTail[vap]!=wd->ap.bcmcHead[vap])?1:0;
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        wd->ap.timBcmcBit[vap] = 0;
+    }
+
+    /* Update Unicast bitmap */
+    /* reset bit map */
+    for (i=0; i<9; i++)
+    {
+        uniBitMap[i] = 0;
+    }
+    highestByte = 0;
+#if 1
+
+    zmw_enter_critical_section(dev);
+
+    id = wd->ap.uniHead;
+    while (id != wd->ap.uniTail)
+    {
+        psBuf = wd->ap.uniArray[id];
+
+        /* TODO : Aging PS frame after queuing for more than 10 seconds */
+
+        /* get destination STA's aid */
+        dst[0] = zmw_tx_buf_readh(dev, psBuf, 0);
+        dst[1] = zmw_tx_buf_readh(dev, psBuf, 2);
+        dst[2] = zmw_tx_buf_readh(dev, psBuf, 4);
+        if ((aid = zfApFindSta(dev, dst)) != 0xffff)
+        {
+            if (wd->ap.staTable[aid].psMode != 0)
+            {
+                zm_msg1_mm(ZM_LV_0, "aid=",aid);
+                aid++;
+                zm_assert(aid<=64);
+                bitPosition = (1 << (aid & 0x7));
+                bytePosition = (aid >> 3);
+                uniBitMap[bytePosition] |= bitPosition;
+
+                if (bytePosition>highestByte)
+                {
+                    highestByte = bytePosition;
+                }
+                id = (id+1) & (ZM_UNI_ARRAY_SIZE-1);
+            }
+            else
+            {
+                zm_msg0_mm(ZM_LV_0, "Send PS frame which STA no longer in PS mode");
+                /* Send PS frame which STA no longer in PS mode */
+                zfApRemoveFromPsQueue(dev, id, dst);
+                tmpBufArray[tmpBufArraySize++] = psBuf;
+            }
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_0, "Free garbage PS frame");
+            /* Free garbage PS frame */
+            zfApRemoveFromPsQueue(dev, id, dst);
+            zfwBufFree(dev, psBuf, 0);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+#endif
+
+    zfQueueGenerateUapsdTim(dev, wd->ap.uapsdQ, uniBitMap, &highestByte);
+
+    zm_msg1_mm(ZM_LV_3, "bm=",uniBitMap[0]);
+    zm_msg1_mm(ZM_LV_3, "highestByte=",highestByte);
+    zm_msg1_mm(ZM_LV_3, "timBcmcBit[]=",wd->ap.timBcmcBit[vap]);
+
+    /* bitmap */
+    zmw_tx_buf_writeb(dev, buf, offset++,
+                         uniBitMap[0] | wd->ap.timBcmcBit[vap]);
+    for (i=0; i<highestByte; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, uniBitMap[i+1]);
+    }
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, lenOffset, highestByte+4);
+
+    for (i=0; i<tmpBufArraySize; i++)
+    {
+        /* Put to VTXQ[ac] */
+        zfPutVtxq(dev, tmpBufArray[i]);
+    }
+    /* Push VTXQ[ac] */
+    zfPushVtxq(dev);
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApRemoveFromPsQueue       */
+/*      Remove zbuf from PS queue.                                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      id : index in ps queue                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      more data bit                                                   */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfApRemoveFromPsQueue(zdev_t* dev, u16_t id, u16_t* addr)
+{
+    u16_t dst[3];
+    u16_t nid;
+    u8_t moreData = 0;
+    zmw_get_wlan_dev(dev);
+
+    wd->ap.uniTail = (wd->ap.uniTail-1) & (ZM_UNI_ARRAY_SIZE-1);
+    while (id != wd->ap.uniTail)
+    {
+        nid = (id + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+        wd->ap.uniArray[id] = wd->ap.uniArray[nid];
+
+        /* Search until tail to config more data bit */
+        dst[0] = zmw_buf_readh(dev, wd->ap.uniArray[id], 0);
+        dst[1] = zmw_buf_readh(dev, wd->ap.uniArray[id], 2);
+        dst[2] = zmw_buf_readh(dev, wd->ap.uniArray[id], 4);
+        if ((addr[0] == dst[0]) && (addr[1] == dst[1])
+                && (addr[2] == dst[2]))
+        {
+            moreData = 0x20;
+        }
+
+        id = nid;
+    }
+    return moreData;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeWmePara            */
+/*      Add WME Parameter Element to buffer.                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeWmePara(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WIFI_IE);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 24);
+
+    /* OUI */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x50);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xF2);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x02);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+
+    /* QoS Info */
+    if (wd->ap.uapsdEnabled)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, 0x81);
+    }
+    else
+    {
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+    }
+
+    /* Reserved */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+
+    /* Best Effort AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x03);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xA4);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Backfround AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x27);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xA4);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Video AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x42);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x43);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x5E);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Voice AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x62);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x32);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x2F);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApSendBeacon              */
+/*      Sned AP mode beacon.                                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+void zfApSendBeacon(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t offset;
+    u16_t vap;
+    u16_t seq;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    wd->ap.beaconCounter++;
+    if (wd->ap.beaconCounter >= wd->ap.vapNumber)
+    {
+        wd->ap.beaconCounter = 0;
+    }
+    vap = wd->ap.beaconCounter;
+
+
+    zm_msg1_mm(ZM_LV_2, "Send beacon, vap=", vap);
+
+    /* TBD : Maximum size of beacon */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc beacon buf Fail!");
+        return;
+    }
+
+    offset = 0;
+
+    /* wlan header */
+    /* Frame control */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0080);
+    offset+=2;
+    /* Duration */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0000);
+    offset+=2;
+    /* Address 1 */
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    /* Address 2 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]); //Multiple SSID
+#else
+    zmw_tx_buf_writeh(dev, buf, offset, (wd->macAddr[2]+(vap<<8))); //VAP
+#endif
+    offset+=2;
+    /* Address 3 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]); //Multiple SSID
+#else
+    zmw_tx_buf_writeh(dev, buf, offset, (wd->macAddr[2]+(vap<<8))); //VAP
+#endif
+    offset+=2;
+
+    /* Sequence number */
+    zmw_enter_critical_section(dev);
+    seq = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+    zmw_tx_buf_writeh(dev, buf, offset, seq);
+    offset+=2;
+
+    /* 24-31 Time Stamp : hardware will fill this field */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+2, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+4, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+6, 0);
+    offset+=8;
+
+    /* Beacon Interval */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+    offset+=2;
+
+    /* Capability */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+    offset+=2;
+
+    /* SSID */
+    if (wd->ap.hideSsid[vap] == 0)
+    {
+        offset = zfApAddIeSsid(dev, buf, offset, vap);
+    }
+    else
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+        zmw_tx_buf_writeb(dev, buf, offset++, 0);
+
+    }
+
+    /* Support Rate */
+    if ( wd->frequency < 3000 )
+    {
+    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+    }
+    else
+    {
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+    }
+
+    /* DS parameter set */
+    offset = zfMmAddIeDs(dev, buf, offset);
+
+    /* TIM */
+    offset = zfApAddIeTim(dev, buf, offset, vap);
+
+    /* If WLAN Type is not PURE B */
+    if (wd->ap.wlanType[vap] != ZM_WLAN_TYPE_PURE_B)
+    {
+        if ( wd->frequency < 3000 )
+        {
+        /* ERP Information */
+        offset = zfMmAddIeErp(dev, buf, offset);
+
+        /* Extended Supported Rates */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+    }
+    }
+
+    /* TODO : country information */
+    /* TODO : RSN */
+    if (wd->ap.wpaSupport[vap] == 1)
+    {
+        offset = zfMmAddIeWpa(dev, buf, offset, vap);
+    }
+
+    /* WME Parameters */
+    if (wd->ap.qosMode == 1)
+    {
+        offset = zfApAddIeWmePara(dev, buf, offset, vap);
+    }
+
+    /* HT Capabilities Info */
+    offset = zfMmAddHTCapability(dev, buf, offset);
+
+    /* Extended HT Capabilities Info */
+    offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+
+    /* 1212 : write to beacon fifo */
+    /* 1221 : write to share memory */
+    zfHpSendBeacon(dev, buf, offset);
+
+    /* Free beacon buffer */
+    /* TODO: In order to fit the madwifi beacon architecture, we need to
+       free beacon buffer in the HAL layer.
+     */
+
+    //zfwBufFree(dev, buf, 0);
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfIntrabssForward           */
+/*      Called to transmit intra-BSS frame from upper layer.            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      vap : virtual AP                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      1 : unicast intras-BSS frame                                    */
+/*      0 : other frames                                                */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfIntrabssForward(zdev_t* dev, zbuf_t* buf, u8_t srcVap)
+{
+    u16_t err;
+    u16_t asocFlag = 0;
+    u16_t dst[3];
+    u16_t aid;
+    u16_t staState;
+    zbuf_t* txBuf;
+    u16_t len;
+    u16_t i;
+    u16_t temp;
+    u16_t ret;
+    u8_t vap = 0;
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    dst[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+    dst[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+#else
+    dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+    dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+#endif  // ZM_ENABLE_NATIVE_WIFI
+
+    /* Do Intra-BSS forward(data copy) if necessary*/
+    if ((dst[0]&0x1) != 0x1)
+    {
+        aid = zfApGetSTAInfo(dev, dst, &staState, &vap);
+        if ((aid != 0xffff) && (staState == ZM_STATE_ASOC) && (srcVap == vap))
+        {
+            asocFlag = 1;
+            zm_msg0_rx(ZM_LV_2, "Intra-BSS forward : asoc STA");
+        }
+
+    }
+    else
+    {
+        vap = srcVap;
+        zm_msg0_rx(ZM_LV_2, "Intra-BSS forward : BCorMC");
+    }
+
+    /* destination address = associated STA or BC/MC */
+    if ((asocFlag == 1) || ((dst[0]&0x1) == 0x1))
+    {
+        /* Allocate frame */
+        if ((txBuf = zfwBufAllocate(dev, ZM_RX_FRAME_SIZE))
+                == NULL)
+        {
+            zm_msg0_rx(ZM_LV_1, "Alloc intra-bss buf Fail!");
+            goto zlAllocError;
+        }
+
+        /* Copy frame */
+        len = zfwBufGetSize(dev, buf);
+        for (i=0; i<len; i+=2)
+        {
+            temp = zmw_rx_buf_readh(dev, buf, i);
+            zmw_tx_buf_writeh(dev, txBuf, i, temp);
+        }
+        zfwBufSetSize(dev, txBuf, len);
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+        /* Tx-A2 = Rx-A1, Tx-A3 = Rx-A2, Tx-A1 = Rx-A3 */
+        for (i=0; i<6; i+=2)
+        {
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A2_OFFSET+i, temp);
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A3_OFFSET+i, temp);
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A1_OFFSET+i, temp);
+        }
+
+        #endif
+
+        /* Transmit frame */
+        /* Return error if port is disabled */
+        if ((err = zfTxPortControl(dev, txBuf, vap)) == ZM_PORT_DISABLED)
+        {
+            err = ZM_ERR_TX_PORT_DISABLED;
+            goto zlTxError;
+        }
+
+#if 1
+        /* AP : Buffer frame for power saving STA */
+        if ((ret = zfApBufferPsFrame(dev, txBuf, vap)) == 0)
+        {
+            /* forward frame if not been buffered */
+            #if 1
+            /* Put to VTXQ[ac] */
+            ret = zfPutVtxq(dev, txBuf);
+            /* Push VTXQ[ac] */
+            zfPushVtxq(dev);
+            #else
+            zfTxSendEth(dev, txBuf, vap, ZM_INTERNAL_ALLOC_BUF, 0);
+            #endif
+
+        }
+#endif
+    }
+    return asocFlag;
+
+zlTxError:
+    zfwBufFree(dev, txBuf, 0);
+zlAllocError:
+    return asocFlag;
+}
+
+struct zsMicVar* zfApGetRxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t sa[6];
+    u16_t id = 0, macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+    macAddr[0] = sa[0] + (sa[1] << 8);
+    macAddr[1] = sa[2] + (sa[3] << 8);
+    macAddr[2] = sa[4] + (sa[5] << 8);
+
+    if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+        return (&wd->ap.staTable[id].rxMicKey);
+
+    return NULL;
+}
+
+struct zsMicVar* zfApGetTxMicKey(zdev_t* dev, zbuf_t* buf, u8_t* qosType)
+{
+    u8_t da[6];
+    u16_t id = 0, macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zfCopyFromIntTxBuffer(dev, buf, da, 0, 6);
+
+    macAddr[0] = da[0] + (da[1] << 8);
+    macAddr[1] = da[2] + (da[3] << 8);
+    macAddr[2] = da[4] + (da[5] << 8);
+
+    if ((macAddr[0] & 0x1))
+    {
+        return (&wd->ap.bcMicKey[0]);
+    }
+    else if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+    {
+        *qosType = wd->ap.staTable[id].qosType;
+        return (&wd->ap.staTable[id].txMicKey);
+    }
+
+    return NULL;
+}
+
+u16_t zfApUpdatePsBit(zdev_t* dev, zbuf_t* buf, u8_t* vap, u8_t* uapsdTrig)
+{
+    u16_t staState;
+    u16_t aid;
+    u16_t psBit;
+    u16_t src[3];
+    u16_t dst[1];
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    src[0] = zmw_rx_buf_readh(dev, buf, 10);
+    src[1] = zmw_rx_buf_readh(dev, buf, 12);
+    src[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3)
+    {
+        /* AP */
+        dst[0] = zmw_rx_buf_readh(dev, buf, 4);
+
+        psBit = (zmw_rx_buf_readb(dev, buf, 1) & 0x10) >> 4;
+        /* Get AID and update STA PS mode */
+        aid = zfApGetSTAInfoAndUpdatePs(dev, src, &staState, vap, psBit, uapsdTrig);
+
+        /* if STA not associated, send deauth */
+        if ((aid == 0xffff) || (staState != ZM_STATE_ASOC))
+        {
+            if ((dst[0]&0x1)==0)
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 0x7,
+                        0, 0);
+            }
+
+            return ZM_ERR_STA_NOT_ASSOCIATED;
+        }
+    } /* if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3) */
+    else
+    {
+        /* WDS */
+        for (i=0; i<ZM_MAX_WDS_SUPPORT; i++)
+        {
+            if ((wd->ap.wds.wdsBitmap & (1<<i)) != 0)
+            {
+                if ((src[0] == wd->ap.wds.macAddr[i][0])
+                        && (src[1] == wd->ap.wds.macAddr[i][1])
+                        && (src[2] == wd->ap.wds.macAddr[i][2]))
+                {
+                    *vap = 0x20 + i;
+                    break;
+                }
+            }
+        }
+    }
+    return ZM_SUCCESS;
+}
+
+void zfApProcessPsPoll(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t src[3];
+    u16_t dst[3];
+    zbuf_t* psBuf = NULL;
+    u16_t id;
+    u8_t moreData = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    src[0] = zmw_tx_buf_readh(dev, buf, 10);
+    src[1] = zmw_tx_buf_readh(dev, buf, 12);
+    src[2] = zmw_tx_buf_readh(dev, buf, 14);
+
+    /* Find ps buffer for PsPoll */
+    zmw_enter_critical_section(dev);
+    id = wd->ap.uniHead;
+    while (id != wd->ap.uniTail)
+    {
+        psBuf = wd->ap.uniArray[id];
+
+        dst[0] = zmw_tx_buf_readh(dev, psBuf, 0);
+        dst[1] = zmw_tx_buf_readh(dev, psBuf, 2);
+        dst[2] = zmw_tx_buf_readh(dev, psBuf, 4);
+
+        if ((src[0] == dst[0]) && (src[1] == dst[1]) && (src[2] == dst[2]))
+        {
+            moreData = zfApRemoveFromPsQueue(dev, id, src);
+            break;
+        }
+        else
+        {
+            psBuf = NULL;
+        }
+        id = (id + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+    }
+    zmw_leave_critical_section(dev);
+
+    /* Send ps buffer */
+    if (psBuf != NULL)
+    {
+        /* Send with more data bit */
+        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, moreData);
+    }
+
+    return;
+}
+
+void zfApSetProtectionMode(zdev_t* dev, u16_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (mode == 0)
+    {
+        if (wd->ap.protectionMode != mode)
+        {
+            /* Write MAC&PHY registers to disable protection */
+
+            wd->ap.protectionMode = mode;
+        }
+
+    }
+    else
+    {
+        if (wd->ap.protectionMode != mode)
+        {
+            /* Write MAC&PHY registers to enable protection */
+
+            wd->ap.protectionMode = mode;
+        }
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApSendFailure             */
+/*      Send failure.                                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : receiver address                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfApSendFailure(zdev_t* dev, u8_t* addr)
+{
+    u16_t id;
+    u16_t staAddr[3];
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    staAddr[0] = addr[0] + (((u16_t)addr[1])<<8);
+    staAddr[1] = addr[2] + (((u16_t)addr[3])<<8);
+    staAddr[2] = addr[4] + (((u16_t)addr[5])<<8);
+    zmw_enter_critical_section(dev);
+    if ((id = zfApFindSta(dev, staAddr)) != 0xffff)
+    {
+        /* Send failture : Add 3 minutes to inactive time that will */
+        /*                 will make STA been kicked out soon */
+        wd->ap.staTable[id].time -= (3*ZM_TICK_PER_MINUTE);
+    }
+    zmw_leave_critical_section(dev);
+}
+
+
+void zfApProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t category;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+
+    switch (category)
+    {
+    case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+        zfAggBlockAckActionFrame(dev, buf);
+        break;
+    default:
+        break;
+    }
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/cmmsta.c b/drivers/staging/otus/80211core/cmmsta.c
new file mode 100644
index 000000000000..c75ba11ee43d
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmmsta.c
@@ -0,0 +1,5782 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+#include "../hal/hpreg.h"
+
+/* TODO : change global variable to constant */
+u8_t   zgWpaRadiusOui[] = { 0x00, 0x50, 0xf2, 0x01 };
+u8_t   zgWpaAesOui[] = { 0x00, 0x50, 0xf2, 0x04 };
+u8_t   zgWpa2RadiusOui[] = { 0x00, 0x0f, 0xac, 0x01 };
+u8_t   zgWpa2AesOui[] = { 0x00, 0x0f, 0xac, 0x04 };
+
+const u16_t zcCwTlb[16] = {   0,    1,    3,    7,   15,   31,   63,  127,
+                            255,  511, 1023, 2047, 4095, 4095, 4095, 4095};
+
+void zfStaStartConnectCb(zdev_t* dev);
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaPutApIntoBlockingList  */
+/*      Put AP into blocking AP list.                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : AP's BSSID                                              */
+/*      weight : weight of AP                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaPutApIntoBlockingList(zdev_t* dev, u8_t* bssid, u8_t weight)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if (weight > 0)
+    {
+        zmw_enter_critical_section(dev);
+        /*Find same bssid entry first*/
+        for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+        {
+            for (j=0; j<6; j++)
+            {
+                if(wd->sta.blockingApList[i].addr[j]!= bssid[j])
+                {
+                    break;
+                }
+            }
+
+            if(j==6)
+            {
+                break;
+            }
+        }
+        /*This bssid doesn't have old record.Find an empty entry*/
+        if (i == ZM_MAX_BLOCKING_AP_LIST_SIZE)
+        {
+            for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+            {
+                if (wd->sta.blockingApList[i].weight == 0)
+                {
+                    break;
+                }
+            }
+        }
+
+        /* If the list is full, pick one entry for replacement */
+        if (i == ZM_MAX_BLOCKING_AP_LIST_SIZE)
+        {
+            i = bssid[5] & (ZM_MAX_BLOCKING_AP_LIST_SIZE-1);
+        }
+
+        /* Update AP address and weight */
+        for (j=0; j<6; j++)
+        {
+            wd->sta.blockingApList[i].addr[j] = bssid[j];
+        }
+
+        wd->sta.blockingApList[i].weight = weight;
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaIsApInBlockingList     */
+/*      Is AP in blocking list.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : AP's BSSID                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      TRUE : AP in blocking list                                      */
+/*      FALSE : AP not in blocking list                                 */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaIsApInBlockingList(zdev_t* dev, u8_t* bssid)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    //zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+    {
+        if (wd->sta.blockingApList[i].weight != 0)
+        {
+            for (j=0; j<6; j++)
+            {
+                if (wd->sta.blockingApList[i].addr[j] != bssid[j])
+                {
+                    break;
+                }
+            }
+            if (j == 6)
+            {
+                //zmw_leave_critical_section(dev);
+                return TRUE;
+            }
+        }
+    }
+    //zmw_leave_critical_section(dev);
+    return FALSE;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaRefreshBlockList       */
+/*      Is AP in blocking list.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      flushFlag : flush whole blocking list                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaRefreshBlockList(zdev_t* dev, u16_t flushFlag)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+    {
+        if (wd->sta.blockingApList[i].weight != 0)
+        {
+            if (flushFlag != 0)
+            {
+                wd->sta.blockingApList[i].weight = 0;
+            }
+            else
+            {
+                wd->sta.blockingApList[i].weight--;
+            }
+        }
+    }
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaConnectFail            */
+/*      Handle Connect failure.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : BSSID                                                   */
+/*      reason : reason of failure                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaConnectFail(zdev_t* dev, u16_t reason, u16_t* bssid, u8_t weight)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Change internal state */
+    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    /* Notify wrapper of connection status changes */
+    if (wd->zfcbConnectNotify != NULL)
+    {
+        wd->zfcbConnectNotify(dev, reason, bssid);
+    }
+
+    /* Put AP into internal blocking list */
+    zfStaPutApIntoBlockingList(dev, (u8_t *)bssid, weight);
+
+    /* Issue another SCAN */
+    if ( wd->sta.bAutoReconnect )
+    {
+        zm_debug_msg0("Start internal scan...");
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+    }
+}
+
+u8_t zfiWlanIBSSGetPeerStationsCount(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.oppositeCount;
+}
+
+u8_t zfiWlanIBSSIteratePeerStations(zdev_t* dev, u8_t numToIterate, zfpIBSSIteratePeerStationCb callback, void *ctx)
+{
+    u8_t oppositeCount;
+    u8_t i;
+    u8_t index = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+    if ( oppositeCount > numToIterate )
+    {
+        oppositeCount = numToIterate;
+    }
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        callback(dev, &wd->sta.oppositeInfo[i], ctx, index++);
+        oppositeCount--;
+
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return index;
+}
+
+
+s8_t zfStaFindFreeOpposite(zdev_t* dev, u16_t *sa, int *pFoundIdx)
+{
+    int oppositeCount;
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+        if ( zfMemoryIsEqual((u8_t*) sa, wd->sta.oppositeInfo[i].macAddr, 6) )
+        {
+            //wd->sta.oppositeInfo[i].aliveCounter++;
+            wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+
+            /* it is already stored */
+            return 1;
+        }
+    }
+
+    // Check if there's still space for new comer
+    if ( wd->sta.oppositeCount == ZM_MAX_OPPOSITE_COUNT )
+    {
+        return -1;
+    }
+
+    // Find an unused slot for new peer station
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            break;
+        }
+    }
+
+    *pFoundIdx = i;
+    return 0;
+}
+
+s8_t zfStaFindOppositeByMACAddr(zdev_t* dev, u16_t *sa, u8_t *pFoundIdx)
+{
+    u32_t oppositeCount;
+    u32_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+        if ( zfMemoryIsEqual((u8_t*) sa, wd->sta.oppositeInfo[i].macAddr, 6) )
+        {
+            *pFoundIdx = (u8_t)i;
+
+            return 0;
+        }
+    }
+
+    *pFoundIdx = 0;
+    return 1;
+}
+
+static void zfStaInitCommonOppositeInfo(zdev_t* dev, int i)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* set the default rate to the highest rate */
+    wd->sta.oppositeInfo[i].valid = 1;
+    wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+    wd->sta.oppositeCount++;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* Set parameters for new opposite peer station !!! */
+    wd->sta.oppositeInfo[i].camIdx = 0xff;  // Not set key in this location
+    wd->sta.oppositeInfo[i].pkInstalled = 0;
+    wd->sta.oppositeInfo[i].wpaState = ZM_STA_WPA_STATE_INIT ;  // No encryption
+#endif
+}
+
+int zfStaSetOppositeInfoFromBSSInfo(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    int i;
+    u8_t*  dst;
+    u16_t  sa[3];
+    int res;
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t*) sa, pBssInfo->macaddr, 6);
+
+    res = zfStaFindFreeOpposite(dev, sa, &i);
+    if ( res != 0 )
+    {
+        goto zlReturn;
+    }
+
+    dst = wd->sta.oppositeInfo[i].macAddr;
+    zfMemoryCopy(dst, (u8_t *)sa, 6);
+
+    oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+
+    if (pBssInfo->extSupportedRates[1] != 0)
+    {
+        /* TODO : Handle 11n */
+        if (pBssInfo->frequency < 3000)
+        {
+            /* 2.4GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 1, pBssInfo->SG40);
+        }
+        else
+        {
+            /* 5GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, pBssInfo->SG40);
+        }
+    }
+    else
+    {
+        /* TODO : Handle 11n */
+        if (pBssInfo->frequency < 3000)
+        {
+            /* 2.4GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 0, 1, pBssInfo->SG40);
+        }
+        else
+        {
+            /* 5GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, pBssInfo->SG40);
+        }
+    }
+
+
+    zfStaInitCommonOppositeInfo(dev, i);
+zlReturn:
+    return 0;
+}
+
+int zfStaSetOppositeInfoFromRxBuf(zdev_t* dev, zbuf_t* buf)
+{
+    int   i;
+    u8_t*  dst;
+    u16_t  sa[3];
+    int res = 0;
+    u16_t  offset;
+    u8_t   bSupportExtRate;
+    u32_t rtsctsRate = 0xffffffff; /* CTS:OFDM 6M, RTS:OFDM 6M */
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+    sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+    sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+    zmw_enter_critical_section(dev);
+
+    res = zfStaFindFreeOpposite(dev, sa, &i);
+    if ( res != 0 )
+    {
+        goto zlReturn;
+    }
+
+    dst = wd->sta.oppositeInfo[i].macAddr;
+    zfCopyFromRxBuffer(dev, buf, dst, ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+    if ( (wd->sta.currentFrequency < 3000) && !(wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+    {
+        bSupportExtRate = 0;
+    } else {
+        bSupportExtRate = 1;
+    }
+
+    if ( (bSupportExtRate == 1)
+         && (wd->sta.currentFrequency < 3000)
+         && (wd->wlanMode == ZM_MODE_IBSS)
+         && (wd->wfc.bIbssGMode == 0) )
+    {
+        bSupportExtRate = 0;
+    }
+
+    wd->sta.connection_11b = 0;
+    oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+
+    if ( ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff)
+         && (bSupportExtRate == 1) )
+    {
+        /* TODO : Handle 11n */
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* 2.4GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11ng
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, wd->sta.SG40);
+            }
+            else
+            {
+                //11g
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 1, wd->sta.SG40);
+            }
+            rtsctsRate = 0x00001bb; /* CTS:CCK 1M, RTS:OFDM 6M */
+        }
+        else
+        {
+            /* 5GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11na
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, wd->sta.SG40);
+            }
+            else
+            {
+                //11a
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, wd->sta.SG40);
+            }
+            rtsctsRate = 0x10b01bb; /* CTS:OFDM 6M, RTS:OFDM 6M */
+        }
+    }
+    else
+    {
+        /* TODO : Handle 11n */
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* 2.4GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11ng
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, wd->sta.SG40);
+                rtsctsRate = 0x00001bb; /* CTS:CCK 1M, RTS:OFDM 6M */
+            }
+            else
+            {
+                //11b
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 0, 1, wd->sta.SG40);
+                rtsctsRate = 0x0; /* CTS:CCK 1M, RTS:CCK 1M */
+                wd->sta.connection_11b = 1;
+            }
+        }
+        else
+        {
+            /* 5GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11na
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, wd->sta.SG40);
+            }
+            else
+            {
+                //11a
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, wd->sta.SG40);
+            }
+            rtsctsRate = 0x10b01bb; /* CTS:OFDM 6M, RTS:OFDM 6M */
+        }
+    }
+
+    zfStaInitCommonOppositeInfo(dev, i);
+
+zlReturn:
+    zmw_leave_critical_section(dev);
+
+    if (rtsctsRate != 0xffffffff)
+    {
+        zfHpSetRTSCTSRate(dev, rtsctsRate);
+    }
+    return res;
+}
+
+void zfStaProtErpMonitor(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   offset;
+    u8_t    erp;
+    u8_t    bssid[6];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&(zfStaIsConnected(dev)) )
+    {
+        ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+        if (zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6))
+        {
+            if ( (offset=zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+            {
+                erp = zmw_rx_buf_readb(dev, buf, offset+2);
+
+                if ( erp & ZM_BIT_1 )
+                {
+                    //zm_debug_msg0("protection mode on");
+                    if (wd->sta.bProtectionMode == FALSE)
+                    {
+                        wd->sta.bProtectionMode = TRUE;
+                        zfHpSetSlotTime(dev, 0);
+                    }
+                }
+                else
+                {
+                    //zm_debug_msg0("protection mode off");
+                    if (wd->sta.bProtectionMode == TRUE)
+                    {
+                        wd->sta.bProtectionMode = FALSE;
+                        zfHpSetSlotTime(dev, 1);
+                    }
+                }
+            }
+        }
+		//Check the existence of Non-N AP
+		//Follow the check the "pBssInfo->EnableHT"
+			if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+			{}
+			else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+			{}
+			else
+			{wd->sta.NonNAPcount++;}
+    }
+}
+
+void zfStaUpdateWmeParameter(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   tmp;
+    u16_t   aifs[5];
+    u16_t   cwmin[5];
+    u16_t   cwmax[5];
+    u16_t   txop[5];
+    u8_t    acm;
+    u8_t    ac;
+    u16_t   len;
+    u16_t   i;
+   	u16_t   offset;
+    u8_t    rxWmeParameterSetCount;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Update if WME parameter set count is changed */
+    /* If connect to WME AP */
+    if (wd->sta.wmeConnected != 0)
+    {
+        /* Find WME parameter element */
+        if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+        {
+            if ((len = zmw_rx_buf_readb(dev, buf, offset+1)) >= 7)
+            {
+                rxWmeParameterSetCount=zmw_rx_buf_readb(dev, buf, offset+8);
+                if (rxWmeParameterSetCount != wd->sta.wmeParameterSetCount)
+                {
+                    zm_msg0_mm(ZM_LV_0, "wmeParameterSetCount changed!");
+                    wd->sta.wmeParameterSetCount = rxWmeParameterSetCount;
+                    /* retrieve WME parameter and update TxQ parameters */
+                    acm = 0xf;
+                    for (i=0; i<4; i++)
+                    {
+                        if (len >= (8+(i*4)+4))
+                        {
+                            tmp=zmw_rx_buf_readb(dev, buf, offset+10+i*4);
+                            ac = (tmp >> 5) & 0x3;
+                            if ((tmp & 0x10) == 0)
+                            {
+                                acm &= (~(1<<ac));
+                            }
+                            aifs[ac] = ((tmp & 0xf) * 9) + 10;
+                            tmp=zmw_rx_buf_readb(dev, buf, offset+11+i*4);
+                            /* Convert to 2^n */
+                            cwmin[ac] = zcCwTlb[(tmp & 0xf)];
+                            cwmax[ac] = zcCwTlb[(tmp >> 4)];
+                            txop[ac]=zmw_rx_buf_readh(dev, buf,
+                                    offset+12+i*4);
+                        }
+                    }
+
+                    if ((acm & 0x4) != 0)
+                    {
+                        cwmin[2] = cwmin[0];
+                        cwmax[2] = cwmax[0];
+                        aifs[2] = aifs[0];
+                        txop[2] = txop[0];
+                    }
+                    if ((acm & 0x8) != 0)
+                    {
+                        cwmin[3] = cwmin[2];
+                        cwmax[3] = cwmax[2];
+                        aifs[3] = aifs[2];
+                        txop[3] = txop[2];
+                    }
+                    cwmin[4] = 3;
+                    cwmax[4] = 7;
+                    aifs[4] = 28;
+
+                    if ((cwmin[2]+aifs[2]) > ((cwmin[0]+aifs[0])+1))
+                    {
+                        wd->sta.ac0PriorityHigherThanAc2 = 1;
+                    }
+                    else
+                    {
+                        wd->sta.ac0PriorityHigherThanAc2 = 0;
+                    }
+                    zfHpUpdateQosParameter(dev, cwmin, cwmax, aifs, txop);
+                }
+            }
+        }
+    } //if (wd->sta.wmeConnected != 0)
+}
+/* process 802.11h Dynamic Frequency Selection */
+void zfStaUpdateDot11HDFS(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    /*
+    Channel Switch Announcement Element Format
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Format|Element ID|Length|Channel Switch Mode|New Channel Number|Channel Switch Count|
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Bytes |   1      |  1   |	     1           |       1          |          1         |
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Value |   37     |  3   |       0 or 1      |unsigned integer  |unsigned integer    |
+    +------+----------+------+-------------------+------------------+--------------------+
+    */
+    //u8_t    length, channel, is5G;
+    u16_t   offset;
+
+    /* get EID(Channel Switch Announcement) */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CHANNEL_SWITCH_ANNOUNCE)) == 0xffff )
+    {
+        //zm_debug_msg0("EID(Channel Switch Announcement) not found");
+        return;
+    }
+    else if ( zmw_rx_buf_readb(dev, buf, offset+1) == 0x3 )
+    {
+        zm_debug_msg0("EID(Channel Switch Announcement) found");
+
+        //length = zmw_rx_buf_readb(dev, buf, offset+1);
+        //zfCopyFromRxBuffer(dev, buf, pBssInfo->supportedRates, offset, length+2);
+
+        //Chanell Switch Mode set to 1, driver should disable transmit immediate
+        //we do this by poll CCA high
+        if (zmw_rx_buf_readb(dev, buf, offset+2) == 0x1 )
+        {
+        	//use ZM_OID_INTERNAL_WRITE,ZM_CMD_RESET to notice firmware flush quene and stop dma,
+        	//then restart rx dma but not tx dma
+        	if (wd->sta.DFSDisableTx != TRUE)
+        	{
+                /* TODO : zfHpResetTxRx would cause Rx hang */
+                //zfHpResetTxRx(dev);
+                wd->sta.DFSDisableTx = TRUE;
+                /* Trgger Rx DMA */
+                zfHpStartRecv(dev);
+            }
+        	//Adapter->ZD80211HSetting.DisableTxBy80211H=TRUE;
+        	//AcquireCtrOfPhyReg(Adapter);
+        	//ZD1205_WRITE_REGISTER(Adapter,CR24, 0x0);
+        	//ReleaseDoNotSleep(Adapter);
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+4) <= 0x2 )
+        {
+        	//Channel Switch
+        	//if Channel Switch Count = 0 , STA should change channel immediately.
+        	//if Channel Switch Count > 0 , STA should change channel after TBTT*count
+        	//But it won't be accurate to let driver calculate TBTT*count, and the value of
+        	//Channel Switch Count will decrease by one each when continue receving beacon
+        	//So we change channel here when we receive count <=2.
+
+            zfHpDeleteAllowChannel(dev, wd->sta.currentFrequency);
+        	wd->frequency = zfChNumToFreq(dev, zmw_rx_buf_readb(dev, buf, offset+3), 0);
+        	//zfHpAddAllowChannel(dev, wd->frequency);
+        	zm_debug_msg1("CWY - jump to frequency = ", wd->frequency);
+        	zfCoreSetFrequency(dev, wd->frequency);
+        	wd->sta.DFSDisableTx = FALSE;
+            /* Increase rxBeaconCount to prevent beacon lost */
+            if (zfStaIsConnected(dev))
+            {
+                wd->sta.rxBeaconCount = 1 << 6; // 2 times of check would pass
+            }
+        	//start tx dma to transmit packet
+
+        	//if (zmw_rx_buf_readb(dev, buf, offset+3) != wd->frequency)
+        	//{
+        	//	//ZDDbgPrint(("Radar Detect by AP\n"));
+        	//	zfCoreSetFrequency();
+        	//	ProcessRadarDetectEvent(Adapter);
+        	//	Set_RF_Channel(Adapter, SwRfd->Rfd->RxBuffer[index+3], (UCHAR)Adapter->RF_Mode, 1);
+        	//	Adapter->CardSetting.Channel = SwRfd->Rfd->RxBuffer[index+3];
+        	//	Adapter->SaveChannel = Adapter->CardSetting.Channel;
+        	//	Adapter->UtilityChannel = Adapter->CardSetting.Channel;
+        	//}
+        }
+    }
+
+}
+/* TODO : process 802.11h Transmission Power Control */
+void zfStaUpdateDot11HTPC(zdev_t* dev, zbuf_t* buf)
+{
+}
+
+/* IBSS power-saving mode */
+void zfStaIbssPSCheckState(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   i, frameCtrl;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return ;
+    }
+
+    /* check BSSID */
+    if ( !zfRxBufferEqualToStr(dev, buf, (u8_t*) wd->sta.bssid,
+                               ZM_WLAN_HEADER_A3_OFFSET, 6) )
+    {
+        return;
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+
+    /* check power management bit */
+    if ( frameCtrl & ZM_BIT_4 )
+    {
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( !wd->sta.staPSList.entity[i].bUsed )
+            {
+                continue;
+            }
+
+            /* check source address */
+            if ( zfRxBufferEqualToStr(dev, buf,
+                                      wd->sta.staPSList.entity[i].macAddr,
+                                      ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+                return;
+            }
+        }
+
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( !wd->sta.staPSList.entity[i].bUsed )
+            {
+                wd->sta.staPSList.entity[i].bUsed = TRUE;
+                wd->sta.staPSList.entity[i].bDataQueued = FALSE;
+                break;
+            }
+        }
+
+        if ( i == ZM_MAX_PS_STA )
+        {
+            /* STA list is full */
+            return;
+        }
+
+        zfCopyFromRxBuffer(dev, buf, wd->sta.staPSList.entity[i].macAddr,
+                           ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+        if ( wd->sta.staPSList.count == 0 )
+        {
+            // enable ATIM window
+            //zfEnableAtimWindow(dev);
+        }
+
+        wd->sta.staPSList.count++;
+    }
+    else if ( wd->sta.staPSList.count )
+    {
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( wd->sta.staPSList.entity[i].bUsed )
+            {
+                if ( zfRxBufferEqualToStr(dev, buf,
+                                          wd->sta.staPSList.entity[i].macAddr,
+                                          ZM_WLAN_HEADER_A2_OFFSET, 6) )
+                {
+                    wd->sta.staPSList.entity[i].bUsed = FALSE;
+                    wd->sta.staPSList.count--;
+
+                    if ( wd->sta.staPSList.entity[i].bDataQueued )
+                    {
+                        /* send queued data */
+                    }
+                }
+            }
+        }
+
+        if ( wd->sta.staPSList.count == 0 )
+        {
+            /* disable ATIM window */
+            //zfDisableAtimWindow(dev);
+        }
+
+    }
+}
+
+/* IBSS power-saving mode */
+u8_t zfStaIbssPSQueueData(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   i;
+    u16_t  da[3];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return 0;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return 0;
+    }
+
+    if ( wd->sta.staPSList.count == 0 && wd->sta.powerSaveMode <= ZM_STA_PS_NONE )
+    {
+        return 0;
+    }
+
+    /* DA */
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    da[0] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+    da[1] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET + 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET + 4);
+#else
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+#endif
+
+    if ( ZM_IS_MULTICAST_OR_BROADCAST(da) )
+    {
+        wd->sta.staPSList.entity[0].bDataQueued = TRUE;
+        wd->sta.ibssPSDataQueue[wd->sta.ibssPSDataCount++] = buf;
+        return 1;
+    }
+
+    // Unicast packet...
+
+    for(i=1; i<ZM_MAX_PS_STA; i++)
+    {
+        if ( zfMemoryIsEqual(wd->sta.staPSList.entity[i].macAddr,
+                             (u8_t*) da, 6) )
+        {
+            wd->sta.staPSList.entity[i].bDataQueued = TRUE;
+            wd->sta.ibssPSDataQueue[wd->sta.ibssPSDataCount++] = buf;
+
+            return 1;
+        }
+    }
+
+#if 0
+    if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+    {
+        wd->sta.staPSDataQueue[wd->sta.staPSDataCount++] = buf;
+
+        return 1;
+    }
+#endif
+
+    return 0;
+}
+
+/* IBSS power-saving mode */
+void zfStaIbssPSSend(zdev_t* dev)
+{
+    u8_t   i;
+    u16_t  bcastAddr[3] = {0xffff, 0xffff, 0xffff};
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return ;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return ;
+    }
+
+    for(i=0; i<ZM_MAX_PS_STA; i++)
+    {
+        if ( wd->sta.staPSList.entity[i].bDataQueued )
+        {
+            if ( i == 0 )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ATIM,
+                              bcastAddr,
+                              0, 0, 0);
+            }
+            else if ( wd->sta.staPSList.entity[i].bUsed )
+            {
+                // Send ATIM to prevent the peer to go to sleep
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ATIM,
+                              (u16_t*) wd->sta.staPSList.entity[i].macAddr,
+                              0, 0, 0);
+            }
+
+            wd->sta.staPSList.entity[i].bDataQueued = FALSE;
+        }
+    }
+
+    for(i=0; i<wd->sta.ibssPSDataCount; i++)
+    {
+        zfTxSendEth(dev, wd->sta.ibssPSDataQueue[i], 0,
+                    ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    wd->sta.ibssPrevPSDataCount = wd->sta.ibssPSDataCount;
+    wd->sta.ibssPSDataCount = 0;
+}
+
+
+void zfStaReconnect(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE &&
+         wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return;
+    }
+
+    if ( (zfStaIsConnected(dev))||(zfStaIsConnecting(dev)) )
+    {
+        return;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    /* Recover zero SSID length  */
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) && (wd->ws.ssidLen == 0))
+    {
+        zm_debug_msg0("zfStaReconnect: NOT Support!! Set SSID to any BSS");
+        /* ANY BSS */
+        zmw_enter_critical_section(dev);
+        wd->sta.ssid[0] = 0;
+        wd->sta.ssidLen = 0;
+        zmw_leave_critical_section(dev);
+    }
+
+    // RAY: To ensure no TX pending before re-connecting
+    zfFlushVtxq(dev);
+    zfWlanEnable(dev);
+    zfScanMgrScanAck(dev);
+}
+
+void zfStaTimer100ms(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( (wd->tick % 10) == 0 )
+    {
+        zfPushVtxq(dev);
+//        zfPowerSavingMgrMain(dev);
+    }
+}
+
+
+void zfStaCheckRxBeacon(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE ) && (zfStaIsConnected(dev)))
+    {
+        if (wd->beaconInterval == 0)
+        {
+            wd->beaconInterval = 100;
+        }
+        if ( (wd->tick % ((wd->beaconInterval * 10) / ZM_MS_PER_TICK)) == 0 )
+        {
+            /* Check rxBeaconCount */
+            if (wd->sta.rxBeaconCount == 0)
+            {
+                if (wd->sta.beaconMissState == 1)
+                {
+            	/*notify AP that we left*/
+            	zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                /* Beacon Lost */
+                zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_BEACON_MISS,
+                        wd->sta.bssid, 0);
+                }
+                else
+                {
+                    wd->sta.beaconMissState = 1;
+                    /* Reset channel */
+                    zfCoreSetFrequencyExV2(dev, wd->frequency, wd->BandWidth40,
+                            wd->ExtOffset, NULL, 1);
+                }
+            }
+            else
+            {
+                wd->sta.beaconMissState = 0;
+            }
+            wd->sta.rxBeaconCount = 0;
+        }
+    }
+}
+
+
+
+void zfStaCheckConnectTimeout(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE )
+    {
+        return;
+    }
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    if ( (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_OPEN)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_1)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_2)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_ASSOCIATE) )
+    {
+        if ( (wd->tick - wd->sta.connectTimer) > ZM_INTERVAL_CONNECT_TIMEOUT )
+        {
+            if ( wd->sta.connectByReasso )
+            {
+                wd->sta.failCntOfReasso++;
+                if ( wd->sta.failCntOfReasso > 2 )
+                {
+                    wd->sta.connectByReasso = FALSE;
+                }
+            }
+
+            wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+            zm_debug_msg1("connect timeout, state = ", wd->sta.connectState);
+            //zfiWlanDisable(dev);
+            goto failed;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+
+failed:
+    zmw_leave_critical_section(dev);
+    if(wd->sta.authMode == ZM_AUTH_MODE_AUTO)
+	{ // Fix some AP not send authentication failed message to sta and lead to connect timeout !
+            wd->sta.connectTimeoutCount++;
+	}
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_TIMEOUT, wd->sta.bssid, 2);
+    return;
+}
+
+void zfMmStaTimeTick(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* airopeek */
+    if (wd->wlanMode != ZM_MODE_AP && !wd->swSniffer)
+    {
+        if ( wd->tick & 1 )
+        {
+            zfTimerCheckAndHandle(dev);
+        }
+
+        zfStaCheckRxBeacon(dev);
+        zfStaTimer100ms(dev);
+        zfStaCheckConnectTimeout(dev);
+        zfPowerSavingMgrMain(dev);
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /*
+     * add by honda
+     */
+    zfAggScanAndClear(dev, wd->tick);
+#endif
+}
+
+void zfStaSendBeacon(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t offset, seq;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //zm_debug_msg0("\n");
+
+    /* TBD : Maximum size of beacon */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_debug_msg0("Allocate beacon buffer failed");
+        return;
+    }
+
+    offset = 0;
+    /* wlan header */
+    /* Frame control */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0080);
+    offset+=2;
+    /* Duration */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0000);
+    offset+=2;
+    /* Address 1 */
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    /* Address 2 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]);
+    offset+=2;
+    /* Address 3 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[1]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[2]);
+    offset+=2;
+
+    /* Sequence number */
+    zmw_enter_critical_section(dev);
+    seq = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+    zmw_tx_buf_writeh(dev, buf, offset, seq);
+    offset+=2;
+
+    /* 24-31 Time Stamp : hardware will fill this field */
+    offset+=8;
+
+    /* Beacon Interval */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+    offset+=2;
+
+    /* Capability */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+
+    /* SSID */
+    offset = zfStaAddIeSsid(dev, buf, offset);
+
+    if(wd->frequency <= ZM_CH_G_14)  // 2.4 GHz  b+g
+    {
+
+    	/* Support Rate */
+    	offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  		ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+    	/* DS parameter set */
+    	offset = zfMmAddIeDs(dev, buf, offset);
+
+    	offset = zfStaAddIeIbss(dev, buf, offset);
+
+        if( wd->wfc.bIbssGMode
+            && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+        {
+      	    /* ERP Information */
+       	    wd->erpElement = 0;
+       	    offset = zfMmAddIeErp(dev, buf, offset);
+       	}
+
+       	/* TODO : country information */
+        /* RSN */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+        }
+
+        if( wd->wfc.bIbssGMode
+            && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+        {
+            /* Enable G Mode */
+            /* Extended Supported Rates */
+       	    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                   		    ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+	    }
+    }
+    else    // 5GHz a
+    {
+        /* Support Rate a Mode */
+    	offset = zfMmAddIeSupportRate(dev, buf, offset,
+        	                            ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+
+        /* DS parameter set */
+    	offset = zfMmAddIeDs(dev, buf, offset);
+
+    	offset = zfStaAddIeIbss(dev, buf, offset);
+
+        /* TODO : country information */
+        /* RSN */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+        }
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        /* TODO : Need to check if it is ok */
+        /* HT Capabilities Info */
+        offset = zfMmAddHTCapability(dev, buf, offset);
+
+        /* Extended HT Capabilities Info */
+        offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+    }
+
+    if ( wd->sta.ibssAdditionalIESize )
+        offset = zfStaAddIbssAdditionalIE(dev, buf, offset);
+
+    /* 1212 : write to beacon fifo */
+    /* 1221 : write to share memory */
+    zfHpSendBeacon(dev, buf, offset);
+
+    /* Free beacon buffer */
+    //zfwBufFree(dev, buf, 0);
+}
+
+void zfStaSignalStatistic(zdev_t* dev, u8_t SignalStrength, u8_t SignalQuality) //CWYang(+)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Add Your Code to Do Works Like Moving Average Here */
+    wd->SignalStrength = (wd->SignalStrength * 7 + SignalStrength * 3)/10;
+    wd->SignalQuality = (wd->SignalQuality * 7 + SignalQuality * 3)/10;
+
+}
+
+struct zsBssInfo* zfStaFindBssInfo(zdev_t* dev, zbuf_t* buf, struct zsWlanProbeRspFrameHeader *pProbeRspHeader)
+{
+    u8_t    i;
+    u8_t    j;
+    u8_t    k;
+    u8_t    isMatched, length, channel;
+    u16_t   offset, frequency;
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((pBssInfo = wd->sta.bssList.head) == NULL)
+    {
+        return NULL;
+    }
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        //zm_debug_msg2("check pBssInfo = ", pBssInfo);
+
+        /* Check BSSID */
+        for( j=0; j<6; j++ )
+        {
+            if ( pBssInfo->bssid[j] != pProbeRspHeader->bssid[j] )
+            {
+                break;
+            }
+        }
+
+		/* Check SSID */
+        if (j == 6)
+        {
+            if (pProbeRspHeader->ssid[1] <= 32)
+            {
+                /* compare length and ssid */
+                isMatched = 1;
+				if((pProbeRspHeader->ssid[1] != 0) && (pBssInfo->ssid[1] != 0))
+				{
+                for( k=1; k<pProbeRspHeader->ssid[1] + 1; k++ )
+                {
+                    if ( pBssInfo->ssid[k] != pProbeRspHeader->ssid[k] )
+                    {
+                        isMatched = 0;
+                        break;
+                    }
+                }
+            }
+            }
+            else
+            {
+                isMatched = 0;
+            }
+        }
+        else
+        {
+            isMatched = 0;
+        }
+
+        /* Check channel */
+        /* Add check channel to solve the bug #31222 */
+        if (isMatched) {
+            if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_DS)) != 0xffff) {
+                if ((length = zmw_rx_buf_readb(dev, buf, offset+1)) == 1) {
+                    channel = zmw_rx_buf_readb(dev, buf, offset+2);
+                    if (zfHpIsAllowedChannel(dev, zfChNumToFreq(dev, channel, 0)) == 0) {
+                        frequency = 0;
+                    } else {
+                        frequency = zfChNumToFreq(dev, channel, 0);;
+                    }
+                } else {
+                    frequency = 0;
+                }
+            } else {
+                frequency = wd->sta.currentFrequency;
+            }
+
+            if (frequency != 0) {
+                if ( ((frequency > 3000) && (pBssInfo->frequency > 3000))
+                     || ((frequency < 3000) && (pBssInfo->frequency < 3000)) ) {
+                    /* redundant */
+                    break;
+                }
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+    if ( i == wd->sta.bssList.bssCount )
+    {
+        pBssInfo = NULL;
+    }
+
+    return pBssInfo;
+}
+
+u8_t zfStaInitBssInfo(zdev_t* dev, zbuf_t* buf,
+        struct zsWlanProbeRspFrameHeader *pProbeRspHeader,
+        struct zsBssInfo* pBssInfo, struct zsAdditionInfo* AddInfo, u8_t type)
+{
+    u8_t    length, channel, is5G;
+    u16_t   i, offset;
+    u8_t    apQosInfo;
+    u16_t    eachIElength = 0;
+    u16_t   accumulateLen = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((type == 1) && ((pBssInfo->flag & ZM_BSS_INFO_VALID_BIT) != 0))
+    {
+        goto zlUpdateRssi;
+    }
+
+    /* get SSID */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff )
+    {
+        zm_debug_msg0("EID(SSID) not found");
+        goto zlError;
+    }
+
+    length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+	{
+		u8_t Show_Flag = 0;
+		zfwGetShowZeroLengthSSID(dev, &Show_Flag);
+
+		if(Show_Flag)
+		{
+			if (length > ZM_MAX_SSID_LENGTH )
+			{
+				zm_debug_msg0("EID(SSID) is invalid");
+				goto zlError;
+			}
+		}
+		else
+		{
+    if ( length == 0 || length > ZM_MAX_SSID_LENGTH )
+    {
+        zm_debug_msg0("EID(SSID) is invalid");
+        goto zlError;
+    }
+
+		}
+	}
+    zfCopyFromRxBuffer(dev, buf, pBssInfo->ssid, offset, length+2);
+
+    /* get DS parameter */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_DS)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if ( length != 1 )
+        {
+            zm_msg0_mm(ZM_LV_0, "Abnormal DS Param Set IE");
+            goto zlError;
+        }
+        channel = zmw_rx_buf_readb(dev, buf, offset+2);
+
+        if (zfHpIsAllowedChannel(dev, zfChNumToFreq(dev, channel, 0)) == 0)
+        {
+            goto zlError2;
+        }
+
+        pBssInfo->frequency = zfChNumToFreq(dev, channel, 0); // auto check
+        pBssInfo->channel = channel;
+
+
+    }
+    else
+    {
+        /* DS parameter not found */
+        pBssInfo->frequency = wd->sta.currentFrequency;
+        pBssInfo->channel = zfChFreqToNum(wd->sta.currentFrequency, &is5G);
+    }
+
+    /* initialize security type */
+    pBssInfo->securityType = ZM_SECURITY_TYPE_NONE;
+
+    /* get macaddr */
+    for( i=0; i<6; i++ )
+    {
+        pBssInfo->macaddr[i] = pProbeRspHeader->sa[i];
+    }
+
+    /* get bssid */
+    for( i=0; i<6; i++ )
+    {
+        pBssInfo->bssid[i] = pProbeRspHeader->bssid[i];
+    }
+
+    /* get timestamp */
+    for( i=0; i<8; i++ )
+    {
+        pBssInfo->timeStamp[i] = pProbeRspHeader->timeStamp[i];
+    }
+
+    /* get beacon interval */
+    pBssInfo->beaconInterval[0] = pProbeRspHeader->beaconInterval[0];
+    pBssInfo->beaconInterval[1] = pProbeRspHeader->beaconInterval[1];
+
+    /* get capability */
+    pBssInfo->capability[0] = pProbeRspHeader->capability[0];
+    pBssInfo->capability[1] = pProbeRspHeader->capability[1];
+
+    /* Copy frame body */
+    offset = 36;            // Copy from the start of variable IE
+    pBssInfo->frameBodysize = zfwBufGetSize(dev, buf)-offset;
+    if (pBssInfo->frameBodysize > (ZM_MAX_PROBE_FRAME_BODY_SIZE-1))
+    {
+        pBssInfo->frameBodysize = ZM_MAX_PROBE_FRAME_BODY_SIZE-1;
+    }
+    accumulateLen = 0;
+    do
+    {
+        eachIElength = zmw_rx_buf_readb(dev, buf, offset + accumulateLen+1) + 2;  //Len+(EID+Data)
+
+        if ( (eachIElength >= 2)
+             && ((accumulateLen + eachIElength) <= pBssInfo->frameBodysize) )
+        {
+            zfCopyFromRxBuffer(dev, buf, pBssInfo->frameBody+accumulateLen, offset+accumulateLen, eachIElength);
+            accumulateLen+=(u16_t)eachIElength;
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_1, "probersp frameBodysize abnormal");
+            break;
+        }
+    }
+    while(accumulateLen < pBssInfo->frameBodysize);
+    pBssInfo->frameBodysize = accumulateLen;
+
+    /* get supported rates */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SUPPORT_RATE)) == 0xffff )
+    {
+        zm_debug_msg0("EID(supported rates) not found");
+        goto zlError;
+    }
+
+    length = zmw_rx_buf_readb(dev, buf, offset+1);
+    if ( length == 0 || length > ZM_MAX_SUPP_RATES_IE_SIZE)
+    {
+        zm_msg0_mm(ZM_LV_0, "Supported rates IE length abnormal");
+        goto zlError;
+    }
+    zfCopyFromRxBuffer(dev, buf, pBssInfo->supportedRates, offset, length+2);
+
+
+
+    /* get Country information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_COUNTRY)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_COUNTRY_INFO_SIZE)
+        {
+            length = ZM_MAX_COUNTRY_INFO_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->countryInfo, offset, length+2);
+        /* check 802.11d support data */
+        if (wd->sta.b802_11D)
+        {
+            zfHpGetRegulationTablefromISO(dev, (u8_t *)&pBssInfo->countryInfo, 3);
+            /* only set regulatory one time */
+            wd->sta.b802_11D = 0;
+        }
+    }
+
+    /* get ERP information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+    {
+        pBssInfo->erp = zmw_rx_buf_readb(dev, buf, offset+2);
+    }
+
+    /* get extended supported rates */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_SUPP_RATES_IE_SIZE)
+        {
+            zm_msg0_mm(ZM_LV_0, "Extended rates IE length abnormal");
+            goto zlError;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->extSupportedRates, offset, length+2);
+    }
+    else
+    {
+        pBssInfo->extSupportedRates[0] = 0;
+        pBssInfo->extSupportedRates[1] = 0;
+    }
+
+    /* get WPA IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_WPA_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE)
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->wpaIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_WPA;
+    }
+    else
+    {
+        pBssInfo->wpaIe[1] = 0;
+    }
+
+    /* get WPS IE */
+    if ((offset = zfFindWifiElement(dev, buf, 4, 0xff)) != 0xffff)
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_WPS_IE_SIZE )
+        {
+            length = ZM_MAX_WPS_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->wscIe, offset, length+2);
+    }
+    else
+    {
+        pBssInfo->wscIe[1] = 0;
+    }
+
+    /* get SuperG IE */
+    if ((offset = zfFindSuperGElement(dev, buf, ZM_WLAN_EID_VENDOR_PRIVATE)) != 0xffff)
+    {
+        pBssInfo->apCap |= ZM_SuperG_AP;
+    }
+
+    /* get XR IE */
+    if ((offset = zfFindXRElement(dev, buf, ZM_WLAN_EID_VENDOR_PRIVATE)) != 0xffff)
+    {
+        pBssInfo->apCap |= ZM_XR_AP;
+    }
+
+    /* get RSN IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_RSN_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE)
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->rsnIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_WPA;
+    }
+    else
+    {
+        pBssInfo->rsnIe[1] = 0;
+    }
+#ifdef ZM_ENABLE_CENC
+    /* get CENC IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CENC_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE )
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->cencIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_CENC;
+        pBssInfo->capability[0] &= 0xffef;
+    }
+    else
+    {
+        pBssInfo->cencIe[1] = 0;
+    }
+#endif //ZM_ENABLE_CENC
+    /* get WME Parameter IE, probe rsp may contain WME parameter element */
+    //if ( wd->bQoSEnable )
+    {
+        if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+        {
+            apQosInfo = zmw_rx_buf_readb(dev, buf, offset+8) & 0x80;
+            pBssInfo->wmeSupport = 1 | apQosInfo;
+        }
+        else if ((offset = zfFindWifiElement(dev, buf, 2, 0)) != 0xffff)
+        {
+            apQosInfo = zmw_rx_buf_readb(dev, buf, offset+8) & 0x80;
+            pBssInfo->wmeSupport = 1  | apQosInfo;
+        }
+        else
+        {
+            pBssInfo->wmeSupport = 0;
+        }
+    }
+    //CWYang(+)
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* 11n AP */
+        pBssInfo->EnableHT = 1;
+        if (zmw_rx_buf_readb(dev, buf, offset+1) & 0x02)
+        {
+            pBssInfo->enableHT40 = 1;
+        }
+        else
+        {
+            pBssInfo->enableHT40 = 0;
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+1) & 0x40)
+        {
+            pBssInfo->SG40 = 1;
+        }
+        else
+        {
+            pBssInfo->SG40 = 0;
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+    {
+        /* 11n AP */
+        pBssInfo->EnableHT = 1;
+        pBssInfo->apCap |= ZM_All11N_AP;
+        if (zmw_rx_buf_readb(dev, buf, offset+2) & 0x02)
+        {
+            pBssInfo->enableHT40 = 1;
+        }
+        else
+        {
+            pBssInfo->enableHT40 = 0;
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+2) & 0x40)
+        {
+            pBssInfo->SG40 = 1;
+        }
+        else
+        {
+            pBssInfo->SG40 = 0;
+        }
+    }
+    else
+    {
+        pBssInfo->EnableHT = 0;
+    }
+    /* HT information */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        pBssInfo->extChOffset = zmw_rx_buf_readb(dev, buf, offset+2) & 0x03;
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTINFORMATION)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        pBssInfo->extChOffset = zmw_rx_buf_readb(dev, buf, offset+3) & 0x03;
+    }
+    else
+    {
+        pBssInfo->extChOffset = 0;
+    }
+
+    if ( (pBssInfo->enableHT40 == 1)
+         && ((pBssInfo->extChOffset != 1) && (pBssInfo->extChOffset != 3)) )
+    {
+        pBssInfo->enableHT40 = 0;
+    }
+
+    if (pBssInfo->enableHT40 == 1)
+    {
+        if (zfHpIsAllowedChannel(dev, pBssInfo->frequency+((pBssInfo->extChOffset==1)?20:-20)) == 0)
+        {
+            /* if extension channel is not an allowed channel, treat AP as non-HT mode */
+            pBssInfo->EnableHT = 0;
+            pBssInfo->enableHT40 = 0;
+            pBssInfo->extChOffset = 0;
+        }
+    }
+
+    /* get ATH Extended Capability */
+    if ( ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)&&
+        ((offset = zfFindBrdcmMrvlRlnkExtCap(dev, buf)) == 0xffff))
+
+    {
+        pBssInfo->athOwlAp = 1;
+    }
+    else
+    {
+        pBssInfo->athOwlAp = 0;
+    }
+
+    /* get Broadcom Extended Capability */
+    if ( (pBssInfo->EnableHT == 1) //((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+         && ((offset = zfFindBroadcomExtCap(dev, buf)) != 0xffff) )
+    {
+        pBssInfo->broadcomHTAp = 1;
+    }
+    else
+    {
+        pBssInfo->broadcomHTAp = 0;
+    }
+
+    /* get Marvel Extended Capability */
+    if ((offset = zfFindMarvelExtCap(dev, buf)) != 0xffff)
+    {
+        pBssInfo->marvelAp = 1;
+    }
+    else
+    {
+        pBssInfo->marvelAp = 0;
+    }
+
+    /* get ATIM window */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_IBSS)) != 0xffff )
+    {
+        pBssInfo->atimWindow = zmw_rx_buf_readh(dev, buf,offset+2);
+    }
+
+    /* Fit for support mode */
+    if (pBssInfo->frequency > 3000) {
+        if (wd->supportMode & ZM_WIRELESS_MODE_5_N) {
+#if 0
+            if (wd->supportMode & ZM_WIRELESS_MODE_5_54) {
+                /* support mode: a, n */
+                /* do nothing */
+            } else {
+                /* support mode: n */
+                /* reject non-n bss info */
+                if (!pBssInfo->EnableHT) {
+                    goto zlError2;
+                }
+            }
+#endif
+        } else {
+            if (wd->supportMode & ZM_WIRELESS_MODE_5_54) {
+                /* support mode: a */
+                /* delete n mode information */
+                pBssInfo->EnableHT = 0;
+                pBssInfo->enableHT40 = 0;
+                pBssInfo->apCap &= (~ZM_All11N_AP);
+                pBssInfo->extChOffset = 0;
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_EID_HT_CAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTINFORMATION);
+            } else {
+                /* support mode: none */
+                goto zlError2;
+            }
+        }
+    } else {
+        if (wd->supportMode & ZM_WIRELESS_MODE_24_N) {
+#if 0
+            if (wd->supportMode & ZM_WIRELESS_MODE_24_54) {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, g, n */
+                    /* do nothing */
+                } else {
+                    /* support mode: g, n */
+                    /* reject b-only bss info */
+                    if ( (!pBssInfo->EnableHT)
+                         && (pBssInfo->extSupportedRates[1] == 0) ) {
+                         goto zlError2;
+                    }
+                }
+            } else {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, n */
+                    /* 1. reject g-only bss info
+                     * 2. if non g-only, delete g mode information
+                     */
+                    if ( !pBssInfo->EnableHT ) {
+                        if ( zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->supportedRates)
+                             || zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->extSupportedRates) ) {
+                            goto zlError2;
+                        } else {
+                            zfGatherBMode(dev, pBssInfo->supportedRates,
+                                          pBssInfo->extSupportedRates);
+                            pBssInfo->erp = 0;
+
+                            pBssInfo->frameBodysize = zfRemoveElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                ZM_WLAN_EID_ERP);
+                            pBssInfo->frameBodysize = zfRemoveElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                ZM_WLAN_EID_EXTENDED_RATE);
+
+                            pBssInfo->frameBodysize = zfUpdateElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                pBssInfo->supportedRates);
+                        }
+                    }
+                } else {
+                    /* support mode: n */
+                    /* reject non-n bss info */
+                    if (!pBssInfo->EnableHT) {
+                        goto zlError2;
+                    }
+                }
+            }
+#endif
+        } else {
+            /* delete n mode information */
+            pBssInfo->EnableHT = 0;
+            pBssInfo->enableHT40 = 0;
+            pBssInfo->apCap &= (~ZM_All11N_AP);
+            pBssInfo->extChOffset = 0;
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_EID_HT_CAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTINFORMATION);
+
+            if (wd->supportMode & ZM_WIRELESS_MODE_24_54) {
+#if 0
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, g */
+                    /* delete n mode information */
+                } else {
+                    /* support mode: g */
+                    /* delete n mode information */
+                    /* reject b-only bss info */
+                    if (pBssInfo->extSupportedRates[1] == 0) {
+                         goto zlError2;
+                    }
+                }
+#endif
+            } else {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b */
+                    /* delete n mode information */
+                    if ( zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->supportedRates)
+                         || zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->extSupportedRates) ) {
+                        goto zlError2;
+                    } else {
+                        zfGatherBMode(dev, pBssInfo->supportedRates,
+                                          pBssInfo->extSupportedRates);
+                        pBssInfo->erp = 0;
+
+                        pBssInfo->frameBodysize = zfRemoveElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            ZM_WLAN_EID_ERP);
+                        pBssInfo->frameBodysize = zfRemoveElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            ZM_WLAN_EID_EXTENDED_RATE);
+
+                        pBssInfo->frameBodysize = zfUpdateElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            pBssInfo->supportedRates);
+                    }
+                } else {
+                    /* support mode: none */
+                    goto zlError2;
+                }
+            }
+        }
+    }
+
+    pBssInfo->flag |= ZM_BSS_INFO_VALID_BIT;
+
+zlUpdateRssi:
+    /* Update Timer information */
+    pBssInfo->tick = wd->tick;
+
+    /* Update ERP information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+    {
+        pBssInfo->erp = zmw_rx_buf_readb(dev, buf, offset+2);
+    }
+
+    if( (s8_t)pBssInfo->signalStrength < (s8_t)AddInfo->Tail.Data.SignalStrength1 )
+    {
+        /* Update signal strength */
+        pBssInfo->signalStrength = (u8_t)AddInfo->Tail.Data.SignalStrength1;
+        /* Update signal quality */
+        pBssInfo->signalQuality = (u8_t)(AddInfo->Tail.Data.SignalStrength1 * 2);
+
+        /* Update the sorting value  */
+        pBssInfo->sortValue = zfComputeBssInfoWeightValue(dev,
+                                               (pBssInfo->supportedRates[6] + pBssInfo->extSupportedRates[0]),
+                                               pBssInfo->EnableHT,
+                                               pBssInfo->enableHT40,
+                                               pBssInfo->signalStrength);
+    }
+
+    return 0;
+
+zlError:
+
+    return 1;
+
+zlError2:
+
+    return 2;
+}
+
+void zfStaProcessBeacon(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo) //CWYang(m)
+{
+    /* Parse TIM and send PS-POLL in power saving mode */
+    struct zsWlanBeaconFrameHeader*  pBeaconHeader;
+    struct zsBssInfo* pBssInfo;
+    u8_t   pBuf[sizeof(struct zsWlanBeaconFrameHeader)];
+    u8_t   bssid[6];
+    int    res;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* sta routine jobs */
+    zfStaProtErpMonitor(dev, buf);  /* check protection mode */
+
+    if (zfStaIsConnected(dev))
+    {
+        ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            if ( zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+                zfPowerSavingMgrProcessBeacon(dev, buf);
+                zfStaUpdateWmeParameter(dev, buf);
+                if (wd->sta.DFSEnable)
+                    zfStaUpdateDot11HDFS(dev, buf);
+                if (wd->sta.TPCEnable)
+                    zfStaUpdateDot11HTPC(dev, buf);
+                /* update signal strength and signal quality */
+                zfStaSignalStatistic(dev, AddInfo->Tail.Data.SignalStrength1,
+                        AddInfo->Tail.Data.SignalQuality); //CWYang(+)
+                wd->sta.rxBeaconCount++;
+            }
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if ( zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A3_OFFSET, 6) )
+            {
+                int res;
+                struct zsPartnerNotifyEvent event;
+
+                zm_debug_msg0("20070916 Receive opposite Beacon!");
+                zmw_enter_critical_section(dev);
+                wd->sta.ibssReceiveBeaconCount++;
+                zmw_leave_critical_section(dev);
+
+                res = zfStaSetOppositeInfoFromRxBuf(dev, buf);
+                if ( res == 0 )
+                {
+                    // New peer station found. Notify the wrapper now
+                    zfInitPartnerNotifyEvent(dev, buf, &event);
+                    if (wd->zfcbIbssPartnerNotify != NULL)
+                    {
+                        wd->zfcbIbssPartnerNotify(dev, 1, &event);
+                    }
+                }
+                /* update signal strength and signal quality */
+                zfStaSignalStatistic(dev, AddInfo->Tail.Data.SignalStrength1,
+                        AddInfo->Tail.Data.SignalQuality); //CWYang(+)
+            }
+            //else if ( wd->sta.ibssPartnerStatus == ZM_IBSS_PARTNER_LOST )
+            // Why does this happen in IBSS?? The impact of Vista since
+            // we need to tell it the BSSID
+#if 0
+            else if ( wd->sta.oppositeCount == 0 )
+            {   /* IBSS merge if SSID matched */
+                if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) != 0xffff )
+                {
+                    if ( (wd->sta.ssidLen == zmw_buf_readb(dev, buf, offset+1))&&
+                         (zfRxBufferEqualToStr(dev, buf, wd->sta.ssid,
+                                               offset+2, wd->sta.ssidLen)) )
+                    {
+                        capabilityInfo = zmw_buf_readh(dev, buf, 34);
+
+                        if ( capabilityInfo & ZM_BIT_1 )
+                        {
+                            if ( (wd->sta.capability[0] & ZM_BIT_4) ==
+                                 (capabilityInfo & ZM_BIT_4) )
+                            {
+                                zm_debug_msg0("IBSS merge");
+                                zfCopyFromRxBuffer(dev, buf, bssid,
+                                                   ZM_WLAN_HEADER_A3_OFFSET, 6);
+                                zfUpdateBssid(dev, bssid);
+                            }
+                        }
+                    }
+                }
+            }
+#endif
+        }
+    }
+
+    /* return if not channel scan */
+    if ( !wd->sta.bChannelScan )
+    {
+        goto zlReturn;
+    }
+
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanBeaconFrameHeader));
+    pBeaconHeader = (struct zsWlanBeaconFrameHeader*) pBuf;
+
+    zmw_enter_critical_section(dev);
+
+    //zm_debug_msg1("bss count = ", wd->sta.bssList.bssCount);
+
+    pBssInfo = zfStaFindBssInfo(dev, buf, pBeaconHeader);
+
+    if ( pBssInfo == NULL )
+    {
+        /* Allocate a new entry if BSS not in the scan list */
+        pBssInfo = zfBssInfoAllocate(dev);
+        if (pBssInfo != NULL)
+        {
+            res = zfStaInitBssInfo(dev, buf, pBeaconHeader, pBssInfo, AddInfo, 0);
+            //zfDumpSSID(pBssInfo->ssid[1], &(pBssInfo->ssid[2]));
+            if ( res != 0 )
+            {
+                zfBssInfoFree(dev, pBssInfo);
+            }
+            else
+            {
+                zfBssInfoInsertToList(dev, pBssInfo);
+            }
+        }
+    }
+    else
+    {
+        res = zfStaInitBssInfo(dev, buf, pBeaconHeader, pBssInfo, AddInfo, 1);
+        if (res == 2)
+        {
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            int idx;
+
+            // It would reset the alive counter if the peer station is found!
+            zfStaFindFreeOpposite(dev, (u16_t *)pBssInfo->macaddr, &idx);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+zlReturn:
+
+    return;
+}
+
+
+void zfAuthFreqCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_COMPLETED)
+    {
+        zm_debug_msg0("ZM_STA_CONN_STATE_ASSOCIATE");
+        wd->sta.connectTimer = wd->tick;
+        wd->sta.connectState = ZM_STA_CONN_STATE_ASSOCIATE;
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessAuth               */
+/*      Process authenticate management frame.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* Note : AP allows one authenticating STA at a time, does not          */
+/*        support multiple authentication process. Make sure            */
+/*        authentication state machine will not be blocked due          */
+/*        to incompleted authentication handshake.                      */
+void zfStaProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    struct zsWlanAuthFrameHeader* pAuthFrame;
+    u8_t  pBuf[sizeof(struct zsWlanAuthFrameHeader)];
+    u32_t p1, p2;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    pAuthFrame = (struct zsWlanAuthFrameHeader*) pBuf;
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanAuthFrameHeader));
+
+    if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_OPEN )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->seq) == 2)&&
+             (zmw_le16_to_cpu(pAuthFrame->algo) == 0)&&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0) )
+        {
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+            zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_COMPLETED");
+            wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_COMPLETED;
+            zmw_leave_critical_section(dev);
+
+            //Set channel according to AP's configuration
+            //Move to here because of Cisco 11n AP feature
+            zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, zfAuthFreqCompleteCb);
+
+            /* send association frame */
+            if ( wd->sta.connectByReasso )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_REASOCREQ,
+                              wd->sta.bssid, 0, 0, 0);
+            }
+            else
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCREQ,
+                              wd->sta.bssid, 0, 0, 0);
+            }
+
+
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            if (wd->sta.authMode == ZM_AUTH_MODE_AUTO)
+            {
+                wd->sta.bIsSharedKey = 1;
+                zfStaStartConnect(dev, wd->sta.bIsSharedKey);
+            }
+            else
+            {
+                zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+                zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+            }
+        }
+    }
+    else if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_1 )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->algo) == 1) &&
+             (zmw_le16_to_cpu(pAuthFrame->seq) == 2) &&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0))
+              //&& (pAuthFrame->challengeText[1] <= 255) )
+        {
+            zfMemoryCopy(wd->sta.challengeText, pAuthFrame->challengeText,
+                         pAuthFrame->challengeText[1]+2);
+
+            /* send the 3rd authentication frame */
+            p1 = 0x30001;
+            p2 = 0;
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH,
+                          wd->sta.bssid, p1, p2, 0);
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+
+            zm_debug_msg0("ZM_STA_SUB_STATE_AUTH_SHARE_2");
+            wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_SHARE_2;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+        }
+    }
+    else if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_2 )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->algo) == 1)&&
+             (zmw_le16_to_cpu(pAuthFrame->seq) == 4)&&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0) )
+        {
+            //Set channel according to AP's configuration
+            //Move to here because of Cisco 11n AP feature
+            zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, NULL);
+
+            /* send association frame */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCREQ,
+                          wd->sta.bssid, 0, 0, 0);
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+
+            zm_debug_msg0("ZM_STA_SUB_STATE_ASSOCIATE");
+            wd->sta.connectState = ZM_STA_CONN_STATE_ASSOCIATE;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+        }
+    }
+    else
+    {
+        zm_debug_msg0("unknown case");
+    }
+}
+
+void zfStaProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+
+    return;
+}
+
+void zfStaProcessAsocRsp(zdev_t* dev, zbuf_t* buf)
+{
+    struct zsWlanAssoFrameHeader* pAssoFrame;
+    u8_t  pBuf[sizeof(struct zsWlanAssoFrameHeader)];
+    u16_t offset;
+    u32_t i;
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    pAssoFrame = (struct zsWlanAssoFrameHeader*) pBuf;
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanAssoFrameHeader));
+
+    if ( wd->sta.connectState == ZM_STA_CONN_STATE_ASSOCIATE )
+    {
+        if ( pAssoFrame->status == 0 )
+        {
+            zm_debug_msg0("ZM_STA_STATE_CONNECTED");
+
+            if (wd->sta.EnableHT == 1)
+            {
+                wd->sta.wmeConnected = 1;
+            }
+            if ((wd->sta.wmeEnabled & ZM_STA_WME_ENABLE_BIT) != 0) //WME enabled
+            {
+                /* Asoc rsp may contain WME parameter element */
+                if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+                {
+                    zm_debug_msg0("WME enable");
+                    wd->sta.wmeConnected = 1;
+                    if ((wd->sta.wmeEnabled & ZM_STA_UAPSD_ENABLE_BIT) != 0)
+                    {
+                        if ((zmw_rx_buf_readb(dev, buf, offset+8) & 0x80) != 0)
+                        {
+                            zm_debug_msg0("UAPSD enable");
+                            wd->sta.qosInfo = wd->sta.wmeQosInfo;
+                        }
+                    }
+
+                    zfStaUpdateWmeParameter(dev, buf);
+                }
+            }
+
+
+            //Store asoc response frame body, for VISTA only
+            wd->sta.asocRspFrameBodySize = zfwBufGetSize(dev, buf)-24;
+            if (wd->sta.asocRspFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+            {
+                wd->sta.asocRspFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+            }
+            for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+            {
+                wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+            }
+
+            zfStaStoreAsocRspIe(dev, buf);
+            if (wd->sta.EnableHT &&
+                ((wd->sta.ie.HtCap.HtCapInfo & HTCAP_SupChannelWidthSet) != 0) &&
+                (wd->ExtOffset != 0))
+            {
+                wd->sta.htCtrlBandwidth = 1;
+            }
+            else
+            {
+                wd->sta.htCtrlBandwidth = 0;
+            }
+
+            //Set channel according to AP's configuration
+            //zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+            //        wd->ExtOffset, NULL);
+
+            if (wd->sta.EnableHT == 1)
+            {
+                wd->addbaComplete = 0;
+
+                if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0 &&
+                    (wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) == 0)
+                {
+                    wd->addbaCount = 1;
+                    zfAggSendAddbaRequest(dev, wd->sta.bssid, 0, 0);
+                    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_ADDBA, 100);
+                }
+            }
+
+            /* set RIFS support */
+            if(wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_RIFSMode)
+            {
+                wd->sta.HT2040 = 1;
+//                zfHpSetRifs(dev, wd->sta.EnableHT, 1, (wd->sta.currentFrequency < 3000)? 1:0);
+            }
+
+            wd->sta.aid = pAssoFrame->aid & 0x3fff;
+            wd->sta.oppositeCount = 0;    /* reset opposite count */
+            zfStaSetOppositeInfoFromRxBuf(dev, buf);
+
+            wd->sta.rxBeaconCount = 16;
+
+            zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+            wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                if (wd->sta.EnableHT != 0) /* 11n */
+            	{
+    		        oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+    		        if (wd->sta.htCtrlBandwidth == 1) /* HT40*/
+    		        {
+    					if(oneTxStreamCap) /* one Tx stream */
+    				    {
+    				        if (wd->sta.SG40)
+    				        {
+    				            wd->CurrentTxRateKbps = 150000;
+    						    wd->CurrentRxRateKbps = 300000;
+    				        }
+    				        else
+    				        {
+    				            wd->CurrentTxRateKbps = 135000;
+    						    wd->CurrentRxRateKbps = 270000;
+    				        }
+    				    }
+    				    else /* Two Tx streams */
+    				    {
+    				        if (wd->sta.SG40)
+    				        {
+    				            wd->CurrentTxRateKbps = 300000;
+    						    wd->CurrentRxRateKbps = 300000;
+    				        }
+    				        else
+    				        {
+    				            wd->CurrentTxRateKbps = 270000;
+    						    wd->CurrentRxRateKbps = 270000;
+    				        }
+    				    }
+    		        }
+    		        else /* HT20 */
+    		        {
+    		            if(oneTxStreamCap) /* one Tx stream */
+    				    {
+    				        wd->CurrentTxRateKbps = 650000;
+    						wd->CurrentRxRateKbps = 130000;
+    				    }
+    				    else /* Two Tx streams */
+    				    {
+    				        wd->CurrentTxRateKbps = 130000;
+    					    wd->CurrentRxRateKbps = 130000;
+    				    }
+    		        }
+                }
+                else /* 11abg */
+                {
+                    if (wd->sta.connection_11b != 0)
+                    {
+                        wd->CurrentTxRateKbps = 11000;
+    			        wd->CurrentRxRateKbps = 11000;
+                    }
+                    else
+                    {
+                        wd->CurrentTxRateKbps = 54000;
+    			        wd->CurrentRxRateKbps = 54000;
+    			    }
+                }
+
+
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+            }
+            wd->sta.connectByReasso = TRUE;
+            wd->sta.failCntOfReasso = 0;
+
+            zfPowerSavingMgrConnectNotify(dev);
+
+            /* Disable here because fixed rate is only for test, TBD. */
+            //if (wd->sta.EnableHT)
+            //{
+            //    wd->txMCS = 7; //Rate = 65Mbps
+            //    wd->txMT = 2; // Ht rate
+            //    wd->enableAggregation = 2; // Enable Aggregation
+            //}
+        }
+        else
+        {
+            zm_debug_msg1("association failed, status = ",
+                          pAssoFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            wd->sta.connectByReasso = FALSE;
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED, wd->sta.bssid, 3);
+        }
+    }
+
+}
+
+void zfStaStoreAsocRspIe(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t offset;
+    u32_t i;
+    u16_t length;
+    u8_t  *htcap;
+    u8_t  asocBw40 = 0;
+    u8_t  asocExtOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+    {
+        wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+    }
+
+    /* HT capabilities: 28 octets */
+    if (    ((wd->sta.currentFrequency > 3000) && !(wd->supportMode & ZM_WIRELESS_MODE_5_N))
+         || ((wd->sta.currentFrequency < 3000) && !(wd->supportMode & ZM_WIRELESS_MODE_24_N)) )
+    {
+        /* not 11n AP */
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = 0;
+        }
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+        return;
+    }
+
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        zm_debug_msg0("atheros pre n");
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 26;
+        for (i=1; i<=26; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Capabilities, htcap=", htcap[i+1]);
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        zm_debug_msg0("pre n 2.0 standard");
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Capabilities, htcap=", htcap[i]);
+        }
+    }
+    else
+    {
+        /* not 11n AP */
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = 0;
+        }
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+        return;
+    }
+
+    asocBw40 = (u8_t)((wd->sta.ie.HtCap.HtCapInfo & HTCAP_SupChannelWidthSet) >> 1);
+
+    /* HT information */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        zm_debug_msg0("atheros pre n HTINFO");
+        length = 22;
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 22;
+        for (i=1; i<=22; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Info, htinfo=", htcap[i+1]);
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTINFORMATION)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        zm_debug_msg0("pre n 2.0 standard HTINFO");
+        length = zmw_rx_buf_readb(dev, buf, offset + 1);
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        for (i=0; i<24; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Info, htinfo=", htcap[i]);
+        }
+    }
+    else
+    {
+        zm_debug_msg0("no HTINFO");
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        for (i=0; i<24; i++)
+        {
+            htcap[i] = 0;
+        }
+    }
+    asocExtOffset = wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_ExtChannelOffsetBelow;
+
+    if ((wd->sta.EnableHT == 1) && (asocBw40 == 1) && ((asocExtOffset == 1) || (asocExtOffset == 3)))
+    {
+        wd->BandWidth40 = asocBw40;
+        wd->ExtOffset = asocExtOffset;
+    }
+    else
+    {
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+    }
+
+    return;
+}
+
+void zfStaProcessDeauth(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t apMacAddr[3];
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* STA : if SA=connected AP then disconnect with AP */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        apMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+        apMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+        apMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+  	if ((apMacAddr[0] == wd->sta.bssid[0]) && (apMacAddr[1] == wd->sta.bssid[1]) && (apMacAddr[2] == wd->sta.bssid[2]))
+        {
+            if (zfwBufGetSize(dev, buf) >= 24+2) //not a malformed frame
+            {
+                if ( zfStaIsConnected(dev) )
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_DEAUTH, wd->sta.bssid, 2);
+                }
+                else if (zfStaIsConnecting(dev))
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+                }
+                else
+                {
+                }
+            }
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        u16_t peerMacAddr[3];
+        u8_t  peerIdx;
+        s8_t  res;
+
+        if ( zfStaIsConnected(dev) )
+        {
+            peerMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+            peerMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+            peerMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, peerMacAddr, &peerIdx);
+            if ( res == 0 )
+            {
+                wd->sta.oppositeInfo[peerIdx].aliveCounter = 0;
+            }
+            zmw_leave_critical_section(dev);
+        }
+    }
+}
+
+void zfStaProcessDisasoc(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t apMacAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    /* STA : if SA=connected AP then disconnect with AP */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        apMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+        apMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+        apMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+
+        if ((apMacAddr[0] == wd->sta.bssid[0]) && (apMacAddr[1] == wd->sta.bssid[1]) && (apMacAddr[2] == wd->sta.bssid[2]))
+        {
+            if (zfwBufGetSize(dev, buf) >= 24+2) //not a malformed frame
+            {
+                if ( zfStaIsConnected(dev) )
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_DISASOC, wd->sta.bssid, 2);
+                }
+                else
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED, wd->sta.bssid, 3);
+                }
+            }
+        }
+    }
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeReq           */
+/*      Process probe request management frame.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfStaProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src)
+{
+    u16_t offset;
+    u8_t len;
+    u16_t i, j;
+    u16_t sendFlag;
+
+    zmw_get_wlan_dev(dev);
+
+    /* check mode : AP/IBSS */
+    if ((wd->wlanMode != ZM_MODE_AP) || (wd->wlanMode != ZM_MODE_IBSS))
+    {
+        zm_msg0_mm(ZM_LV_3, "Ignore probe req");
+        return;
+    }
+
+    /* check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_3, "probe req SSID not found");
+        return;
+    }
+
+    len = zmw_rx_buf_readb(dev, buf, offset+1);
+
+    for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+    {
+        if ((wd->ap.apBitmap & (i<<i)) != 0)
+        {
+            sendFlag = 0;
+            /* boardcast SSID */
+            if ((len == 0) && (wd->ap.hideSsid[i] == 0))
+            {
+                sendFlag = 1;
+            }
+            /* Not broadcast SSID */
+            else if (wd->ap.ssidLen[i] == len)
+            {
+                for (j=0; j<len; j++)
+                {
+                    if (zmw_rx_buf_readb(dev, buf, offset+1+j)
+                            != wd->ap.ssid[i][j])
+                    {
+                        break;
+                    }
+                }
+                if (j == len)
+                {
+                    sendFlag = 1;
+                }
+            }
+            if (sendFlag == 1)
+            {
+                /* Send probe response */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, i, 0, 0);
+            }
+        }
+    }
+}
+
+void zfStaProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+    /* return if not channel scan */
+    // Probe response is sent with unicast. Is this required?
+    // IBSS would send probe request and the code below would prevent
+    // the probe response from handling.
+    #if 0
+    zmw_get_wlan_dev(dev);
+
+    if ( !wd->sta.bChannelScan )
+    {
+        return;
+    }
+    #endif
+
+    zfProcessProbeRsp(dev, buf, AddInfo);
+}
+
+void zfIBSSSetupBssDesc(zdev_t *dev)
+{
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t i;
+#endif
+    struct zsBssInfo *pBssInfo;
+    u16_t offset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    pBssInfo = &wd->sta.ibssBssDesc;
+    zfZeroMemory((u8_t *)pBssInfo, sizeof(struct zsBssInfo));
+
+    pBssInfo->signalStrength = 100;
+
+    zfMemoryCopy((u8_t *)pBssInfo->macaddr, (u8_t *)wd->macAddr,6);
+    zfMemoryCopy((u8_t *)pBssInfo->bssid, (u8_t *)wd->sta.bssid, 6);
+
+    pBssInfo->beaconInterval[0] = (u8_t)(wd->beaconInterval) ;
+    pBssInfo->beaconInterval[1] = (u8_t)((wd->beaconInterval) >> 8) ;
+
+    pBssInfo->capability[0] = wd->sta.capability[0];
+    pBssInfo->capability[1] = wd->sta.capability[1];
+
+    pBssInfo->ssid[0] = ZM_WLAN_EID_SSID;
+    pBssInfo->ssid[1] = wd->sta.ssidLen;
+    zfMemoryCopy((u8_t *)&pBssInfo->ssid[2], (u8_t *)wd->sta.ssid, wd->sta.ssidLen);
+    zfMemoryCopy((u8_t *)&pBssInfo->frameBody[offset], (u8_t *)pBssInfo->ssid,
+                 wd->sta.ssidLen + 2);
+    offset += wd->sta.ssidLen + 2;
+
+    /* support rate */
+
+    /* DS parameter set */
+    pBssInfo->channel = zfChFreqToNum(wd->frequency, NULL);
+    pBssInfo->frequency = wd->frequency;
+    pBssInfo->atimWindow = wd->sta.atimWindow;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+    {
+        u8_t rsn[64]=
+        {
+                    /* Element ID */
+                    0x30,
+                    /* Length */
+                    0x14,
+                    /* Version */
+                    0x01, 0x00,
+                    /* Group Cipher Suite, default=TKIP */
+                    0x00, 0x0f, 0xac, 0x04,
+                    /* Pairwise Cipher Suite Count */
+                    0x01, 0x00,
+                    /* Pairwise Cipher Suite, default=TKIP */
+                    0x00, 0x0f, 0xac, 0x02,
+                    /* Authentication and Key Management Suite Count */
+                    0x01, 0x00,
+                    /* Authentication type, default=PSK */
+                    0x00, 0x0f, 0xac, 0x02,
+                    /* RSN capability */
+                    0x00, 0x00
+        };
+
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, zgWpa2AesOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        // RSN element id
+        pBssInfo->frameBody[offset++] = ZM_WLAN_EID_RSN_IE ;
+
+        // RSN length
+        pBssInfo->frameBody[offset++] = rsn[1] ;
+
+        // RSN information
+        for(i=0; i<rsn[1]; i++)
+        {
+            pBssInfo->frameBody[offset++] = rsn[i+2] ;
+        }
+
+        zfMemoryCopy(pBssInfo->rsnIe, rsn, rsn[1]+2);
+    }
+#endif
+}
+
+void zfIbssConnectNetwork(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo tmpBssInfo;
+    u8_t   macAddr[6], bssid[6], bssNotFound = TRUE;
+    u16_t  i, j=100;
+    u16_t  k;
+    struct zsPartnerNotifyEvent event;
+    u32_t  channelFlags;
+    u16_t  oppositeWepStatus;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* change state to CONNECTING and stop the channel scanning */
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfPowerSavingMgrWakeup(dev);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME STA default. */
+    zfUpdateDefaultQosParameter(dev, 0);
+
+    wd->sta.bProtectionMode = FALSE;
+    zfHpSetSlotTime(dev, 1);
+
+    /* ESS bit off */
+    wd->sta.capability[0] &= ~ZM_BIT_0;
+    /* IBSS bit on */
+    wd->sta.capability[0] |= ZM_BIT_1;
+    /* not not use short slot time */
+    wd->sta.capability[1] &= ~ZM_BIT_2;
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    wd->sta.EnableHT = 0;
+    wd->BandWidth40 = 0;
+    wd->ExtOffset = 0;
+
+    if ( wd->sta.bssList.bssCount )
+    {
+        //Reorder BssList by RSSI--CWYang(+)
+        zfBssInfoReorderList(dev);
+
+        zmw_enter_critical_section(dev);
+
+        pBssInfo = wd->sta.bssList.head;
+
+        for(i=0; i<wd->sta.bssList.bssCount; i++)
+        {
+            // 20070806 #1 Privacy bit
+            if ( pBssInfo->capability[0] & ZM_BIT_4 )
+            { // Privacy Ibss network
+//                zm_debug_msg0("Privacy bit on");
+                oppositeWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+
+                if ( pBssInfo->rsnIe[1] != 0 )
+                {
+                    if ( (pBssInfo->rsnIe[7] == 0x01) || (pBssInfo->rsnIe[7] == 0x05) )
+                    { // WEP-40 & WEP-104
+//                        zm_debug_msg0("WEP40 or WEP104");
+                        oppositeWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+                    }
+                    else if ( pBssInfo->rsnIe[7] == 0x02 )
+                    { // TKIP
+//                        zm_debug_msg0("TKIP");
+                        oppositeWepStatus = ZM_ENCRYPTION_TKIP;
+                    }
+                    else if ( pBssInfo->rsnIe[7] == 0x04 )
+                    { // AES
+//                        zm_debug_msg0("CCMP-AES");
+                        oppositeWepStatus = ZM_ENCRYPTION_AES;
+                    }
+                }
+            }
+            else
+            {
+//                zm_debug_msg0("Privacy bit off");
+                oppositeWepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+            }
+
+            if ( (zfMemoryIsEqual(&(pBssInfo->ssid[2]), wd->sta.ssid,
+                                  wd->sta.ssidLen))&&
+                 (wd->sta.ssidLen == pBssInfo->ssid[1])&&
+                 (oppositeWepStatus == wd->sta.wepStatus) )
+            {
+                /* Check support mode */
+                if (pBssInfo->frequency > 3000) {
+                    if (   (pBssInfo->EnableHT == 1)
+                        || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                    {
+                        channelFlags = CHANNEL_A_HT;
+                        if (pBssInfo->enableHT40 == 1) {
+                            channelFlags |= CHANNEL_HT40;
+                        }
+                    } else {
+                        channelFlags = CHANNEL_A;
+                    }
+                } else {
+                    if (   (pBssInfo->EnableHT == 1)
+                        || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                    {
+                        channelFlags = CHANNEL_G_HT;
+                        if(pBssInfo->enableHT40 == 1) {
+                            channelFlags |= CHANNEL_HT40;
+                        }
+                    } else {
+                        if (pBssInfo->extSupportedRates[1] == 0) {
+                            channelFlags = CHANNEL_B;
+                        } else {
+                            channelFlags = CHANNEL_G;
+                        }
+                    }
+                }
+
+                if (   ((channelFlags == CHANNEL_B) && (wd->connectMode & ZM_BIT_0))
+                    || ((channelFlags == CHANNEL_G) && (wd->connectMode & ZM_BIT_1))
+                    || ((channelFlags == CHANNEL_A) && (wd->connectMode & ZM_BIT_2))
+                    || ((channelFlags & CHANNEL_HT20) && (wd->connectMode & ZM_BIT_3)) )
+                {
+                    pBssInfo = pBssInfo->next;
+                    continue;
+                }
+
+                /* Bypass DFS channel */
+                if (zfHpIsDfsChannelNCS(dev, pBssInfo->frequency))
+                {
+                    zm_debug_msg0("Bypass DFS channel");
+                    continue;
+                }
+
+                /* check IBSS bit */
+                if ( pBssInfo->capability[0] & ZM_BIT_1 )
+                {
+                    /* may check timestamp here */
+                    j = i;
+                    break;
+                }
+            }
+
+            pBssInfo = pBssInfo->next;
+        }
+
+        if ((j < wd->sta.bssList.bssCount) && (pBssInfo != NULL))
+        {
+            zfwMemoryCopy((u8_t*)&tmpBssInfo, (u8_t*)(pBssInfo), sizeof(struct zsBssInfo));
+            pBssInfo = &tmpBssInfo;
+        }
+        else
+        {
+            pBssInfo = NULL;
+        }
+
+        zmw_leave_critical_section(dev);
+
+        //if ( j < wd->sta.bssList.bssCount )
+        if (pBssInfo != NULL)
+        {
+            int res;
+
+            zm_debug_msg0("IBSS found");
+
+            /* Found IBSS, reset bssNotFoundCount */
+            zmw_enter_critical_section(dev);
+            wd->sta.bssNotFoundCount = 0;
+            zmw_leave_critical_section(dev);
+
+            bssNotFound = FALSE;
+            wd->sta.atimWindow = pBssInfo->atimWindow;
+            wd->frequency = pBssInfo->frequency;
+            //wd->sta.flagFreqChanging = 1;
+            zfCoreSetFrequency(dev, wd->frequency);
+            zfUpdateBssid(dev, pBssInfo->bssid);
+            zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_ZERO);
+            zfUpdateSupportRate(dev, pBssInfo->supportedRates);
+            zfUpdateSupportRate(dev, pBssInfo->extSupportedRates);
+            wd->beaconInterval = pBssInfo->beaconInterval[0] +
+                                 (((u16_t) pBssInfo->beaconInterval[1]) << 8);
+
+            if (wd->beaconInterval == 0)
+            {
+                wd->beaconInterval = 100;
+            }
+
+            /* rsn information element */
+            if ( pBssInfo->rsnIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.rsnIe, pBssInfo->rsnIe,
+                             pBssInfo->rsnIe[1]+2);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                /* If not use RSNA , run traditional */
+                zmw_enter_critical_section(dev);
+                wd->sta.ibssWpa2Psk = 1;
+                zmw_leave_critical_section(dev);
+#endif
+            }
+            else
+            {
+                wd->sta.rsnIe[1] = 0;
+            }
+
+            /* privacy bit */
+            if ( pBssInfo->capability[0] & ZM_BIT_4 )
+            {
+                wd->sta.capability[0] |= ZM_BIT_4;
+            }
+            else
+            {
+                wd->sta.capability[0] &= ~ZM_BIT_4;
+            }
+
+            /* preamble type */
+            wd->preambleTypeInUsed = wd->preambleType;
+            if ( wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_AUTO )
+            {
+                if (pBssInfo->capability[0] & ZM_BIT_5)
+                {
+                    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+                }
+                else
+                {
+                    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_LONG;
+                }
+            }
+
+            if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+            {
+                wd->sta.capability[0] &= ~ZM_BIT_5;
+            }
+            else
+            {
+                wd->sta.capability[0] |= ZM_BIT_5;
+            }
+
+            wd->sta.beaconFrameBodySize = pBssInfo->frameBodysize + 12;
+
+            if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+            {
+                wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+            }
+
+            for (k=0; k<8; k++)
+            {
+                wd->sta.beaconFrameBody[k] = pBssInfo->timeStamp[k];
+            }
+            wd->sta.beaconFrameBody[8] = pBssInfo->beaconInterval[0];
+            wd->sta.beaconFrameBody[9] = pBssInfo->beaconInterval[1];
+            wd->sta.beaconFrameBody[10] = pBssInfo->capability[0];
+            wd->sta.beaconFrameBody[11] = pBssInfo->capability[1];
+            //for (k=12; k<wd->sta.beaconFrameBodySize; k++)
+            for (k=0; k<pBssInfo->frameBodysize; k++)
+            {
+                wd->sta.beaconFrameBody[k+12] = pBssInfo->frameBody[k];
+            }
+
+            zmw_enter_critical_section(dev);
+            res = zfStaSetOppositeInfoFromBSSInfo(dev, pBssInfo);
+            if ( res == 0 )
+            {
+                zfMemoryCopy(event.bssid, (u8_t *)(pBssInfo->bssid), 6);
+                zfMemoryCopy(event.peerMacAddr, (u8_t *)(pBssInfo->macaddr), 6);
+            }
+            zmw_leave_critical_section(dev);
+
+            //zfwIbssPartnerNotify(dev, 1, &event);
+            goto connect_done;
+        }
+    }
+
+    /* IBSS not found */
+    if ( bssNotFound )
+    {
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        u16_t offset ;
+#endif
+        if ( wd->sta.ibssJoinOnly )
+        {
+            zm_debug_msg0("IBSS join only...retry...");
+            goto retry_ibss;
+        }
+
+        if(wd->sta.bssNotFoundCount<2)
+        {
+            zmw_enter_critical_section(dev);
+            zm_debug_msg1("IBSS not found, do sitesurvey!!  bssNotFoundCount=", wd->sta.bssNotFoundCount);
+            wd->sta.bssNotFoundCount++;
+            zmw_leave_critical_section(dev);
+            goto retry_ibss;
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            /* Fail IBSS found, TODO create IBSS */
+            wd->sta.bssNotFoundCount = 0;
+            zmw_leave_critical_section(dev);
+        }
+
+
+        if (zfHpIsDfsChannel(dev, wd->frequency))
+        {
+            wd->frequency = zfHpFindFirstNonDfsChannel(dev, wd->frequency > 3000);
+        }
+
+        if( wd->ws.autoSetFrequency == 0 )
+        { /* Auto set frequency */
+            zm_debug_msg1("Create Ad Hoc Network Band ", wd->ws.adhocMode);
+            wd->frequency = zfFindCleanFrequency(dev, wd->ws.adhocMode);
+            wd->ws.autoSetFrequency = 0xff;
+        }
+        zm_debug_msg1("IBSS not found, created one in channel ", wd->frequency);
+
+        wd->sta.ibssBssIsCreator = 1;
+
+        //wd->sta.flagFreqChanging = 1;
+        zfCoreSetFrequency(dev, wd->frequency);
+        if (wd->sta.bDesiredBssid == TRUE)
+        {
+            for (k=0; k<6; k++)
+            {
+                bssid[k] = wd->sta.desiredBssid[k];
+            }
+        }
+        else
+        {
+            #if 1
+            macAddr[0] = (wd->macAddr[0] & 0xff);
+            macAddr[1] = (wd->macAddr[0] >> 8);
+            macAddr[2] = (wd->macAddr[1] & 0xff);
+            macAddr[3] = (wd->macAddr[1] >> 8);
+            macAddr[4] = (wd->macAddr[2] & 0xff);
+            macAddr[5] = (wd->macAddr[2] >> 8);
+            zfGenerateRandomBSSID(dev, (u8_t *)wd->macAddr, (u8_t *)bssid);
+            #else
+            for (k=0; k<6; k++)
+            {
+                bssid[k] = (u8_t) zfGetRandomNumber(dev, 0);
+            }
+            bssid[0] &= ~ZM_BIT_0;
+            bssid[0] |= ZM_BIT_1;
+            #endif
+        }
+
+        zfUpdateBssid(dev, bssid);
+        //wd->sta.atimWindow = 0x0a;
+
+        /* rate information */
+        if(wd->frequency <= ZM_CH_G_14)  // 2.4 GHz  b+g
+        {
+            if ( wd->wfc.bIbssGMode
+                 && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+            {
+                zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_AG);
+            }
+            else
+            {
+                zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_B);
+            }
+        } else {
+            zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_AG);
+        }
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED )
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_4;
+        }
+        else
+        {
+            wd->sta.capability[0] |= ZM_BIT_4;
+        }
+
+        wd->preambleTypeInUsed = wd->preambleType;
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_5;
+        }
+        else
+        {
+            wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+            wd->sta.capability[0] |= ZM_BIT_5;
+        }
+
+        zfIBSSSetupBssDesc(dev);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+
+        // 20070411 Add WPA2PSK information to its IBSS network !!!
+        offset = 0 ;
+
+        /* timestamp */
+        offset += 8 ;
+
+        /* beacon interval */
+        wd->sta.beaconFrameBody[offset++] = (u8_t)(wd->beaconInterval) ;
+        wd->sta.beaconFrameBody[offset++] = (u8_t)((wd->beaconInterval) >> 8) ;
+
+        /* capability information */
+        wd->sta.beaconFrameBody[offset++] = wd->sta.capability[0] ;
+        wd->sta.beaconFrameBody[offset++] = wd->sta.capability[1] ;
+        #if 0
+        /* ssid */
+        // ssid element id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_SSID ;
+        // ssid length
+        wd->sta.beaconFrameBody[offset++] = wd->sta.ssidLen ;
+        // ssid information
+        for(i=0; i<wd->sta.ssidLen; i++)
+        {
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ssid[i] ;
+        }
+
+        /* support rate */
+        rateSet = ZM_RATE_SET_CCK ;
+        if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+        {
+            offset += 0 ;
+        }
+        else
+        {
+            // support rate element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_SUPPORT_RATE ;
+
+            // support rate length
+            lenOffset = offset++;
+
+            // support rate information
+            for (i=0; i<4; i++)
+            {
+                if ((wd->bRate & (0x1<<i)) == (0x1<<i))
+                {
+                    wd->sta.beaconFrameBody[offset++] =
+                	    zg11bRateTbl[i]+((wd->bRateBasic & (0x1<<i))<<(7-i)) ;
+                    len++;
+                }
+            }
+
+            // support rate length
+            wd->sta.beaconFrameBody[lenOffset] = len ;
+        }
+
+        /* DS parameter set */
+        // DS parameter set elemet id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_DS ;
+
+        // DS parameter set length
+        wd->sta.beaconFrameBody[offset++] = 1 ;
+
+        // DS parameter set information
+        wd->sta.beaconFrameBody[offset++] =
+         	zfChFreqToNum(wd->frequency, NULL) ;
+
+        /* IBSS parameter set */
+        // IBSS parameter set element id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_IBSS ;
+
+        // IBSS parameter set length
+        wd->sta.beaconFrameBody[offset++] = 2 ;
+
+        // IBSS parameter set information
+        wd->sta.beaconFrameBody[offset] = wd->sta.atimWindow ;
+        offset += 2 ;
+
+        /* ERP Information and Extended Supported Rates */
+        if ( wd->wfc.bIbssGMode
+             && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+        {
+            /* ERP Information */
+            wd->erpElement = 0;
+            // ERP element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_ERP ;
+
+            // ERP length
+            wd->sta.beaconFrameBody[offset++] = 1 ;
+
+            // ERP information
+            wd->sta.beaconFrameBody[offset++] = wd->erpElement ;
+
+            /* Extended Supported Rates */
+            if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+            {
+                offset += 0 ;
+            }
+            else
+            {
+                len = 0 ;
+
+                // Extended Supported Rates element id
+                wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_EXTENDED_RATE ;
+
+                // Extended Supported Rates length
+                lenOffset = offset++ ;
+
+                // Extended Supported Rates information
+                for (i=0; i<8; i++)
+                {
+                    if ((wd->gRate & (0x1<<i)) == (0x1<<i))
+                    {
+                        wd->sta.beaconFrameBody[offset++] =
+                                     zg11gRateTbl[i]+((wd->gRateBasic & (0x1<<i))<<(7-i));
+                        len++;
+                    }
+                }
+
+                // extended support rate length
+            	  wd->sta.beaconFrameBody[lenOffset] = len ;
+            }
+        }
+        #endif
+
+        /* RSN : important information influence the result of creating an IBSS network */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            u8_t frameType = ZM_WLAN_FRAME_TYPE_AUTH ;
+            u8_t    rsn[64]=
+            {
+                        /* Element ID */
+                        0x30,
+                        /* Length */
+                        0x14,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x04,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* RSN capability */
+                        0x00, 0x00
+            };
+
+            /* Overwrite Group Cipher Suite by AP's setting */
+            zfMemoryCopy(rsn+4, zgWpa2AesOui, 4);
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+            {
+                /* Overwrite Pairwise Cipher Suite by AES */
+                zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+            }
+
+            // RSN element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_RSN_IE ;
+
+            // RSN length
+            wd->sta.beaconFrameBody[offset++] = rsn[1] ;
+
+            // RSN information
+            for(i=0; i<rsn[1]; i++)
+                wd->sta.beaconFrameBody[offset++] = rsn[i+2] ;
+
+            zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            /* If not use RSNA , run traditional */
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssWpa2Psk = 1;
+            zmw_leave_critical_section(dev);
+#endif
+        }
+
+        #if 0
+        /* HT Capabilities Info */
+        {
+            u8_t OUI[3] = { 0x0 , 0x90 , 0x4C } ;
+
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_WPA_IE ;
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Data.Length + 4 ;
+
+            for (i = 0; i < 3; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = OUI[i] ;
+            }
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Data.ElementID ;
+
+            for (i = 0; i < 26; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Byte[i+2] ;
+            }
+        }
+
+        /* Extended HT Capabilities Info */
+        {
+            u8_t OUI[3] = { 0x0 , 0x90 , 0x4C } ;
+
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_WPA_IE ;
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Data.Length + 4 ;
+
+            for (i = 0; i < 3; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = OUI[i] ;
+            }
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Data.ElementID ;
+
+            for (i = 0; i < 22; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Byte[i+2] ;
+            }
+        }
+        #endif
+
+        wd->sta.beaconFrameBodySize = offset ;
+
+        if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+        {
+            wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+        }
+
+        // 20070416 Let Create IBSS network could enter the zfwIbssPartnerNotify function
+        // bssNotFound = FALSE ;
+
+        printk("The capability info 1 = %02x\n", wd->sta.capability[0]) ;
+        printk("The capability info 2 = %02x\n", wd->sta.capability[1]) ;
+        for(k=0; k<wd->sta.beaconFrameBodySize; k++)
+        {
+	     printk("%02x ", wd->sta.beaconFrameBody[k]) ;
+        }
+        #if 0
+        zmw_enter_critical_section(dev);
+        zfMemoryCopy(event.bssid, (u8_t *)bssid, 6);
+        zfMemoryCopy(event.peerMacAddr, (u8_t *)wd->macAddr, 6);
+        zmw_leave_critical_section(dev);
+        #endif
+#endif
+
+        //zmw_enter_critical_section(dev);
+        //wd->sta.ibssPartnerStatus = ZM_IBSS_PARTNER_LOST;
+        //zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        wd->sta.ibssBssIsCreator = 0;
+    }
+
+connect_done:
+    zfHpEnableBeacon(dev, ZM_MODE_IBSS, wd->beaconInterval, wd->dtim, (u8_t)wd->sta.atimWindow);
+    zfStaSendBeacon(dev); // Refresh Beacon content for ZD1211B HalPlus
+    zfHpSetAtimWindow(dev, wd->sta.atimWindow);
+
+    // Start the IBSS timer to monitor for new stations
+    zmw_enter_critical_section(dev);
+    zfTimerSchedule(dev, ZM_EVENT_IBSS_MONITOR, ZM_TICK_IBSS_MONITOR);
+    zmw_leave_critical_section(dev);
+
+
+    if (wd->zfcbConnectNotify != NULL)
+    {
+        wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+    }
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+
+#ifdef ZM_ENABLE_IBSS_DELAYED_JOIN_INDICATION
+    if ( !bssNotFound )
+    {
+        wd->sta.ibssDelayedInd = 1;
+        zfMemoryCopy((u8_t *)&wd->sta.ibssDelayedIndEvent, (u8_t *)&event, sizeof(struct zsPartnerNotifyEvent));
+    }
+#else
+    if ( !bssNotFound )
+    {
+        if (wd->zfcbIbssPartnerNotify != NULL)
+        {
+            wd->zfcbIbssPartnerNotify(dev, 1, &event);
+        }
+    }
+#endif
+
+    return;
+
+retry_ibss:
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND, wd->sta.bssid, 0);
+    return;
+}
+
+void zfStaProcessAtim(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("Receiving Atim window notification");
+
+    wd->sta.recvAtim = 1;
+}
+
+static struct zsBssInfo* zfInfraFindAPToConnect(zdev_t* dev,
+        struct zsBssInfo* candidateBss)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo* pNowBssInfo=NULL;
+    u16_t i;
+    u16_t ret, apWepStatus;
+    u32_t k;
+    u32_t channelFlags;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+
+    for(i=0; i<wd->sta.bssList.bssCount; i++)
+    {
+        if ( pBssInfo->capability[0] & ZM_BIT_4 )
+        {
+            apWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+        }
+        else
+        {
+            apWepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+        }
+
+        if ( ((zfMemoryIsEqual(&(pBssInfo->ssid[2]), wd->sta.ssid,
+                               wd->sta.ssidLen))&&
+              (wd->sta.ssidLen == pBssInfo->ssid[1]))||
+             ((wd->sta.ssidLen == 0)&&
+               /* connect to any BSS: AP's ans STA's WEP status must match */
+              (wd->sta.wepStatus == apWepStatus )&&
+              (pBssInfo->securityType != ZM_SECURITY_TYPE_WPA) ))
+        {
+            if ( wd->sta.ssidLen == 0 )
+            {
+                zm_debug_msg0("ANY BSS found");
+            }
+
+            if ( ((wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED && apWepStatus == ZM_ENCRYPTION_WEP_ENABLED) ||
+                 (wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED &&
+                 (apWepStatus == ZM_ENCRYPTION_WEP_DISABLED && wd->sta.dropUnencryptedPkts == 1))) &&
+                 (wd->sta.authMode >= ZM_AUTH_MODE_OPEN && wd->sta.authMode <= ZM_AUTH_MODE_AUTO) )
+            {
+                zm_debug_msg0("Privacy policy is inconsistent");
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* for WPA negative test */
+            if ( !zfCheckAuthentication(dev, pBssInfo) )
+            {
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* Check bssid */
+            if (wd->sta.bDesiredBssid == TRUE)
+            {
+                for (k=0; k<6; k++)
+                {
+                    if (wd->sta.desiredBssid[k] != pBssInfo->bssid[k])
+                    {
+                        zm_msg0_mm(ZM_LV_1, "desired bssid not matched 1");
+                        break;
+                    }
+                }
+
+                if (k != 6)
+                {
+                    zm_msg0_mm(ZM_LV_1, "desired bssid not matched 2");
+                    pBssInfo = pBssInfo->next;
+                    continue;
+                }
+            }
+
+            /* Check support mode */
+            if (pBssInfo->frequency > 3000) {
+                if (   (pBssInfo->EnableHT == 1)
+                    || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                {
+                    channelFlags = CHANNEL_A_HT;
+                    if (pBssInfo->enableHT40 == 1) {
+                        channelFlags |= CHANNEL_HT40;
+                    }
+                } else {
+                    channelFlags = CHANNEL_A;
+                }
+            } else {
+                if (   (pBssInfo->EnableHT == 1)
+                    || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                {
+                    channelFlags = CHANNEL_G_HT;
+                    if(pBssInfo->enableHT40 == 1) {
+                        channelFlags |= CHANNEL_HT40;
+                    }
+                } else {
+                    if (pBssInfo->extSupportedRates[1] == 0) {
+                        channelFlags = CHANNEL_B;
+                    } else {
+                        channelFlags = CHANNEL_G;
+                    }
+                }
+            }
+
+            if (   ((channelFlags == CHANNEL_B) && (wd->connectMode & ZM_BIT_0))
+                || ((channelFlags == CHANNEL_G) && (wd->connectMode & ZM_BIT_1))
+                || ((channelFlags == CHANNEL_A) && (wd->connectMode & ZM_BIT_2))
+                || ((channelFlags & CHANNEL_HT20) && (wd->connectMode & ZM_BIT_3)) )
+            {
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* Skip if AP in blocking list */
+            if ((ret = zfStaIsApInBlockingList(dev, pBssInfo->bssid)) == TRUE)
+            {
+                zm_msg0_mm(ZM_LV_0, "Candidate AP in blocking List, skip if there's stilla choice!");
+                pNowBssInfo = pBssInfo;
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            if ( pBssInfo->capability[0] & ZM_BIT_0 )  // check if infra-BSS
+            {
+                    pNowBssInfo = pBssInfo;
+                    wd->sta.apWmeCapability = pBssInfo->wmeSupport;
+
+
+                    goto done;
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+done:
+    if (pNowBssInfo != NULL)
+    {
+        zfwMemoryCopy((void*)candidateBss, (void*)pNowBssInfo, sizeof(struct zsBssInfo));
+        pNowBssInfo = candidateBss;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return pNowBssInfo;
+}
+
+
+void zfInfraConnectNetwork(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo* pNowBssInfo=NULL;
+    struct zsBssInfo candidateBss;
+    //u16_t i, j=100, quality=10000;
+    //u8_t ret=FALSE, apWepStatus;
+    u8_t ret=FALSE;
+    u16_t k;
+    u8_t density = ZM_MPDU_DENSITY_NONE;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* Reset bssNotFoundCount for Ad-Hoc:IBSS */
+    /* Need review : IbssConn -> InfraConn -> IbssConn etc, flag/counter reset? */
+    zmw_enter_critical_section(dev);
+    wd->sta.bssNotFoundCount = 0;
+    zmw_leave_critical_section(dev);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME STA default. */
+    zfUpdateDefaultQosParameter(dev, 0);
+
+    zfStaRefreshBlockList(dev, 0);
+
+    /* change state to CONNECTING and stop the channel scanning */
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfPowerSavingMgrWakeup(dev);
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    zfQueueFlush(dev, wd->sta.uapsdQ);
+
+    wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+
+    //Reorder BssList by RSSI--CWYang(+)
+    zfBssInfoReorderList(dev);
+
+    pNowBssInfo = zfInfraFindAPToConnect(dev, &candidateBss);
+
+	if (wd->sta.SWEncryptEnable != 0)
+	{
+	    if (wd->sta.bSafeMode == 0)
+	    {
+		    zfStaDisableSWEncryption(dev);//Quickly reboot
+	    }
+	}
+    if ( pNowBssInfo != NULL )
+    {
+        //zm_assert(pNowBssInfo != NULL);
+
+        pBssInfo = pNowBssInfo;
+        wd->sta.ssidLen = pBssInfo->ssid[1];
+        zfMemoryCopy(wd->sta.ssid, &(pBssInfo->ssid[2]), pBssInfo->ssid[1]);
+        wd->frequency = pBssInfo->frequency;
+        //wd->sta.flagFreqChanging = 1;
+
+        //zfCoreSetFrequency(dev, wd->frequency);
+        zfUpdateBssid(dev, pBssInfo->bssid);
+        zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_ZERO);
+        zfUpdateSupportRate(dev, pBssInfo->supportedRates);
+        zfUpdateSupportRate(dev, pBssInfo->extSupportedRates);
+
+        wd->beaconInterval = pBssInfo->beaconInterval[0] +
+                             (((u16_t) pBssInfo->beaconInterval[1]) << 8);
+        if (wd->beaconInterval == 0)
+        {
+            wd->beaconInterval = 100;
+        }
+
+        /* ESS bit on */
+        wd->sta.capability[0] |= ZM_BIT_0;
+        /* IBSS bit off */
+        wd->sta.capability[0] &= ~ZM_BIT_1;
+
+        /* 11n AP flag */
+        wd->sta.EnableHT = pBssInfo->EnableHT;
+        wd->sta.SG40 = pBssInfo->SG40;
+#ifdef ZM_ENABLE_CENC
+        if ( pBssInfo->securityType == ZM_SECURITY_TYPE_CENC )
+        {
+            wd->sta.wmeEnabled = 0; //Disable WMM in CENC
+            cencInit(dev);
+            cencSetCENCMode(dev, NdisCENC_PSK);
+            wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+            /* CENC */
+            if ( pBssInfo->cencIe[1] != 0 )
+            {
+                //wd->sta.wepStatus = ZM_ENCRYPTION_CENC;
+                //wd->sta.encryMode = ZM_CENC;
+                zfwCencHandleBeaconProbrespon(dev, (u8_t *)&pBssInfo->cencIe,
+                        (u8_t *)&pBssInfo->ssid, (u8_t *)&pBssInfo->macaddr);
+                zfMemoryCopy(wd->sta.cencIe, pBssInfo->cencIe,
+                        pBssInfo->cencIe[1]+2);
+            }
+            else
+            {
+                wd->sta.cencIe[1] = 0;
+            }
+        }
+#endif //ZM_ENABLE_CENC
+        if ( pBssInfo->securityType == ZM_SECURITY_TYPE_WPA )
+        {
+            wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_TKIP )
+            {
+                wd->sta.encryMode = ZM_TKIP;
+
+                /* Turn on software encryption/decryption for TKIP */
+                if (wd->sta.EnableHT == 1)
+                {
+                    zfStaEnableSWEncryption(dev, (ZM_SW_TKIP_ENCRY_EN|ZM_SW_TKIP_DECRY_EN));
+                }
+
+                /* Do not support TKIP in 11n mode */
+                //wd->sta.EnableHT = 0;
+                //pBssInfo->enableHT40 = 0;
+            }
+            else if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+            {
+                wd->sta.encryMode = ZM_AES;
+
+                /* If AP supports HT mode */
+                if (wd->sta.EnableHT)
+                {
+                    /* Set MPDU density to 8 us*/
+                    density = ZM_MPDU_DENSITY_8US;
+                }
+            }
+
+            if ( pBssInfo->wpaIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.wpaIe, pBssInfo->wpaIe,
+                             pBssInfo->wpaIe[1]+2);
+            }
+            else
+            {
+                wd->sta.wpaIe[1] = 0;
+            }
+
+            if ( pBssInfo->rsnIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.rsnIe, pBssInfo->rsnIe,
+                             pBssInfo->rsnIe[1]+2);
+            }
+            else
+            {
+                wd->sta.rsnIe[1] = 0;
+            }
+        }
+
+
+
+        /* check preamble bit */
+        wd->preambleTypeInUsed = wd->preambleType;
+        if ( wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_AUTO )
+        {
+            if (pBssInfo->capability[0] & ZM_BIT_5)
+            {
+                wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+            }
+            else
+            {
+                wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_LONG;
+            }
+        }
+
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_5;
+        }
+        else
+        {
+            wd->sta.capability[0] |= ZM_BIT_5;
+        }
+
+        /* check 802.11n 40MHz Setting */
+        if ((pBssInfo->enableHT40 == 1) &&
+            ((pBssInfo->extChOffset == 1) || (pBssInfo->extChOffset == 3)))
+        {
+            wd->BandWidth40 = pBssInfo->enableHT40;
+            wd->ExtOffset = pBssInfo->extChOffset;
+        }
+        else
+        {
+            wd->BandWidth40 = 0;
+            wd->ExtOffset = 0;
+        }
+
+        /* check 802.11H support bit */
+
+        /* check Owl Ap */
+        if ( pBssInfo->athOwlAp & ZM_BIT_0 )
+        {
+            /* In this function, FW retry will be enable, ZM_MAC_REG_RETRY_MAX
+               will be set to 0.
+             */
+            zfHpDisableHwRetry(dev);
+            wd->sta.athOwlAp = 1;
+            /* Set MPDU density to 8 us*/
+            density = ZM_MPDU_DENSITY_8US;
+        }
+        else
+        {
+            /* In this function, FW retry will be disable, ZM_MAC_REG_RETRY_MAX
+               will be set to 3.
+             */
+            zfHpEnableHwRetry(dev);
+            wd->sta.athOwlAp = 0;
+        }
+        wd->reorder = 1;
+
+        /* Set MPDU density */
+        zfHpSetMPDUDensity(dev, density);
+
+        /* check short slot time bit */
+        if ( pBssInfo->capability[1] & ZM_BIT_2 )
+        {
+            wd->sta.capability[1] |= ZM_BIT_2;
+        }
+
+        if ( pBssInfo->erp & ZM_BIT_1 )
+        {
+            //zm_debug_msg0("protection mode on");
+            wd->sta.bProtectionMode = TRUE;
+            zfHpSetSlotTime(dev, 0);
+        }
+        else
+        {
+            //zm_debug_msg0("protection mode off");
+            wd->sta.bProtectionMode = FALSE;
+            zfHpSetSlotTime(dev, 1);
+        }
+
+        if (pBssInfo->marvelAp == 1)
+        {
+            wd->sta.enableDrvBA = 0;
+            /*
+             * 8701 : NetGear 3500 (MARVELL)
+             * Downlink issue : set slottime to 20.
+             */
+            zfHpSetSlotTimeRegister(dev, 0);
+        }
+        else
+        {
+            wd->sta.enableDrvBA = 1;
+
+            /*
+             * This is not good for here do reset slot time.
+             * I think it should reset when leave MARVELL ap
+             * or enter disconnect state etc.
+             */
+            zfHpSetSlotTimeRegister(dev, 1);
+        }
+
+        //Store probe response frame body, for VISTA only
+        wd->sta.beaconFrameBodySize = pBssInfo->frameBodysize + 12;
+        if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+        {
+            wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+        }
+        for (k=0; k<8; k++)
+        {
+            wd->sta.beaconFrameBody[k] = pBssInfo->timeStamp[k];
+        }
+        wd->sta.beaconFrameBody[8] = pBssInfo->beaconInterval[0];
+        wd->sta.beaconFrameBody[9] = pBssInfo->beaconInterval[1];
+        wd->sta.beaconFrameBody[10] = pBssInfo->capability[0];
+        wd->sta.beaconFrameBody[11] = pBssInfo->capability[1];
+        for (k=0; k<(wd->sta.beaconFrameBodySize - 12); k++)
+        {
+            wd->sta.beaconFrameBody[k+12] = pBssInfo->frameBody[k];
+        }
+
+        if ( ( pBssInfo->capability[0] & ZM_BIT_4 )&&
+             (( wd->sta.authMode == ZM_AUTH_MODE_OPEN )||
+              ( wd->sta.authMode == ZM_AUTH_MODE_SHARED_KEY)||
+              (wd->sta.authMode == ZM_AUTH_MODE_AUTO)) )
+        {   /* privacy enabled */
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED )
+            {
+                zm_debug_msg0("Adapter is no WEP, try to connect to WEP AP");
+                ret = FALSE;
+            }
+
+            /* Do not support WEP in 11n mode */
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+            {
+                /* Turn on software encryption/decryption for WEP */
+                if (wd->sta.EnableHT == 1)
+                {
+                    zfStaEnableSWEncryption(dev, (ZM_SW_WEP_ENCRY_EN|ZM_SW_WEP_DECRY_EN));
+                }
+
+                //wd->sta.EnableHT = 0;
+                //wd->BandWidth40 = 0;
+                //wd->ExtOffset = 0;
+            }
+
+            wd->sta.capability[0] |= ZM_BIT_4;
+
+            if ( wd->sta.authMode == ZM_AUTH_MODE_AUTO )
+            { /* Try to use open and shared-key authehtication alternatively */
+                if ( (wd->sta.connectTimeoutCount % 2) == 0 )
+                    wd->sta.bIsSharedKey = 0;
+                else
+                    wd->sta.bIsSharedKey = 1;
+            }
+            else if ( wd->sta.authMode != ZM_AUTH_MODE_SHARED_KEY )
+            {   /* open  or auto */
+                //zfStaStartConnect(dev, 0);
+                wd->sta.bIsSharedKey = 0;
+            }
+            else if ( wd->sta.authMode != ZM_AUTH_MODE_OPEN )
+            {   /* shared key */
+                //zfStaStartConnect(dev, 1) ;
+                wd->sta.bIsSharedKey = 1;
+            }
+        }
+        else
+        {
+            if ( (pBssInfo->securityType == ZM_SECURITY_TYPE_WPA)||
+                 (pBssInfo->capability[0] & ZM_BIT_4) )
+            {
+                wd->sta.capability[0] |= ZM_BIT_4;
+                /* initialize WPA related parameters */
+            }
+            else
+            {
+                wd->sta.capability[0] &= (~ZM_BIT_4);
+            }
+
+            /* authentication with open system */
+            //zfStaStartConnect(dev, 0);
+            wd->sta.bIsSharedKey = 0;
+        }
+
+        /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+        /*
+        if ( (pBssInfo->broadcomHTAp == 1)
+             && (wd->sta.SWEncryptEnable != 0) )
+        {
+            zfHpSetTTSIFSTime(dev, 0xa);
+        }
+        else
+        {
+            zfHpSetTTSIFSTime(dev, 0x8);
+        }
+        */
+    }
+    else
+    {
+        zm_debug_msg0("Desired SSID not found");
+        goto zlConnectFailed;
+    }
+
+
+    zfCoreSetFrequencyV2(dev, wd->frequency, zfStaStartConnectCb);
+    return;
+
+zlConnectFailed:
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND, wd->sta.bssid, 0);
+    return;
+}
+
+u8_t zfCheckWPAAuth(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    u8_t   ret=TRUE;
+    u8_t   pmkCount;
+    u8_t   i;
+    u16_t   encAlgoType = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.wepStatus == ZM_ENCRYPTION_TKIP )
+    {
+        encAlgoType = ZM_TKIP;
+    }
+    else if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+    {
+        encAlgoType = ZM_AES;
+    }
+
+    switch(wd->sta.authMode)
+    {
+        case ZM_AUTH_MODE_WPA:
+        case ZM_AUTH_MODE_WPAPSK:
+            if ( pBssInfo->wpaIe[1] == 0 )
+            {
+                ret = FALSE;
+                break;
+            }
+
+            pmkCount = pBssInfo->wpaIe[12];
+            for(i=0; i < pmkCount; i++)
+            {
+                if ( pBssInfo->wpaIe[17 + 4*i] == encAlgoType )
+                {
+                    ret = TRUE;
+                    goto done;
+                }
+            }
+
+            ret = FALSE;
+            break;
+
+        case ZM_AUTH_MODE_WPA2:
+        case ZM_AUTH_MODE_WPA2PSK:
+            if ( pBssInfo->rsnIe[1] == 0 )
+            {
+                ret = FALSE;
+                break;
+            }
+
+            pmkCount = pBssInfo->rsnIe[8];
+            for(i=0; i < pmkCount; i++)
+            {
+                if ( pBssInfo->rsnIe[13 + 4*i] == encAlgoType )
+                {
+                    ret = TRUE;
+                    goto done;
+                }
+            }
+
+            ret = FALSE;
+            break;
+    }
+
+done:
+    return ret;
+}
+
+u8_t zfCheckAuthentication(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    u8_t   ret=TRUE;
+    u16_t  encAlgoType;
+    u16_t UnicastCipherNum;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Connecting to ANY has been checked */
+    if ( wd->sta.ssidLen == 0 )
+    {
+        return ret;
+    }
+
+
+	switch(wd->sta.authMode)
+	//switch(wd->ws.authMode)//Quickly reboot
+    {
+        case ZM_AUTH_MODE_WPA_AUTO:
+        case ZM_AUTH_MODE_WPAPSK_AUTO:
+            encAlgoType = 0;
+            if(pBssInfo->rsnIe[1] != 0)
+            {
+                UnicastCipherNum = (pBssInfo->rsnIe[8]) +
+                                   (pBssInfo->rsnIe[9] << 8);
+
+                /* If there is only one unicast cipher */
+                if (UnicastCipherNum == 1)
+                {
+                    encAlgoType = pBssInfo->rsnIe[13];
+                    //encAlgoType = pBssInfo->rsnIe[7];
+                }
+                else
+                {
+                    u16_t ii;
+                    u16_t desiredCipher = 0;
+                    u16_t IEOffSet = 13;
+
+                    /* Enumerate all the supported unicast cipher */
+                    for (ii = 0; ii < UnicastCipherNum; ii++)
+                    {
+                        if (pBssInfo->rsnIe[IEOffSet+ii*4] > desiredCipher)
+                        {
+                            desiredCipher = pBssInfo->rsnIe[IEOffSet+ii*4];
+                        }
+                    }
+
+                    encAlgoType = desiredCipher;
+                }
+
+                if ( encAlgoType == 0x02 )
+                {
+    			    wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+
+    			    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2PSK;
+                    }
+                }
+                else if ( encAlgoType == 0x04 )
+                {
+                    wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2PSK;
+                    }
+                }
+                else
+                {
+                    ret = FALSE;
+                }
+            }
+            else if(pBssInfo->wpaIe[1] != 0)
+            {
+                UnicastCipherNum = (pBssInfo->wpaIe[12]) +
+                                   (pBssInfo->wpaIe[13] << 8);
+
+                /* If there is only one unicast cipher */
+                if (UnicastCipherNum == 1)
+                {
+                    encAlgoType = pBssInfo->wpaIe[17];
+                    //encAlgoType = pBssInfo->wpaIe[11];
+                }
+                else
+                {
+                    u16_t ii;
+                    u16_t desiredCipher = 0;
+                    u16_t IEOffSet = 17;
+
+                    /* Enumerate all the supported unicast cipher */
+                    for (ii = 0; ii < UnicastCipherNum; ii++)
+                    {
+                        if (pBssInfo->wpaIe[IEOffSet+ii*4] > desiredCipher)
+                        {
+                            desiredCipher = pBssInfo->wpaIe[IEOffSet+ii*4];
+                        }
+                    }
+
+                    encAlgoType = desiredCipher;
+                }
+
+                if ( encAlgoType == 0x02 )
+                {
+    			    wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+
+    			    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPAPSK;
+                    }
+                }
+                else if ( encAlgoType == 0x04 )
+                {
+                    wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPAPSK;
+                    }
+                }
+                else
+                {
+                    ret = FALSE;
+                }
+
+
+            }
+            else
+            {
+                ret = FALSE;
+            }
+
+            break;
+
+        case ZM_AUTH_MODE_WPA:
+        case ZM_AUTH_MODE_WPAPSK:
+        case ZM_AUTH_MODE_WPA_NONE:
+        case ZM_AUTH_MODE_WPA2:
+        case ZM_AUTH_MODE_WPA2PSK:
+            {
+                if ( pBssInfo->securityType != ZM_SECURITY_TYPE_WPA )
+                {
+                    ret = FALSE;
+                }
+
+                ret = zfCheckWPAAuth(dev, pBssInfo);
+            }
+            break;
+
+        case ZM_AUTH_MODE_OPEN:
+        case ZM_AUTH_MODE_SHARED_KEY:
+        case ZM_AUTH_MODE_AUTO:
+            {
+                if ( pBssInfo->wscIe[1] )
+                {
+                    // If the AP is a Jumpstart AP, it's ok!! Ray
+                    break;
+                }
+                else if ( pBssInfo->securityType == ZM_SECURITY_TYPE_WPA )
+                {
+                    ret = FALSE;
+                }
+            }
+            break;
+
+        default:
+            break;
+    }
+
+    return ret;
+}
+
+u8_t zfStaIsConnected(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_CONNECTED )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfStaIsConnecting(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_CONNECTING )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfStaIsDisconnect(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_DISCONNECT )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfChangeAdapterState(zdev_t* dev, u8_t newState)
+{
+    u8_t ret = TRUE;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //if ( newState == wd->sta.adapterState )
+    //{
+    //    return FALSE;
+    //}
+
+    switch(newState)
+    {
+    case ZM_STA_STATE_DISCONNECT:
+        zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_DISCONNECT);
+
+        #if 1
+            zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        #else
+            if ( wd->sta.bChannelScan )
+            {
+                /* stop the action of channel scanning */
+                wd->sta.bChannelScan = FALSE;
+                ret =  TRUE;
+                break;
+            }
+        #endif
+
+        break;
+    case ZM_STA_STATE_CONNECTING:
+        #if 1
+            zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        #else
+            if ( wd->sta.bChannelScan )
+            {
+                /* stop the action of channel scanning */
+                wd->sta.bChannelScan = FALSE;
+                ret =  TRUE;
+                break;
+            }
+        #endif
+
+        break;
+    case ZM_STA_STATE_CONNECTED:
+        break;
+    default:
+        break;
+    }
+
+    //if ( ret )
+    //{
+        zmw_enter_critical_section(dev);
+        wd->sta.adapterState = newState;
+        zmw_leave_critical_section(dev);
+
+        zm_debug_msg1("change adapter state = ", newState);
+    //}
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaMmAddIeSsid            */
+/*      Add information element SSID to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ssidLen);
+
+    /* Information : SSID */
+    for (i=0; i<wd->sta.ssidLen; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ssid[i]);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaMmAddIeWpa             */
+/*      Add information element SSID to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2006.01     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+{
+    u32_t  i;
+    u8_t    ssn[64]={
+                        /* Element ID */
+                        0xdd,
+                        /* Length */
+                        0x18,
+                        /* OUI type */
+                        0x00, 0x50, 0xf2, 0x01,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* WPA capability */
+                        0x00, 0x00
+                    };
+
+    u8_t    rsn[64]={
+                        /* Element ID */
+                        0x30,
+                        /* Length */
+                        0x14,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* RSN capability */
+                        0x00, 0x00
+                    };
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPAPSK )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(ssn+8, wd->sta.wpaIe+8, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(ssn+14, zgWpaAesOui, 4);
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, ssn, offset, ssn[1]+2);
+        zfMemoryCopy(wd->sta.wpaIe, ssn, ssn[1]+2);
+        offset += (ssn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(ssn+8, wd->sta.wpaIe+8, 4);
+        /* Overwrite Key Management Suite by WPA-Radius */
+        zfMemoryCopy(ssn+20, zgWpaRadiusOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(ssn+14, zgWpaAesOui, 4);
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, ssn, offset, ssn[1]+2);
+        zfMemoryCopy(wd->sta.wpaIe, ssn, ssn[1]+2);
+        offset += (ssn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA2PSK )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, wd->sta.rsnIe+4, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        if ( frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ )
+        {
+            for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+            {
+                if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid,
+                                     (u8_t*) wd->sta.bssid, 6) )
+                {
+                    /* matched */
+                    break;
+                }
+
+                if ( i < wd->sta.pmkidInfo.bssidCount )
+                {
+                    // Fill PMKID Count in RSN information element
+                    rsn[22] = 0x01;
+                    rsn[23] = 0x00;
+
+                    // Fill PMKID in RSN information element
+                    zfMemoryCopy(rsn+24,
+                                 wd->sta.pmkidInfo.bssidInfo[i].pmkid, 16);
+			                 rsn[1] += 18;
+                }
+            }
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, rsn, offset, rsn[1]+2);
+        zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+        offset += (rsn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA2 )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, wd->sta.rsnIe+4, 4);
+        /* Overwrite Key Management Suite by WPA2-Radius */
+        zfMemoryCopy(rsn+16, zgWpa2RadiusOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        if (( frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ || ( frameType == ZM_WLAN_FRAME_TYPE_ASOCREQ )))
+        {
+
+            if (wd->sta.pmkidInfo.bssidCount != 0) {
+                // Fill PMKID Count in RSN information element
+                rsn[22] = 1;
+                rsn[23] = 0;
+                /*
+                 *  The caller is respnsible to give us the relevant PMKID.
+                 *  We'll only accept 1 PMKID for now.
+                 */
+                for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+                {
+                    if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid, (u8_t*) wd->sta.bssid, 6) )
+                    {
+                        zfMemoryCopy(rsn+24, wd->sta.pmkidInfo.bssidInfo[i].pmkid, 16);
+                        break;
+                    }
+                }
+                rsn[1] += 18;
+            }
+
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, rsn, offset, rsn[1]+2);
+        zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+        offset += (rsn[1]+2);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeIbss              */
+/*      Add information element IBSS parameter to buffer.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeIbss(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_IBSS);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+
+    /* ATIM window */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.atimWindow);
+    offset += 2;
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeWmeInfo           */
+/*      Add WME Information Element to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeWmeInfo(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t qosInfo)
+{
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WIFI_IE);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 7);
+
+    /* OUI */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x50);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xF2);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x02);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+
+    /* QoS Info */
+    zmw_tx_buf_writeb(dev, buf, offset++, qosInfo);
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIePowerCap          */
+/*      Add information element Power capability to buffer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Sharon                                            2007.12       */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIePowerCap(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t MaxTxPower;
+    u8_t MinTxPower;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_POWER_CAPABILITY);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+
+    MinTxPower = (u8_t)(zfHpGetMinTxPower(dev)/2);
+    MaxTxPower = (u8_t)(zfHpGetMaxTxPower(dev)/2);
+
+    /* Min Transmit Power Cap */
+    zmw_tx_buf_writeh(dev, buf, offset++, MinTxPower);
+
+    /* Max Transmit Power Cap */
+    zmw_tx_buf_writeh(dev, buf, offset++, MaxTxPower);
+
+    return offset;
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeSupportCh              */
+/*      Add information element supported channels to buffer.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Sharon            2007.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeSupportCh(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+
+    u8_t   i;
+    u16_t  count_24G = 0;
+    u16_t  count_5G = 0;
+    u16_t  channelNum;
+    u8_t   length;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+    zmw_enter_critical_section(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel < 3000)
+        { // 2.4Hz
+            count_24G++;
+        }
+        else
+        { // 5GHz
+            count_5G++;
+        }
+    }
+
+    length = (u8_t)(count_5G * 2 + 2); //5G fill by pair, 2,4G (continuous channels) fill 2 bytes
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SUPPORTED_CHANNELS );
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, length);
+
+    // 2.4GHz (continuous channels)
+    /* First channel number */
+    zmw_tx_buf_writeh(dev, buf, offset++, 1); //Start from channle 1
+    /* Number of channels */
+    zmw_tx_buf_writeh(dev, buf, offset++, count_24G);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt ; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel > 4000 && wd->regulationTable.allowChannel[i].channel < 5000)
+        { // 5GHz 4000 -5000Mhz
+            channelNum = (wd->regulationTable.allowChannel[i].channel-4000)/5;
+            /* First channel number */
+            zmw_tx_buf_writeh(dev, buf, offset++, channelNum);
+            /* Number of channels */
+            zmw_tx_buf_writeh(dev, buf, offset++, 1);
+        }
+        else if (wd->regulationTable.allowChannel[i].channel >= 5000)
+        { // 5GHz >5000Mhz
+            channelNum = (wd->regulationTable.allowChannel[i].channel-5000)/5;
+            /* First channel number */
+            zmw_tx_buf_writeh(dev, buf, offset++, channelNum);
+            /* Number of channels */
+            zmw_tx_buf_writeh(dev, buf, offset++, 1);
+        }
+    }
+   zmw_leave_critical_section(dev);
+
+    return offset;
+}
+
+void zfStaStartConnectCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfStaStartConnect(dev, wd->sta.bIsSharedKey);
+}
+
+void zfStaStartConnect(zdev_t* dev, u8_t bIsSharedKey)
+{
+    u32_t p1, p2;
+    u8_t newConnState;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* p1_low = algorithm number, p1_high = transaction sequence number */
+    if ( bIsSharedKey )
+    {
+        //wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_SHARE_1;
+        newConnState = ZM_STA_CONN_STATE_AUTH_SHARE_1;
+        zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_SHARE_1");
+        p1 = ZM_AUTH_ALGO_SHARED_KEY;
+    }
+    else
+    {
+        //wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_OPEN;
+        newConnState = ZM_STA_CONN_STATE_AUTH_OPEN;
+        zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_OPEN");
+        if( wd->sta.leapEnabled )
+            p1 = ZM_AUTH_ALGO_LEAP;
+        else
+            p1 = ZM_AUTH_ALGO_OPEN_SYSTEM;
+    }
+
+    /* status code */
+    p2 = 0x0;
+
+    zmw_enter_critical_section(dev);
+    wd->sta.connectTimer = wd->tick;
+    wd->sta.connectState = newConnState;
+    zmw_leave_critical_section(dev);
+
+    /* send the 1st authentication frame */
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH, wd->sta.bssid, p1, p2, 0);
+
+    return;
+}
+
+void zfSendNullData(zdev_t* dev, u8_t type)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(34+8+1)/2];
+    u16_t bcastAddr[3] = {0xffff,0xffff,0xffff};
+    u16_t *dstAddr;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 0);
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    if ( wd->wlanMode == ZM_MODE_IBSS)
+    {
+        dstAddr = bcastAddr;
+    }
+    else
+    {
+        dstAddr = wd->sta.bssid;
+    }
+
+    if (wd->sta.wmeConnected != 0)
+    {
+        /* If connect to a WMM AP, Send QoS Null data */
+        hlen = zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_QOS_NULL, dstAddr, header, 0, buf, 0, 0);
+    }
+    else
+    {
+        hlen = zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_NULL, dstAddr, header, 0, buf, 0, 0);
+    }
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        header[4] |= 0x0100; //TODS bit
+    }
+
+    if ( type == 1 )
+    {
+        header[4] |= 0x1000;
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    /*increase unicast frame counter*/
+    wd->commTally.txUnicastFrm++;
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfSendPSPoll(zdev_t* dev)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(8+24+1)/2];
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 0);
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_PSPOLL, wd->sta.bssid, header, 0, buf, 0, 0);
+
+    header[0] = 20;
+    header[4] |= 0x1000;
+    header[5] = wd->sta.aid | 0xc000; //Both bit-14 and bit-15 are 1
+    hlen = 16 + 8;
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfSendBA(zdev_t* dev, u16_t start_seq, u8_t *bitmap)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(8+24+1)/2];
+    u16_t i, offset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 12); // 28 = FC 2 + DU 2 + RA 6 + TA 6 + BAC 2 + SEQ 2 + BitMap 8
+                                 // 12 = BAC 2 + SEQ 2 + BitMap 8
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_BA, wd->sta.bssid, header, 0, buf, 0, 0);
+
+    header[0] = 32; /* MAC header 16 + BA control 2 + BA info 10 + FCS 4*/
+    header[1] = 0x4;  /* No ACK */
+
+    /* send by OFDM 6M */
+    header[2] = (u16_t)(zcRateToPhyCtrl[4] & 0xffff);
+    header[3] = (u16_t)(zcRateToPhyCtrl[4]>>16) & 0xffff;
+
+    hlen = 16 + 8;  /* MAC header 16 + control 8*/
+    offset = 0;
+    zmw_tx_buf_writeh(dev, buf, offset, 0x05); /*compressed bitmap on*/
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+    offset+=2;
+
+    for (i=0; i<8; i++) {
+        zmw_tx_buf_writeb(dev, buf, offset, bitmap[i]);
+        offset++;
+    }
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfStaGetTxRate(zdev_t* dev, u16_t* macAddr, u32_t* phyCtrl,
+        u16_t* rcProbingFlag)
+{
+    u8_t   addr[6], i;
+    u8_t   rate;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    ZM_MAC_WORD_TO_BYTE(macAddr, addr);
+    *phyCtrl = 0;
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        zmw_enter_critical_section(dev);
+        rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[0].rcCell, rcProbingFlag);
+//#ifdef ZM_FB50
+        //rate = 27;
+//#endif
+        *phyCtrl = zcRateToPhyCtrl[rate];
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        for(i=0; i<wd->sta.oppositeCount; i++)
+        {
+            if ( addr[0] && 0x01 == 1 ) // The default beacon transmitted rate is CCK and 1 Mbps , but the a mode should use
+                                        // OFDM modulation and 6Mbps to transmit beacon.
+            {
+                //rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[i].rcCell, rcProbingFlag);
+                rate = wd->sta.oppositeInfo[i].rcCell.operationRateSet[0];
+                *phyCtrl = zcRateToPhyCtrl[rate];
+                break;
+            }
+            else if ( zfMemoryIsEqual(addr, wd->sta.oppositeInfo[i].macAddr, 6) )
+            {
+                rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[i].rcCell, rcProbingFlag);
+                *phyCtrl = zcRateToPhyCtrl[rate];
+                break;
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+struct zsMicVar* zfStaGetRxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t keyIndex;
+    u8_t da0;
+
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+         (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) == 0x80)
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+5); /* Qos Packet*/
+    else
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+3); /* normal Packet*/
+    keyIndex = (keyIndex & 0xc0) >> 6;
+
+    return (&wd->sta.rxMicKey[keyIndex]);
+}
+
+struct zsMicVar* zfStaGetTxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    //if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+    //     (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    if ( (wd->sta.encryMode != ZM_TKIP) || (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    return (&wd->sta.txMicKey);
+}
+
+u16_t zfStaRxValidateFrame(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   frameType, frameCtrl;
+    u8_t   da0;
+    //u16_t  sa[3];
+    u16_t  ret;
+    u16_t  i;
+    //u8_t    sa0;
+
+    zmw_get_wlan_dev(dev);
+
+    frameType = zmw_rx_buf_readb(dev, buf, 0);
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+    //sa0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+
+    if ( (!zfStaIsConnected(dev))&&((frameType & 0xf) == ZM_WLAN_DATA_FRAME) )
+    {
+        return ZM_ERR_DATA_BEFORE_CONNECTED;
+    }
+
+
+    if ( (zfStaIsConnected(dev))&&((frameType & 0xf) == ZM_WLAN_DATA_FRAME) )
+    {
+        /* check BSSID */
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            /* Big Endian and Little Endian Compatibility */
+            u16_t mac[3];
+            mac[0] = zmw_cpu_to_le16(wd->sta.bssid[0]);
+            mac[1] = zmw_cpu_to_le16(wd->sta.bssid[1]);
+            mac[2] = zmw_cpu_to_le16(wd->sta.bssid[2]);
+            if ( !zfRxBufferEqualToStr(dev, buf, (u8_t *)mac,
+                                       ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+/*We will get lots of garbage data, especially in AES mode.*/
+/*To avoid sending too many deauthentication frames in STA mode, mark it.*/
+#if 0
+                /* If unicast frame, send deauth to the transmitter */
+                if (( da0 & 0x01 ) == 0)
+                {
+                    for (i=0; i<3; i++)
+                    {
+                        sa[i] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+(i*2));
+                    }
+					/* If mutilcast address, don't send deauthentication*/
+					if (( sa0 & 0x01 ) == 0)
+	                  	zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, sa, 7, 0, 0);
+                }
+#endif
+                return ZM_ERR_DATA_BSSID_NOT_MATCHED;
+            }
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            /* Big Endian and Little Endian Compatibility */
+            u16_t mac[3];
+            mac[0] = zmw_cpu_to_le16(wd->sta.bssid[0]);
+            mac[1] = zmw_cpu_to_le16(wd->sta.bssid[1]);
+            mac[2] = zmw_cpu_to_le16(wd->sta.bssid[2]);
+            if ( !zfRxBufferEqualToStr(dev, buf, (u8_t *)mac,
+                                       ZM_WLAN_HEADER_A3_OFFSET, 6) )
+            {
+                return ZM_ERR_DATA_BSSID_NOT_MATCHED;
+            }
+        }
+
+        frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+
+        /* check security bit */
+        if ( wd->sta.dropUnencryptedPkts &&
+             (wd->sta.wepStatus != ZM_ENCRYPTION_WEP_DISABLED )&&
+             ( !(frameCtrl & ZM_BIT_6) ) )
+        {   /* security on, but got data without encryption */
+
+            #if 1
+            ret = ZM_ERR_DATA_NOT_ENCRYPTED;
+            if ( wd->sta.pStaRxSecurityCheckCb != NULL )
+            {
+                ret = wd->sta.pStaRxSecurityCheckCb(dev, buf);
+            }
+            else
+            {
+                ret = ZM_ERR_DATA_NOT_ENCRYPTED;
+            }
+            if (ret == ZM_ERR_DATA_NOT_ENCRYPTED)
+            {
+                wd->commTally.swRxDropUnencryptedCount++;
+            }
+            return ret;
+            #else
+            if ( (wd->sta.wepStatus != ZM_ENCRYPTION_TKIP)&&
+                 (wd->sta.wepStatus != ZM_ENCRYPTION_AES) )
+            {
+                return ZM_ERR_DATA_NOT_ENCRYPTED;
+            }
+            #endif
+        }
+    }
+
+    return ZM_SUCCESS;
+}
+
+void zfStaMicFailureHandling(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   da0;
+    u8_t   micNotify = 1;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.wpaState <  ZM_STA_WPA_STATE_PK_OK )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.cmMicFailureCount++;
+
+    if ( wd->sta.cmMicFailureCount == 1 )
+    {
+        zm_debug_msg0("get the first MIC failure");
+        //zfTimerSchedule(dev, ZM_EVENT_CM_TIMER, ZM_TICK_CM_TIMEOUT);
+
+        /* Timer Resolution on WinXP is 15/16 ms  */
+        /* Decrease Time offset for <XP> Counter Measure */
+        zfTimerSchedule(dev, ZM_EVENT_CM_TIMER, ZM_TICK_CM_TIMEOUT - ZM_TICK_CM_TIMEOUT_OFFSET);
+    }
+    else if ( wd->sta.cmMicFailureCount == 2 )
+    {
+        zm_debug_msg0("get the second MIC failure");
+        /* reserve 2 second for OS to send MIC failure report to AP */
+        wd->sta.cmDisallowSsidLength = wd->sta.ssidLen;
+        zfMemoryCopy(wd->sta.cmDisallowSsid, wd->sta.ssid, wd->sta.ssidLen);
+        //wd->sta.cmMicFailureCount = 0;
+        zfTimerCancel(dev, ZM_EVENT_CM_TIMER);
+        //zfTimerSchedule(dev, ZM_EVENT_CM_DISCONNECT, ZM_TICK_CM_DISCONNECT);
+
+        /* Timer Resolution on WinXP is 15/16 ms  */
+        /* Decrease Time offset for <XP> Counter Measure */
+        zfTimerSchedule(dev, ZM_EVENT_CM_DISCONNECT, ZM_TICK_CM_DISCONNECT - ZM_TICK_CM_DISCONNECT_OFFSET);
+    }
+    else
+    {
+        micNotify = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (micNotify == 1)
+    {
+        da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+        if ( da0 & 0x01 )
+        {
+            if (wd->zfcbMicFailureNotify != NULL)
+            {
+                wd->zfcbMicFailureNotify(dev, wd->sta.bssid, ZM_MIC_GROUP_ERROR);
+            }
+        }
+        else
+        {
+            if (wd->zfcbMicFailureNotify != NULL)
+            {
+                wd->zfcbMicFailureNotify(dev, wd->sta.bssid, ZM_MIC_PAIRWISE_ERROR);
+            }
+        }
+    }
+}
+
+
+u8_t zfStaBlockWlanScan(zdev_t* dev)
+{
+    u8_t   ret=FALSE;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.bChannelScan )
+    {
+        return TRUE;
+    }
+
+    return ret;
+}
+
+void zfStaResetStatus(zdev_t* dev, u8_t bInit)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfHpDisableBeacon(dev);
+
+    wd->dtim = 1;
+    wd->sta.capability[0] = 0x01;
+    wd->sta.capability[1] = 0x00;
+    /* 802.11h */
+    if (wd->sta.DFSEnable || wd->sta.TPCEnable)
+        wd->sta.capability[1] |= ZM_BIT_0;
+
+    /* release queued packets */
+    for(i=0; i<wd->sta.ibssPSDataCount; i++)
+    {
+        zfwBufFree(dev, wd->sta.ibssPSDataQueue[i], 0);
+    }
+
+    for(i=0; i<wd->sta.staPSDataCount; i++)
+    {
+        zfwBufFree(dev, wd->sta.staPSDataQueue[i], 0);
+    }
+
+    wd->sta.ibssPSDataCount = 0;
+    wd->sta.staPSDataCount = 0;
+    zfZeroMemory((u8_t*) &wd->sta.staPSList, sizeof(struct zsStaPSList));
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    zfQueueFlush(dev, wd->sta.uapsdQ);
+
+    return;
+
+}
+
+void zfStaIbssMonitoring(zdev_t* dev, u8_t reset)
+{
+    u16_t i;
+    u16_t oppositeCount;
+    struct zsPartnerNotifyEvent event;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //zm_debug_msg1("zfStaIbssMonitoring %d", wd->sta.oppositeCount);
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.oppositeCount == 0 )
+    {
+        goto done;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        goto done;
+    }
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( reset )
+        {
+            wd->sta.oppositeInfo[i].valid = 0;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+
+        if ( wd->sta.oppositeInfo[i].aliveCounter )
+        {
+            zm_debug_msg1("Setting alive to ", wd->sta.oppositeInfo[i].aliveCounter);
+
+            zmw_leave_critical_section(dev);
+
+            if ( wd->sta.oppositeInfo[i].aliveCounter != ZM_IBSS_PEER_ALIVE_COUNTER )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ,
+                              (u16_t*)wd->sta.oppositeInfo[i].macAddr, 1, 0, 0);
+            }
+
+            zmw_enter_critical_section(dev);
+            wd->sta.oppositeInfo[i].aliveCounter--;
+        }
+        else
+        {
+            zm_debug_msg0("zfStaIbssMonitoring remove the peer station");
+            zfMemoryCopy(event.bssid, (u8_t *)(wd->sta.bssid), 6);
+            zfMemoryCopy(event.peerMacAddr, wd->sta.oppositeInfo[i].macAddr, 6);
+
+            wd->sta.oppositeInfo[i].valid = 0;
+            wd->sta.oppositeCount--;
+            if (wd->zfcbIbssPartnerNotify != NULL)
+            {
+                zmw_leave_critical_section(dev);
+                wd->zfcbIbssPartnerNotify(dev, 0, &event);
+                zmw_enter_critical_section(dev);
+            }
+        }
+    }
+
+done:
+    if ( reset == 0 )
+    {
+        zfTimerSchedule(dev, ZM_EVENT_IBSS_MONITOR, ZM_TICK_IBSS_MONITOR);
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfInitPartnerNotifyEvent(zdev_t* dev, zbuf_t* buf, struct zsPartnerNotifyEvent *event)
+{
+    u16_t  *peerMacAddr;
+
+    zmw_get_wlan_dev(dev);
+
+    peerMacAddr = (u16_t *)event->peerMacAddr;
+
+    zfMemoryCopy(event->bssid, (u8_t *)(wd->sta.bssid), 6);
+    peerMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+    peerMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET + 2);
+    peerMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET + 4);
+}
+
+void zfStaInitOppositeInfo(zdev_t* dev)
+{
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=0; i<ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        wd->sta.oppositeInfo[i].valid = 0;
+        wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+    }
+}
+#ifdef ZM_ENABLE_CENC
+u16_t zfStaAddIeCenc(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.cencIe[1] != 0)
+    {
+        zfCopyToIntTxBuffer(dev, buf, wd->sta.cencIe, offset, wd->sta.cencIe[1]+2);
+        offset += (wd->sta.cencIe[1]+2);
+    }
+    return offset;
+}
+#endif //ZM_ENABLE_CENC
+u16_t zfStaProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t category, actionDetails;
+    zmw_get_wlan_dev(dev);
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+    actionDetails = zmw_rx_buf_readb(dev, buf, 25);
+    switch (category)
+    {
+        case 0:		//Spectrum Management
+	        switch(actionDetails)
+	        {
+	        	case 0:			//Measurement Request
+	        		break;
+	        	case 1:			//Measurement Report
+	        		//ProcessActionSpectrumFrame_MeasurementReport(Adapter,pActionBody+3);
+	        		break;
+	        	case 2:			//TPC request
+                    //if (wd->sta.TPCEnable)
+                    //    zfStaUpdateDot11HTPC(dev, buf);
+	        		break;
+	        	case 3:			//TPC report
+                    //if (wd->sta.TPCEnable)
+                    //    zfStaUpdateDot11HTPC(dev, buf);
+	        		break;
+	        	case 4:			//Channel Switch Announcement
+                    if (wd->sta.DFSEnable)
+                        zfStaUpdateDot11HDFS(dev, buf);
+	        		break;
+	        	default:
+	        		zm_debug_msg1("Action Frame contain not support action field ", actionDetails);
+	        		break;
+	        }
+        	break;
+        case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+            zfAggBlockAckActionFrame(dev, buf);
+            break;
+        case 17:	//Qos Management
+        	break;
+    }
+
+    return 0;
+}
+
+/* Determine the time not send beacon , if more than some value ,
+   re-write the beacon start address */
+void zfReWriteBeaconStartAddress(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->tickIbssSendBeacon++;    // Increase 1 per 10ms .
+    zmw_leave_critical_section(dev);
+
+    if ( wd->tickIbssSendBeacon == 40 )
+    {
+//        DbgPrint("20070727");
+        zfHpEnableBeacon(dev, ZM_MODE_IBSS, wd->beaconInterval, wd->dtim, (u8_t)wd->sta.atimWindow);
+        zmw_enter_critical_section(dev);
+        wd->tickIbssSendBeacon = 0;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+struct zsTkipSeed* zfStaGetRxSeed(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t keyIndex;
+    u8_t da0;
+
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+         (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) == 0x80)
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+5); /* Qos Packet*/
+    else
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+3); /* normal Packet*/
+    keyIndex = (keyIndex & 0xc0) >> 6;
+
+    return (&wd->sta.rxSeed[keyIndex]);
+}
+
+void zfStaEnableSWEncryption(zdev_t *dev, u8_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.SWEncryptEnable = value;
+    zfHpSWDecrypt(dev, 1);
+    zfHpSWEncrypt(dev, 1);
+}
+
+void zfStaDisableSWEncryption(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.SWEncryptEnable = 0;
+    zfHpSWDecrypt(dev, 0);
+    zfHpSWEncrypt(dev, 0);
+}
+
+u16_t zfComputeBssInfoWeightValue(zdev_t *dev, u8_t isBMode, u8_t isHT, u8_t isHT40, u8_t signalStrength)
+{
+	u8_t  weightOfB           = 0;
+	u8_t  weightOfAGBelowThr  = 0;
+	u8_t  weightOfAGUpThr     = 15;
+	u8_t  weightOfN20BelowThr = 15;
+	u8_t  weightOfN20UpThr    = 30;
+	u8_t  weightOfN40BelowThr = 16;
+	u8_t  weightOfN40UpThr    = 32;
+
+    zmw_get_wlan_dev(dev);
+
+    if( isBMode == 0 )
+        return (signalStrength + weightOfB);    // pure b mode , do not add the weight value for this AP !
+    else
+    {
+        if( isHT == 0 && isHT40 == 0 )
+        { // a , g , b/g mode ! add the weight value 15 for this AP if it's signal strength is more than some value !
+            if( signalStrength < 18 ) // -77 dBm
+				return signalStrength + weightOfAGBelowThr;
+			else
+				return (signalStrength + weightOfAGUpThr);
+        }
+        else if( isHT == 1 && isHT40 == 0 )
+        { // 80211n mode use 20MHz
+            if( signalStrength < 23 ) // -72 dBm
+                return (signalStrength + weightOfN20BelowThr);
+            else
+                return (signalStrength + weightOfN20UpThr);
+        }
+        else // isHT == 1 && isHT40 == 1
+        { // 80211n mode use 40MHz
+            if( signalStrength < 16 ) // -79 dBm
+                return (signalStrength + weightOfN40BelowThr);
+            else
+                return (signalStrength + weightOfN40UpThr);
+        }
+    }
+}
+
+u16_t zfStaAddIbssAdditionalIE(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+	u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.ibssAdditionalIESize; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ibssAdditionalIE[i]);
+    }
+
+    return offset;
+}
diff --git a/drivers/staging/otus/80211core/coid.c b/drivers/staging/otus/80211core/coid.c
new file mode 100644
index 000000000000..6007f3131f8f
--- /dev/null
+++ b/drivers/staging/otus/80211core/coid.c
@@ -0,0 +1,2695 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : iod.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains OID functions.                             */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiWlanQueryMacAddress      */
+/*      Query OWN MAC address.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      addr : for return MAC address                                   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    vapId = zfwGetVapId(dev);
+
+    addr[0] = (u8_t)(wd->macAddr[0] & 0xff);
+    addr[1] = (u8_t)(wd->macAddr[0] >> 8);
+    addr[2] = (u8_t)(wd->macAddr[1] & 0xff);
+    addr[3] = (u8_t)(wd->macAddr[1] >> 8);
+    addr[4] = (u8_t)(wd->macAddr[2] & 0xff);
+    if (vapId == 0xffff)
+        addr[5] = (u8_t)(wd->macAddr[2] >> 8);
+    else
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        addr[5] = (u8_t)(wd->macAddr[2] >> 8); // Multiple SSID
+#else
+        addr[5] = vapId + 1 + (u8_t)(wd->macAddr[2] >> 8); //VAP
+#endif
+    }
+
+    return;
+}
+
+void zfiWlanQueryBssList(zdev_t* dev, struct zsBssList* pBssList)
+{
+    struct zsBssInfo*   pBssInfo;
+    struct zsBssInfo*   pDstBssInfo;
+    u8_t   i;
+    u8_t*  pMemList;
+    u8_t*  pMemInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    pMemList = (u8_t*) pBssList;
+    pMemInfo = pMemList + sizeof(struct zsBssList);
+    pBssList->head = (struct zsBssInfo*) pMemInfo;
+
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+    pDstBssInfo = (struct zsBssInfo*) pMemInfo;
+    pBssList->bssCount = wd->sta.bssList.bssCount;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        zfMemoryCopy((u8_t*)pDstBssInfo, (u8_t*)pBssInfo,
+                sizeof(struct zsBssInfo));
+
+        if ( pBssInfo->next != NULL )
+        {
+            pBssInfo = pBssInfo->next;
+            pDstBssInfo->next = pDstBssInfo + 1;
+            pDstBssInfo++;
+        }
+        else
+        {
+            zm_assert(i==(wd->sta.bssList.bssCount-1));
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanAck(dev);
+}
+
+void zfiWlanQueryBssListV1(zdev_t* dev, struct zsBssListV1* bssListV1)
+{
+    struct zsBssInfo*   pBssInfo;
+    //struct zsBssInfo*   pDstBssInfo;
+    u8_t   i, j, bdrop = 0, k = 0, Same_Count = 0;
+    u8_t   bssid[6];
+    //u8_t*  pMemList;
+    //u8_t*  pMemInfo;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    bssListV1->bssCount = wd->sta.bssList.bssCount;
+
+    pBssInfo = wd->sta.bssList.head;
+    ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        bdrop = 0;
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE ) )
+        {
+			for (j = 0; j < 6; j++)
+            {
+                if ( pBssInfo->bssid[j] != bssid[j] )
+                {
+                    break;
+                }
+            }
+
+            if  ( (j == 6)
+                  &&((pBssInfo->ssid[1] == wd->sta.ssidLen) || (pBssInfo->ssid[1] == 0) )&& (pBssInfo->frequency == wd->frequency) )
+            {
+				if(pBssInfo->ssid[1] == 0)
+					pBssInfo->ssid[1] = wd->sta.ssidLen;
+
+				if(Same_Count == 0)
+				{//First meet
+					Same_Count++;
+				}
+				else
+				{//same one
+					bdrop = 1;
+					bssListV1->bssCount--;
+				}
+
+            }
+        }
+
+        if (bdrop == 0)
+        {
+            zfMemoryCopy((u8_t*)(&bssListV1->bssInfo[k]), (u8_t*)pBssInfo,
+                sizeof(struct zsBssInfo));
+
+			if(Same_Count == 1)
+			{
+				zfMemoryCopy(&(bssListV1->bssInfo[k].ssid[2]), wd->sta.ssid, wd->sta.ssidLen);
+				Same_Count++;
+			}
+
+			k++;
+        }
+
+        if ( pBssInfo->next != NULL )
+        {
+            pBssInfo = pBssInfo->next;
+        }
+        else
+        {
+            zm_assert(i==(wd->sta.bssList.bssCount-1));
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanAck(dev);
+}
+
+void zfiWlanQueryAdHocCreatedBssDesc(zdev_t* dev, struct zsBssInfo *pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t *)pBssInfo, (u8_t *)&wd->sta.ibssBssDesc, sizeof(struct zsBssInfo));
+}
+
+u8_t zfiWlanQueryAdHocIsCreator(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.ibssBssIsCreator;
+}
+
+u32_t zfiWlanQuerySupportMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->supportMode;
+}
+
+u32_t zfiWlanQueryTransmitPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    u32_t ret = 0;
+
+    if (zfStaIsConnected(dev)) {
+        ret = wd->sta.connPowerInHalfDbm;
+    } else {
+        ret = zfHpGetTransmitPower(dev);
+    }
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiWlanFlushBssList         */
+/*      Flush BSSID List.                                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanFlushBssList(zdev_t* dev)
+{
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* Call zfBssInfoRefresh() twice to remove all entry */
+    zfBssInfoRefresh(dev, 1);
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetWlanMode(zdev_t* dev, u8_t wlanMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.wlanMode = wlanMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetAuthenticationMode(zdev_t* dev, u8_t authMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.authMode = authMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetWepStatus(zdev_t* dev, u8_t wepStatus)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.wepStatus = wepStatus;
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfiWlanSetSSID(zdev_t* dev, u8_t* ssid, u8_t ssidLength)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( ssidLength <= 32 )
+    {
+        zmw_enter_critical_section(dev);
+
+        wd->ws.ssidLen = ssidLength;
+        zfMemoryCopy(wd->ws.ssid, ssid, ssidLength);
+
+        if ( ssidLength < 32 )
+        {
+            wd->ws.ssid[ssidLength] = 0;
+        }
+
+        wd->ws.probingSsidList[0].ssidLen = ssidLength;
+        zfMemoryCopy(wd->ws.probingSsidList[0].ssid, ssid, ssidLength);
+        for (i=1; i<ZM_MAX_PROBE_HIDDEN_SSID_SIZE; i++)
+        {
+            wd->ws.probingSsidList[i].ssidLen = 0;
+        }
+
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfiWlanSetFragThreshold(zdev_t* dev, u16_t fragThreshold)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (fragThreshold == 0)
+    {   /* fragmentation is disabled */
+        wd->fragThreshold = 32767;
+    }
+    else if (fragThreshold < 256)
+    {
+        /* Minimum fragment threshold */
+        wd->fragThreshold = 256;
+    }
+    else if (fragThreshold > 2346)
+    {
+        wd->fragThreshold = 2346;
+    }
+    else
+    {
+        wd->fragThreshold = fragThreshold & 0xfffe;
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetRtsThreshold(zdev_t* dev, u16_t rtsThreshold)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->rtsThreshold = rtsThreshold;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetFrequency(zdev_t* dev, u32_t frequency, u8_t bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->frequency = (u16_t) (frequency/1000);
+        zmw_leave_critical_section(dev);
+        zfCoreSetFrequency(dev, wd->frequency);
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        if( frequency == 0 )
+        { // Auto select clean channel depend on wireless environment !
+            wd->ws.autoSetFrequency = 0;
+        }
+        wd->ws.frequency = (u16_t) (frequency/1000);
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfiWlanSetBssid(zdev_t* dev, u8_t* bssid)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<6; i++)
+    {
+        wd->ws.desiredBssid[i] = bssid[i];
+    }
+    wd->ws.bDesiredBssid = TRUE;
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfiWlanSetBeaconInterval(zdev_t* dev,
+                              u16_t  beaconInterval,
+                              u8_t   bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->beaconInterval = beaconInterval;
+        zmw_leave_critical_section(dev);
+
+        /* update beacon interval here */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.beaconInterval = beaconInterval;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+
+void zfiWlanSetDtimCount(zdev_t* dev, u8_t  dtim)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (dtim > 0)
+    {
+        wd->ws.dtim = dtim;
+    }
+    zmw_leave_critical_section(dev);
+}
+
+
+void zfiWlanSetAtimWindow(zdev_t* dev, u16_t atimWindow, u8_t bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.atimWindow = atimWindow;
+        zmw_leave_critical_section(dev);
+
+        /* atim window here */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.atimWindow = atimWindow;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+
+void zfiWlanSetEncryMode(zdev_t* dev, u8_t encryMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Hostapd Issue */
+        if ((wd->ws.encryMode != ZM_AES) && (wd->ws.encryMode != ZM_TKIP))
+            wd->ws.encryMode = encryMode;
+    }
+    else
+        wd->ws.encryMode = encryMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetDefaultKeyId(zdev_t* dev, u8_t keyId)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.keyId = keyId;
+}
+
+u8_t zfiWlanQueryIsPKInstalled(zdev_t *dev, u8_t *staMacAddr)
+{
+    u8_t isInstalled = 0;
+
+#if 1
+//#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t   res, peerIdx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    res = zfStaFindOppositeByMACAddr(dev, (u16_t *)staMacAddr, &peerIdx);
+    if( res == 0 )
+    {
+        isInstalled = wd->sta.oppositeInfo[peerIdx].pkInstalled;
+    }
+    zmw_leave_critical_section(dev);
+//#endif
+#endif
+
+    return isInstalled;
+}
+
+u8_t zfiWlanSetKey(zdev_t* dev, struct zsKeyInfo keyInfo)
+{
+    u16_t  broadcast[3] = {0xffff, 0xffff, 0xffff};
+    u32_t* key;
+    u8_t   encryMode = ZM_NO_WEP;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t   encryType = ZM_NO_WEP;
+#endif
+    u8_t   micKey[16];
+    u16_t  id = 0;
+    u8_t   vapId, i, addr[6];
+    u8_t   userIdx=0;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* Determine opposite exist or not */
+    u8_t   res, peerIdx;
+//    u8_t   userIdx=0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.ibssWpa2Psk == 1 )
+    {
+        zmw_enter_critical_section(dev);
+        res = zfStaFindOppositeByMACAddr(dev, (u16_t*)keyInfo.macAddr, &peerIdx);
+        if( res == 0 )
+        {
+            userIdx = peerIdx;
+            if ( wd->sta.oppositeInfo[userIdx].camIdx == 0xff )
+                wd->sta.oppositeInfo[userIdx].camIdx = userIdx;
+        }
+        zmw_leave_critical_section(dev);
+    }
+#else
+    zmw_get_wlan_dev(dev);
+#endif
+
+    if ( keyInfo.flag & ZM_KEY_FLAG_AUTHENTICATOR )
+    {   /* set key by authenticator */
+        /* set pairwise key */
+        if (keyInfo.flag & ZM_KEY_FLAG_PK)
+        {
+            /* Find STA's information */
+            if ((id = zfApFindSta(dev, keyInfo.macAddr)) == 0xffff)
+            {
+                /* Can't STA in the staTable */
+                return ZM_STATUS_FAILURE;
+            }
+
+            wd->ap.staTable[id].iv16 = 0;
+            wd->ap.staTable[id].iv32 = 0;
+
+            if (keyInfo.keyLength == 32)
+            {   /* TKIP */
+                //u8_t KeyRsc[6] = {0, 0, 0, 0, 0, 0};
+
+                /* In the current AP mode, we set KeyRsc to zero */
+                //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                //           &(wd->ap.staTable[id].txSeed), KeyRsc);
+                //zfTkipInit(keyInfo.key, (u8_t*) keyInfo.macAddr,
+                //           &(wd->ap.staTable[id].rxSeed), KeyRsc);
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    zm_debug_msg0("Set CENC pairwise Key");
+
+                    wd->ap.staTable[id].encryMode = ZM_CENC;
+
+                    /* Reset txiv and rxiv */
+                    wd->ap.staTable[id].txiv[0] = 0x5c365c37;
+                    wd->ap.staTable[id].txiv[1] = 0x5c365c36;
+                    wd->ap.staTable[id].txiv[2] = 0x5c365c36;
+                    wd->ap.staTable[id].txiv[3] = 0x5c365c36;
+
+                    wd->ap.staTable[id].rxiv[0] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[1] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[2] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[3] = 0x5c365c36;
+
+                    /* Set Key Index */
+                    wd->ap.staTable[id].cencKeyIdx = keyInfo.keyIndex;
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_CENC, (u16_t *)keyInfo.macAddr,
+                    //          (u32_t*) &keyInfo.key[16]);
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    wd->ap.staTable[id].encryMode = ZM_TKIP;
+
+                    zfMemoryCopy(micKey, &keyInfo.key[16], 8);
+                    zfMemoryCopy(&micKey[8], &keyInfo.key[24], 8);
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_TKIP, (u16_t *)keyInfo.macAddr,
+                    //           (u32_t*) micKey);
+
+                    /* For fragmentation, we use software MIC */
+                    zfMemoryCopy((u8_t *)&(wd->ap.staTable[id].txMicKey), &(keyInfo.key[16]), 8);
+                    zfMemoryCopy((u8_t *)&(wd->ap.staTable[id].rxMicKey), &(keyInfo.key[24]), 8);
+
+                }
+            }
+            else if (keyInfo.keyLength == 16)
+            {   /* AES */
+                wd->ap.staTable[id].encryMode = ZM_AES;
+            }
+            else if (keyInfo.keyLength == 0)
+            {
+                /* Clear Key Info */
+                zfApClearStaKey(dev, (u16_t *)keyInfo.macAddr);
+
+                return ZM_STATUS_SUCCESS;
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            //zfCoreSetKey(dev, id+1, 0, wd->ap.staTable[id].encryMode,
+            //      (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            zfHpSetApPairwiseKey(dev, (u16_t *)keyInfo.macAddr,
+                    wd->ap.staTable[id].encryMode, (u32_t*) keyInfo.key,
+                    (u32_t*) &keyInfo.key[16], id+1);
+            wd->ap.staTable[id].keyIdx = id + 1 + 4;
+        }
+        else if (keyInfo.flag & ZM_KEY_FLAG_GK)
+        {
+            vapId = keyInfo.vapId;
+
+            wd->ap.iv16[vapId] = 0;
+            wd->ap.iv32[vapId] = 0;
+
+            if (keyInfo.keyLength == 32)
+            {   /* TKIP */
+                //u8_t KeyRsc[6] = {0, 0, 0, 0, 0, 0};
+
+                //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                //           &(wd->ap.bcSeed), KeyRsc);
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    encryMode = ZM_CENC;
+                    zm_debug_msg0("Set CENC group Key");
+
+                    /* Reset txiv and rxiv */
+                    wd->ap.txiv[vapId][0] = 0x5c365c36;
+                    wd->ap.txiv[vapId][1] = 0x5c365c36;
+                    wd->ap.txiv[vapId][2] = 0x5c365c36;
+                    wd->ap.txiv[vapId][3] = 0x5c365c36;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_CENC, keyInfo.vapAddr,
+                    //          (u32_t*) &keyInfo.key[16]);
+                    key = (u32_t*) keyInfo.key;
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    encryMode = ZM_TKIP;
+                    key = (u32_t *)keyInfo.key;
+
+                    /* set MIC key to HMAC */
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, broadcast,
+                    //         (u32_t*) (&keyInfo.key[16]));
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, keyInfo.vapAddr,
+                    //           (u32_t*) (&keyInfo.key[16]));
+
+                    zfMicSetKey(&(keyInfo.key[16]), &(wd->ap.bcMicKey[0]));
+                    key = (u32_t*) keyInfo.key;
+                }
+            }
+            else if (keyInfo.keyLength == 16)
+            {   /* AES */
+                encryMode = ZM_AES;
+                key = (u32_t *)keyInfo.key;
+                zm_debug_msg0("CWY - Set AES Group Key");
+            }
+            else if (keyInfo.keyLength == 0)
+            {
+                /* Clear Key Info */
+                zfApClearStaKey(dev, broadcast);
+
+                /* Turn off WEP bit in the capability field */
+                wd->ap.capab[vapId] &= 0xffef;
+
+                return ZM_STATUS_SUCCESS;
+            }
+            else
+            {   /* WEP */
+                if (keyInfo.keyLength == 5)
+                {
+                    encryMode = ZM_WEP64;
+                }
+                else if (keyInfo.keyLength == 13)
+                {
+                    encryMode = ZM_WEP128;
+                }
+                else if (keyInfo.keyLength == 29)
+                {
+                    encryMode = ZM_WEP256;
+                }
+
+                key = (u32_t*) keyInfo.key;
+            }
+
+            // Modification for CAM not support VAP search
+            //zfCoreSetKey(dev, 0, 0, encryMode, broadcast, key);
+            //zfCoreSetKey(dev, 0, 0, encryMode, wd->macAddr, key);
+            //zfCoreSetKey(dev, 0, 0, encryMode, keyInfo.vapAddr, key);
+            zfHpSetApGroupKey(dev, wd->macAddr, encryMode,
+                    key, (u32_t*) &keyInfo.key[16], vapId);
+
+            //zfiWlanSetEncryMode(dev, encryMode);
+            wd->ws.encryMode = encryMode;
+
+            /* set the multicast address encryption type */
+            wd->ap.encryMode[vapId] = encryMode;
+
+            /* set the multicast key index */
+            wd->ap.bcKeyIndex[vapId] = keyInfo.keyIndex;
+            wd->ap.bcHalKeyIdx[vapId] = vapId + 60;
+
+            /* Turn on WEP bit in the capability field */
+            wd->ap.capab[vapId] |= 0x10;
+        }
+    }
+    else
+    {   /* set by supplicant */
+
+        if ( keyInfo.flag & ZM_KEY_FLAG_PK )
+        {   /* set pairwise key */
+
+            //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+            //           &wd->sta.txSeed, keyInfo.initIv);
+            //zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+            //           &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( wd->sta.ibssWpa2Psk == 1 )
+            {
+                /* unicast -- > pairwise key */
+                wd->sta.oppositeInfo[userIdx].iv16 = 0;
+                wd->sta.oppositeInfo[userIdx].iv32 = 0;
+            }
+            else
+            {
+                wd->sta.iv16 = 0;
+                wd->sta.iv32 = 0;
+            }
+
+            wd->sta.oppositeInfo[userIdx].pkInstalled = 1;
+#else
+            wd->sta.iv16 = 0;
+            wd->sta.iv32 = 0;
+
+            wd->sta.oppositeInfo[userIdx].pkInstalled = 1;
+#endif
+
+            if ( keyInfo.keyLength == 32 )
+            {   /* TKIP */
+                zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                        &wd->sta.txSeed, keyInfo.initIv);
+                zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                        &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    zm_debug_msg0("Set CENC pairwise Key");
+
+                    wd->sta.encryMode = ZM_CENC;
+
+                    /* Reset txiv and rxiv */
+                    wd->sta.txiv[0] = 0x5c365c36;
+                    wd->sta.txiv[1] = 0x5c365c36;
+                    wd->sta.txiv[2] = 0x5c365c36;
+                    wd->sta.txiv[3] = 0x5c365c36;
+
+                    wd->sta.rxiv[0] = 0x5c365c37;
+                    wd->sta.rxiv[1] = 0x5c365c36;
+                    wd->sta.rxiv[2] = 0x5c365c36;
+                    wd->sta.rxiv[3] = 0x5c365c36;
+
+                    /* Set Key Index */
+                    wd->sta.cencKeyId = keyInfo.keyIndex;
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_CENC, (u16_t *)keyInfo.macAddr,
+                    //         (u32_t*) &keyInfo.key[16]);
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    wd->sta.encryMode = ZM_TKIP;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, wd->sta.bssid,
+                    //         (u32_t*) &keyInfo.key[16]);
+
+                    zfMicSetKey(&keyInfo.key[16], &wd->sta.txMicKey);
+                    zfMicSetKey(&keyInfo.key[24],
+                                &wd->sta.rxMicKey[keyInfo.keyIndex]);
+                }
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {   /* AES */
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                if ( wd->sta.ibssWpa2Psk == 1 )
+                {
+                    wd->sta.oppositeInfo[userIdx].encryMode = ZM_AES;
+                    encryType = wd->sta.oppositeInfo[userIdx].encryMode;
+                }
+                else
+                {
+                    wd->sta.encryMode = ZM_AES;
+                    encryType = wd->sta.encryMode;
+                }
+#else
+                wd->sta.encryMode = ZM_AES;
+#endif
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            /* user 0 */
+            //zfCoreSetKey(dev, 0, 0, wd->sta.encryMode,
+            //         wd->sta.bssid, (u32_t*) keyInfo.key);
+            //zfHpSetStaPairwiseKey(dev, wd->sta.bssid, wd->sta.encryMode,
+            //    (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            { /* If not AES-CCMP and ibss network , use traditional */
+                zfHpSetPerUserKey(dev,
+                                userIdx,
+                                keyInfo.keyIndex,  // key id == 0 ( Pairwise key = 0 )
+                                (u8_t*)keyInfo.macAddr,   // RX need Source Address ( Address 2 )
+                                encryType,
+//                              wd->sta.encryMode,
+                                (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.oppositeInfo[userIdx].wpaState = ZM_STA_WPA_STATE_PK_OK ;
+            }
+            else
+            {/* Big Endian and Little Endian Compatibility */
+                for (i = 0; i < 3; i++)
+                {
+                    addr[2 * i] = wd->sta.bssid[i] & 0xff;
+                    addr[2 * i + 1] = wd->sta.bssid[i] >> 8;
+                }
+                zfHpSetPerUserKey(dev,
+                                    ZM_USER_KEY_PK,   // user id
+                                    0,                // key id
+                                    addr,//(u8_t *)wd->sta.bssid,
+                              wd->sta.encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.keyId = 4;
+            }
+#else
+            /* Big Endian and Little Endian Compatibility */
+            for (i = 0; i < 3; i++)
+            {
+                addr[2 * i] = wd->sta.bssid[i] & 0xff;
+                addr[2 * i + 1] = wd->sta.bssid[i] >> 8;
+            }
+            zfHpSetPerUserKey(dev,
+                              ZM_USER_KEY_PK,   // user id
+                              0,                // key id
+                              addr,//(u8_t *)wd->sta.bssid,
+                              wd->sta.encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+            wd->sta.keyId = 4;
+#endif
+
+            wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+        }
+        else if ( keyInfo.flag & ZM_KEY_FLAG_GK )
+        {   /* set group key */
+
+            zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                       &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+            if ( keyInfo.keyLength == 32 )
+            {   /* TKIP */
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    encryMode = ZM_CENC;
+                    zm_debug_msg0("Set CENC group Key");
+
+                    /* Reset txiv and rxiv */
+                    wd->sta.rxivGK[0] = 0x5c365c36;
+                    wd->sta.rxivGK[1] = 0x5c365c36;
+                    wd->sta.rxivGK[2] = 0x5c365c36;
+                    wd->sta.rxivGK[3] = 0x5c365c36;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_CENC, keyInfo.vapAddr,
+                    //         (u32_t*) &keyInfo.key[16]);
+                    key = (u32_t*) keyInfo.key;
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    encryMode = ZM_TKIP;
+                    key = (u32_t*) wd->sta.rxSeed[keyInfo.keyIndex].tk;
+
+                    if ( !(keyInfo.flag & ZM_KEY_FLAG_INIT_IV) )
+                    {
+                        wd->sta.rxSeed[keyInfo.keyIndex].iv16 = 0;
+                        wd->sta.rxSeed[keyInfo.keyIndex].iv32 = 0;
+                    }
+
+                    /* set MIC key to HMAC */
+                    //zfCoreSetKey(dev, 8, 1, ZM_TKIP, broadcast,
+                    //         (u32_t*) (&keyInfo.key[16]));
+
+                    zfMicSetKey(&keyInfo.key[24],
+                                &wd->sta.rxMicKey[keyInfo.keyIndex]);
+                }
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {   /* AES */
+                encryMode = ZM_AES;
+                //key = (u32_t*) wd->sta.rxSeed[keyInfo.keyIndex].tk;
+            }
+            else
+            {   /* WEP */
+                if ( keyInfo.keyLength == 5 )
+                {
+                    encryMode = ZM_WEP64;
+                }
+                else if ( keyInfo.keyLength == 13 )
+                {
+                    encryMode = ZM_WEP128;
+                }
+                else if ( keyInfo.keyLength == 29 )
+                {
+                    encryMode = ZM_WEP256;
+                }
+
+                key = (u32_t*) keyInfo.key;
+            }
+
+            /* user 8 */
+            //zfCoreSetKey(dev, 8, 0, encryMode, broadcast, key);
+            //zfHpSetStaGroupKey(dev, broadcast, encryMode,
+            //        (u32_t*) keyInfo.key, (u32_t*) (&keyInfo.key[16]));
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            {/* If not AES-CCMP and ibss network , use traditional */
+                zfHpSetPerUserKey(dev,
+                              userIdx,
+                              keyInfo.keyIndex,                // key id
+                              // (u8_t *)broadcast,                  // for only 2 stations IBSS netwrl ( A2 )
+                              (u8_t*)keyInfo.macAddr,   // for multiple ( > 2 ) stations IBSS network ( A2 )
+                              encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+            }
+            else
+            {
+                zfHpSetPerUserKey(dev,
+                                ZM_USER_KEY_GK,   // user id
+                                0,                // key id
+                                (u8_t *)broadcast,
+                                encryMode,
+                                (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+            }
+#else
+            zfHpSetPerUserKey(dev,
+                              ZM_USER_KEY_GK,   // user id
+                              0,                // key id
+                              (u8_t *)broadcast,
+                              encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+            wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+#endif
+        }
+        else
+        {   /* legacy WEP */
+            zm_debug_msg0("legacy WEP");
+
+            if ( keyInfo.keyIndex >= 4 )
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            if ( keyInfo.keyLength == 5 )
+            {
+                zm_debug_msg0("WEP 64");
+
+                encryMode = ZM_WEP64;
+            }
+            else if ( keyInfo.keyLength == 13 )
+            {
+                zm_debug_msg0("WEP 128");
+
+                encryMode = ZM_WEP128;
+            }
+            else if ( keyInfo.keyLength == 32 )
+            {
+                /* TKIP */
+                #if 0
+                // Don't reset the IV since some AP would fail in IV check and drop our connection
+                if ( wd->sta.wpaState != ZM_STA_WPA_STATE_PK_OK )
+                {
+                    wd->sta.iv16 = 0;
+                    wd->sta.iv32 = 0;
+                }
+                #endif
+
+                encryMode = ZM_TKIP;
+
+                zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                           &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+                zfMicSetKey(&keyInfo.key[24],
+                           &wd->sta.rxMicKey[keyInfo.keyIndex]);
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {
+                /* AES */
+                #if 0
+                // Don't reset the IV since some AP would fail in IV check and drop our connection
+                if ( wd->sta.wpaState != ZM_STA_WPA_STATE_PK_OK )
+                {
+                    /* broadcast -- > group key */
+                    /* Only initialize when set our default key ! */
+                    wd->sta.iv16 = 0;
+                    wd->sta.iv32 = 0;
+                }
+                #endif
+
+                encryMode = ZM_AES;
+            }
+            else if ( keyInfo.keyLength == 29 )
+            {
+                zm_debug_msg0("WEP 256");
+
+                encryMode = ZM_WEP256;
+                //zfCoreSetKey(dev, 64, 1, wd->sta.encryMode,
+                //         wd->sta.bssid, (u32_t*) (&keyInfo.key[16]));
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            {
+                u8_t i;
+
+                zm_debug_msg0("key = ");
+                for(i = 0; i < keyInfo.keyLength; i++)
+                {
+                    zm_debug_msg2("", keyInfo.key[i]);
+                }
+            }
+
+            if ( keyInfo.flag & ZM_KEY_FLAG_DEFAULT_KEY )
+            {
+                //for WEP default key 1~3 and ATOM platform--CWYang(+)
+                vapId = 0;
+                wd->ap.bcHalKeyIdx[vapId] = keyInfo.keyIndex;
+                wd->ap.bcKeyIndex[vapId] = keyInfo.keyIndex;
+                wd->sta.keyId = keyInfo.keyIndex;
+            }
+
+			if(encryMode == ZM_TKIP)
+			{
+				if(wd->TKIP_Group_KeyChanging == 0x1)
+				{
+					zm_debug_msg0("Countermeasure : Cancel Old Timer ");
+					zfTimerCancel(dev,	ZM_EVENT_SKIP_COUNTERMEASURE);
+				}
+				else
+				{
+					zm_debug_msg0("Countermeasure : Create New Timer ");
+				}
+
+				wd->TKIP_Group_KeyChanging = 0x1;
+				zfTimerSchedule(dev, ZM_EVENT_SKIP_COUNTERMEASURE, 150);
+			}
+
+
+
+			//------------------------------------------------------------------------
+
+            /* use default key */
+            //zfCoreSetKey(dev, ZM_USER_KEY_DEFAULT+keyInfo.keyIndex, 0,
+            //         wd->sta.encryMode, wd->sta.bssid, (u32_t*) keyInfo.key);
+
+            if ( encryMode == ZM_TKIP ||
+                 encryMode == ZM_AES )
+            {
+                zfHpSetDefaultKey(dev, keyInfo.keyIndex, encryMode,
+                                 (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            {/* If not AES-CCMP and ibss network , use traditional */
+                wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+            }
+            else
+            {
+                if (wd->sta.wpaState == ZM_STA_WPA_STATE_PK_OK)
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+                else
+                {
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+                    wd->sta.encryMode = encryMode;
+                    wd->ws.encryMode = encryMode;
+                }
+            }
+#else
+                if (wd->sta.wpaState == ZM_STA_WPA_STATE_PK_OK)
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+                else if ( wd->sta.wpaState == ZM_STA_WPA_STATE_INIT )
+                {
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+                    wd->sta.encryMode = encryMode;
+                    wd->ws.encryMode = encryMode;
+                }
+#endif
+            }
+            else
+            {
+                zfHpSetDefaultKey(dev, keyInfo.keyIndex, encryMode,
+                               (u32_t*) keyInfo.key, NULL);
+
+                /* Save key for software WEP */
+                zfMemoryCopy(wd->sta.wepKey[keyInfo.keyIndex], keyInfo.key,
+                        keyInfo.keyLength);
+
+                /* TODO: Check whether we need to save the SWEncryMode */
+                wd->sta.SWEncryMode[keyInfo.keyIndex] = encryMode;
+
+                wd->sta.encryMode = encryMode;
+                wd->ws.encryMode = encryMode;
+            }
+        }
+    }
+
+//    wd->sta.flagKeyChanging = 1;
+    return ZM_STATUS_SUCCESS;
+}
+
+/* PSEUDO test */
+u8_t zfiWlanPSEUDOSetKey(zdev_t* dev, struct zsKeyInfo keyInfo)
+{
+    //u16_t  broadcast[3] = {0xffff, 0xffff, 0xffff};
+    //u32_t* key;
+    u8_t   micKey[16];
+
+    zmw_get_wlan_dev(dev);
+
+    switch (keyInfo.keyLength)
+    {
+        case 5:
+            wd->sta.encryMode = ZM_WEP64;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 0, ZM_WEP64, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+		          break;
+
+       	case 13:
+            wd->sta.encryMode = ZM_WEP128;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 0, ZM_WEP128, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+          		break;
+
+       	case 29:
+            wd->sta.encryMode = ZM_WEP256;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 1, ZM_WEP256,  (u16_t *)keyInfo.macAddr, (u32_t*) (&keyInfo.key[16]));
+            zfCoreSetKey(dev, 64, 0, ZM_WEP256, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+		          break;
+
+       	case 16:
+            wd->sta.encryMode = ZM_AES;
+            //zfCoreSetKey(dev, 0, 0, ZM_AES, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            zfCoreSetKey(dev, 64, 0, ZM_AES, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            break;
+
+       	case 32:
+#ifdef ZM_ENABLE_CENC
+            if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+            {
+                u16_t boardcastAddr[3] = {0xffff, 0xffff, 0xffff};
+                u16_t Addr_a[] = { 0x0000, 0x0080, 0x0901};
+                u16_t Addr_b[] = { 0x0000, 0x0080, 0x0902};
+                /* CENC test: user0,1 and user2 for boardcast */
+                wd->sta.encryMode = ZM_CENC;
+                zfCoreSetKey(dev, 0, 1, ZM_CENC, (u16_t *)Addr_a, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 0, 0, ZM_CENC, (u16_t *)Addr_a, (u32_t*) keyInfo.key);
+
+                zfCoreSetKey(dev, 1, 1, ZM_CENC, (u16_t *)Addr_b, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 1, 0, ZM_CENC, (u16_t *)Addr_b, (u32_t*) keyInfo.key);
+
+                zfCoreSetKey(dev, 2, 1, ZM_CENC, (u16_t *)boardcastAddr, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 2, 0, ZM_CENC, (u16_t *)boardcastAddr, (u32_t*) keyInfo.key);
+
+                /* Initialize PN sequence */
+                wd->sta.txiv[0] = 0x5c365c36;
+                wd->sta.txiv[1] = 0x5c365c36;
+                wd->sta.txiv[2] = 0x5c365c36;
+                wd->sta.txiv[3] = 0x5c365c36;
+            }
+            else
+#endif //ZM_ENABLE_CENC
+            {
+                wd->sta.encryMode = ZM_TKIP;
+                zfCoreSetKey(dev, 64, 1, ZM_TKIP, (u16_t *)keyInfo.macAddr, (u32_t*) micKey);
+                zfCoreSetKey(dev, 64, 0, ZM_TKIP, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            }
+            break;
+        default:
+            wd->sta.encryMode = ZM_NO_WEP;
+    }
+
+    return ZM_STATUS_SUCCESS;
+}
+
+void zfiWlanSetPowerSaveMode(zdev_t* dev, u8_t mode)
+{
+#if 0
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.powerSaveMode = mode;
+
+    /* send null data with PwrBit to inform AP */
+    if ( mode > ZM_STA_PS_NONE )
+    {
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            zfSendNullData(dev, 1);
+        }
+
+        /* device into PS mode */
+        zfPSDeviceSleep(dev);
+    }
+#endif
+
+    zfPowerSavingMgrSetMode(dev, mode);
+}
+
+void zfiWlanSetMacAddress(zdev_t* dev, u16_t* mac)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->macAddr[0] = mac[0];
+    wd->macAddr[1] = mac[1];
+    wd->macAddr[2] = mac[2];
+
+    zfHpSetMacAddress(dev, mac, 0);
+}
+
+u8_t zfiWlanQueryWlanMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->wlanMode;
+}
+
+u8_t zfiWlanQueryAdapterState(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->state;
+}
+
+u8_t zfiWlanQueryAuthenticationMode(zdev_t* dev, u8_t bWrapper)
+{
+    u8_t   authMode;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        authMode = wd->ws.authMode;
+    }
+    else
+    {
+        //authMode = wd->sta.authMode;
+        authMode = wd->sta.currentAuthMode;
+    }
+
+    return authMode;
+}
+
+u8_t zfiWlanQueryWepStatus(zdev_t* dev, u8_t bWrapper)
+{
+    u8_t wepStatus;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        wepStatus = wd->ws.wepStatus;
+    }
+    else
+    {
+        wepStatus = wd->sta.wepStatus;
+    }
+
+    return wepStatus;
+}
+
+void zfiWlanQuerySSID(zdev_t* dev, u8_t* ssid, u8_t* pSsidLength)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            *pSsidLength = wd->ap.ssidLen[0];
+            zfMemoryCopy(ssid, wd->ap.ssid[0], wd->ap.ssidLen[0]);
+        }
+        else
+        {
+            *pSsidLength = wd->ap.ssidLen[vapId + 1];
+            zfMemoryCopy(ssid, wd->ap.ssid[vapId + 1], wd->ap.ssidLen[vapId + 1]);
+        }
+    }
+    else
+    {
+        *pSsidLength = wd->sta.ssidLen;
+        zfMemoryCopy(ssid, wd->sta.ssid, wd->sta.ssidLen);
+    }
+}
+
+u16_t zfiWlanQueryFragThreshold(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->fragThreshold;
+}
+
+u16_t zfiWlanQueryRtsThreshold(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->rtsThreshold;
+}
+
+u32_t zfiWlanQueryFrequency(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return (wd->frequency*1000);
+}
+
+/***********************************************************
+ * Function: zfiWlanQueryCurrentFrequency
+ * Return value:
+ *   -   0 : no validate current frequency
+ *   - (>0): current frequency depend on "qmode"
+ * Input:
+ *   - qmode:
+ *      0: return value depend on the support mode, this
+           qmode is use to solve the bug #31223
+ *      1: return the actually current frequency
+ ***********************************************************/
+u32_t zfiWlanQueryCurrentFrequency(zdev_t* dev, u8_t qmode)
+{
+    u32_t frequency;
+
+    zmw_get_wlan_dev(dev);
+
+    switch (qmode)
+    {
+    case 0:
+        if (wd->sta.currentFrequency > 3000)
+        {
+            if (wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                frequency = wd->sta.currentFrequency;
+            }
+            else if (wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                frequency = zfChGetFirst2GhzChannel(dev);
+            }
+            else
+            {
+                frequency = 0;
+            }
+        }
+        else
+        {
+            if (wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                frequency = wd->sta.currentFrequency;
+            }
+            else if (wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                frequency = zfChGetLast5GhzChannel(dev);
+            }
+            else
+            {
+                frequency = 0;
+            }
+        }
+        break;
+
+    case 1:
+        frequency = wd->sta.currentFrequency;
+        break;
+
+    default:
+        frequency = 0;
+    }
+
+    return (frequency*1000);
+}
+
+u32_t zfiWlanQueryFrequencyAttribute(zdev_t* dev, u32_t freq)
+{
+    zmw_get_wlan_dev(dev);
+
+    u8_t  i;
+    u16_t frequency = (u16_t) (freq/1000);
+    u32_t ret = 0;
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if ( wd->regulationTable.allowChannel[i].channel == frequency )
+        {
+            ret = wd->regulationTable.allowChannel[i].channelFlags;
+        }
+    }
+
+    return ret;
+}
+
+/* BandWidth  0=>20  1=>40 */
+/* ExtOffset  0=>20  1=>high control 40   3=>low control 40 */
+void zfiWlanQueryFrequencyHT(zdev_t* dev, u32_t *bandWidth, u32_t *extOffset)
+{
+    zmw_get_wlan_dev(dev);
+
+    *bandWidth = wd->BandWidth40;
+    *extOffset = wd->ExtOffset;
+}
+
+u8_t zfiWlanQueryCWMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->cwm.cw_mode;
+}
+
+u32_t zfiWlanQueryCWEnable(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->cwm.cw_enable;
+}
+
+void zfiWlanQueryBssid(zdev_t* dev, u8_t* bssid)
+{
+    u8_t   addr[6];
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, addr);
+    zfMemoryCopy(bssid, addr, 6);
+}
+
+u16_t zfiWlanQueryBeaconInterval(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->beaconInterval;
+}
+
+u32_t zfiWlanQueryRxBeaconTotal(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    wd->sta.rxBeaconTotal += wd->sta.rxBeaconCount;
+
+    return wd->sta.rxBeaconTotal;
+}
+
+u16_t zfiWlanQueryAtimWindow(zdev_t* dev)
+{
+    u16_t atimWindow;
+
+    zmw_get_wlan_dev(dev);
+
+    atimWindow = wd->sta.atimWindow;
+
+    return atimWindow;
+}
+
+u8_t zfiWlanQueryEncryMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+        return wd->ap.encryMode[0];
+    else
+        return wd->sta.encryMode;
+}
+
+u16_t zfiWlanQueryCapability(zdev_t* dev)
+{
+    u16_t capability;
+
+    zmw_get_wlan_dev(dev);
+
+    capability = wd->sta.capability[0] +
+                 (((u16_t) wd->sta.capability[1]) << 8);
+
+    return capability;
+
+}
+
+u16_t zfiWlanQueryAid(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.aid;
+}
+
+void zfiWlanQuerySupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength)
+{
+    u8_t   i, j=0;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<4; i++ )
+    {
+        if ( wd->bRate & (0x1 << i) )
+        {
+            rateArray[j] = zg11bRateTbl[i] +
+                           ((wd->bRateBasic & (0x1<<i))<<(7-i));
+            j++;
+        }
+    }
+
+    *pLength = j;
+}
+
+void zfiWlanQueryExtSupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength)
+{
+    u8_t   i, j=0;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<8; i++ )
+    {
+        if ( wd->gRate & (0x1 << i) )
+        {
+            rateArray[j] = zg11gRateTbl[i] +
+                           ((wd->gRateBasic & (0x1<<i))<<(7-i));
+            j++;
+        }
+    }
+
+    *pLength = j;
+}
+
+void zfiWlanQueryRsnIe(zdev_t* dev, u8_t* ie, u8_t* pLength)
+{
+    u8_t len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = wd->sta.rsnIe[1] + 2;
+    zfMemoryCopy(ie, wd->sta.rsnIe, len);
+    *pLength = len;
+}
+
+void zfiWlanQueryWpaIe(zdev_t* dev, u8_t* ie, u8_t* pLength)
+{
+    u8_t len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = wd->sta.wpaIe[1] + 2;
+    zfMemoryCopy(ie, wd->sta.wpaIe, len);
+    *pLength = len;
+
+}
+
+u8_t zfiWlanQueryMulticastCipherAlgo(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch( wd->sta.currentAuthMode )
+    {
+        case ZM_AUTH_MODE_WPA2PSK:
+        case ZM_AUTH_MODE_WPA2:
+            if ( wd->sta.rsnIe[7] == 2 )
+            {
+                return ZM_TKIP;
+            }
+            else
+            {
+                return ZM_AES;
+            }
+            break;
+
+        case ZM_AUTH_MODE_WPAPSK:
+        case ZM_AUTH_MODE_WPA:
+            if ( wd->sta.rsnIe[11] == 2 )
+            {
+                return ZM_TKIP;
+            }
+            else
+            {
+                return ZM_AES;
+            }
+            break;
+
+        default:
+            return wd->sta.encryMode;
+    }
+}
+
+u8_t zfiWlanQueryHTMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    // 0:Legancy, 1:N
+    return wd->sta.EnableHT;
+}
+
+u8_t zfiWlanQueryBandWidth40(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    // 0:20M, 1:40M
+    return wd->BandWidth40;
+}
+
+u16_t zfiWlanQueryRegionCode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->regulationTable.regionCode;
+}
+void zfiWlanSetWpaIe(zdev_t* dev, u8_t* ie, u8_t Length)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+            vapId = 0;
+        else
+            vapId++;
+
+        zm_assert(Length < ZM_MAX_WPAIE_SIZE);
+        if (Length < ZM_MAX_WPAIE_SIZE)
+        {
+            wd->ap.wpaLen[vapId] = Length;
+            zfMemoryCopy(wd->ap.wpaIe[vapId], ie, wd->ap.wpaLen[vapId]);
+        }
+
+    }
+    else
+    {
+        wd->sta.wpaLen = Length;
+        zfMemoryCopy(wd->sta.wpaIe, ie, wd->sta.wpaLen);
+    }
+    //zfiWlanSetWpaSupport(dev, 1);
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        wd->ap.wpaSupport[vapId] = 1;
+    }
+    else
+    {
+        wd->sta.wpaSupport = 1;
+    }
+
+}
+
+void zfiWlanSetWpaSupport(zdev_t* dev, u8_t WpaSupport)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+            vapId = 0;
+        else
+            vapId++;
+
+        wd->ap.wpaSupport[vapId] = WpaSupport;
+    }
+    else
+    {
+        wd->sta.wpaSupport = WpaSupport;
+    }
+
+}
+
+void zfiWlanSetProtectionMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bProtectionMode = mode;
+    if (wd->sta.bProtectionMode == TRUE)
+    {
+        zfHpSetSlotTime(dev, 0);
+    }
+    else
+    {
+        zfHpSetSlotTime(dev, 1);
+    }
+
+    zm_msg1_mm(ZM_LV_1, "wd->protectionMode=", wd->sta.bProtectionMode);
+}
+
+void zfiWlanSetBasicRate(zdev_t* dev, u8_t bRateSet, u8_t gRateSet,
+                         u32_t nRateSet)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.bRateBasic = bRateSet;
+    wd->ws.gRateBasic = gRateSet;
+    wd->ws.nRateBasic = nRateSet;
+}
+
+void zfiWlanSetBGMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.bgMode = mode;
+}
+
+void zfiWlanSetpreambleType(zdev_t* dev, u8_t type)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.preambleType = type;
+}
+
+u8_t zfiWlanQuerypreambleType(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.preambleType;
+}
+
+u8_t zfiWlanQueryPowerSaveMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.powerSaveMode;
+}
+
+u8_t zfiWlanSetPmkidInfo(zdev_t* dev, u16_t* bssid, u8_t* pmkid)
+{
+    u32_t  i;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+    {
+        if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid,
+                             (u8_t*) bssid, 6) )
+        {
+            /* matched */
+            break;
+        }
+    }
+
+    if ( i < wd->sta.pmkidInfo.bssidCount )
+    {
+        /* overwrite the original one */
+        zfMemoryCopy(wd->sta.pmkidInfo.bssidInfo[i].pmkid, pmkid, 16);
+    }
+    else
+    {
+        if ( i < ZM_PMKID_MAX_BSS_CNT )
+        {
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[0] = bssid[0];
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[1] = bssid[1];
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[2] = bssid[2];
+
+            zfMemoryCopy(wd->sta.pmkidInfo.bssidInfo[i].pmkid, pmkid, 16);
+            wd->sta.pmkidInfo.bssidCount++;
+        }
+    }
+
+    return 0;
+}
+
+u32_t zfiWlanQueryPmkidInfo(zdev_t* dev, u8_t* buf, u32_t len)
+{
+    //struct zsPmkidInfo* pPmkidInfo = ( struct zsPmkidInfo* ) buf;
+    u32_t  size;
+
+    zmw_get_wlan_dev(dev);
+
+    size = sizeof(u32_t) +
+           wd->sta.pmkidInfo.bssidCount * sizeof(struct zsPmkidBssidInfo);
+
+    if ( len < size )
+    {
+        return wd->sta.pmkidInfo.bssidCount;
+    }
+
+    zfMemoryCopy(buf, (u8_t*) &wd->sta.pmkidInfo, (u16_t) size);
+
+    return 0;
+}
+
+void zfiWlanSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList)
+{
+    struct zsMulticastAddr* pMacList = (struct zsMulticastAddr*) pList;
+    u8_t   i;
+    u8_t   bAllMulticast = 0;
+    //u32_t  value;
+
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.multicastList.size = size;
+    for(i=0; i<size; i++)
+    {
+        zfMemoryCopy(wd->sta.multicastList.macAddr[i].addr,
+                     pMacList[i].addr, 6);
+    }
+
+    if ( wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST )
+        bAllMulticast = 1;
+    zfHpSetMulticastList(dev, size, pList, bAllMulticast);
+
+}
+
+void zfiWlanRemoveKey(zdev_t* dev, u8_t keyType, u8_t keyId)
+{
+    u16_t  fakeMacAddr[3] = {0, 0, 0};
+    u32_t  fakeKey[4] = {0, 0, 0, 0};
+
+    zmw_get_wlan_dev(dev);
+
+    if ( keyType == 0 )
+    {
+        /* remove WEP key */
+        zm_debug_msg0("remove WEP key");
+        zfCoreSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0,
+                 ZM_NO_WEP, fakeMacAddr, fakeKey);
+        wd->sta.encryMode = ZM_NO_WEP;
+    }
+    else if ( keyType == 1 )
+    {
+        /* remove pairwise key */
+        zm_debug_msg0("remove pairwise key");
+        zfHpRemoveKey(dev, ZM_USER_KEY_PK);
+        wd->sta.encryMode = ZM_NO_WEP;
+    }
+    else
+    {
+        /* remove group key */
+        zm_debug_msg0("remove group key");
+        zfHpRemoveKey(dev, ZM_USER_KEY_GK);
+    }
+}
+
+
+void zfiWlanQueryRegulationTable(zdev_t* dev, struct zsRegulationTable* pEntry)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t*) pEntry, (u8_t*) &wd->regulationTable,
+                 sizeof(struct zsRegulationTable));
+}
+
+/* parameter "time" is specified in ms */
+void zfiWlanSetScanTimerPerChannel(zdev_t* dev, u16_t time)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("scan time (ms) = ", time);
+
+    wd->sta.activescanTickPerChannel = time / ZM_MS_PER_TICK;
+}
+
+void zfiWlanSetAutoReconnect(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bAutoReconnect = enable;
+    //wd->sta.bAutoReconnectEnabled = enable;
+}
+
+void zfiWlanSetStaWme(zdev_t* dev, u8_t enable, u8_t uapsdInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.staWmeEnabled = enable & 0x3;
+    if ((enable & 0x2) != 0)
+    {
+        wd->ws.staWmeQosInfo = uapsdInfo & 0x6f;
+    }
+    else
+    {
+        wd->ws.staWmeQosInfo = 0;
+    }
+}
+
+void zfiWlanSetApWme(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.apWmeEnabled = enable;
+}
+
+u8_t zfiWlanQuerywmeEnable(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.staWmeEnabled;
+}
+
+void zfiWlanSetProbingHiddenSsid(zdev_t* dev, u8_t* ssid, u8_t ssidLen,
+    u16_t entry)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    if ((ssidLen <= 32) && (entry < ZM_MAX_PROBE_HIDDEN_SSID_SIZE))
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.probingSsidList[entry].ssidLen = ssidLen;
+        zfMemoryCopy(wd->ws.probingSsidList[entry].ssid, ssid, ssidLen);
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+void zfiWlanSetDisableProbingWithSsid(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.disableProbingWithSsid = mode;
+
+    return;
+}
+
+void zfiWlanSetDropUnencryptedPackets(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.dropUnencryptedPkts = enable;
+}
+
+void zfiWlanSetStaRxSecurityCheckCb(zdev_t* dev, zfpStaRxSecurityCheckCb pStaRxSecurityCheckCb)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.pStaRxSecurityCheckCb = pStaRxSecurityCheckCb;
+}
+
+void zfiWlanSetIBSSJoinOnly(zdev_t* dev, u8_t joinOnly)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.ibssJoinOnly = joinOnly;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiConfigWdsPort            */
+/*      Configure WDS port.                                             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      wdsPortId : WDS port ID, start from 0                           */
+/*      flag : 0=>disable WDS port, 1=>enable WDS port                  */
+/*      wdsAddr : WDS neighbor MAC address                              */
+/*      encType : encryption type for WDS port                          */
+/*      wdsKey : encryption key for WDS port                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiConfigWdsPort(zdev_t* dev, u8_t wdsPortId, u16_t flag, u16_t* wdsAddr,
+        u16_t encType, u32_t* wdsKey)
+{
+    u16_t addr[3];
+    u32_t key[4];
+
+    zmw_get_wlan_dev(dev);
+
+    if (wdsPortId > ZM_MAX_WDS_SUPPORT)
+    {
+        return ZM_ERR_WDS_PORT_ID;
+    }
+
+    if (flag == 1)
+    {
+        /* Enable WDS port */
+        wd->ap.wds.macAddr[wdsPortId][0] = wdsAddr[0];
+        wd->ap.wds.macAddr[wdsPortId][1] = wdsAddr[1];
+        wd->ap.wds.macAddr[wdsPortId][2] = wdsAddr[2];
+
+        wd->ap.wds.wdsBitmap |= (1 << wdsPortId);
+        wd->ap.wds.encryMode[wdsPortId] = (u8_t) encType;
+
+        zfCoreSetKey(dev, 10+ZM_MAX_WDS_SUPPORT, 0, (u8_t) encType, wdsAddr, wdsKey);
+    }
+    else
+    {
+        /* Disable WDS port */
+        addr[0] = addr[1] = addr[2] = 0;
+        key[0] = key[1] = key[2] = key[3] = 0;
+        wd->ap.wds.wdsBitmap &= (~(1 << wdsPortId));
+        zfCoreSetKey(dev, 10+ZM_MAX_WDS_SUPPORT, 0, ZM_NO_WEP, addr, key);
+    }
+
+    return ZM_SUCCESS;
+}
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+void zfiWlanQueryGSN(zdev_t* dev, u8_t *gsn, u16_t vapId)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u32_t txiv[4];
+    zmw_get_wlan_dev(dev);
+
+    /* convert little endian to big endian for 32 bits */
+    txiv[3] = wd->ap.txiv[vapId][0];
+    txiv[2] = wd->ap.txiv[vapId][1];
+    txiv[1] = wd->ap.txiv[vapId][2];
+    txiv[0] = wd->ap.txiv[vapId][3];
+
+    zfMemoryCopy(gsn, (u8_t*)txiv, 16);
+}
+#endif //ZM_ENABLE_CENC
+//CWYang(+)
+void zfiWlanQuerySignalInfo(zdev_t* dev, u8_t *buffer)
+{
+    zmw_get_wlan_dev(dev);
+
+    /*Change Signal Strength/Quality Value to Human Sense Here*/
+
+    buffer[0] = wd->SignalStrength;
+    buffer[1] = wd->SignalQuality;
+}
+
+/* OS-XP */
+u16_t zfiStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+{
+    return  zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+}
+
+/* zfiDebugCmd                                                                        */
+/*     cmd       value-description                                                  */
+/*         0       schedule timer                                                     */
+/*         1       cancel timer                                                         */
+/*         2       clear timer                                                           */
+/*         3       test timer                                                            */
+/*         4                                                                                 */
+/*         5                                                                                 */
+/*         6       checksum test     0/1                                           */
+/*         7       enableProtectionMode                                          */
+/*         8       rx packet content dump    0/1                               */
+
+u32_t zfiDebugCmd(zdev_t* dev, u32_t cmd, u32_t value)
+{
+    u16_t event;
+    u32_t tick;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ( cmd == 0 )
+    {   /* schedule timer */
+        event = (u16_t) ((value >> 16) & 0xffff);
+        tick = value & 0xffff;
+        zfTimerSchedule(dev, event, tick);
+    }
+    else if ( cmd == 1 )
+    {   /* cancel timer */
+        event = (u16_t) (value & 0xffff);
+        zfTimerCancel(dev, event);
+    }
+    else if ( cmd == 2 )
+    {   /* clear timer */
+        zfTimerClear(dev);
+    }
+    else if ( cmd == 3 )
+    {   /* test timer */
+        zfTimerSchedule(dev, 1,  500);
+        zfTimerSchedule(dev, 2, 1000);
+        zfTimerSchedule(dev, 3, 1000);
+        zfTimerSchedule(dev, 4, 1000);
+        zfTimerSchedule(dev, 5, 1500);
+        zfTimerSchedule(dev, 6, 2000);
+        zfTimerSchedule(dev, 7, 2200);
+        zfTimerSchedule(dev, 6, 2500);
+        zfTimerSchedule(dev, 8, 2800);
+    }
+    else if ( cmd == 4)
+    {
+        zfTimerSchedule(dev, 1,  500);
+        zfTimerSchedule(dev, 2, 1000);
+        zfTimerSchedule(dev, 3, 1000);
+        zfTimerSchedule(dev, 4, 1000);
+        zfTimerSchedule(dev, 5, 1500);
+        zfTimerSchedule(dev, 6, 2000);
+        zfTimerSchedule(dev, 7, 2200);
+        zfTimerSchedule(dev, 6, 2500);
+        zfTimerSchedule(dev, 8, 2800);
+        zfTimerCancel(dev, 1);
+        zfTimerCancel(dev, 3);
+        zfTimerCancel(dev, 6);
+    }
+    else if ( cmd == 5 )
+    {
+        wd->sta.keyId = (u8_t) value;
+    }
+	else if ( cmd == 6 )
+	{
+	    /* 0: normal    1: always set TCP/UDP checksum zero */
+        wd->checksumTest = value;
+	}
+	else if ( cmd == 7 )
+	{
+        wd->enableProtectionMode = value;
+   	    zm_msg1_mm(ZM_LV_1, "wd->enableProtectionMode=", wd->enableProtectionMode);
+	}
+	else if ( cmd == 8 )
+	{
+        /* rx packet content dump */
+        if (value)
+        {
+            wd->rxPacketDump = 1;
+        }
+        else
+        {
+            wd->rxPacketDump = 0;
+        }
+	}
+
+
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+#ifdef ZM_ENABLE_CENC
+u8_t zfiWlanSetCencPairwiseKey(zdev_t* dev, u8_t keyid, u32_t *txiv, u32_t *rxiv,
+        u8_t *key, u8_t *mic)
+{
+    struct zsKeyInfo keyInfo;
+    u8_t cencKey[32];
+    u8_t i;
+    u16_t macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < 16; i++)
+        cencKey[i] = key[i];
+    for (i = 0; i < 16; i++)
+        cencKey[i + 16] = mic[i];
+    keyInfo.key = cencKey;
+    keyInfo.keyLength = 32;
+    keyInfo.keyIndex = keyid;
+    keyInfo.flag = ZM_KEY_FLAG_CENC | ZM_KEY_FLAG_PK;
+    for (i = 0; i < 3; i++)
+        macAddr[i] = wd->sta.bssid[i];
+    keyInfo.macAddr = macAddr;
+
+    zfiWlanSetKey(dev, keyInfo);
+
+    /* Reset txiv and rxiv */
+    //wd->sta.txiv[0] = txiv[0];
+    //wd->sta.txiv[1] = txiv[1];
+    //wd->sta.txiv[2] = txiv[2];
+    //wd->sta.txiv[3] = txiv[3];
+    //
+    //wd->sta.rxiv[0] = rxiv[0];
+    //wd->sta.rxiv[1] = rxiv[1];
+    //wd->sta.rxiv[2] = rxiv[2];
+    //wd->sta.rxiv[3] = rxiv[3];
+
+    return 0;
+}
+
+u8_t zfiWlanSetCencGroupKey(zdev_t* dev, u8_t keyid, u32_t *rxiv,
+        u8_t *key, u8_t *mic)
+{
+    struct zsKeyInfo keyInfo;
+    u8_t cencKey[32];
+    u8_t i;
+    u16_t macAddr[6] = {0xffff, 0xffff, 0xffff};
+
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < 16; i++)
+        cencKey[i] = key[i];
+    for (i = 0; i < 16; i++)
+        cencKey[i + 16] = mic[i];
+    keyInfo.key = cencKey;
+    keyInfo.keyLength = 32;
+    keyInfo.keyIndex = keyid;
+    keyInfo.flag = ZM_KEY_FLAG_CENC | ZM_KEY_FLAG_GK;
+    keyInfo.vapId = 0;
+    for (i = 0; i < 3; i++)
+        keyInfo.vapAddr[i] = wd->macAddr[i];
+    keyInfo.macAddr = macAddr;
+
+    zfiWlanSetKey(dev, keyInfo);
+
+    /* Reset txiv and rxiv */
+    wd->sta.rxivGK[0] = ((rxiv[3] >> 24) & 0xFF)
+                      + (((rxiv[3] >> 16) & 0xFF) << 8)
+                      + (((rxiv[3] >> 8) & 0xFF) << 16)
+                      + ((rxiv[3] & 0xFF) << 24);
+    wd->sta.rxivGK[1] = ((rxiv[2] >> 24) & 0xFF)
+                      + (((rxiv[2] >> 16) & 0xFF) << 8)
+                      + (((rxiv[2] >> 8) & 0xFF) << 16)
+                      + ((rxiv[2] & 0xFF) << 24);
+    wd->sta.rxivGK[2] = ((rxiv[1] >> 24) & 0xFF)
+                      + (((rxiv[1] >> 16) & 0xFF) << 8)
+                      + (((rxiv[1] >> 8) & 0xFF) << 16)
+                      + ((rxiv[1] & 0xFF) << 24);
+    wd->sta.rxivGK[3] = ((rxiv[0] >> 24) & 0xFF)
+                      + (((rxiv[0] >> 16) & 0xFF) << 8)
+                      + (((rxiv[0] >> 8) & 0xFF) << 16)
+                      + ((rxiv[0] & 0xFF) << 24);
+
+    wd->sta.authMode = ZM_AUTH_MODE_CENC;
+    wd->sta.currentAuthMode = ZM_AUTH_MODE_CENC;
+
+    return 0;
+}
+#endif //ZM_ENABLE_CENC
+
+u8_t zfiWlanSetDot11DMode(zdev_t* dev, u8_t mode)
+{
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.b802_11D = mode;
+    if (mode) //Enable 802.11d
+    {
+        wd->regulationTable.regionCode = NO_ENUMRD;
+        for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+            wd->regulationTable.allowChannel[i].channelFlags |= ZM_REG_FLAG_CHANNEL_PASSIVE;
+    }
+    else //Disable
+    {
+        for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+            wd->regulationTable.allowChannel[i].channelFlags &= ~ZM_REG_FLAG_CHANNEL_PASSIVE;
+    }
+
+    return 0;
+}
+
+u8_t zfiWlanSetDot11HDFSMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg0("CWY - Enable 802.11h DFS");
+
+    // TODO : DFS Enable in 5250 to 5350 MHz and 5470 to 5725 MHz .
+    //if ( Adapter->ZD80211HSupport &&
+    //   Adapter->CardSetting.NetworkTypeInUse == Ndis802_11OFDM5 &&
+    //   ((ChannelNo >=52 && ChannelNo <= 64)	||				//5250~5350 MHZ
+    //    (ChannelNo >=100 && ChannelNo <= 140))) 			//5470~5725 MHZ
+    //{
+    //   Adapter->ZD80211HSetting.DFSEnable=TRUE;
+    //}
+    //else
+    //{
+    //   Adapter->ZD80211HSetting.DFSEnable=FALSE;
+    //}
+
+    wd->sta.DFSEnable = mode;
+    if (mode)
+        wd->sta.capability[1] |= ZM_BIT_0;
+    else
+        wd->sta.capability[1] &= (~ZM_BIT_0);
+
+    return 0;
+}
+
+u8_t zfiWlanSetDot11HTPCMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    // TODO : TPC Enable in 5150~5350 MHz and 5470~5725MHz.
+    //if ( Adapter->ZD80211HSupport &&
+    //   Adapter->CardSetting.NetworkTypeInUse == Ndis802_11OFDM5 &&
+    //   ((ChannelNo == 36 || ChannelNo == 40 || ChannelNo == 44 || ChannelNo == 48) ||	//5150~5250 MHZ , Not Japan
+    //    (ChannelNo >=52 && ChannelNo <= 64) ||				//5250~5350 MHZ
+    //    (ChannelNo >=100 && ChannelNo <= 140))) 			//5470~5725 MHZ
+    //{
+    //   Adapter->ZD80211HSetting.TPCEnable=TRUE;
+    //}
+    //else
+    //{
+    //   Adapter->ZD80211HSetting.TPCEnable=FALSE;
+    //}
+
+    wd->sta.TPCEnable = mode;
+    if (mode)
+        wd->sta.capability[1] |= ZM_BIT_0;
+    else
+        wd->sta.capability[1] &= (~ZM_BIT_0);
+
+    return 0;
+}
+
+u8_t zfiWlanSetAniMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->aniEnable = mode;
+    if (mode)
+        zfHpAniAttach(dev);
+
+    return 0;
+}
+
+#ifdef ZM_OS_LINUX_FUNC
+void zfiWlanShowTally(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_mm(ZM_LV_0, "Hw_UnderrunCnt    = ", wd->commTally.Hw_UnderrunCnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_TotalRxFrm     = ", wd->commTally.Hw_TotalRxFrm);
+    zm_msg1_mm(ZM_LV_0, "Hw_CRC32Cnt       = ", wd->commTally.Hw_CRC32Cnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_CRC16Cnt       = ", wd->commTally.Hw_CRC16Cnt);
+    zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_UNI  = ", wd->commTally.Hw_DecrypErr_UNI);
+    zm_msg1_mm(ZM_LV_0, "Hw_RxFIFOOverrun  = ", wd->commTally.Hw_RxFIFOOverrun);
+    zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_Mul  = ", wd->commTally.Hw_DecrypErr_Mul);
+    zm_msg1_mm(ZM_LV_1, "Hw_RetryCnt       = ", wd->commTally.Hw_RetryCnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_TotalTxFrm     = ", wd->commTally.Hw_TotalTxFrm);
+    zm_msg1_mm(ZM_LV_0, "Hw_RxTimeOut      = ", wd->commTally.Hw_RxTimeOut);
+    zm_msg1_mm(ZM_LV_0, "Tx_MPDU           = ", wd->commTally.Tx_MPDU);
+    zm_msg1_mm(ZM_LV_0, "BA_Fail           = ", wd->commTally.BA_Fail);
+    zm_msg1_mm(ZM_LV_0, "Hw_Tx_AMPDU       = ", wd->commTally.Hw_Tx_AMPDU);
+    zm_msg1_mm(ZM_LV_0, "Hw_Tx_MPDU        = ", wd->commTally.Hw_Tx_MPDU);
+
+    zm_msg1_mm(ZM_LV_1, "Hw_RxMPDU          = ", wd->commTally.Hw_RxMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxDropMPDU      = ", wd->commTally.Hw_RxDropMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxDelMPDU       = ", wd->commTally.Hw_RxDelMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyMiscError  = ", wd->commTally.Hw_RxPhyMiscError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyXRError    = ", wd->commTally.Hw_RxPhyXRError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyOFDMError  = ", wd->commTally.Hw_RxPhyOFDMError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyCCKError   = ", wd->commTally.Hw_RxPhyCCKError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyHTError    = ", wd->commTally.Hw_RxPhyHTError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyTotalCount = ", wd->commTally.Hw_RxPhyTotalCount);
+
+    if (!((wd->commTally.Tx_MPDU == 0) && (wd->commTally.BA_Fail == 0)))
+    {
+        zm_debug_msg_p("BA Fail Ratio(%)  = ", wd->commTally.BA_Fail * 100,
+                (wd->commTally.BA_Fail + wd->commTally.Tx_MPDU));
+    }
+
+    if (!((wd->commTally.Hw_Tx_MPDU == 0) && (wd->commTally.Hw_Tx_AMPDU == 0)))
+    {
+        zm_debug_msg_p("Avg Agg Number    = ",
+                wd->commTally.Hw_Tx_MPDU, wd->commTally.Hw_Tx_AMPDU);
+    }
+}
+#endif
+
+void zfiWlanSetMaxTxPower(zdev_t* dev, u8_t power2, u8_t power5)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->maxTxPower2 = power2;
+    wd->maxTxPower5 = power5;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanQueryMaxTxPower(zdev_t* dev, u8_t *power2, u8_t *power5)
+{
+    zmw_get_wlan_dev(dev);
+
+    *power2 = wd->maxTxPower2;
+    *power5 = wd->maxTxPower5;
+}
+
+void zfiWlanSetConnectMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->connectMode = mode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetSupportMode(zdev_t* dev, u32_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->supportMode = mode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetAdhocMode(zdev_t* dev, u32_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.adhocMode = mode;
+}
+
+u32_t zfiWlanQueryAdhocMode(zdev_t* dev, u8_t bWrapper)
+{
+    u32_t adhocMode;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        adhocMode = wd->ws.adhocMode;
+    }
+    else
+    {
+        adhocMode = wd->wfc.bIbssGMode;
+    }
+
+    return adhocMode;
+}
+
+
+u8_t zfiWlanSetCountryIsoName(zdev_t* dev, u8_t *countryIsoName, u8_t length)
+{
+    u8_t buf[5];
+    zmw_get_wlan_dev(dev);
+
+    if (length == 4)
+    {
+        buf[2] = wd->ws.countryIsoName[0] = countryIsoName[2];
+        buf[3] = wd->ws.countryIsoName[1] = countryIsoName[1];
+        buf[4] = wd->ws.countryIsoName[2] = countryIsoName[0];
+    }
+    else if (length == 3)
+    {
+        buf[2] = wd->ws.countryIsoName[0] = countryIsoName[1];
+        buf[3] = wd->ws.countryIsoName[1] = countryIsoName[0];
+        buf[4] = wd->ws.countryIsoName[2] = '\0';
+    }
+    else
+    {
+        return 1;
+    }
+
+    return zfHpGetRegulationTablefromISO(dev, buf, length);
+}
+
+
+const char* zfiWlanQueryCountryIsoName(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.countryIsoName;
+}
+
+
+
+void zfiWlanSetRegulatory(zdev_t* dev, u8_t CCS, u16_t Code, u8_t bfirstChannel)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (CCS)
+    {
+        /* Reset Regulation Table by Country Code */
+        zfHpGetRegulationTablefromCountry(dev, Code);
+    }
+    else
+    {
+        /* Reset Regulation Table by Region Code */
+        zfHpGetRegulationTablefromRegionCode(dev, Code);
+    }
+
+    if (bfirstChannel) {
+        zmw_enter_critical_section(dev);
+        wd->frequency = zfChGetFirstChannel(dev, NULL);
+        zmw_leave_critical_section(dev);
+        zfCoreSetFrequency(dev, wd->frequency);
+    }
+}
+
+
+const char* zfiHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode)
+{
+    return zfHpGetisoNamefromregionCode(dev, regionCode);
+}
+
+u16_t zfiWlanChannelToFrequency(zdev_t* dev, u8_t channel)
+{
+    return zfChNumToFreq(dev, channel, 0);
+}
+
+u8_t zfiWlanFrequencyToChannel(zdev_t* dev, u16_t freq)
+{
+    u8_t is5GBand = 0;
+
+    return zfChFreqToNum(freq, &is5GBand);
+}
+
+void zfiWlanDisableDfsChannel(zdev_t* dev, u8_t disableFlag)
+{
+    zfHpDisableDfsChannel(dev, disableFlag);
+    return;
+}
+
+void zfiWlanSetLEDCtrlParam(zdev_t* dev, u8_t type, u8_t flag)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ledStruct.LEDCtrlType = type;
+    wd->ledStruct.LEDCtrlFlagFromReg  = flag;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanEnableLeapConfig(zdev_t* dev, u8_t leapEnabled)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.leapEnabled = leapEnabled;
+}
+
+u32_t zfiWlanQueryHwCapability(zdev_t* dev)
+{
+    return zfHpCapability(dev);
+}
+
+u32_t zfiWlanQueryReceivedPacket(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.ReceivedPktRatePerSecond;
+}
+
+void zfiWlanCheckSWEncryption(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zfHpSWDecrypt(dev, 1);
+    }
+}
+
+u16_t zfiWlanQueryAllowChannels(zdev_t* dev, u16_t *channels)
+{
+    u16_t ii;
+    zmw_get_wlan_dev(dev);
+
+    for (ii = 0; ii < wd->regulationTable.allowChannelCnt; ii++)
+    {
+        channels[ii] = wd->regulationTable.allowChannel[ii].channel;
+    }
+
+    return wd->regulationTable.allowChannelCnt;
+}
+
+void zfiWlanSetDynamicSIFSParam(zdev_t* dev, u8_t val)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->dynamicSIFSEnable = val;
+
+    zm_debug_msg1("wd->dynamicSIFSEnable = ", wd->dynamicSIFSEnable)
+}
+
+u16_t zfiWlanGetMulticastAddressCount(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.multicastList.size;
+}
+
+void zfiWlanGetMulticastList(zdev_t* dev, u8_t* pMCList)
+{
+    struct zsMulticastAddr* pMacList = (struct zsMulticastAddr*) pMCList;
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for ( i=0; i<wd->sta.multicastList.size; i++ )
+    {
+        zfMemoryCopy(pMacList[i].addr, wd->sta.multicastList.macAddr[i].addr, 6);
+    }
+}
+
+void zfiWlanSetPacketFilter(zdev_t* dev, u32_t PacketFilter)
+{
+    u8_t  bAllMulticast = 0;
+    u32_t oldFilter;
+
+    zmw_get_wlan_dev(dev);
+
+    oldFilter = wd->sta.osRxFilter;
+
+    wd->sta.osRxFilter = PacketFilter;
+
+    if ((oldFilter & ZM_PACKET_TYPE_ALL_MULTICAST) !=
+        (wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST))
+    {
+        if ( wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST )
+            bAllMulticast = 1;
+        zfHpSetMulticastList(dev, wd->sta.multicastList.size,
+                             (u8_t*)wd->sta.multicastList.macAddr, bAllMulticast);
+    }
+}
+
+u8_t zfiCompareWithMulticastListAddress(zdev_t* dev, u16_t* dstMacAddr)
+{
+    u8_t i;
+    u8_t bIsInMCListAddr = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    for ( i=0; i<wd->sta.multicastList.size; i++ )
+    {
+    	if ( zfwMemoryIsEqual((u8_t*)dstMacAddr, (u8_t*)wd->sta.multicastList.macAddr[i].addr, 6) )
+    	{
+            bIsInMCListAddr = 1;
+            break;
+    	}
+    }
+
+    return bIsInMCListAddr;
+}
+
+void zfiWlanSetSafeModeEnabled(zdev_t* dev, u8_t safeMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bSafeMode = safeMode;
+
+    if ( safeMode )
+    	zfStaEnableSWEncryption(dev, 1);
+    else
+        zfStaDisableSWEncryption(dev);
+}
+
+void zfiWlanSetIBSSAdditionalIELength(zdev_t* dev, u32_t ibssAdditionalIESize, u8_t* ibssAdditionalIE)
+{
+	zmw_get_wlan_dev(dev);
+
+	if ( ibssAdditionalIESize )
+    {
+	    wd->sta.ibssAdditionalIESize = ibssAdditionalIESize;
+        zfMemoryCopy(wd->sta.ibssAdditionalIE, ibssAdditionalIE, (u16_t)ibssAdditionalIESize);
+    }
+    else
+    	wd->sta.ibssAdditionalIESize = 0;
+}
diff --git a/drivers/staging/otus/80211core/cprecomp.h b/drivers/staging/otus/80211core/cprecomp.h
new file mode 100644
index 000000000000..1670bfc22587
--- /dev/null
+++ b/drivers/staging/otus/80211core/cprecomp.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _CPRECOMP_H
+#define _CPRECOMP_H
+
+#include "../oal_dt.h"
+#include "../oal_marc.h"
+#include "pub_zfi.h"
+#include "pub_zfw.h"
+#include "pub_usb.h"
+#include "wlan.h"
+#include "struct.h"
+#include "cfunc.h"
+#include "cagg.h"
+#include "cwm.h"
+#include "performance.h"
+#endif
+
diff --git a/drivers/staging/otus/80211core/cpsmgr.c b/drivers/staging/otus/80211core/cpsmgr.c
new file mode 100644
index 000000000000..cf73caca8e52
--- /dev/null
+++ b/drivers/staging/otus/80211core/cpsmgr.c
@@ -0,0 +1,731 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/**
+  *  The power saving manager is to save the power as much as possible.
+  *  Generally speaking, it controls:
+  *
+  *         - when to sleep
+  *         -
+  *
+  */
+#include "cprecomp.h"
+
+void zfPowerSavingMgrInit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.powerSaveMode = ZM_STA_PS_NONE;
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+    wd->sta.psMgr.isSleepAllowed = 0;
+    wd->sta.psMgr.maxSleepPeriods = 1;
+    wd->sta.psMgr.ticks = 0;
+    wd->sta.psMgr.sleepAllowedtick = 0;
+}
+
+static u16_t zfPowerSavingMgrHandlePsNone(zdev_t* dev, u8_t *isWakeUpRequired)
+{
+    u16_t ret = 0;
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            *isWakeUpRequired = 0;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+        case ZM_PS_MSG_STATE_T2:
+        case ZM_PS_MSG_STATE_SLEEP:
+        default:
+            *isWakeUpRequired = 1;
+zm_debug_msg0("zfPowerSavingMgrHandlePsNone: Wake up now\n");
+            if ( zfStaIsConnected(dev) )
+            {
+                zm_debug_msg0("zfPowerSavingMgrOnHandleT1 send Null data\n");
+                //zfSendNullData(dev, 0);
+                ret = 1;
+            }
+
+            wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+            break;
+    }
+    return ret;
+}
+
+static void zfPowerSavingMgrHandlePs(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            //zm_debug_msg0("zfPowerSavingMgrHandlePs: Prepare to sleep...\n");
+            //wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+        case ZM_PS_MSG_STATE_T2:
+        case ZM_PS_MSG_STATE_SLEEP:
+        default:
+            break;
+    }
+}
+
+void zfPowerSavingMgrSetMode(zdev_t* dev, u8_t mode)
+{
+    u16_t sendNull = 0;
+    u8_t isWakeUpRequired = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg1("mode = ", mode);
+
+    if (mode > ZM_STA_PS_LIGHT)
+    {
+        zm_debug_msg0("return - wrong power save mode");
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    #if 1
+    switch(mode)
+    {
+        case ZM_STA_PS_NONE:
+            sendNull = zfPowerSavingMgrHandlePsNone(dev, &isWakeUpRequired);
+            break;
+
+        case ZM_STA_PS_FAST:
+        case ZM_STA_PS_LIGHT:
+            wd->sta.psMgr.maxSleepPeriods = 1;
+            zfPowerSavingMgrHandlePs(dev);
+            break;
+
+        case ZM_STA_PS_MAX:
+            wd->sta.psMgr.maxSleepPeriods = ZM_PS_MAX_SLEEP_PERIODS;
+            zfPowerSavingMgrHandlePs(dev);
+            break;
+    }
+    #else
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            if ( mode != ZM_STA_PS_NONE )
+            {
+zm_debug_msg0("zfPowerSavingMgrSetMode: switch from ZM_PS_MSG_STATE_ACTIVE to ZM_PS_MSG_STATE_T1\n");
+                // Stall the TX & start to wait the pending TX to be completed
+                wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            }
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+    }
+    #endif
+
+    wd->sta.powerSaveMode = mode;
+    zmw_leave_critical_section(dev);
+
+    if ( isWakeUpRequired )
+    {
+        zfHpPowerSaveSetState(dev, 0);
+        wd->sta.psMgr.tempWakeUp = 0;
+    }
+
+    if ( zfStaIsConnected(dev)
+         && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+    {
+        switch(mode)
+        {
+            case ZM_STA_PS_NONE:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+
+            case ZM_STA_PS_FAST:
+            case ZM_STA_PS_MAX:
+            case ZM_STA_PS_LIGHT:
+                zfHpPowerSaveSetMode(dev, 0, 1, wd->beaconInterval);
+                break;
+
+            default:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+        }
+    }
+
+    if (sendNull == 1)
+    {
+        zfSendNullData(dev, 0);
+    }
+
+    return;
+}
+
+static void zfPowerSavingMgrNotifyPSToAP(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( (wd->sta.psMgr.tempWakeUp != 1)&&
+         (wd->sta.psMgr.lastTxUnicastFrm != wd->commTally.txUnicastFrm ||
+          wd->sta.psMgr.lastTxBroadcastFrm != wd->commTally.txBroadcastFrm ||
+          wd->sta.psMgr.lastTxMulticastFrm != wd->commTally.txMulticastFrm) )
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.psMgr.lastTxUnicastFrm = wd->commTally.txUnicastFrm;
+        wd->sta.psMgr.lastTxBroadcastFrm = wd->commTally.txBroadcastFrm;
+        wd->sta.psMgr.lastTxMulticastFrm = wd->commTally.txMulticastFrm;
+        zmw_leave_critical_section(dev);
+
+        zfSendNullData(dev, 1);
+    }
+}
+
+static void zfPowerSavingMgrOnHandleT1(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // If the tx Q is not empty...return
+    if ( zfIsVtxqEmpty(dev) == FALSE )
+    {
+        return;
+    }
+
+zm_debug_msg0("VtxQ is empty now...Check if HAL TXQ is empty\n");
+
+    // The the HAL TX Q is not empty...return
+    if ( zfHpGetFreeTxdCount(dev) != zfHpGetMaxTxdCount(dev) )
+    {
+        return;
+    }
+
+zm_debug_msg0("HAL TXQ is empty now...Could go to sleep...\n");
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.powerSaveMode == ZM_STA_PS_LIGHT)
+    {
+        if (wd->sta.ReceivedPktRatePerSecond > 200)
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+        {
+            if (wd->sta.psMgr.sleepAllowedtick) {
+                wd->sta.psMgr.sleepAllowedtick--;
+                zmw_leave_critical_section(dev);
+                return;
+            }
+        }
+    }
+
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_T2;
+
+    zmw_leave_critical_section(dev);
+
+    // Send the Null pkt to AP to notify that I'm going to sleep
+    if ( zfStaIsConnected(dev) )
+    {
+zm_debug_msg0("zfPowerSavingMgrOnHandleT1 send Null data\n");
+        zfPowerSavingMgrNotifyPSToAP(dev);
+    }
+
+    // Stall the TX now
+    // zfTxEngineStop(dev);
+}
+
+static void zfPowerSavingMgrOnHandleT2(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // Wait until the Null pkt is transmitted
+    if ( zfHpGetFreeTxdCount(dev) != zfHpGetMaxTxdCount(dev) )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_SLEEP;
+    wd->sta.psMgr.lastTxUnicastFrm = wd->commTally.txUnicastFrm;
+    wd->sta.psMgr.lastTxBroadcastFrm = wd->commTally.txBroadcastFrm;
+    wd->sta.psMgr.lastTxMulticastFrm = wd->commTally.txMulticastFrm;
+    zmw_leave_critical_section(dev);
+
+    // Let CHIP sleep now
+zm_debug_msg0("zfPowerSavingMgrOnHandleT2 zzzz....\n");
+    zfHpPowerSaveSetState(dev, 1);
+    wd->sta.psMgr.tempWakeUp = 0;
+}
+
+u8_t zfPowerSavingMgrIsSleeping(zdev_t *dev)
+{
+    u8_t isSleeping = FALSE;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ( wd->sta.psMgr.state == ZM_PS_MSG_STATE_SLEEP ||
+         wd->sta.psMgr.state == ZM_PS_MSG_STATE_T2)
+    {
+        isSleeping = TRUE;
+    }
+    zmw_leave_critical_section(dev);
+    return isSleeping;
+}
+
+static u8_t zfPowerSavingMgrIsIdle(zdev_t *dev)
+{
+    u8_t isIdle = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ( zfStaIsConnected(dev) && wd->sta.psMgr.isSleepAllowed == 0 )
+    {
+        goto done;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        goto done;
+    }
+
+    if ( zfStaIsConnecting(dev) )
+    {
+        goto done;
+    }
+
+    if (wd->sta.powerSaveMode == ZM_STA_PS_LIGHT)
+    {
+        if (wd->sta.ReceivedPktRatePerSecond > 200)
+        {
+            goto done;
+        }
+
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+        {
+            if (wd->sta.psMgr.sleepAllowedtick) {
+                wd->sta.psMgr.sleepAllowedtick--;
+                goto done;
+            }
+        }
+    }
+
+    isIdle = 1;
+
+done:
+    zmw_leave_critical_section(dev);
+
+    if ( zfIsVtxqEmpty(dev) == FALSE )
+    {
+        isIdle = 0;
+    }
+
+    return isIdle;
+}
+
+static void zfPowerSavingMgrSleepIfIdle(zdev_t *dev)
+{
+    u8_t isIdle;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    isIdle = zfPowerSavingMgrIsIdle(dev);
+
+    if ( isIdle == 0 )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    switch(wd->sta.powerSaveMode)
+    {
+        case ZM_STA_PS_NONE:
+            break;
+
+        case ZM_STA_PS_MAX:
+        case ZM_STA_PS_FAST:
+        case ZM_STA_PS_LIGHT:
+            zm_debug_msg0("zfPowerSavingMgrSleepIfIdle: IDLE so slep now...\n");
+            wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            break;
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+static void zfPowerSavingMgrDisconnectMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+#ifdef ZM_ENABLE_DISCONNECT_PS
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            zfPowerSavingMgrSleepIfIdle(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+#else
+    zfPowerSavingMgrWakeup(dev);
+#endif
+}
+
+static void zfPowerSavingMgrInfraMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            zfPowerSavingMgrSleepIfIdle(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+}
+
+void zfPowerSavingMgrAtimWinExpired(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+//printk("zfPowerSavingMgrAtimWinExpired #1\n");
+    if ( wd->sta.powerSaveMode == ZM_STA_PS_NONE )
+    {
+        return;
+    }
+
+//printk("zfPowerSavingMgrAtimWinExpired #2\n");
+    // if we received any ATIM window from the others to indicate we have buffered data
+    // at the other station, we can't go to sleep
+    if ( wd->sta.recvAtim )
+    {
+        wd->sta.recvAtim = 0;
+        zm_debug_msg0("Can't sleep due to receving ATIM window!");
+        return;
+    }
+
+    // if we are the one to tx beacon during last beacon interval. we can't go to sleep
+    // since we need to be alive to respond the probe request!
+    if ( wd->sta.txBeaconInd )
+    {
+        zm_debug_msg0("Can't sleep due to just transmit a beacon!");
+        return;
+    }
+
+    // If we buffer any data for the other stations. we could not go to sleep
+    if ( wd->sta.ibssPrevPSDataCount != 0 )
+    {
+        zm_debug_msg0("Can't sleep due to buffering data for the others!");
+        return;
+    }
+
+    // before sleeping, we still need to notify the others by transmitting null
+    // pkt with power mgmt bit turned on.
+    zfPowerSavingMgrOnHandleT1(dev);
+}
+
+static void zfPowerSavingMgrIBSSMain(zdev_t* dev)
+{
+    // wait for the end of
+    // if need to wait to know if we are the one to transmit the beacon
+    // during the beacon interval. If it's me, we can't go to sleep.
+
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+        case ZM_PS_MSG_STATE_SLEEP:
+        case ZM_PS_MSG_STATE_T1:
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+
+    return;
+}
+
+#if 1
+void zfPowerSavingMgrMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->sta.adapterState)
+    {
+    case ZM_STA_STATE_DISCONNECT:
+        zfPowerSavingMgrDisconnectMain(dev);
+        break;
+    case ZM_STA_STATE_CONNECTED:
+        {
+            if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) {
+                zfPowerSavingMgrInfraMain(dev);
+            } else if (wd->wlanMode == ZM_MODE_IBSS) {
+                zfPowerSavingMgrIBSSMain(dev);
+            }
+        }
+        break;
+    case ZM_STA_STATE_CONNECTING:
+    default:
+        break;
+    }
+}
+#else
+void zfPowerSavingMgrMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE )
+    {
+        return;
+    }
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            goto check_sleep;
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            goto sleeping;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+
+    return;
+
+sleeping:
+    return;
+
+check_sleep:
+    zfPowerSavingMgrSleepIfIdle(dev);
+    return;
+}
+#endif
+
+#ifdef ZM_ENABLE_POWER_SAVE
+void zfPowerSavingMgrWakeup(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+//zm_debug_msg0("zfPowerSavingMgrWakeup");
+
+    //if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_ACTIVE && ( zfPowerSavingMgrIsIdle(dev) == 0 ))
+    if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_ACTIVE )
+    {
+        zmw_enter_critical_section(dev);
+
+        wd->sta.psMgr.isSleepAllowed = 0;
+        wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+
+        if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+            wd->sta.psMgr.tempWakeUp = 1;
+
+        zmw_leave_critical_section(dev);
+
+        // Wake up the CHIP now!!
+        zfHpPowerSaveSetState(dev, 0);
+    }
+}
+#else
+void zfPowerSavingMgrWakeup(zdev_t* dev)
+{
+}
+#endif
+
+void zfPowerSavingMgrProcessBeacon(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   length, bitmap;
+    u16_t  offset, n1, n2, q, r;
+    zbuf_t* psBuf;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.powerSaveMode == ZM_STA_PS_NONE  )
+    //if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_SLEEP )
+    {
+        return;
+    }
+
+    wd->sta.psMgr.isSleepAllowed = 1;
+
+    if ( (offset=zfFindElement(dev, buf, ZM_WLAN_EID_TIM)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+        if ( length > 3 )
+        {
+            n1 = zmw_rx_buf_readb(dev, buf, offset+4) & (~ZM_BIT_0);
+            n2 = length + n1 - 4;
+            q = wd->sta.aid >> 3;
+            r = wd->sta.aid & 7;
+
+            if ((q >= n1) && (q <= n2))
+            {
+                bitmap = zmw_rx_buf_readb(dev, buf, offset+5+q-n1);
+
+                if ( (bitmap >> r) &  ZM_BIT_0 )
+                {
+                    //if ( wd->sta.powerSaveMode == ZM_STA_PS_FAST )
+                    if ( 0 )
+                    {
+                        wd->sta.psMgr.state = ZM_PS_MSG_STATE_S1;
+                        //zfSendPSPoll(dev);
+                        zfSendNullData(dev, 0);
+                    }
+                    else
+                    {
+                        if ((wd->sta.qosInfo&0xf) != 0xf)
+                        {
+                            /* send ps-poll */
+                            //printk("zfSendPSPoll #1\n");
+
+                            wd->sta.psMgr.isSleepAllowed = 0;
+
+                            switch (wd->sta.powerSaveMode)
+                            {
+                            case ZM_STA_PS_MAX:
+                            case ZM_STA_PS_FAST:
+                                //zm_debug_msg0("wake up and send PS-Poll\n");
+                                zfSendPSPoll(dev);
+                                break;
+                            case ZM_STA_PS_LIGHT:
+                                zm_debug_msg0("wake up and send null data\n");
+
+                                zmw_enter_critical_section(dev);
+                                wd->sta.psMgr.sleepAllowedtick = 400;
+                                zmw_leave_critical_section(dev);
+
+                                zfSendNullData(dev, 0);
+                                break;
+                            }
+
+                            wd->sta.psMgr.tempWakeUp = 0;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    while ((psBuf = zfQueueGet(dev, wd->sta.uapsdQ)) != NULL)
+    {
+        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    //printk("zfPowerSavingMgrProcessBeacon #1\n");
+    zfPowerSavingMgrMain(dev);
+}
+
+void zfPowerSavingMgrConnectNotify(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        switch(wd->sta.powerSaveMode)
+        {
+            case ZM_STA_PS_NONE:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+
+            case ZM_STA_PS_FAST:
+            case ZM_STA_PS_MAX:
+            case ZM_STA_PS_LIGHT:
+                zfHpPowerSaveSetMode(dev, 0, 1, wd->beaconInterval);
+                break;
+
+            default:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+        }
+    }
+}
+
+void zfPowerSavingMgrPreTBTTInterrupt(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* disable TBTT interrupt when change from connection to disconnect */
+    if (zfStaIsDisconnect(dev)) {
+        zfHpPowerSaveSetMode(dev, 0, 0, 0);
+        zfPowerSavingMgrWakeup(dev);
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    wd->sta.psMgr.ticks++;
+
+    if ( wd->sta.psMgr.ticks < wd->sta.psMgr.maxSleepPeriods )
+    {
+        zmw_leave_critical_section(dev);
+        return;
+    }
+    else
+    {
+        wd->sta.psMgr.ticks = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfPowerSavingMgrWakeup(dev);
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
+
diff --git a/drivers/staging/otus/80211core/cscanmgr.c b/drivers/staging/otus/80211core/cscanmgr.c
new file mode 100644
index 000000000000..b32835c87590
--- /dev/null
+++ b/drivers/staging/otus/80211core/cscanmgr.c
@@ -0,0 +1,535 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+void zfScanMgrInit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.scanMgr.scanReqs[0] = 0;
+    wd->sta.scanMgr.scanReqs[1] = 0;
+
+    wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+}
+
+u8_t zfScanMgrScanStart(zdev_t* dev, u8_t scanType)
+{
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("scanType = ", scanType);
+
+    zmw_declare_for_critical_section();
+
+    if ( scanType != ZM_SCAN_MGR_SCAN_INTERNAL &&
+         scanType != ZM_SCAN_MGR_SCAN_EXTERNAL )
+    {
+        zm_debug_msg0("unknown scanType");
+        return 1;
+    }
+    else if (zfStaIsConnecting(dev))
+    {
+        zm_debug_msg0("reject scan request due to connecting");
+        return 1;
+    }
+
+    i = scanType - 1;
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.scanReqs[i] == 1 )
+    {
+        zm_debug_msg1("scan rescheduled", scanType);
+        goto scan_done;
+    }
+
+    wd->sta.scanMgr.scanReqs[i] = 1;
+    zm_debug_msg1("scan scheduled: ", scanType);
+
+    // If there's no scan pending, we do the scan right away.
+    // If there's an internal scan and the new scan request is external one,
+    // we will restart the scan.
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        goto schedule_scan;
+    }
+    else if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_INTERNAL &&
+              scanType == ZM_SCAN_MGR_SCAN_EXTERNAL )
+    {
+        // Stop the internal scan & schedule external scan first
+        zfTimerCancel(dev, ZM_EVENT_SCAN);
+
+        /* Fix for WHQL sendrecv => we do not apply delay time in which the device
+           stop transmitting packet when we already connect to some AP  */
+        wd->sta.bScheduleScan = FALSE;
+
+        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+        zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+
+        wd->sta.bChannelScan = FALSE;
+        goto schedule_scan;
+    }
+    else
+    {
+        zm_debug_msg0("Scan is busy...waiting later to start\n");
+    }
+
+    zmw_leave_critical_section(dev);
+    return 0;
+
+scan_done:
+    zmw_leave_critical_section(dev);
+    return 1;
+
+schedule_scan:
+
+    wd->sta.bScheduleScan = TRUE;
+
+    zfTimerSchedule(dev, ZM_EVENT_SCAN, wd->sta.scanMgr.scanStartDelay);
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+    wd->sta.scanMgr.currScanType = scanType;
+    zmw_leave_critical_section(dev);
+
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 1);
+    }
+    return 0;
+}
+
+void zfScanMgrScanStop(zdev_t* dev, u8_t scanType)
+{
+    u8_t scanNotifyRequired = 0;
+    u8_t theOtherScan = ZM_SCAN_MGR_SCAN_NONE;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        zm_assert(wd->sta.scanMgr.scanReqs[0] == 0);
+        zm_assert(wd->sta.scanMgr.scanReqs[1] == 0);
+        goto done;
+    }
+
+    switch(scanType)
+    {
+        case ZM_SCAN_MGR_SCAN_EXTERNAL:
+            scanNotifyRequired = 1;
+            theOtherScan = ZM_SCAN_MGR_SCAN_INTERNAL;
+            break;
+
+        case ZM_SCAN_MGR_SCAN_INTERNAL:
+            theOtherScan = ZM_SCAN_MGR_SCAN_EXTERNAL;
+            break;
+
+        default:
+            goto done;
+    }
+
+    if ( wd->sta.scanMgr.currScanType != scanType )
+    {
+        goto stop_done;
+    }
+
+    zfTimerCancel(dev, ZM_EVENT_SCAN);
+
+    /* Fix for WHQL sendrecv => we do not apply delay time in which the device
+       stop transmitting packet when we already connect to some AP  */
+    wd->sta.bScheduleScan = FALSE;
+
+    zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+    zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+
+    wd->sta.bChannelScan = FALSE;
+    wd->sta.scanFrequency = 0;
+
+    if ( wd->sta.scanMgr.scanReqs[theOtherScan - 1] )
+    {
+        wd->sta.scanMgr.currScanType = theOtherScan;
+
+        // Schedule the other scan after 1 second later
+        zfTimerSchedule(dev, ZM_EVENT_SCAN, 100);
+    }
+    else
+    {
+        wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+    }
+
+stop_done:
+    wd->sta.scanMgr.scanReqs[scanType - 1] = 0;
+
+    zmw_leave_critical_section(dev);
+
+    /* avoid lose receive packet when site survey */
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 0);
+    }
+
+    if ( scanNotifyRequired )
+    {
+        zm_debug_msg0("Scan notify after reset");
+        if (wd->zfcbScanNotify != NULL)
+        {
+            wd->zfcbScanNotify(dev, NULL);
+        }
+    }
+
+    return;
+
+done:
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void zfScanMgrScanAck(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+extern void zfStaReconnect(zdev_t* dev);
+
+static void zfScanSendProbeRequest(zdev_t* dev)
+{
+    u8_t k;
+    u16_t  dst[3] = { 0xffff, 0xffff, 0xffff };
+
+    zmw_get_wlan_dev(dev);
+
+    /* Increase rxBeaconCount to prevent beacon lost */
+    if (zfStaIsConnected(dev))
+    {
+        wd->sta.rxBeaconCount++;
+    }
+
+    if ( wd->sta.bPassiveScan )
+    {
+        return;
+    }
+    /* enable 802.l11h and in DFS Band , disable sending probe request */
+    if (wd->sta.DFSEnable)
+    {
+        if (zfHpIsDfsChannel(dev, wd->sta.scanFrequency))
+        {
+            return;
+        }
+    }
+
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ, dst, 0, 0, 0);
+
+    if ( wd->sta.disableProbingWithSsid )
+    {
+        return;
+    }
+
+    for (k=1; k<=ZM_MAX_PROBE_HIDDEN_SSID_SIZE; k++)
+    {
+        if ( wd->ws.probingSsidList[k-1].ssidLen != 0 )
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ, dst, k, 0, 0);
+        }
+    }
+}
+
+static void zfScanMgrEventSetFreqCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+//printk("zfScanMgrEventSetFreqCompleteCb #1\n");
+
+    zmw_enter_critical_section(dev);
+    zfTimerSchedule(dev, ZM_EVENT_IN_SCAN, ZM_TICK_IN_SCAN);
+    if (wd->sta.bPassiveScan)
+    {
+        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_SCAN, wd->sta.passiveScanTickPerChannel);
+    }
+    else
+    {
+        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_SCAN, wd->sta.activescanTickPerChannel);
+    }
+    zmw_leave_critical_section(dev);
+
+    zfScanSendProbeRequest(dev);
+}
+
+
+static void zfScanMgrEventScanCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 0);
+    }
+    return;
+}
+
+
+void zfScanMgrScanEventRetry(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( !wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    if ( !wd->sta.bPassiveScan )
+    {
+        zfScanSendProbeRequest(dev);
+        #if 0
+        zmw_enter_critical_section(dev);
+        zfTimerSchedule(dev, ZM_EVENT_IN_SCAN, ZM_TICK_IN_SCAN);
+        zmw_leave_critical_section(dev);
+        #endif
+    }
+}
+
+u8_t zfScanMgrScanEventTimeout(zdev_t* dev)
+{
+    u16_t   nextScanFrequency = 0;
+    u8_t    temp;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ( wd->sta.scanFrequency == 0 )
+    {
+        zmw_leave_critical_section(dev);
+        return -1;
+    }
+
+    nextScanFrequency = zfChGetNextChannel(dev, wd->sta.scanFrequency,
+                                           &wd->sta.bPassiveScan);
+
+    if ( (nextScanFrequency == 0xffff)
+         || (wd->sta.scanFrequency == zfChGetLastChannel(dev, &temp)) )
+    {
+        u8_t currScanType;
+        u8_t isExternalScan = 0;
+        u8_t isInternalScan = 0;
+
+        //zm_debug_msg1("end scan = ", KeQueryInterruptTime());
+        wd->sta.scanFrequency = 0;
+
+        zm_debug_msg1("scan 1 type: ", wd->sta.scanMgr.currScanType);
+        zm_debug_msg1("scan channel count = ", wd->regulationTable.allowChannelCnt);
+
+        //zfBssInfoRefresh(dev);
+        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+
+        if ( wd->sta.bChannelScan == FALSE )
+        {
+            zm_debug_msg0("WOW!! scan is cancelled\n");
+            zmw_leave_critical_section(dev);
+            goto report_scan_result;
+        }
+
+
+        currScanType = wd->sta.scanMgr.currScanType;
+        switch(currScanType)
+        {
+            case ZM_SCAN_MGR_SCAN_EXTERNAL:
+                isExternalScan = 1;
+
+                if ( wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_INTERNAL - 1] )
+                {
+                    wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_INTERNAL - 1] = 0;
+                    isInternalScan = 1;
+                }
+
+                break;
+
+            case ZM_SCAN_MGR_SCAN_INTERNAL:
+                isInternalScan = 1;
+
+                if ( wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_EXTERNAL - 1] )
+                {
+                    // Because the external scan should pre-empts internal scan.
+                    // So this shall not be happened!!
+                    zm_assert(0);
+                }
+
+                break;
+
+            default:
+                zm_assert(0);
+                break;
+        }
+
+        wd->sta.scanMgr.scanReqs[currScanType - 1] = 0;
+        wd->sta.scanMgr.scanStartDelay = 100;
+        wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+        zmw_leave_critical_section(dev);
+
+        //Set channel according to AP's configuration
+        zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                wd->ExtOffset, zfScanMgrEventScanCompleteCb);
+
+        wd->sta.bChannelScan = FALSE;
+
+        #if 1
+        if (zfStaIsConnected(dev))
+        { // Finish site survey, reset the variable to detect using wrong frequency !
+            zfHpFinishSiteSurvey(dev, 1);
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssSiteSurveyStatus = 2;
+            wd->tickIbssReceiveBeacon = 0;
+            wd->sta.ibssReceiveBeaconCount = 0;
+            zmw_leave_critical_section(dev);
+
+            /* #5 Re-enable RIFS function after the site survey ! */
+            /* This is because switch band will reset the BB register to initial value */
+            if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+            {
+                zfHpEnableRifs(dev, ((wd->sta.currentFrequency<3000)?1:0), wd->sta.EnableHT, wd->sta.HT2040);
+            }
+        }
+        else
+        {
+            zfHpFinishSiteSurvey(dev, 0);
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssSiteSurveyStatus = 0;
+            zmw_leave_critical_section(dev);
+        }
+        #endif
+
+report_scan_result:
+        /* avoid lose receive packet when site survey */
+        //if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+        //{
+        //    zfSendNullData(dev, 0);
+        //}
+
+        if ( isExternalScan )//Quickly reboot
+        {
+            if (wd->zfcbScanNotify != NULL)
+            {
+                wd->zfcbScanNotify(dev, NULL);
+            }
+        }
+
+        if ( isInternalScan )
+        {
+            //wd->sta.InternalScanReq = 0;
+            zfStaReconnect(dev);
+        }
+
+        return 0;
+    }
+    else
+    {
+        wd->sta.scanFrequency = nextScanFrequency;
+
+        //zmw_enter_critical_section(dev);
+        zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+        zmw_leave_critical_section(dev);
+
+        zm_debug_msg0("scan 2");
+        zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+        return 1;
+    }
+}
+
+void zfScanMgrScanEventStart(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    zfPowerSavingMgrWakeup(dev);
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        goto no_scan;
+    }
+
+    //zfBssInfoRefresh(dev);
+    zfBssInfoRefresh(dev, 0);
+    wd->sta.bChannelScan = TRUE;
+    wd->sta.bScheduleScan = FALSE;
+    zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+    zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+
+    //zm_debug_msg1("start scan = ", KeQueryInterruptTime());
+    wd->sta.scanFrequency = zfChGetFirstChannel(dev, &wd->sta.bPassiveScan);
+    zmw_leave_critical_section(dev);
+
+    /* avoid lose receive packet when site survey */
+    //if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    //{
+    //    zfSendNullData(dev, 1);
+    //}
+//    zm_debug_msg0("scan 0");
+//    zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+    #if 1
+    if (zfStaIsConnected(dev))
+    {// If doing site survey !
+        zfHpBeginSiteSurvey(dev, 1);
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssSiteSurveyStatus = 1;
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        zfHpBeginSiteSurvey(dev, 0);
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssSiteSurveyStatus = 0;
+        zmw_leave_critical_section(dev);
+    }
+    #endif
+
+    zm_debug_msg0("scan 0");
+    zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+    return;
+
+no_scan:
+    zmw_leave_critical_section(dev);
+    return;
+}
diff --git a/drivers/staging/otus/80211core/ctkip.c b/drivers/staging/otus/80211core/ctkip.c
new file mode 100644
index 000000000000..be42f7aaa37d
--- /dev/null
+++ b/drivers/staging/otus/80211core/ctkip.c
@@ -0,0 +1,598 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : ctkip.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u16_t zgTkipSboxLower[256] =
+    {
+        0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
+        0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
+        0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
+        0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
+        0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
+        0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
+        0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
+        0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
+        0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
+        0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
+        0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
+        0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
+        0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
+        0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
+        0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
+        0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
+        0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
+        0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
+        0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
+        0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
+        0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
+        0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
+        0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
+        0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
+        0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
+        0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
+        0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
+        0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
+        0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
+        0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
+        0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
+        0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+    };
+
+
+u16_t zgTkipSboxUpper[256] =
+    {
+        0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
+        0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
+        0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
+        0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
+        0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
+        0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
+        0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
+        0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
+        0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
+        0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
+        0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
+        0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
+        0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
+        0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
+        0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
+        0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
+        0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
+        0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
+        0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
+        0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
+        0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
+        0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
+        0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
+        0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
+        0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
+        0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
+        0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
+        0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
+        0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
+        0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
+        0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
+        0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+    };
+
+u16_t zfrotr1(u16_t a)
+// rotate right by 1 bit.
+{
+    u16_t b;
+
+    if (a & 0x01)
+    {
+        b = (a >> 1) | 0x8000;
+    }
+    else
+    {
+        b = (a >> 1) & 0x7fff;
+    }
+    return b;
+}
+
+/*************************************************************/
+/* zfTkipSbox()                                              */
+/* Returns a 16 bit value from a 64K entry table. The Table  */
+/* is synthesized from two 256 entry byte wide tables.       */
+/*************************************************************/
+u16_t zfTkipSbox(u16_t index)
+{
+    u16_t   low;
+    u16_t   high;
+    u16_t   left, right;
+
+    low = (index & 0xFF);
+    high = ((index >> 8) & 0xFF);
+
+    left = zgTkipSboxLower[low] + (zgTkipSboxUpper[low] << 8 );
+    right = zgTkipSboxUpper[high] + (zgTkipSboxLower[high] << 8 );
+
+    return (left ^ right);
+}
+
+u8_t zfTkipPhase1KeyMix(u32_t iv32, struct zsTkipSeed* pSeed)
+{
+    u16_t   tsc0;
+    u16_t   tsc1;
+    u16_t   i, j;
+#if 0
+    /* Need not proceed this function with the same iv32 */
+    if ( iv32 == pSeed->iv32 )
+    {
+        return 1;
+    }
+#endif
+    tsc0 = (u16_t) ((iv32 >> 16) & 0xffff); /* msb */
+    tsc1 = (u16_t) (iv32 & 0xffff);
+
+    /* Phase 1, step 1 */
+    pSeed->ttak[0] = tsc1;
+    pSeed->ttak[1] = tsc0;
+    pSeed->ttak[2] = (u16_t) (pSeed->ta[0] + (pSeed->ta[1] <<8));
+    pSeed->ttak[3] = (u16_t) (pSeed->ta[2] + (pSeed->ta[3] <<8));
+    pSeed->ttak[4] = (u16_t) (pSeed->ta[4] + (pSeed->ta[5] <<8));
+
+    /* Phase 1, step 2 */
+    for (i=0; i<8; i++)
+    {
+        j = 2*(i & 1);
+        pSeed->ttak[0] =(pSeed->ttak[0] + zfTkipSbox(pSeed->ttak[4]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[1+j], pSeed->tk[j])))
+                        & 0xffff;
+        pSeed->ttak[1] =(pSeed->ttak[1] + zfTkipSbox(pSeed->ttak[0]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[5+j], pSeed->tk[4+j] )))
+                        & 0xffff;
+        pSeed->ttak[2] =(pSeed->ttak[2] + zfTkipSbox(pSeed->ttak[1]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[9+j], pSeed->tk[8+j] )))
+                        & 0xffff;
+        pSeed->ttak[3] =(pSeed->ttak[3] + zfTkipSbox(pSeed->ttak[2]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[13+j], pSeed->tk[12+j])))
+                        & 0xffff;
+        pSeed->ttak[4] =(pSeed->ttak[4] + zfTkipSbox(pSeed->ttak[3]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[1+j], pSeed->tk[j]  )))
+                        & 0xffff;
+        pSeed->ttak[4] =(pSeed->ttak[4] + i) & 0xffff;
+    }
+
+    if ( iv32 == (pSeed->iv32+1) )
+    {
+        pSeed->iv32tmp = iv32;
+        return 1;
+    }
+
+    return 0;
+}
+
+u8_t zfTkipPhase2KeyMix(u16_t iv16, struct zsTkipSeed* pSeed)
+{
+    u16_t tsc2;
+
+    tsc2 = iv16;
+
+    /* Phase 2, Step 1 */
+    pSeed->ppk[0] = pSeed->ttak[0];
+    pSeed->ppk[1] = pSeed->ttak[1];
+    pSeed->ppk[2] = pSeed->ttak[2];
+    pSeed->ppk[3] = pSeed->ttak[3];
+    pSeed->ppk[4] = pSeed->ttak[4];
+    pSeed->ppk[5] = (pSeed->ttak[4] + tsc2) & 0xffff;
+
+    /* Phase2, Step 2 */
+    pSeed->ppk[0] = pSeed->ppk[0]
+                + zfTkipSbox(pSeed->ppk[5] ^ ZM_BYTE_TO_WORD(pSeed->tk[1],pSeed->tk[0]));
+    pSeed->ppk[1] = pSeed->ppk[1]
+                + zfTkipSbox(pSeed->ppk[0] ^ ZM_BYTE_TO_WORD(pSeed->tk[3],pSeed->tk[2]));
+    pSeed->ppk[2] = pSeed->ppk[2]
+                + zfTkipSbox(pSeed->ppk[1] ^ ZM_BYTE_TO_WORD(pSeed->tk[5],pSeed->tk[4]));
+    pSeed->ppk[3] = pSeed->ppk[3]
+                + zfTkipSbox(pSeed->ppk[2] ^ ZM_BYTE_TO_WORD(pSeed->tk[7],pSeed->tk[6]));
+    pSeed->ppk[4] = pSeed->ppk[4]
+                + zfTkipSbox(pSeed->ppk[3] ^ ZM_BYTE_TO_WORD(pSeed->tk[9],pSeed->tk[8]));
+    pSeed->ppk[5] = pSeed->ppk[5]
+                + zfTkipSbox(pSeed->ppk[4] ^ ZM_BYTE_TO_WORD(pSeed->tk[11],pSeed->tk[10]));
+
+    pSeed->ppk[0] = pSeed->ppk[0]
+                + zfrotr1(pSeed->ppk[5] ^ ZM_BYTE_TO_WORD(pSeed->tk[13],pSeed->tk[12]));
+    pSeed->ppk[1] = pSeed->ppk[1]
+                + zfrotr1(pSeed->ppk[0] ^ ZM_BYTE_TO_WORD(pSeed->tk[15],pSeed->tk[14]));
+    pSeed->ppk[2] = pSeed->ppk[2] + zfrotr1(pSeed->ppk[1]);
+    pSeed->ppk[3] = pSeed->ppk[3] + zfrotr1(pSeed->ppk[2]);
+    pSeed->ppk[4] = pSeed->ppk[4] + zfrotr1(pSeed->ppk[3]);
+    pSeed->ppk[5] = pSeed->ppk[5] + zfrotr1(pSeed->ppk[4]);
+
+    if (iv16 == 0)
+    {
+        if (pSeed->iv16 == 0xffff)
+        {
+            pSeed->iv16tmp=0;
+            return 1;
+        }
+        else
+            return 0;
+    }
+    else if (iv16 == (pSeed->iv16+1))
+    {
+        pSeed->iv16tmp = iv16;
+        return 1;
+    }
+    else
+        return 0;
+}
+
+void zfTkipInit(u8_t* key, u8_t* ta, struct zsTkipSeed* pSeed, u8_t* initIv)
+{
+    u16_t  iv16;
+    u32_t  iv32;
+    u16_t  i;
+
+    /* clear memory */
+    zfZeroMemory((u8_t*) pSeed, sizeof(struct zsTkipSeed));
+    /* set key to seed */
+    zfMemoryCopy(pSeed->ta, ta, 6);
+    zfMemoryCopy(pSeed->tk, key, 16);
+
+    iv16 = *initIv++;
+    iv16 += *initIv<<8;
+    initIv++;
+
+    iv32=0;
+
+    for(i=0; i<4; i++)      // initiv is little endian
+    {
+        iv32 += *initIv<<(i*8);
+        *initIv++;
+    }
+
+    pSeed->iv32 = iv32+1; // Force Recalculating on Tkip Phase1
+    zfTkipPhase1KeyMix(iv32, pSeed);
+
+    pSeed->iv16 = iv16;
+    pSeed->iv32 = iv32;
+}
+
+u32_t zfGetU32t(u8_t* p)
+{
+    u32_t res=0;
+    u16_t i;
+
+    for( i=0; i<4; i++ )
+    {
+        res |= (*p++) << (8*i);
+    }
+
+    return res;
+
+}
+
+void zfPutU32t(u8_t* p, u32_t value)
+{
+    u16_t i;
+
+    for(i=0; i<4; i++)
+    {
+        *p++ = (u8_t) (value & 0xff);
+        value >>= 8;
+    }
+}
+
+void zfMicClear(struct zsMicVar* pMic)
+{
+    pMic->left = pMic->k0;
+    pMic->right = pMic->k1;
+    pMic->nBytes = 0;
+    pMic->m = 0;
+}
+
+void zfMicSetKey(u8_t* key, struct zsMicVar* pMic)
+{
+    pMic->k0 = zfGetU32t(key);
+    pMic->k1 = zfGetU32t(key+4);
+    zfMicClear(pMic);
+}
+
+void zfMicAppendByte(u8_t b, struct zsMicVar* pMic)
+{
+    // Append the byte to our word-sized buffer
+    pMic->m |= b << (8* pMic->nBytes);
+    pMic->nBytes++;
+
+    // Process the word if it is full.
+    if ( pMic->nBytes >= 4 )
+    {
+        pMic->left ^= pMic->m;
+        pMic->right ^= ZM_ROL32(pMic->left, 17 );
+        pMic->left += pMic->right;
+        pMic->right ^= ((pMic->left & 0xff00ff00) >> 8) |
+                       ((pMic->left & 0x00ff00ff) << 8);
+        pMic->left += pMic->right;
+        pMic->right ^= ZM_ROL32( pMic->left, 3 );
+        pMic->left += pMic->right;
+        pMic->right ^= ZM_ROR32( pMic->left, 2 );
+        pMic->left += pMic->right;
+        // Clear the buffer
+        pMic->m = 0;
+        pMic->nBytes = 0;
+    }
+}
+
+void zfMicGetMic(u8_t* dst, struct zsMicVar* pMic)
+{
+    // Append the minimum padding
+    zfMicAppendByte(0x5a, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+
+    // and then zeroes until the length is a multiple of 4
+    while( pMic->nBytes != 0 )
+    {
+        zfMicAppendByte(0, pMic);
+    }
+
+    // The appendByte function has already computed the result.
+    zfPutU32t(dst, pMic->left);
+    zfPutU32t(dst+4, pMic->right);
+
+    // Reset to the empty message.
+    zfMicClear(pMic);
+
+}
+
+u8_t zfMicRxVerify(zdev_t* dev, zbuf_t* buf)
+{
+    struct zsMicVar*  pMicKey;
+    struct zsMicVar    MyMicKey;
+    u8_t   mic[8];
+    u8_t   da[6];
+    u8_t   sa[6];
+    u8_t   bValue;
+    u16_t  i, payloadOffset, tailOffset;
+
+    zmw_get_wlan_dev(dev);
+
+    /* need not check MIC if pMicKEy is equal to NULL */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetRxMicKey(dev, buf);
+
+        if ( pMicKey != NULL )
+        {
+            zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A2_OFFSET, 6);
+            zfCopyFromRxBuffer(dev, buf, da, ZM_WLAN_HEADER_A3_OFFSET, 6);
+        }
+        else
+        {
+            return ZM_MIC_SUCCESS;
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetRxMicKey(dev, buf);
+
+        if ( pMicKey != NULL )
+        {
+            zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A3_OFFSET, 6);
+            zfCopyFromRxBuffer(dev, buf, da, ZM_WLAN_HEADER_A1_OFFSET, 6);
+        }
+        else
+        {
+            return ZM_MIC_SUCCESS;
+        }
+    }
+    else
+    {
+        return ZM_MIC_SUCCESS;
+    }
+
+    MyMicKey.k0=pMicKey->k0;
+    MyMicKey.k1=pMicKey->k1;
+    pMicKey = &MyMicKey;
+
+    zfMicClear(pMicKey);
+    tailOffset = zfwBufGetSize(dev, buf);
+    tailOffset -= 8;
+
+    /* append DA */
+    for(i=0; i<6; i++)
+    {
+        zfMicAppendByte(da[i], pMicKey);
+    }
+    /* append SA */
+    for(i=0; i<6; i++)
+    {
+        zfMicAppendByte(sa[i], pMicKey);
+    }
+
+    /* append for alignment */
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) != 0)
+        zfMicAppendByte(zmw_rx_buf_readb(dev, buf,24)&0x7, pMicKey);
+    else
+        zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    /* append payload */
+    payloadOffset = ZM_SIZE_OF_WLAN_DATA_HEADER +
+                    ZM_SIZE_OF_IV +
+                    ZM_SIZE_OF_EXT_IV;
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) != 0)
+    {
+        /* Qos Packet, Plcpheader + 2 */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            /* TODO : Rx Qos element offset in software MIC check */
+        }
+        else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+        {
+            if (wd->sta.wmeConnected != 0)
+            {
+                payloadOffset += 2;
+            }
+        }
+    }
+
+    for(i=payloadOffset; i<tailOffset; i++)
+    {
+        bValue = zmw_rx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic(mic, pMicKey);
+
+    if ( !zfRxBufferEqualToStr(dev, buf, mic, tailOffset, 8) )
+    {
+        return ZM_MIC_FAILURE;
+    }
+
+    return ZM_MIC_SUCCESS;
+}
+
+void zfTkipGetseeds(u16_t iv16, u8_t *RC4Key, struct zsTkipSeed *Seed)
+{
+    RC4Key[0]  = ZM_HI8(iv16);
+    RC4Key[1]  = (ZM_HI8(iv16) | 0x20) & 0x7f;
+    RC4Key[2]  = ZM_LO8(iv16);
+    RC4Key[3]  = ((Seed->ppk[5] ^ ZM_BYTE_TO_WORD(Seed->tk[1],Seed->tk[0]))>>1) & 0xff;
+    RC4Key[4]  = Seed->ppk[0] & 0xff;
+    RC4Key[5]  = Seed->ppk[0] >> 8;
+    RC4Key[6]  = Seed->ppk[1] & 0xff;
+    RC4Key[7]  = Seed->ppk[1] >> 8;
+    RC4Key[8]  = Seed->ppk[2] & 0xff;
+    RC4Key[9]  = Seed->ppk[2] >> 8;
+    RC4Key[10] = Seed->ppk[3] & 0xff;
+    RC4Key[11] = Seed->ppk[3] >> 8;
+    RC4Key[12] = Seed->ppk[4] & 0xff;
+    RC4Key[13] = Seed->ppk[4] >> 8;
+    RC4Key[14] = Seed->ppk[5] & 0xff;
+    RC4Key[15] = Seed->ppk[5] >> 8;
+}
+
+void zfCalTxMic(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u16_t *da, u16_t *sa, u8_t up, u8_t *mic)
+{
+    struct zsMicVar*  pMicKey;
+    u16_t  i;
+    u16_t len;
+    u8_t bValue;
+    u8_t qosType;
+    u8_t *pDa = (u8_t *)da;
+    u8_t *pSa = (u8_t *)sa;
+
+    zmw_get_wlan_dev(dev);
+
+    /* need not check MIC if pMicKEy is equal to NULL */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetTxMicKey(dev, buf, &qosType);
+
+        if ( pMicKey == NULL )
+            return;
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetTxMicKey(dev, buf);
+
+        if ( pMicKey == NULL )
+        {
+            zm_debug_msg0("pMicKey is NULL");
+            return;
+        }
+    }
+    else
+    {
+        return;
+    }
+
+    zfMicClear(pMicKey);
+    len = zfwBufGetSize(dev, buf);
+
+    /* append DA */
+    for(i = 0; i < 6; i++)
+    {
+        zfMicAppendByte(pDa[i], pMicKey);
+    }
+
+    /* append SA */
+    for(i = 0; i < 6; i++)
+    {
+        zfMicAppendByte(pSa[i], pMicKey);
+    }
+
+    if (up != 0)
+        zfMicAppendByte((up&0x7), pMicKey);
+    else
+        zfMicAppendByte(0, pMicKey);
+
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    /* For Snap header */
+    for(i = 0; i < snapLen; i++)
+    {
+        zfMicAppendByte(snap[i], pMicKey);
+    }
+
+    for(i = offset; i < len; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic(mic, pMicKey);
+}
+
+void zfTKIPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* key, u32_t* icv)
+{
+    u8_t iv[3];
+
+    iv[0] = key[0];
+    iv[1] = key[1];
+    iv[2] = key[2];
+
+    keyLen -= 3;
+
+    zfWEPEncrypt(dev, buf, snap, snapLen, offset, keyLen, &key[3], iv);
+}
+
+u16_t zfTKIPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* key)
+{
+    u16_t ret = ZM_ICV_SUCCESS;
+    u8_t iv[3];
+
+    iv[0] = key[0];
+    iv[1] = key[1];
+    iv[2] = key[2];
+
+    keyLen -= 3;
+
+    ret = zfWEPDecrypt(dev, buf, offset, keyLen, &key[3], iv);
+
+    return ret;
+}
diff --git a/drivers/staging/otus/80211core/ctxrx.c b/drivers/staging/otus/80211core/ctxrx.c
new file mode 100644
index 000000000000..e258a7df5363
--- /dev/null
+++ b/drivers/staging/otus/80211core/ctxrx.c
@@ -0,0 +1,4096 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : htr.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u16_t zfWlanRxValidate(zdev_t* dev, zbuf_t* buf);
+u16_t zfWlanRxFilter(zdev_t* dev, zbuf_t* buf);
+
+
+
+const u8_t zgSnapBridgeTunnel[6] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8 };
+const u8_t zgSnap8021h[6] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00 };
+/* Table for converting IP DSCP P2-P0 bits to 802.11e Access Category */
+const u8_t zcUpToAc[8] = {0, 1, 1, 0, 2, 2, 3, 3}; //WMM default
+//const u8_t zcUpToAc[8] = {0, 1, 1, 0, 0, 0, 0, 0}; //For 2 TxQ
+//const u8_t zcUpToAc[8] = {0, 0, 0, 0, 0, 0, 0, 0}; //For single TxQ
+const u8_t zcMaxspToPktNum[4] = {8, 2, 4, 6};
+
+u8_t zfGetEncryModeFromRxStatus(struct zsAdditionInfo* addInfo)
+{
+    u8_t securityByte;
+    u8_t encryMode;
+
+    securityByte = (addInfo->Tail.Data.SAIndex & 0xc0) >> 4;  /* byte4 */
+    securityByte |= (addInfo->Tail.Data.DAIndex & 0xc0) >> 6; /* byte5 */
+
+    switch( securityByte )
+    {
+        case ZM_NO_WEP:
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+        case ZM_TKIP:
+        case ZM_AES:
+
+            encryMode = securityByte;
+            break;
+
+        default:
+
+            if ( (securityByte & 0xf8) == 0x08 )
+            {
+                // decrypted by software
+            }
+
+            encryMode = ZM_NO_WEP;
+            break;
+    }
+
+    return encryMode;
+}
+
+void zfGetRxIvIcvLength(zdev_t* dev, zbuf_t* buf, u8_t vap, u16_t* pIvLen,
+                        u16_t* pIcvLen, struct zsAdditionInfo* addInfo)
+{
+    u16_t wdsPort;
+    u8_t  encryMode;
+
+    zmw_get_wlan_dev(dev);
+
+    *pIvLen = 0;
+    *pIcvLen = 0;
+
+    encryMode = zfGetEncryModeFromRxStatus(addInfo);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        if (vap < ZM_MAX_AP_SUPPORT)
+        {
+            if (( wd->ap.encryMode[vap] == ZM_WEP64 ) ||
+                    ( wd->ap.encryMode[vap] == ZM_WEP128 ) ||
+                    ( wd->ap.encryMode[vap] == ZM_WEP256 ))
+            {
+                *pIvLen = 4;
+                *pIcvLen = 4;
+            }
+            else
+            {
+                u16_t id;
+                u16_t addr[3];
+
+                addr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+                addr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+                addr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+                /* Find STA's information */
+                if ((id = zfApFindSta(dev, addr)) != 0xffff)
+                {
+                    if (wd->ap.staTable[id].encryMode == ZM_TKIP)
+                    {
+                        *pIvLen = 8;
+                        *pIcvLen = 4;
+                    }
+                    else if (wd->ap.staTable[id].encryMode == ZM_AES)
+                    {
+                        *pIvLen = 8;
+                        *pIcvLen = 8; // AES MIC
+                        //*pIcvLen = 0;
+                    }
+#ifdef ZM_ENABLE_CENC
+                    else if (wd->ap.staTable[id].encryMode == ZM_CENC)
+                    {
+                        *pIvLen = 18;
+                        *pIcvLen= 16;
+                    }
+#endif //ZM_ENABLE_CENC
+                }
+            }
+            /* WDS port checking */
+            if ((wdsPort = vap - 0x20) >= ZM_MAX_WDS_SUPPORT)
+            {
+                wdsPort = 0;
+            }
+
+            switch (wd->ap.wds.encryMode[wdsPort])
+			{
+			case ZM_WEP64:
+			case ZM_WEP128:
+			case ZM_WEP256:
+                *pIvLen = 4;
+                *pIcvLen = 4;
+				break;
+			case ZM_TKIP:
+                *pIvLen = 8;
+                *pIcvLen = 4;
+				break;
+			case ZM_AES:
+                *pIvLen = 8;
+                *pIcvLen = 0;
+				break;
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+                *pIvLen = 18;
+                *pIcvLen = 16;
+				break;
+#endif //ZM_ENABLE_CENC
+			}/* end of switch */
+        }
+    }
+	else if ( wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* test: 6518 for QA auto test */
+        switch (encryMode)
+		{
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            *pIvLen = 4;
+            *pIcvLen = 4;
+			break;
+		case ZM_TKIP:
+            *pIvLen = 8;
+            *pIcvLen = 4;
+			break;
+		case ZM_AES:
+            *pIvLen = 8;
+            *pIcvLen = 0;
+			break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            *pIvLen = 18;
+            *pIcvLen = 16;
+#endif //ZM_ENABLE_CENC
+		}/* end of switch */
+    }
+    else
+    {
+        if ( (encryMode == ZM_WEP64)||
+             (encryMode == ZM_WEP128)||
+             (encryMode == ZM_WEP256) )
+        {
+            *pIvLen = 4;
+            *pIcvLen = 4;
+        }
+        else if ( encryMode == ZM_TKIP )
+        {
+            *pIvLen = 8;
+            *pIcvLen = 4;
+        }
+        else if ( encryMode == ZM_AES )
+        {
+            *pIvLen = 8;
+            *pIcvLen = 8; // AES MIC
+        }
+#ifdef ZM_ENABLE_CENC
+        else if ( encryMode == ZM_CENC)
+        {
+            *pIvLen = 18;
+            *pIcvLen= 16;
+        }
+#endif //ZM_ENABLE_CENC
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAgingDefragList           */
+/*      Force flushing whole defrag list or aging the buffer            */
+/*      in the defrag list.                                             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      flushFlag : 1=>flushing, 0=>Aging                               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfAgingDefragList(zdev_t* dev, u16_t flushFlag)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if (wd->defragTable.defragEntry[i].fragCount != 0 )
+        {
+            if (((wd->tick - wd->defragTable.defragEntry[i].tick) >
+                        (ZM_DEFRAG_AGING_TIME_SEC * ZM_TICK_PER_SECOND))
+               || (flushFlag != 0))
+            {
+                zm_msg1_rx(ZM_LV_2, "Aging defrag list :", i);
+                /* Free the buffers in the defrag list */
+                for (j=0; j<wd->defragTable.defragEntry[i].fragCount; j++)
+                {
+                    zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[j], 0);
+                }
+            }
+        }
+        wd->defragTable.defragEntry[i].fragCount = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAddFirstFragToDefragList  */
+/*      Add first fragment to defragment list, the first empty entry    */
+/*      will be selected. If the list is full, sequentially select      */
+/*      one entry for replacement.                                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : first fragment buffer                                     */
+/*      addr : address of first fragment buffer                         */
+/*      seqNum : sequence of first fragment buffer                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfAddFirstFragToDefragList(zdev_t* dev, zbuf_t* buf, u8_t* addr, u16_t seqNum)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    /* Find an empty one in defrag list */
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if ( wd->defragTable.defragEntry[i].fragCount == 0 )
+        {
+            break;
+        }
+    }
+
+    /* If full, sequentially replace existing one */
+    if (i == ZM_MAX_DEFRAG_ENTRIES)
+    {
+        i = wd->defragTable.replaceNum++ & (ZM_MAX_DEFRAG_ENTRIES-1);
+        /* Free the buffers in the defrag list to be replaced */
+        for (j=0; j<wd->defragTable.defragEntry[i].fragCount; j++)
+        {
+            zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[j], 0);
+        }
+    }
+
+    wd->defragTable.defragEntry[i].fragCount = 1;
+    wd->defragTable.defragEntry[i].fragment[0] = buf;
+    wd->defragTable.defragEntry[i].seqNum = seqNum;
+    wd->defragTable.defragEntry[i].tick = wd->tick;
+
+    for (j=0; j<6; j++)
+    {
+        wd->defragTable.defragEntry[i].addr[j] = addr[j];
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAddFragToDefragList       */
+/*      Add middle or last fragment to defragment list.                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : first fragment buffer                                     */
+/*      addr : address of fragment buffer                               */
+/*      seqNum : sequence fragment buffer                               */
+/*      fragNum : fragment number of fragment buffer                    */
+/*      moreFrag : more frag bit of fragment buffer                     */
+/*      addInfo : addition info of fragment buffer                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfAddFragToDefragList(zdev_t* dev, zbuf_t* buf, u8_t* addr,
+        u16_t seqNum, u8_t fragNum, u8_t moreFrag,
+        struct zsAdditionInfo* addInfo)
+{
+    u16_t i, j, k;
+    zbuf_t* returnBuf = NULL;
+    u16_t defragDone = 0;
+    u16_t lenErr = 0;
+    u16_t startAddr, fragHead, frameLen, ivLen, icvLen;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    /* Find frag in the defrag list */
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if ( wd->defragTable.defragEntry[i].fragCount != 0 )
+        {
+            /* Compare address */
+            for (j=0; j<6; j++)
+            {
+                if (addr[j] != wd->defragTable.defragEntry[i].addr[j])
+                {
+                    break;
+                }
+            }
+            if (j == 6)
+            {
+                /* Compare sequence and fragment number */
+                if (seqNum == wd->defragTable.defragEntry[i].seqNum)
+                {
+                    if ((fragNum == wd->defragTable.defragEntry[i].fragCount)
+                                && (fragNum < 8))
+                    {
+                        /* Add frag frame to defrag list */
+                        wd->defragTable.defragEntry[i].fragment[fragNum] = buf;
+                        wd->defragTable.defragEntry[i].fragCount++;
+                        defragDone = 1;
+
+                        if (moreFrag == 0)
+                        {
+                            /* merge all fragment if more data bit is cleared */
+                            returnBuf = wd->defragTable.defragEntry[i].fragment[0];
+                            startAddr = zfwBufGetSize(dev, returnBuf);
+                            /* skip WLAN header 24(Data) or 26(QoS Data) */
+                            fragHead = 24 + ((zmw_rx_buf_readh(dev, returnBuf, 0) & 0x80) >> 6);
+                            zfGetRxIvIcvLength(dev, returnBuf, 0, &ivLen, &icvLen, addInfo);
+                            fragHead += ivLen; /* skip IV */
+                            for(k=1; k<wd->defragTable.defragEntry[i].fragCount; k++)
+                            {
+                                frameLen = zfwBufGetSize(dev,
+                                                         wd->defragTable.defragEntry[i].fragment[k]);
+                                if ((startAddr+frameLen-fragHead) < 1560)
+                                {
+                                    zfRxBufferCopy(dev, returnBuf, wd->defragTable.defragEntry[i].fragment[k],
+                                               startAddr, fragHead, frameLen-fragHead);
+                                    startAddr += (frameLen-fragHead);
+                                }
+                                else
+                                {
+                                    lenErr = 1;
+                                }
+                                zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[k], 0);
+                            }
+
+                            wd->defragTable.defragEntry[i].fragCount = 0;
+                            zfwBufSetSize(dev, returnBuf, startAddr);
+                        }
+                        break;
+                    }
+                }
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (lenErr == 1)
+    {
+        zfwBufFree(dev, returnBuf, 0);
+        return NULL;
+    }
+    if (defragDone == 0)
+    {
+        zfwBufFree(dev, buf, 0);
+        return NULL;
+    }
+
+    return returnBuf;
+}
+
+
+/* return value = NULL => save or free this frame         */
+zbuf_t* zfDefragment(zdev_t* dev, zbuf_t* buf, u8_t* pbIsDefrag,
+                     struct zsAdditionInfo* addInfo)
+{
+    u8_t fragNum;
+    u16_t seqNum;
+    u8_t moreFragBit;
+    u8_t addr[6];
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    *pbIsDefrag = FALSE;
+    seqNum = zmw_buf_readh(dev, buf, 22);
+    fragNum = (u8_t)(seqNum & 0xf);
+    moreFragBit = (zmw_buf_readb(dev, buf, 1) & ZM_BIT_2) >> 2;
+
+    if ((fragNum == 0) && (moreFragBit == 0))
+    {
+        /* Not part of a fragmentation */
+
+        return buf;
+    }
+    else
+    {
+        wd->commTally.swRxFragmentCount++;
+        seqNum = seqNum >> 4;
+        for (i=0; i<6; i++)
+        {
+            addr[i] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+i);
+        }
+
+        if (fragNum == 0)
+        {
+            /* more frag = 1 */
+            /* First part of a fragmentation */
+            zm_msg1_rx(ZM_LV_2, "First Frag, seq=", seqNum);
+            zfAddFirstFragToDefragList(dev, buf, addr, seqNum);
+            buf = NULL;
+        }
+        else
+        {
+            /* Middle or last part of a fragmentation */
+            zm_msg1_rx(ZM_LV_2, "Frag seq=", seqNum);
+            zm_msg1_rx(ZM_LV_2, "Frag moreFragBit=", moreFragBit);
+            buf = zfAddFragToDefragList(dev, buf, addr, seqNum, fragNum, moreFragBit, addInfo);
+            if (buf != NULL)
+            {
+                *pbIsDefrag = TRUE;
+            }
+        }
+    }
+
+    return buf;
+}
+
+
+#if ZM_PROTOCOL_RESPONSE_SIMULATION
+u16_t zfSwap(u16_t num)
+{
+    return ((num >> 8) + ((num & 0xff) << 8));
+}
+
+
+void zfProtRspSim(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t ethType;
+    u16_t arpOp;
+    u16_t prot;
+    u16_t temp;
+    u16_t i;
+    u16_t dip[2];
+    u16_t dstPort;
+    u16_t srcPort;
+
+    ethType = zmw_rx_buf_readh(dev, buf, 12);
+    zm_msg2_rx(ZM_LV_2, "ethType=", ethType);
+
+    /* ARP */
+    if (ethType == 0x0608)
+    {
+        arpOp = zmw_rx_buf_readh(dev, buf, 20);
+        dip[0] = zmw_rx_buf_readh(dev, buf, 38);
+        dip[1] = zmw_rx_buf_readh(dev, buf, 40);
+        zm_msg2_rx(ZM_LV_2, "arpOp=", arpOp);
+        zm_msg2_rx(ZM_LV_2, "ip0=", dip[0]);
+        zm_msg2_rx(ZM_LV_2, "ip1=", dip[1]);
+
+        //ARP request to 192.168.1.15
+        if ((arpOp == 0x0100) && (dip[0] == 0xa8c0) && (dip[1] == 0x0f01));
+        {
+            zm_msg0_rx(ZM_LV_2, "ARP");
+            /* ARP response */
+            zmw_rx_buf_writeh(dev, buf, 20, 0x0200);
+
+            /* dst hardware address */
+
+            /* src hardware address */
+            //zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            //zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            //zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            /* dst ip address */
+            for (i=0; i<5; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 22+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 32+(i*2), temp);
+            }
+
+            /* src hardware address */
+            zmw_rx_buf_writeh(dev, buf, 22, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 24, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 26, 0x0000);
+
+            /* src ip address */
+            zmw_rx_buf_writeh(dev, buf, 28, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 30, 0x0f01);
+        }
+    }
+    /* ICMP */
+    else if (ethType == 0x0008)
+    {
+        zm_msg0_rx(ZM_LV_2, "IP");
+        prot = zmw_rx_buf_readb(dev, buf, 23);
+        dip[0] = zmw_rx_buf_readh(dev, buf, 30);
+        dip[1] = zmw_rx_buf_readh(dev, buf, 32);
+        zm_msg2_rx(ZM_LV_2, "prot=", prot);
+        zm_msg2_rx(ZM_LV_2, "ip0=", dip[0]);
+        zm_msg2_rx(ZM_LV_2, "ip1=", dip[1]);
+
+        /* PING request to 192.168.1.15 */
+        if ((prot == 0x1) && (dip[0] == 0xa8c0) && (dip[1] == 0x0f01))
+        {
+            zm_msg0_rx(ZM_LV_2, "ICMP");
+            /* change dst */
+            for (i=0; i<3; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                zmw_rx_buf_writeh(dev, buf, i*2, temp);
+            }
+            /* change src */
+            zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            /* exchange src ip and dst ip */
+            for (i=0; i<2; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+            }
+            zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+
+            /* change icmp type to echo reply */
+            zmw_rx_buf_writeb(dev, buf, 34, 0x0);
+
+            /* update icmp checksum */
+            temp = zmw_rx_buf_readh(dev, buf, 36);
+            temp += 8;
+            zmw_rx_buf_writeh(dev, buf, 36, temp);
+        }
+        else if (prot == 0x6)
+        {
+            zm_msg0_rx(ZM_LV_2, "TCP");
+            srcPort = zmw_rx_buf_readh(dev, buf, 34);
+            dstPort = zmw_rx_buf_readh(dev, buf, 36);
+            zm_msg2_rx(ZM_LV_2, "Src Port=", srcPort);
+            zm_msg2_rx(ZM_LV_2, "Dst Port=", dstPort);
+            if ((dstPort == 0x1500) || (srcPort == 0x1500))
+            {
+                zm_msg0_rx(ZM_LV_2, "FTP");
+
+                /* change dst */
+                for (i=0; i<3; i++)
+                {
+                    temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                    zmw_rx_buf_writeh(dev, buf, i*2, temp);
+                }
+                /* change src */
+                zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+                zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+                zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+                /* exchange src ip and dst ip */
+                for (i=0; i<2; i++)
+                {
+                    temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                    zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+                }
+                zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+                zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+#if 0
+                /* Patch src port */
+                temp = zmw_rx_buf_readh(dev, buf, 34);
+                temp = zfSwap(zfSwap(temp) + 1);
+                zmw_rx_buf_writeh(dev, buf, 34, temp);
+                temp = zmw_rx_buf_readh(dev, buf, 38);
+                temp = zfSwap(zfSwap(temp) + 1);
+                zmw_rx_buf_writeh(dev, buf, 38, temp);
+
+                /* Patch checksum */
+                temp = zmw_rx_buf_readh(dev, buf, 50);
+                temp = zfSwap(temp);
+                temp = ~temp;
+                temp += 2;
+                temp = ~temp;
+                temp = zfSwap(temp);
+                zmw_rx_buf_writeh(dev, buf, 50, temp);
+#endif
+            }
+
+        }
+        else if (prot == 0x11)
+        {
+            /* change dst */
+            for (i=0; i<3; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                zmw_rx_buf_writeh(dev, buf, i*2, temp);
+            }
+            /* change src */
+            zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            zm_msg0_rx(ZM_LV_2, "UDP");
+            srcPort = zmw_rx_buf_readh(dev, buf, 34);
+            dstPort = zmw_rx_buf_readh(dev, buf, 36);
+            zm_msg2_rx(ZM_LV_2, "Src Port=", srcPort);
+            zm_msg2_rx(ZM_LV_2, "Dst Port=", dstPort);
+
+            /* exchange src ip and dst ip */
+            for (i=0; i<2; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+            }
+            zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+
+            /* exchange port */
+            zmw_rx_buf_writeh(dev, buf, 34, srcPort+1);
+            zmw_rx_buf_writeh(dev, buf, 36, dstPort);
+
+            /* checksum = 0 */
+            zmw_rx_buf_writeh(dev, buf, 40, 0);
+        }
+
+    }
+    else if (ethType == 0x0060) /* =>0x0060 is port */
+    {
+        /* change src for Evl tool loop back receive */
+        zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+        zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+        zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+    }
+
+}
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiTxSendEth                */
+/*      Called to native 802.11 management frames                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x1-0x7=>VAP, 0x20-0x25=>WDS     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ray             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiTxSend80211Mgmt(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t err;
+    //u16_t addrTblSize = 0;
+    //struct zsAddrTbl addrTbl;
+    u16_t hlen;
+    u16_t header[(24+25+1)/2];
+    int i;
+
+    for(i=0;i<12;i++)
+    {
+        header[i] = zmw_buf_readh(dev, buf, i);
+    }
+    hlen = 24;
+
+    zfwBufRemoveHead(dev, buf, 24);
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_EXTERNAL_ALLOC_BUF, 0, 0)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return 0;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return 0;
+}
+
+u8_t zfiIsTxQueueFull(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ((((wd->vtxqHead[0] + 1) & ZM_VTXQ_SIZE_MASK) != wd->vtxqTail[0]) )
+    {
+        zmw_leave_critical_section(dev);
+        return 0;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 1;
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiTxSendEth                */
+/*      Called to transmit Ethernet frame from upper layer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x1-0x7=>VAP, 0x20-0x25=>WDS     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t err, ret;
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_PERFORMANCE_TX_MSDU(dev, wd->tick);
+    zm_msg1_tx(ZM_LV_2, "zfiTxSendEth(), port=", port);
+    /* Return error if port is disabled */
+    if ((err = zfTxPortControl(dev, buf, port)) == ZM_PORT_DISABLED)
+    {
+        err = ZM_ERR_TX_PORT_DISABLED;
+        goto zlError;
+    }
+
+#if 1
+    if ((wd->wlanMode == ZM_MODE_AP) && (port < 0x20))
+    {
+        /* AP : Buffer frame for power saving STA */
+        if ((ret = zfApBufferPsFrame(dev, buf, port)) == 1)
+        {
+            return ZM_SUCCESS;
+        }
+    }
+    else
+#endif
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if ( zfPowerSavingMgrIsSleeping(dev) )
+        {
+            /*check ZM_ENABLE_POWER_SAVE flag*/
+            zfPowerSavingMgrWakeup(dev);
+        }
+    }
+#ifdef ZM_ENABLE_IBSS_PS
+    /* IBSS power-saving mode */
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIbssPSQueueData(dev, buf) )
+        {
+            return ZM_SUCCESS;
+        }
+    }
+#endif
+
+#if 1
+    //if ( wd->bQoSEnable )
+    if (1)
+    {
+        /* Put to VTXQ[ac] */
+        ret = zfPutVtxq(dev, buf);
+
+        /* Push VTXQ[ac] */
+        zfPushVtxq(dev);
+    }
+    else
+    {
+        ret = zfTxSendEth(dev, buf, port, ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    return ret;
+#else
+    return zfTxSendEth(dev, buf, port, ZM_EXTERNAL_ALLOC_BUF, 0);
+#endif
+
+zlError:
+    zm_msg2_tx(ZM_LV_1, "Tx Comp err=", err);
+
+    zfwBufFree(dev, buf, err);
+    return err;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxSendEth                 */
+/*      Called to transmit Ethernet frame from upper layer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x10-0x17=>VAP, 0x20-0x25=>WDS   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u16_t flag)
+{
+    u16_t err;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t removeLen;
+    u16_t header[(8+30+2+18)/2];    /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t headerLen;
+    u16_t mic[8/2];
+    u16_t micLen;
+    u16_t snap[8/2];
+    u16_t snapLen;
+    u16_t fragLen;
+    u16_t frameLen;
+    u16_t fragNum;
+    struct zsFrag frag;
+    u16_t i, j, id;
+    u16_t offset;
+    u16_t da[3];
+    u16_t sa[3];
+    u8_t up;
+    u8_t qosType, keyIdx = 0;
+    u16_t fragOff;
+    u16_t newFlag;
+    struct zsMicVar*  pMicKey;
+    u8_t tkipFrameOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    newFlag = flag & 0xff00;
+    flag = flag & 0xff;
+
+    zm_msg1_tx(ZM_LV_2, "zfTxSendEth(), port=", port);
+
+    /* Get IP TOS for QoS AC and IP frag offset */
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+    //EOSP bit
+    if (newFlag & 0x100)
+    {
+        up |= 0x10;
+    }
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 16);
+        da[1] = zmw_tx_buf_readh(dev, buf, 18);
+        da[2] = zmw_tx_buf_readh(dev, buf, 20);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 16);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 18);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else
+    {
+        //
+    }
+#else
+    /* DA */
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+    /* SA */
+    sa[0] = zmw_tx_buf_readh(dev, buf, 6);
+    sa[1] = zmw_tx_buf_readh(dev, buf, 8);
+    sa[2] = zmw_tx_buf_readh(dev, buf, 10);
+#endif
+    //Decide Key Index in ATOM, No meaning in OTUS--CWYang(m)
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        keyIdx = wd->ap.bcHalKeyIdx[port];
+        id = zfApFindSta(dev, da);
+        if (id != 0xffff)
+        {
+            switch (wd->ap.staTable[id].encryMode)
+            {
+            case ZM_AES:
+            case ZM_TKIP:
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+                keyIdx = wd->ap.staTable[id].keyIdx;
+                break;
+            }
+        }
+    }
+    else
+    {
+        switch (wd->sta.encryMode)
+        {
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            keyIdx = wd->sta.keyId;
+            break;
+        case ZM_AES:
+        case ZM_TKIP:
+            if ((da[0] & 0x1))
+                keyIdx = 5;
+            else
+                keyIdx = 4;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            keyIdx = wd->sta.cencKeyId;
+            break;
+#endif //ZM_ENABLE_CENC
+        }
+    }
+
+    /* Create SNAP */
+    removeLen = zfTxGenWlanSnap(dev, buf, snap, &snapLen);
+    //zm_msg1_tx(ZM_LV_0, "fragOff=", fragOff);
+
+
+/* ********************************************************************************************** */
+/* Add 20071025 Mxzeng                                                                            */
+/* ********************************************************************************************** */
+/* ---------------------------------------------------------------------------------------------- */
+/*  Ethernet : frameLen = zfwBufGetSize(dev, buf);                                                */
+/* ---+--6--+--6--+--2--+-----20-----+-------------------------+------ Variable -------+--------- */
+/*    |  DA |  SA | Type|  IP Header | TCP(20) UDP(12) ICMP(8) | Application Payload L |          */
+/* ---+-----+-----+-----+------------+-------------------------+-----------------------+--------- */
+/*  MSDU = 6 + 6 + 2 + ( Network Layer header ) + ( Transport Layer header ) + L                  */
+/*                                                                                                */
+/*  MSDU - DA - SA : frameLen -= removeLen;                                                       */
+/* ---+--2--+-----20-----+-------------------------+------ Variable -------+--------------------- */
+/*    | Type| IP Header  | TCP(20) UDP(12) ICMP(8) | Application Payload L |                      */
+/* ---+-----+------------+-------------------------+-----------------------+--------------------- */
+/*												  */
+/*  MPDU : frameLen + mpduLengthOffset ;                                                          */
+/* -+---2---+----2---+-6-+-6-+--6--+---2----+--1--+--1-+---1---+-------3------+-frameLen-+---4--+- */
+/*  | frame |duration| DA|SA |BSSID|sequence|SNAP |SNAP|Control|    RFC 1042  |          |  FCS |  */
+/*  |Control|        |   |   |     | number |DSAP |SSAP|       | encapsulation|          |      |  */
+/* -+-------+--------+---+---+-----+--------+-----+----+-------+--------------+----------+------+- */
+/* ----------------------------------------------------------------------------------------------- */
+
+    if ( wd->sta.encryMode == ZM_TKIP )
+        tkipFrameOffset = 8;
+
+    fragLen = wd->fragThreshold + tkipFrameOffset;   // Fragmentation threshold for MPDU Lengths
+    frameLen = zfwBufGetSize(dev, buf);    // MSDU Lengths
+    frameLen -= removeLen;                 // MSDU Lengths - DA - SA
+
+    /* #1st create MIC Length manually */
+    micLen = 0;
+
+    /* Access Category */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaQosType(dev, da, &qosType);
+        if (qosType == 0)
+        {
+            up = 0;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if (wd->sta.wmeConnected == 0)
+        {
+            up = 0;
+        }
+    }
+    else
+    {
+        /* TODO : STA QoS control field */
+        up = 0;
+    }
+
+    /* #2nd Assign sequence number */
+    zmw_enter_critical_section(dev);
+    frag.seq[0] = ((wd->seq[zcUpToAc[up&0x7]]++) << 4);
+    zmw_leave_critical_section(dev);
+
+    /* #3rd Pass the total payload to generate MPDU length ! */
+    frag.buf[0] = buf;
+    frag.bufType[0] = bufType;
+    frag.flag[0] = (u8_t)flag;
+    fragNum = 1;
+
+    headerLen = zfTxGenWlanHeader(dev, frag.buf[0], header, frag.seq[0],
+                                  frag.flag[0], snapLen+micLen, removeLen, port, da, sa,
+                                  up, &micLen, snap, snapLen, NULL);
+
+    //zm_debug_msg1("#1 headerLen = ", headerLen);
+
+    /* #4th Check the HeaderLen and determine whether the MPDU Lengths bigger than Fragmentation threshold  */
+    /* If MPDU Lengths large than fragmentation threshold --> headerLen = 0 */
+    if( headerLen != 0 )
+    {
+        zf80211FrameSend(dev, frag.buf[0], header, snapLen, da, sa, up,
+                         headerLen, snap, mic, micLen, removeLen, frag.bufType[0],
+                         zcUpToAc[up&0x7], keyIdx);
+    }
+    else //if( headerLen == 0 ) // Need to be fragmented
+    {
+        u16_t mpduLengthOffset;
+        u16_t pseudSnapLen = 0;
+
+        mpduLengthOffset = header[0] - frameLen; // For fragmentation threshold !
+
+        micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic); // Get snap and mic information
+
+        fragLen = fragLen - mpduLengthOffset;
+
+        //zm_debug_msg1("#2 frameLen = ", frameLen);
+        //zm_debug_msg1("#3 fragThreshold = ", fragLen);
+
+        /* fragmentation */
+        if (frameLen >= fragLen)
+        {
+            //copy fragLen to frag
+            i = 0;
+            while( frameLen > 0 )
+            {
+                if ((frag.buf[i] = zfwBufAllocate(dev, fragLen+32)) != NULL)
+                {
+                    frag.bufType[i] = ZM_INTERNAL_ALLOC_BUF;
+                    frag.seq[i] = frag.seq[0] + i;
+                    offset = removeLen + i*fragLen;
+
+                    /* Consider the offset if we consider snap length to the other fragmented frame */
+                    if ( i >= 1 )
+                        offset = offset + pseudSnapLen*(i-1);
+
+                    if (frameLen > fragLen + pseudSnapLen)
+                    {
+                        frag.flag[i] = flag | 0x4; /* More data */
+                        /* First fragment */
+                        if (i == 0)
+                        {
+                            /* Add SNAP */
+                            for (j=0; j<snapLen; j+=2)
+                            {
+                                zmw_tx_buf_writeh(dev, frag.buf[i], j, snap[(j>>1)]);
+                            }
+                            zfTxBufferCopy(dev, frag.buf[i], buf, snapLen, offset, fragLen);
+                            zfwBufSetSize(dev, frag.buf[i], snapLen+fragLen);
+
+                            /* Add pseud snap length to the other fragmented frame */
+                            pseudSnapLen = snapLen;
+
+                            frameLen -= fragLen;
+                        }
+                        /* Intermediate Fragment */
+                        else
+                        {
+                            //zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, fragLen);
+                            //zfwBufSetSize(dev, frag.buf[i], fragLen);
+
+                            zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, fragLen+pseudSnapLen );
+                            zfwBufSetSize(dev, frag.buf[i], fragLen+pseudSnapLen);
+
+                            frameLen -= (fragLen+pseudSnapLen);
+                        }
+                        //frameLen -= fragLen;
+                    }
+                    else
+                    {
+                        /* Last fragment  */
+                        zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, frameLen);
+                        /* Add MIC if need */
+                        if ( micLen )
+                        {
+                            zfCopyToRxBuffer(dev, frag.buf[i], (u8_t*) mic, frameLen, micLen);
+                        }
+                        zfwBufSetSize(dev, frag.buf[i], frameLen+micLen);
+                        frameLen = 0;
+                        frag.flag[i] = (u8_t)flag; /* No more data */
+                    }
+                    i++;
+                }
+                else
+                {
+                    break;
+                }
+
+                // Please pay attention to the index of the buf !!!
+                // If write to null buf , the OS will crash !!!
+                zfwCopyBufContext(dev, buf, frag.buf[i-1]);
+            }
+            fragNum = i;
+            snapLen = micLen = removeLen = 0;
+
+            zfwBufFree(dev, buf, 0);
+        }
+
+        for (i=0; i<fragNum; i++)
+        {
+            /* Create WLAN header(Control Setting + 802.11 header + IV) */
+            headerLen = zfTxGenWlanHeader(dev, frag.buf[i], header, frag.seq[i],
+                                    frag.flag[i], snapLen+micLen, removeLen, port, da, sa, up, &micLen,
+                                    snap, snapLen, NULL);
+
+            zf80211FrameSend(dev, frag.buf[i], header, snapLen, da, sa, up,
+                             headerLen, snap, mic, micLen, removeLen, frag.bufType[i],
+                             zcUpToAc[up&0x7], keyIdx);
+
+        } /* for (i=0; i<fragNum; i++) */
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxPortControl             */
+/*      Check port status.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      buf : buffer pointer                                            */
+/*      port : port number, 0=>standard, 10-17=>Virtual AP, 20-25=>WDS  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_PORT_ENABLED or ZM_PORT_DISABLE                              */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Signature           ZyDAS Technology Corporation    2005.4      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxPortControl(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        if ( wd->sta.adapterState == ZM_STA_STATE_DISCONNECT )
+        {
+            zm_msg0_tx(ZM_LV_3, "Packets dropped due to disconnect state");
+            return ZM_PORT_DISABLED;
+        }
+    }
+
+    return ZM_PORT_ENABLED;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfIdlRecv                   */
+/*      Do frame validation and filtering then pass to zfwRecv80211().  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
+{
+    u16_t ret = 0;
+    u16_t bssid[3];
+    struct agg_tid_rx *tid_rx;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    /* tally */
+    wd->commTally.DriverRxFrmCnt++;
+
+    bssid[0] = zmw_buf_readh(dev, buf, 16);
+    bssid[1] = zmw_buf_readh(dev, buf, 18);
+    bssid[2] = zmw_buf_readh(dev, buf, 20);
+
+    /* Validate Rx frame */
+    if ((ret = zfWlanRxValidate(dev, buf)) != ZM_SUCCESS)
+    {
+        zm_msg1_rx(ZM_LV_1, "Rx invalid:", ret);
+        goto zlError;
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    //#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION
+    /*
+     * add by honda
+     */
+    tid_rx = zfAggRxEnabled(dev, buf);
+    if (tid_rx && wd->reorder)
+    {
+        zfAggRx(dev, buf, addInfo, tid_rx);
+
+        return;
+    }
+    /*
+     * end of add by honda
+     */
+    //#endif
+#endif
+
+    /* Filter Rx frame */
+    if ((ret = zfWlanRxFilter(dev, buf)) != ZM_SUCCESS)
+    {
+        zm_msg1_rx(ZM_LV_1, "Rx duplicated:", ret);
+        goto zlError;
+    }
+
+    /* Discard error frame except mic failure */
+    if ((addInfo->Tail.Data.ErrorIndication & 0x3f) != 0)
+    {
+        if ( wd->XLinkMode && ((addInfo->Tail.Data.ErrorIndication & 0x3f)==0x10) &&
+             zfCompareWithBssid(dev, bssid) )
+        {
+            // Bypass frames !!!
+        }
+        else
+        {
+            goto zlError;
+        }
+    }
+
+
+    /* OTUS command-8212 dump rx packet */
+    if (wd->rxPacketDump)
+    {
+        zfwDumpBuf(dev, buf);
+    }
+
+    /* Call zfwRecv80211() wrapper function to deliver Rx packet */
+    /* to driver framework.                                      */
+
+    if (wd->zfcbRecv80211 != NULL)
+    {
+        wd->zfcbRecv80211(dev, buf, addInfo); //CWYang(m)
+    }
+    else
+    {
+        zfiRecv80211(dev, buf, addInfo);
+    }
+    return;
+
+zlError:
+    zm_msg1_rx(ZM_LV_1, "Free packet, error code:", ret);
+
+    wd->commTally.DriverDiscardedFrm++;
+
+    /* Free Rx buffer */
+    zfwBufFree(dev, buf, 0);
+
+    return;
+}
+
+
+void zfShowRxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t   packetType, keyType, code, identifier, type, flags;
+    u16_t  packetLen, keyInfo, keyLen, keyDataLen, length, Op_Code;
+    u32_t  replayCounterH, replayCounterL, vendorId, VendorType;
+
+    /* EAPOL packet type */
+    packetType = zmw_rx_buf_readb(dev, buf, offset+1); // 0: EAP-Packet
+                                                       // 1: EAPOL-Start
+                                                       // 2: EAPOL-Logoff
+                                                       // 3: EAPOL-Key
+                                                       // 4: EAPOL-Encapsulated-ASF-Alert
+
+    /* EAPOL frame format */
+    /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+    /* -----------------------------------------------   */
+    /*            PAE Ethernet Type (0x888e)             */
+    /* ----------------------------------------------- 2 */
+    /*     Protocol Version    |         Type            */
+    /* ----------------------------------------------- 4 */
+    /*                       Length                      */
+    /* ----------------------------------------------- 6 */
+    /*                    Packet Body                    */
+    /* ----------------------------------------------- N */
+
+    /* EAPOL body length */
+    packetLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+2)) << 8) +
+                zmw_rx_buf_readb(dev, buf, offset+3);
+
+    if( packetType == 0 )
+    { // EAP-Packet
+
+        /* EAP-Packet Code */
+        code = zmw_rx_buf_readb(dev, buf, offset+4); // 1 : Request
+                                                     // 2 : Response
+                                                     // 3 : Success
+                                                     // 4 : Failure
+        // An EAP packet of the type of Success and Failure has no Data field, and has a length of 4.
+
+        /* EAP Packet format */
+        /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+        /* -----------------------------------------------   */
+        /*           Code          |        Identifier       */
+        /* ----------------------------------------------- 2 */
+        /*                       Length                      */
+        /* ----------------------------------------------- 4 */
+        /*                        Data                       */
+        /* ----------------------------------------------- N */
+
+        zm_debug_msg0("EAP-Packet");
+        zm_debug_msg1("Packet Length = ", packetLen);
+        zm_debug_msg1("EAP-Packet Code = ", code);
+
+        if( code == 1 )
+        {
+            zm_debug_msg0("EAP-Packet Request");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_rx_buf_readb(dev, buf, offset+8); // 1   : Identity
+                                                         // 2   : Notification
+                                                         // 3   : Nak (Response Only)
+                                                         // 4   : MD5-Challenge
+                                                         // 5   : One Time Password (OTP)
+                                                         // 6   : Generic Token Card (GTC)
+                                                         // 254 : (Expanded Types)Wi-Fi Protected Setup
+                                                         // 255 : Experimental Use
+
+            /* The data field in an EAP packet of the type of Request or Response is in the format shown bellowing */
+            /*  0  1  2  3  4  5  6  7             N             */
+            /* -----------------------------------------------   */
+            /*           Type          |        Type Data        */
+            /* -----------------------------------------------   */
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Request Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Request Wi-Fi Protected Setup");
+
+                /* 0                   1                   2                   3   */
+                /* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|     Type      |               Vendor-Id                       |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|                          Vendor-Type                          |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|              Vendor data...                                    */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+                        */
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_rx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_rx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_rx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_rx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 2 )
+        {
+            zm_debug_msg0("EAP-Packet Response");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_rx_buf_readb(dev, buf, offset+8);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Response Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 3 )
+            {
+                zm_debug_msg0("EAP-Packet Request Nak");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Response Wi-Fi Protected Setup");
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_rx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_rx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_rx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_rx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 3 )
+        {
+            zm_debug_msg0("EAP-Packet Success");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+        else if( code == 4 )
+        {
+            zm_debug_msg0("EAP-Packet Failure");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+    }
+    else if( packetType == 1 )
+    { // EAPOL-Start
+        zm_debug_msg0("EAPOL-Start");
+    }
+    else if( packetType == 2 )
+    { // EAPOL-Logoff
+        zm_debug_msg0("EAPOL-Logoff");
+    }
+    else if( packetType == 3 )
+    { // EAPOL-Key
+        /* EAPOL-Key type */
+        keyType = zmw_rx_buf_readb(dev, buf, offset+4);
+        /* EAPOL-Key information */
+        keyInfo = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+5)) << 8) +
+                  zmw_rx_buf_readb(dev, buf, offset+6);
+        /* EAPOL-Key length */
+        keyLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+7)) << 8) +
+                 zmw_rx_buf_readb(dev, buf, offset+8);
+        /* EAPOL-Key replay counter (high double word) */
+        replayCounterH = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 24) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 16) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+11)) << 8) +
+                         zmw_rx_buf_readb(dev, buf, offset+12);
+        /* EAPOL-Key replay counter (low double word) */
+        replayCounterL = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 24) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 16) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+15)) << 8) +
+                         zmw_rx_buf_readb(dev, buf, offset+16);
+        /* EAPOL-Key data length */
+        keyDataLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+97)) << 8) +
+                     zmw_rx_buf_readb(dev, buf, offset+98);
+
+        zm_debug_msg0("EAPOL-Key");
+        zm_debug_msg1("packet length = ", packetLen);
+
+        if ( keyType == 254 )
+        {
+            zm_debug_msg0("key type = 254 (SSN key descriptor)");
+        }
+        else
+        {
+            zm_debug_msg2("key type = 0x", keyType);
+        }
+
+        zm_debug_msg2("replay counter(L) = ", replayCounterL);
+
+        zm_debug_msg2("key information = ", keyInfo);
+
+        if ( keyInfo & ZM_BIT_3 )
+        {
+            zm_debug_msg0("    - pairwise key");
+        }
+        else
+        {
+            zm_debug_msg0("    - group key");
+        }
+
+        if ( keyInfo & ZM_BIT_6 )
+        {
+            zm_debug_msg0("    - Tx key installed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Tx key not set");
+        }
+
+        if ( keyInfo & ZM_BIT_7 )
+        {
+            zm_debug_msg0("    - Ack needed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Ack not needed");
+        }
+
+        if ( keyInfo & ZM_BIT_8 )
+        {
+            zm_debug_msg0("    - MIC set");
+        }
+        else
+        {
+            zm_debug_msg0("    - MIC not set");
+        }
+
+        if ( keyInfo & ZM_BIT_9 )
+        {
+            zm_debug_msg0("    - packet encrypted");
+        }
+        else
+        {
+            zm_debug_msg0("    - packet not encrypted");
+        }
+
+        zm_debug_msg1("keyLen = ", keyLen);
+        zm_debug_msg1("keyDataLen = ", keyDataLen);
+    }
+    else if( packetType == 4 )
+    {
+        zm_debug_msg0("EAPOL-Encapsulated-ASF-Alert");
+    }
+}
+
+void zfShowTxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t   packetType, keyType, code, identifier, type, flags;
+    u16_t  packetLen, keyInfo, keyLen, keyDataLen, length, Op_Code;
+    u32_t  replayCounterH, replayCounterL, vendorId, VendorType;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("EAPOL Packet size = ", zfwBufGetSize(dev, buf));
+
+    /* EAPOL packet type */
+    // 0: EAP-Packet
+    // 1: EAPOL-Start
+    // 2: EAPOL-Logoff
+    // 3: EAPOL-Key
+    // 4: EAPOL-Encapsulated-ASF-Alert
+
+    /* EAPOL frame format */
+    /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+    /* -----------------------------------------------   */
+    /*            PAE Ethernet Type (0x888e)             */
+    /* ----------------------------------------------- 2 */
+    /*     Protocol Version    |         Type            */
+    /* ----------------------------------------------- 4 */
+    /*                       Length                      */
+    /* ----------------------------------------------- 6 */
+    /*                    Packet Body                    */
+    /* ----------------------------------------------- N */
+
+    packetType = zmw_tx_buf_readb(dev, buf, offset+1);
+    /* EAPOL body length */
+    packetLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+2)) << 8) +
+                zmw_tx_buf_readb(dev, buf, offset+3);
+
+    if( packetType == 0 )
+    { // EAP-Packet
+        /* EAP-Packet Code */
+        code = zmw_tx_buf_readb(dev, buf, offset+4); // 1 : Request
+                                                     // 2 : Response
+                                                     // 3 : Success
+                                                     // 4 : Failure
+
+        // An EAP packet of the type of Success and Failure has no Data field, and has a length of 4.
+
+        /* EAP Packet format */
+        /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+        /* -----------------------------------------------   */
+        /*           Code          |        Identifier       */
+        /* ----------------------------------------------- 2 */
+        /*                       Length                      */
+        /* ----------------------------------------------- 4 */
+        /*                        Data                       */
+        /* ----------------------------------------------- N */
+
+        zm_debug_msg0("EAP-Packet");
+        zm_debug_msg1("Packet Length = ", packetLen);
+        zm_debug_msg1("EAP-Packet Code = ", code);
+
+        if( code == 1 )
+        {
+            zm_debug_msg0("EAP-Packet Request");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_tx_buf_readb(dev, buf, offset+8); // 1   : Identity
+                                                         // 2   : Notification
+                                                         // 3   : Nak (Response Only)
+                                                         // 4   : MD5-Challenge
+                                                         // 5   : One Time Password (OTP)
+                                                         // 6   : Generic Token Card (GTC)
+                                                         // 254 : (Expanded Types)Wi-Fi Protected Setup
+                                                         // 255 : Experimental Use
+
+            /* The data field in an EAP packet of the type of Request or Response is in the format shown bellowing */
+            /*  0  1  2  3  4  5  6  7             N             */
+            /* -----------------------------------------------   */
+            /*           Type          |        Type Data        */
+            /* -----------------------------------------------   */
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Request Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Request Wi-Fi Protected Setup");
+
+                /* 0                   1                   2                   3   */
+                /* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|     Type      |               Vendor-Id                       |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|                          Vendor-Type                          |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|              Vendor data...                                    */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+                        */
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_tx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_tx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_tx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_tx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 2 )
+        {
+            zm_debug_msg0("EAP-Packet Response");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_tx_buf_readb(dev, buf, offset+8);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Response Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 3 )
+            {
+                zm_debug_msg0("EAP-Packet Request Nak");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Response Wi-Fi Protected Setup");
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_tx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_tx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_tx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_tx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 3 )
+        {
+            zm_debug_msg0("EAP-Packet Success");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+        else if( code == 4 )
+        {
+            zm_debug_msg0("EAP-Packet Failure");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+    }
+    else if( packetType == 1 )
+    { // EAPOL-Start
+        zm_debug_msg0("EAPOL-Start");
+    }
+    else if( packetType == 2 )
+    { // EAPOL-Logoff
+        zm_debug_msg0("EAPOL-Logoff");
+    }
+    else if( packetType == 3 )
+    { // EAPOL-Key
+        /* EAPOL-Key type */
+        keyType = zmw_tx_buf_readb(dev, buf, offset+4);
+        /* EAPOL-Key information */
+        keyInfo = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+5)) << 8) +
+                  zmw_tx_buf_readb(dev, buf, offset+6);
+        /* EAPOL-Key length */
+        keyLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+7)) << 8) +
+                 zmw_tx_buf_readb(dev, buf, offset+8);
+        /* EAPOL-Key replay counter (high double word) */
+        replayCounterH = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 24) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 16) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+11)) << 8) +
+                         zmw_tx_buf_readb(dev, buf, offset+12);
+        /* EAPOL-Key replay counter (low double word) */
+        replayCounterL = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 24) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 16) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+15)) << 8) +
+                         zmw_tx_buf_readb(dev, buf, offset+16);
+        /* EAPOL-Key data length */
+        keyDataLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+97)) << 8) +
+                     zmw_tx_buf_readb(dev, buf, offset+98);
+
+        zm_debug_msg0("EAPOL-Key");
+        zm_debug_msg1("packet length = ", packetLen);
+
+        if ( keyType == 254 )
+        {
+            zm_debug_msg0("key type = 254 (SSN key descriptor)");
+        }
+        else
+        {
+            zm_debug_msg2("key type = 0x", keyType);
+        }
+
+        zm_debug_msg2("replay counter(L) = ", replayCounterL);
+
+        zm_debug_msg2("key information = ", keyInfo);
+
+        if ( keyInfo & ZM_BIT_3 )
+        {
+            zm_debug_msg0("    - pairwise key");
+        }
+        else
+        {
+            zm_debug_msg0("    - group key");
+        }
+
+        if ( keyInfo & ZM_BIT_6 )
+        {
+            zm_debug_msg0("    - Tx key installed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Tx key not set");
+        }
+
+        if ( keyInfo & ZM_BIT_7 )
+        {
+            zm_debug_msg0("    - Ack needed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Ack not needed");
+        }
+
+        if ( keyInfo & ZM_BIT_8 )
+        {
+            zm_debug_msg0("    - MIC set");
+        }
+        else
+        {
+            zm_debug_msg0("    - MIC not set");
+        }
+
+        if ( keyInfo & ZM_BIT_9 )
+        {
+            zm_debug_msg0("    - packet encrypted");
+        }
+        else
+        {
+            zm_debug_msg0("    - packet not encrypted");
+        }
+
+        zm_debug_msg1("keyLen = ", keyLen);
+        zm_debug_msg1("keyDataLen = ", keyDataLen);
+    }
+    else if( packetType == 4 )
+    {
+        zm_debug_msg0("EAPOL-Encapsulated-ASF-Alert");
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiRecv80211                */
+/*      Called to receive 802.11 frame.                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
+{
+    u8_t snapCase=0, encryMode;
+    u16_t frameType, typeLengthField;
+    u16_t frameCtrl;
+    u16_t frameSubtype;
+    u16_t ret;
+    u16_t len;
+    u8_t bIsDefrag = 0;
+    u16_t offset, tailLen;
+    u8_t vap = 0;
+    u16_t da[3], sa[3];
+    u16_t ii;
+    u8_t uapsdTrig = 0;
+    zbuf_t* psBuf;
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    u8_t i;
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    //zm_msg2_rx(ZM_LV_2, "zfiRecv80211(), buf=", buf);
+
+    //zm_msg2_rx(ZM_LV_0, "h[0]=", zmw_rx_buf_readh(dev, buf, 0));
+    //zm_msg2_rx(ZM_LV_0, "h[2]=", zmw_rx_buf_readh(dev, buf, 2));
+    //zm_msg2_rx(ZM_LV_0, "h[4]=", zmw_rx_buf_readh(dev, buf, 4));
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 0);
+    frameType = frameCtrl & 0xf;
+    frameSubtype = frameCtrl & 0xf0;
+
+#if 0   // Move to ProcessBeacon to judge if there's a new peer station
+    if ( (wd->wlanMode == ZM_MODE_IBSS)&&
+         (wd->sta.ibssPartnerStatus != ZM_IBSS_PARTNER_ALIVE) )
+    {
+        zfStaIbssMonitoring(dev, buf);
+    }
+#endif
+
+    /* If data frame */
+    if (frameType == ZM_WLAN_DATA_FRAME)
+    {
+        wd->sta.TotalNumberOfReceivePackets++;
+        wd->sta.TotalNumberOfReceiveBytes += zfwBufGetSize(dev, buf);
+        //zm_debug_msg1("Receive packets     = ", wd->sta.TotalNumberOfReceivePackets);
+
+        //zm_msg0_rx(ZM_LV_0, "Rx data");
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            if ((ret = zfApUpdatePsBit(dev, buf, &vap, &uapsdTrig)) != ZM_SUCCESS)
+            {
+                zfwBufFree(dev, buf, 0);
+                return;
+            }
+
+            if (((uapsdTrig&0xf) != 0) && ((frameSubtype & 0x80) != 0))
+            {
+                u8_t ac = zcUpToAc[zmw_buf_readb(dev, buf, 24)&0x7];
+                u8_t pktNum;
+                u8_t mb;
+                u16_t flag;
+                u8_t src[6];
+
+                //printk("QoS ctrl=%d\n", zmw_buf_readb(dev, buf, 24));
+                //printk("UAPSD trigger, ac=%d\n", ac);
+
+                if (((0x8>>ac) & uapsdTrig) != 0)
+                {
+                    pktNum = zcMaxspToPktNum[(uapsdTrig>>4) & 0x3];
+
+                    for (ii=0; ii<6; ii++)
+                    {
+                        src[ii] = zmw_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+ii);
+                    }
+
+                    for (ii=0; ii<pktNum; ii++)
+                    {
+                        //if ((psBuf = zfQueueGet(dev, wd->ap.uapsdQ)) != NULL)
+                        if ((psBuf = zfQueueGetWithMac(dev, wd->ap.uapsdQ, src, &mb)) != NULL)
+                        {
+                            if ((ii+1) == pktNum)
+                            {
+                                //EOSP anyway
+                                flag = 0x100 | (mb<<5);
+                            }
+                            else
+                            {
+                                if (mb != 0)
+                                {
+                                    //more data, not EOSP
+                                    flag = 0x20;
+                                }
+                                else
+                                {
+                                    //no more data, EOSP
+                                    flag = 0x100;
+                                }
+                            }
+                            zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, flag);
+                        }
+
+                        if ((psBuf == NULL) || (mb == 0))
+                        {
+                            if ((ii == 0) && (psBuf == NULL))
+                            {
+                                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_QOS_NULL, (u16_t*)src, 0, 0, 0);
+                            }
+                            break;
+                        }
+                    }
+                }
+            }
+
+        }
+        else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            u16_t frameCtrlMSB;
+    		u8_t   bssid[6];
+
+            /* Check Is RIFS frame and decide to enable RIFS or not */
+            if( wd->sta.EnableHT )
+                zfCheckIsRIFSFrame(dev, buf, frameSubtype);
+
+            if ( zfPowerSavingMgrIsSleeping(dev) || wd->sta.psMgr.tempWakeUp == 1)
+            {
+                frameCtrlMSB = zmw_rx_buf_readb(dev, buf, 1);
+
+                /* check more data */
+                if ( frameCtrlMSB & ZM_BIT_5 )
+                {
+                    //if rx frame's AC is not delivery-enabled
+                    if ((wd->sta.qosInfo&0xf) != 0xf)
+                    {
+                        u8_t rxAc = 0;
+                        if ((frameSubtype & 0x80) != 0)
+                        {
+                            rxAc = zcUpToAc[zmw_buf_readb(dev, buf, 24)&0x7];
+                        }
+
+                        if (((0x8>>rxAc) & wd->sta.qosInfo) == 0)
+                        {
+                            zfSendPSPoll(dev);
+                            wd->sta.psMgr.tempWakeUp = 0;
+                        }
+                    }
+                }
+            }
+			/*increase beacon count when receive vaild data frame from AP*/
+        	ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+			if (zfStaIsConnected(dev)&&
+				zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6))
+			{
+                wd->sta.rxBeaconCount++;
+			}
+        }
+
+        zm_msg1_rx(ZM_LV_2, "Rx VAP=", vap);
+
+        /* handle IV, EXT-IV, ICV, and EXT-ICV */
+        zfGetRxIvIcvLength(dev, buf, vap, &offset, &tailLen, addInfo);
+
+        zfStaIbssPSCheckState(dev, buf);
+        //QoS data frame
+        if ((frameSubtype & 0x80) == 0x80)
+        {
+            offset += 2;
+        }
+
+        len = zfwBufGetSize(dev, buf);
+        /* remove ICV */
+        if (tailLen > 0)
+        {
+            if (len > tailLen)
+            {
+                len -= tailLen;
+                zfwBufSetSize(dev, buf, len);
+            }
+        }
+
+        /* Filter NULL data */
+        if (((frameSubtype&0x40) != 0) || ((len = zfwBufGetSize(dev, buf))<=24))
+        {
+            zm_msg1_rx(ZM_LV_1, "Free Rx NULL data, len=", len);
+            zfwBufFree(dev, buf, 0);
+            return;
+        }
+
+        /* check and handle defragmentation */
+        if ( wd->sta.bSafeMode && (wd->sta.wepStatus == ZM_ENCRYPTION_AES) && wd->sta.SWEncryptEnable )
+        {
+            zm_msg0_rx(ZM_LV_1, "Bypass defragmentation packets in safe mode");
+        }
+        else
+        {
+            if ( (buf = zfDefragment(dev, buf, &bIsDefrag, addInfo)) == NULL )
+            {
+                /* In this case, the buffer has been freed in zfDefragment */
+                return;
+            }
+        }
+
+        ret = ZM_MIC_SUCCESS;
+
+        /* If SW WEP/TKIP are not turned on */
+        if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_DECRY_EN) == 0 &&
+            (wd->sta.SWEncryptEnable & ZM_SW_WEP_DECRY_EN) == 0)
+        {
+            encryMode = zfGetEncryModeFromRxStatus(addInfo);
+
+            /* check if TKIP */
+            if ( encryMode == ZM_TKIP )
+            {
+                if ( bIsDefrag )
+                {
+                    ret = zfMicRxVerify(dev, buf);
+                }
+                else
+                {
+                    /* check MIC failure bit */
+                    if ( ZM_RX_STATUS_IS_MIC_FAIL(addInfo) )
+                    {
+                        ret = ZM_MIC_FAILURE;
+                    }
+                }
+
+                if ( ret == ZM_MIC_FAILURE )
+                {
+                    u8_t Unicast_Pkt = 0x0;
+
+                    if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+                    {
+                        wd->commTally.swRxUnicastMicFailCount++;
+                        Unicast_Pkt = 0x1;
+                    }/*
+                    else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+                    {
+                        wd->commTally.swRxMulticastMicFailCount++;
+                    }*/
+                    else
+                    {
+                        wd->commTally.swRxMulticastMicFailCount++;
+                    }
+                    if ( wd->wlanMode == ZM_MODE_AP )
+                    {
+                        u16_t idx;
+                        u8_t addr[6];
+
+                        for (idx=0; idx<6; idx++)
+                        {
+                            addr[idx] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+idx);
+                        }
+
+                        if (wd->zfcbApMicFailureNotify != NULL)
+                        {
+                            wd->zfcbApMicFailureNotify(dev, addr, buf);
+                        }
+                    }
+                    else
+                    {
+                        if(Unicast_Pkt)
+                        {
+                            zm_debug_msg0("Countermeasure : Unicast_Pkt ");
+                        }
+                        else
+                        {
+                            zm_debug_msg0("Countermeasure : Non-Unicast_Pkt ");
+                        }
+
+                        if((wd->TKIP_Group_KeyChanging == 0x0) || (Unicast_Pkt == 0x1))
+                        {
+                            zm_debug_msg0("Countermeasure : Do MIC Check ");
+                            zfStaMicFailureHandling(dev, buf);
+                        }
+                        else
+                        {
+                            zm_debug_msg0("Countermeasure : SKIP MIC Check due to Group Keychanging ");
+                        }
+                    }
+                    /* Discard MIC failed frame */
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+            }
+        }
+        else
+        {
+            u8_t IsEncryFrame;
+
+            /* TODO: Check whether WEP bit is turned on in MAC header */
+            encryMode = ZM_NO_WEP;
+
+            IsEncryFrame = (zmw_rx_buf_readb(dev, buf, 1) & 0x40);
+
+            if (IsEncryFrame)
+            {
+                /* Software decryption for TKIP */
+                if (wd->sta.SWEncryptEnable & ZM_SW_TKIP_DECRY_EN)
+                {
+                    u16_t iv16;
+                    u16_t iv32;
+                    u8_t RC4Key[16];
+                    u16_t IvOffset;
+                    struct zsTkipSeed *rxSeed;
+
+                    IvOffset = offset + ZM_SIZE_OF_WLAN_DATA_HEADER;
+
+                    rxSeed = zfStaGetRxSeed(dev, buf);
+
+                    if (rxSeed == NULL)
+                    {
+                        zm_debug_msg0("rxSeed is NULL");
+
+                        /* Discard this frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    iv16 = (zmw_rx_buf_readb(dev, buf, IvOffset) << 8) + zmw_rx_buf_readb(dev, buf, IvOffset+2);
+                    iv32 = zmw_rx_buf_readb(dev, buf, IvOffset+4) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+5) << 8) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+6) << 16) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+7) << 24);
+
+                    /* TKIP Key Mixing */
+                    zfTkipPhase1KeyMix(iv32, rxSeed);
+                    zfTkipPhase2KeyMix(iv16, rxSeed);
+                    zfTkipGetseeds(iv16, RC4Key, rxSeed);
+
+                    /* Decrypt Data */
+                    ret = zfTKIPDecrypt(dev, buf, IvOffset+ZM_SIZE_OF_IV+ZM_SIZE_OF_EXT_IV, 16, RC4Key);
+
+                    if (ret == ZM_ICV_FAILURE)
+                    {
+                        zm_debug_msg0("TKIP ICV fail");
+
+                        /* Discard ICV failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    /* Remove ICV from buffer */
+                    zfwBufSetSize(dev, buf, len-4);
+
+                    /* Check MIC */
+                    ret = zfMicRxVerify(dev, buf);
+
+                    if (ret == ZM_MIC_FAILURE)
+                    {
+                        if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+                        {
+                            wd->commTally.swRxUnicastMicFailCount++;
+                        }
+                        else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+                        {
+                            wd->commTally.swRxMulticastMicFailCount++;
+                        }
+                        else
+                        {
+                            wd->commTally.swRxMulticastMicFailCount++;
+                        }
+                        if ( wd->wlanMode == ZM_MODE_AP )
+                        {
+                            u16_t idx;
+                            u8_t addr[6];
+
+                            for (idx=0; idx<6; idx++)
+                            {
+                                addr[idx] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+idx);
+                            }
+
+                            if (wd->zfcbApMicFailureNotify != NULL)
+                            {
+                                wd->zfcbApMicFailureNotify(dev, addr, buf);
+                            }
+                        }
+                        else
+                        {
+                            zfStaMicFailureHandling(dev, buf);
+                        }
+
+                        zm_debug_msg0("MIC fail");
+                        /* Discard MIC failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    encryMode = ZM_TKIP;
+                    offset += ZM_SIZE_OF_IV + ZM_SIZE_OF_EXT_IV;
+                }
+                else if(wd->sta.SWEncryptEnable & ZM_SW_WEP_DECRY_EN)
+                {
+                    u16_t IvOffset;
+                    u8_t keyLen = 5;
+                    u8_t iv[3];
+                    u8_t *wepKey;
+                    u8_t keyIdx;
+
+                    IvOffset = offset + ZM_SIZE_OF_WLAN_DATA_HEADER;
+
+                    /* Retrieve IV */
+                    iv[0] = zmw_rx_buf_readb(dev, buf, IvOffset);
+                    iv[1] = zmw_rx_buf_readb(dev, buf, IvOffset+1);
+                    iv[2] = zmw_rx_buf_readb(dev, buf, IvOffset+2);
+
+                    keyIdx = ((zmw_rx_buf_readb(dev, buf, IvOffset+3) >> 6) & 0x03);
+
+                    IvOffset += ZM_SIZE_OF_IV;
+
+                    if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP64)
+                    {
+                        keyLen = 5;
+                    }
+                    else if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP128)
+                    {
+                        keyLen = 13;
+                    }
+                    else if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP256)
+                    {
+                        keyLen = 29;
+                    }
+
+                    zfWEPDecrypt(dev, buf, IvOffset, keyLen, wd->sta.wepKey[keyIdx], iv);
+
+                    if (ret == ZM_ICV_FAILURE)
+                    {
+                        zm_debug_msg0("WEP ICV fail");
+
+                        /* Discard ICV failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    encryMode = wd->sta.SWEncryMode[keyIdx];
+
+                    /* Remove ICV from buffer */
+                    zfwBufSetSize(dev, buf, len-4);
+
+                    offset += ZM_SIZE_OF_IV;
+                }
+            }
+        }
+
+#ifdef ZM_ENABLE_CENC
+        //else if ( encryMode == ZM_CENC ) /* check if CENC */
+        if ( encryMode == ZM_CENC )
+        {
+            u32_t rxIV[4];
+
+            rxIV[0] = (zmw_rx_buf_readh(dev, buf, 28) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 26);
+            rxIV[1] = (zmw_rx_buf_readh(dev, buf, 32) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 30);
+            rxIV[2] = (zmw_rx_buf_readh(dev, buf, 36) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 34);
+            rxIV[3] = (zmw_rx_buf_readh(dev, buf, 40) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 38);
+
+            //zm_debug_msg2("rxIV[0] = 0x", rxIV[0]);
+            //zm_debug_msg2("rxIV[1] = 0x", rxIV[1]);
+            //zm_debug_msg2("rxIV[2] = 0x", rxIV[2]);
+            //zm_debug_msg2("rxIV[3] = 0x", rxIV[3]);
+
+            /* destination address*/
+            da[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+            da[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+2);
+            da[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+4);
+
+            if ( wd->wlanMode == ZM_MODE_AP )
+            {
+            }
+            else
+            {
+                if ((da[0] & 0x1))
+                { //multicast frame
+                    /* Accumlate the PN sequence */
+                    wd->sta.rxivGK[0] ++;
+
+                    if (wd->sta.rxivGK[0] == 0)
+                    {
+                        wd->sta.rxivGK[1]++;
+                    }
+
+                    if (wd->sta.rxivGK[1] == 0)
+                    {
+                        wd->sta.rxivGK[2]++;
+                    }
+
+                    if (wd->sta.rxivGK[2] == 0)
+                    {
+                        wd->sta.rxivGK[3]++;
+                    }
+
+                    if (wd->sta.rxivGK[3] == 0)
+                    {
+                        wd->sta.rxivGK[0] = 0;
+                        wd->sta.rxivGK[1] = 0;
+                        wd->sta.rxivGK[2] = 0;
+                    }
+
+                    //zm_debug_msg2("wd->sta.rxivGK[0] = 0x", wd->sta.rxivGK[0]);
+                    //zm_debug_msg2("wd->sta.rxivGK[1] = 0x", wd->sta.rxivGK[1]);
+                    //zm_debug_msg2("wd->sta.rxivGK[2] = 0x", wd->sta.rxivGK[2]);
+                    //zm_debug_msg2("wd->sta.rxivGK[3] = 0x", wd->sta.rxivGK[3]);
+
+                    if ( !((wd->sta.rxivGK[0] == rxIV[0])
+                        && (wd->sta.rxivGK[1] == rxIV[1])
+                        && (wd->sta.rxivGK[2] == rxIV[2])
+                        && (wd->sta.rxivGK[3] == rxIV[3])))
+                    {
+                        u8_t PacketDiscard = 0;
+                        /* Discard PN Code Error frame */
+                        if (rxIV[0] < wd->sta.rxivGK[0])
+                        {
+                            PacketDiscard = 1;
+                        }
+                        if (wd->sta.rxivGK[0] > 0xfffffff0)
+                        { //boundary case
+                            if ((rxIV[0] < 0xfffffff0)
+                                && (((0xffffffff - wd->sta.rxivGK[0]) + rxIV[0]) > 16))
+                            {
+                                PacketDiscard = 1;
+                            }
+                        }
+                        else
+                        { //normal case
+                            if ((rxIV[0] - wd->sta.rxivGK[0]) > 16)
+                            {
+                                PacketDiscard = 1;
+                            }
+                        }
+                        // sync sta pn code with ap because of losting some packets
+                        wd->sta.rxivGK[0] = rxIV[0];
+                        wd->sta.rxivGK[1] = rxIV[1];
+                        wd->sta.rxivGK[2] = rxIV[2];
+                        wd->sta.rxivGK[3] = rxIV[3];
+                        if (PacketDiscard)
+                        {
+                            zm_debug_msg0("Discard PN Code lost too much multicast frame");
+                            zfwBufFree(dev, buf, 0);
+                            return;
+                        }
+                    }
+                }
+                else
+                { //unicast frame
+                    /* Accumlate the PN sequence */
+                    wd->sta.rxiv[0] += 2;
+
+                    if (wd->sta.rxiv[0] == 0 || wd->sta.rxiv[0] == 1)
+                    {
+                        wd->sta.rxiv[1]++;
+                    }
+
+                    if (wd->sta.rxiv[1] == 0)
+                    {
+                        wd->sta.rxiv[2]++;
+                    }
+
+                    if (wd->sta.rxiv[2] == 0)
+                    {
+                        wd->sta.rxiv[3]++;
+                    }
+
+                    if (wd->sta.rxiv[3] == 0)
+                    {
+                        wd->sta.rxiv[0] = 0;
+                        wd->sta.rxiv[1] = 0;
+                        wd->sta.rxiv[2] = 0;
+                    }
+
+                    //zm_debug_msg2("wd->sta.rxiv[0] = 0x", wd->sta.rxiv[0]);
+                    //zm_debug_msg2("wd->sta.rxiv[1] = 0x", wd->sta.rxiv[1]);
+                    //zm_debug_msg2("wd->sta.rxiv[2] = 0x", wd->sta.rxiv[2]);
+                    //zm_debug_msg2("wd->sta.rxiv[3] = 0x", wd->sta.rxiv[3]);
+
+                    if ( !((wd->sta.rxiv[0] == rxIV[0])
+                        && (wd->sta.rxiv[1] == rxIV[1])
+                        && (wd->sta.rxiv[2] == rxIV[2])
+                        && (wd->sta.rxiv[3] == rxIV[3])))
+                    {
+                        zm_debug_msg0("PN Code mismatch, lost unicast frame, sync pn code to recv packet");
+                        // sync sta pn code with ap because of losting some packets
+                        wd->sta.rxiv[0] = rxIV[0];
+                        wd->sta.rxiv[1] = rxIV[1];
+                        wd->sta.rxiv[2] = rxIV[2];
+                        wd->sta.rxiv[3] = rxIV[3];
+                        /* Discard PN Code Error frame */
+                        //zm_debug_msg0("Discard PN Code mismatch unicast frame");
+                        //zfwBufFree(dev, buf, 0);
+                        //return;
+                    }
+                }
+            }
+        }
+#endif //ZM_ENABLE_CENC
+
+        /* for tally */
+        if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+        {
+            /* for ACU to display RxRate */
+            zfWlanUpdateRxRate(dev, addInfo);
+
+            wd->commTally.rxUnicastFrm++;
+            wd->commTally.rxUnicastOctets += (len-24);
+        }
+        else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+        {
+            wd->commTally.rxBroadcastFrm++;
+            wd->commTally.rxBroadcastOctets += (len-24);
+        }
+        else
+        {
+            wd->commTally.rxMulticastFrm++;
+            wd->commTally.rxMulticastOctets += (len-24);
+        }
+        wd->ledStruct.rxTraffic++;
+
+        if ((frameSubtype & 0x80) == 0x80)
+        {
+            /* if QoS control bit-7 is 1 => A-MSDU frame */
+            if ((zmw_rx_buf_readh(dev, buf, 24) & 0x80) != 0)
+            {
+                zfDeAmsdu(dev, buf, vap, encryMode);
+                return;
+            }
+        }
+
+        // Remove MIC of TKIP
+        if ( encryMode == ZM_TKIP )
+        {
+            zfwBufSetSize(dev, buf, zfwBufGetSize(dev, buf) - 8);
+        }
+
+        /* Convert 802.11 and SNAP header to ethernet header */
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)||
+             (wd->wlanMode == ZM_MODE_IBSS) )
+        {
+            /* destination address*/
+            da[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+            da[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+2);
+            da[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+4);
+
+            /* check broadcast frame */
+            if ( (da[0] == 0xffff) && (da[1] == 0xffff) && (da[2] == 0xffff) )
+            {
+                // Ap send broadcast frame to the DUT !
+            }
+            /* check multicast frame */
+            /* TODO : Remove these code, hardware should be able to block */
+            /*        multicast frame on the multicast address list       */
+            /*        or bypass all multicast packet by flag bAllMulticast */
+            else if ((da[0] & 0x01) && (wd->sta.bAllMulticast == 0))
+            {
+                for(ii=0; ii<wd->sta.multicastList.size; ii++)
+                {
+                    if ( zfMemoryIsEqual(wd->sta.multicastList.macAddr[ii].addr,
+                                         (u8_t*) da, 6))
+                    {
+                        break;
+                    }
+                }
+
+                if ( ii == wd->sta.multicastList.size )
+                {   /* not found */
+                    zm_debug_msg0("discard unknown multicast frame");
+
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+            }
+
+#ifdef ZM_ENABLE_NATIVE_WIFI //Native Wifi : 1, Ethernet format : 0
+            //To remove IV
+            if (offset > 0)
+            {
+                for (i=12; i>0; i--)
+                {
+                    zmw_rx_buf_writeh(dev, buf, ((i-1)*2)+offset,
+                            zmw_rx_buf_readh(dev, buf, (i-1)*2));
+                }
+                zfwBufRemoveHead(dev, buf, offset);
+            }
+#else
+
+            if (zfRxBufferEqualToStr(dev, buf, zgSnapBridgeTunnel,
+                                     24+offset, 6))
+            {
+                snapCase = 1;
+            }
+            else if ( zfRxBufferEqualToStr(dev, buf, zgSnap8021h,
+                                           24+offset, 6) )
+            {
+                typeLengthField =
+                    (((u16_t) zmw_rx_buf_readb(dev, buf, 30+offset)) << 8) +
+                    zmw_rx_buf_readb(dev, buf, 31+offset);
+
+                //zm_debug_msg2("tpyeLengthField = ", typeLengthField);
+
+                //8137 : IPX, 80F3 : Appletalk
+                if ( (typeLengthField != 0x8137)&&
+                     (typeLengthField != 0x80F3) )
+                {
+                    snapCase = 2;
+                }
+
+                if ( typeLengthField == 0x888E )
+                {
+                    zfShowRxEAPOL(dev, buf, 32);
+                }
+            }
+            else
+            {
+                //zfwDumpBuf(dev, buf);
+            }
+
+            /* source address */
+            if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+            {
+                /* SA = Address 3 */
+                sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+                sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+                sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+            }
+            else
+            {
+                /* SA = Address 2 */
+                sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+                sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+                sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+            }
+
+            if ( snapCase )
+            {
+                /* SA */
+                zmw_rx_buf_writeh(dev, buf, 24+offset, sa[0]);
+                zmw_rx_buf_writeh(dev, buf, 26+offset, sa[1]);
+                zmw_rx_buf_writeh(dev, buf, 28+offset, sa[2]);
+
+                /* DA = Address 1 */
+                zmw_rx_buf_writeh(dev, buf, 18+offset, da[0]);
+                zmw_rx_buf_writeh(dev, buf, 20+offset, da[1]);
+                zmw_rx_buf_writeh(dev, buf, 22+offset, da[2]);
+                zfwBufRemoveHead(dev, buf, 18+offset);
+            }
+            else
+            {
+                /* SA */
+                zmw_rx_buf_writeh(dev, buf, 16+offset, sa[0]);
+                zmw_rx_buf_writeh(dev, buf, 18+offset, sa[1]);
+                zmw_rx_buf_writeh(dev, buf, 20+offset, sa[2]);
+
+                /* DA = Address 1 */
+                zmw_rx_buf_writeh(dev, buf, 10+offset, da[0]);
+                zmw_rx_buf_writeh(dev, buf, 12+offset, da[1]);
+                zmw_rx_buf_writeh(dev, buf, 14+offset, da[2]);
+                zfwBufRemoveHead(dev, buf, 10+offset);
+                /* Ethernet payload length */
+                typeLengthField = zfwBufGetSize(dev, buf) - 14;
+                zmw_rx_buf_writeh(dev, buf, 12, (typeLengthField<<8)+(typeLengthField>>8));
+            }
+#endif  // ZM_ENABLE_NATIVE_WIFI
+        }
+        else if (wd->wlanMode == ZM_MODE_AP)
+        {
+            //if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3)
+            if (vap < ZM_MAX_AP_SUPPORT)
+            /* AP mode */
+            {
+#ifdef ZM_ENABLE_NATIVE_WIFI //Native Wifi : 1, Ethernet format : 0
+                //To remove IV
+                if (offset > 0)
+                {
+                    for (i=12; i>0; i--)
+                    {
+                        zmw_rx_buf_writeh(dev, buf, ((i-1)*2)+offset,
+                                zmw_rx_buf_readh(dev, buf, (i-1)*2));
+                    }
+                    zfwBufRemoveHead(dev, buf, offset);
+                }
+#else
+                /* SA = Address 2 */
+                zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET));
+                zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET+4));
+                /* DA = Address 3 */
+                /* Seq : Read 20 write 22, read 18 write 20, read 16 write 18 */
+                /* sequence must not be inverted */
+                zmw_rx_buf_writeh(dev, buf, 22+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+4));
+                zmw_rx_buf_writeh(dev, buf, 20+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 18+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET));
+                zfwBufRemoveHead(dev, buf, 18+offset);
+#endif  // ZM_ENABLE_NATIVE_WIFI
+                #if 1
+                if ((ret = zfIntrabssForward(dev, buf, vap)) == 1)
+                {
+                    /* Free Rx buffer if intra-BSS unicast frame */
+                    zm_msg0_rx(ZM_LV_2, "Free intra-BSS unicast frame");
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+                #endif
+            }
+            else
+            /* WDS mode */
+            {
+                zm_msg0_rx(ZM_LV_2, "Rx WDS data");
+
+                /* SA = Address 4 */
+                zmw_rx_buf_writeh(dev, buf, 30+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET));
+                zmw_rx_buf_writeh(dev, buf, 32+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 34+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET+4));
+                /* DA = Address 3 */
+                /* Seq : Read 20 write 22, read 18 write 20, read 16 write 18 */
+                /* sequence must not be inverted */
+                zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+4));
+                zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET));
+                zfwBufRemoveHead(dev, buf, 24+offset);
+            }
+        }
+        else if (wd->wlanMode == ZM_MODE_PSEUDO)
+        {
+			/* WDS test: remove add4 */
+            if (wd->enableWDS)
+            {
+                offset += 6;
+            }
+
+            /* SA = Address 2 */
+            zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET));
+            zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET+2));
+            zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET+4));
+            /* DA = Address 1 */
+            zmw_rx_buf_writeh(dev, buf, 18+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET));
+            zmw_rx_buf_writeh(dev, buf, 20+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET+2));
+            zmw_rx_buf_writeh(dev, buf, 22+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET+4));
+            zfwBufRemoveHead(dev, buf, 18+offset);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+
+        /* Call zfwRecvEth() to notify upper layer */
+        //zm_msg2_rx(ZM_LV_2, "Call zfwRecvEth(), buf=", buf);
+        //zfwDumpBuf(dev, buf);
+
+        #if ZM_PROTOCOL_RESPONSE_SIMULATION == 1
+        zfProtRspSim(dev, buf);
+        #endif
+        //zfwDumpBuf(dev, buf);
+
+        /* tally */
+    	wd->commTally.NotifyNDISRxFrmCnt++;
+
+    	if (wd->zfcbRecvEth != NULL)
+    	{
+            wd->zfcbRecvEth(dev, buf, vap);
+            ZM_PERFORMANCE_RX_MSDU(dev, wd->tick)
+        }
+    }
+    /* if management frame */
+    else if (frameType == ZM_WLAN_MANAGEMENT_FRAME)
+    {
+        zm_msg2_rx(ZM_LV_2, "Rx management,FC=", frameCtrl);
+        /* Call zfProcessManagement() to handle management frame */
+        zfProcessManagement(dev, buf, addInfo); //CWYang(m)
+        zfwBufFree(dev, buf, 0);
+    }
+    /* PsPoll */
+    else if ((wd->wlanMode == ZM_MODE_AP) && (frameCtrl == 0xa4))
+    {
+        zm_msg0_rx(ZM_LV_0, "Rx PsPoll");
+        zfApProcessPsPoll(dev, buf);
+        zfwBufFree(dev, buf, 0);
+    }
+    else
+    {
+        zm_msg0_rx(ZM_LV_1, "Rx discard!!");
+        wd->commTally.DriverDiscardedFrm++;
+
+        zfwBufFree(dev, buf, 0);
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfWlanRxValidate            */
+/*      Validate Rx frame.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfWlanRxValidate(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t frameType;
+    u16_t frameCtrl;
+    u16_t frameLen;
+    u16_t ret;
+    u8_t  frameSubType;
+
+    zmw_get_wlan_dev(dev);
+
+    frameCtrl = zmw_rx_buf_readh(dev, buf, 0);
+    frameType = frameCtrl & 0xC;
+    frameSubType = (frameCtrl & 0xF0) >> 4;
+
+    frameLen = zfwBufGetSize(dev, buf);
+
+    /* Accept Data/Management frame with protocol version = 0 */
+    if ((frameType == 0x8) || (frameType == 0x0))
+    {
+
+        /* TODO : check rx status => erro bit */
+
+        /* Check Minimum Length with Wep */
+        if ((frameCtrl & 0x4000) != 0)
+        {
+            /* Minimum Length =                                       */
+            /*     PLCP(5)+Header(24)+IV(4)+ICV(4)+CRC(4)+RxStatus(8) */
+            if (frameLen < 32)
+            {
+                return ZM_ERR_MIN_RX_ENCRYPT_FRAME_LENGTH;
+            }
+        }
+        else if ( frameSubType == 0x5 || frameSubType == 0x8 )
+        {
+            /* Minimum Length = PLCP(5)+MACHeader(24)+Timestamp(8)+BeaconInterval(2)+Cap(2)+CRC(4)+RxStatus(8) */
+            if (frameLen < 36)
+            {
+                return ZM_ERR_MIN_RX_FRAME_LENGTH;
+            }
+        }
+        else
+        {
+            /* Minimum Length = PLCP(5)+MACHeader(24)+CRC(4)+RxStatus(8) */
+            if (frameLen < 24)
+            {
+                return ZM_ERR_MIN_RX_FRAME_LENGTH;
+            }
+        }
+
+        /* Check if frame Length > ZM_WLAN_MAX_RX_SIZE. */
+        if (frameLen > ZM_WLAN_MAX_RX_SIZE)
+        {
+            return ZM_ERR_MAX_RX_FRAME_LENGTH;
+        }
+    }
+    else if ((frameCtrl&0xff) == 0xa4)
+    {
+        /* PsPoll */
+        //zm_msg0_rx(ZM_LV_0, "rx pspoll");
+    }
+    else if ((frameCtrl&0xff) == ZM_WLAN_FRAME_TYPE_BAR)
+    {
+        if (wd->sta.enableDrvBA == 1)
+        {
+            zfAggRecvBAR(dev, buf);
+        }
+
+        return ZM_ERR_RX_BAR_FRAME;
+    }
+    else
+    {
+        return ZM_ERR_RX_FRAME_TYPE;
+    }
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+    }
+    else if ( wd->wlanMode != ZM_MODE_PSEUDO )
+    {
+        if ( (ret=zfStaRxValidateFrame(dev, buf))!=ZM_SUCCESS )
+        {
+            //zm_debug_msg1("discard frame, code = ", ret);
+            return ret;
+        }
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfWlanRxFilter              */
+/*      Filter duplicated frame.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfWlanRxFilter(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t src[3];
+    u16_t dst0;
+    u16_t frameType;
+    u16_t seq;
+    u16_t offset;
+    u16_t index;
+    u16_t col;
+    u16_t i;
+    u8_t up = 0; /* User priority */
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    /* RX PREFIX */
+    offset = 0;
+
+    frameType = zmw_rx_buf_readh(dev, buf, offset);
+
+    // Don't divide 2^4 because we don't want the fragementation pkt to be treated as
+    // duplicated frames
+    seq = zmw_rx_buf_readh(dev, buf, offset+22);
+    dst0 = zmw_rx_buf_readh(dev, buf, offset+4);
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+
+    /* QoS data frame */
+    if ((frameType & 0x88) == 0x88)
+    {
+        up = zmw_rx_buf_readb(dev, buf, offset+24);
+        up &= 0x7;
+    }
+
+    index = (src[2]+up) & (ZM_FILTER_TABLE_ROW-1);
+
+    /* TBD : filter frame with source address == own MAC adress */
+    if ((wd->macAddr[0] == src[0]) && (wd->macAddr[1] == src[1])
+            && (wd->macAddr[2] == src[2]))
+    {
+        //zm_msg0_rx(ZM_LV_0, "Rx filter=>src is own MAC");
+        wd->trafTally.rxSrcIsOwnMac++;
+#if 0
+        return ZM_ERR_RX_SRC_ADDR_IS_OWN_MAC;
+#endif
+    }
+
+    zm_msg2_rx(ZM_LV_2, "Rx seq=", seq);
+
+    /* Filter unicast frame only */
+    if ((dst0 & 0x1) == 0)
+    {
+        zmw_enter_critical_section(dev);
+
+        for(i=0; i<ZM_FILTER_TABLE_COL; i++)
+        {
+            if ((wd->rxFilterTbl[i][index].addr[0] == src[0])
+                    && (wd->rxFilterTbl[i][index].addr[1] == src[1])
+                    && (wd->rxFilterTbl[i][index].addr[2] == src[2])
+                    && (wd->rxFilterTbl[i][index].up == up))
+            {
+                if (((frameType&0x800)==0x800)
+                        &&(wd->rxFilterTbl[i][index].seq==seq))
+                {
+                    zmw_leave_critical_section(dev);
+                    /* hit : duplicated frame */
+                    zm_msg0_rx(ZM_LV_1, "Rx filter hit=>duplicated");
+                    wd->trafTally.rxDuplicate++;
+                    return ZM_ERR_RX_DUPLICATE;
+                }
+                else
+                {
+                    /* hit : not duplicated frame, update sequence number */
+                    wd->rxFilterTbl[i][index].seq = seq;
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_rx(ZM_LV_2, "Rx filter hit");
+                    return ZM_SUCCESS;
+                }
+            }
+        } /* for(i=0; i<ZM_FILTER_TABLE_COL; i++) */
+
+        /* miss : add to table */
+        zm_msg0_rx(ZM_LV_1, "Rx filter miss");
+        /* TODO : Random select a column */
+        col = (u16_t)(wd->tick & (ZM_FILTER_TABLE_COL-1));
+        wd->rxFilterTbl[col][index].addr[0] = src[0];
+        wd->rxFilterTbl[col][index].addr[1] = src[1];
+        wd->rxFilterTbl[col][index].addr[2] = src[2];
+        wd->rxFilterTbl[col][index].seq = seq;
+        wd->rxFilterTbl[col][index].up = up;
+
+        zmw_leave_critical_section(dev);
+    } /* if ((dst0 & 0x1) == 0) */
+
+    return ZM_SUCCESS;
+}
+
+
+
+u16_t zfTxGenWlanTail(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t snaplen,
+                      u16_t* mic)
+{
+    struct zsMicVar*  pMicKey;
+    u16_t  i, length, payloadOffset;
+    u8_t   bValue, qosType = 0;
+    u8_t   snapByte[12];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetTxMicKey(dev, buf, &qosType);
+
+        if ( pMicKey == NULL )
+        {
+            return 0;
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetTxMicKey(dev, buf);
+
+        if ( pMicKey == NULL )
+        {
+            return 0;
+        }
+    }
+    else
+    {
+        return 0;
+    }
+
+    length = zfwBufGetSize(dev, buf);
+
+    zfMicClear(pMicKey);
+
+    /* append DA and SA */
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    for(i=16; i<22; i++)
+    { // VISTA DA
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+    for(i=10; i<16; i++)
+    { // VISTA SA
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+#else
+    for(i=0; i<12; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+#endif
+
+    /* append for alignment */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        if (wd->sta.wmeConnected != 0)
+            zfMicAppendByte(zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1) >> 5, pMicKey);
+        else
+            zfMicAppendByte(0, pMicKey);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        if (qosType == 1)
+            zfMicAppendByte(zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1) >> 5, pMicKey);
+        else
+            zfMicAppendByte(0, pMicKey);
+    }
+    else
+    {
+        /* TODO : Qos Software MIC in IBSS Mode */
+        zfMicAppendByte(0, pMicKey);
+    }
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    if ( snaplen == 0 )
+    {
+        payloadOffset = ZM_80211_FRAME_IP_OFFSET;
+    }
+    else
+    {
+        payloadOffset = ZM_80211_FRAME_TYPE_OFFSET;
+
+        for(i=0; i<(snaplen>>1); i++)
+        {
+            snapByte[i*2] = (u8_t) (snap[i] & 0xff);
+            snapByte[i*2+1] = (u8_t) ((snap[i] >> 8) & 0xff);
+        }
+
+        for(i=0; i<snaplen; i++)
+        {
+            zfMicAppendByte(snapByte[i], pMicKey);
+        }
+    }
+
+    for(i=payloadOffset; i<length; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic( (u8_t*) mic, pMicKey);
+
+    return ZM_SIZE_OF_MIC;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxGetIpTosAndFrag         */
+/*      Get IP TOS and frag offset from Tx buffer                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*      up : pointer for returning user priority                        */
+/*      fragOff : pointer for returning ip frag offset                  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfTxGetIpTosAndFrag(zdev_t* dev, zbuf_t* buf, u8_t* up, u16_t* fragOff)
+{
+    u8_t ipv;
+    u16_t len;
+	u16_t etherType;
+    u8_t tos;
+
+    *up = 0;
+    *fragOff = 0;
+
+    len = zfwBufGetSize(dev, buf);
+
+    if (len >= 34) //Minimum IPv4 packet size, 14(Ether header)+20(IPv4 header)
+    {
+        etherType = (((u16_t)zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_TYPE_OFFSET))<<8)
+                    + zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_TYPE_OFFSET + 1);
+
+        /* protocol type = IP */
+        if (etherType == 0x0800)
+        {
+            ipv = zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET) >> 4;
+            if (ipv == 0x4) //IPv4
+            {
+                tos = zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1);
+                *up = (tos >> 5);
+                *fragOff = zmw_tx_buf_readh(dev, buf, ZM_80211_FRAME_IP_OFFSET + 6);
+            }
+            /* TODO : handle VLAN tag and IPv6 packet */
+        }
+    }
+    return;
+}
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen)
+{
+    snap[0] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 0);
+    snap[1] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 2);
+    snap[2] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 4);
+    *snaplen = 6;
+
+    return ZM_80211_FRAME_HEADER_LEN + *snaplen;
+}
+#else
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen)
+{
+    u16_t removed;
+	   u16_t etherType;
+   	u16_t len;
+
+	   len = zfwBufGetSize(dev, buf);
+    if (len < 14) //Minimum Ethernet packet size, 14(Ether header)
+    {
+        /* TODO : Assert? */
+        *snaplen = 0;
+        return 0;
+    }
+
+    /* Generate RFC1042 header */
+    etherType = (((u16_t)zmw_tx_buf_readb(dev, buf, 12))<<8)
+                + zmw_tx_buf_readb(dev, buf, 13);
+
+    //zm_debug_msg2("ethernet type or length = ", etherType);
+
+    if (etherType > 1500)
+    {
+        /* ETHERNET format */
+        removed = 12;
+        snap[0] = 0xaaaa;
+        snap[1] = 0x0003;
+        if ((etherType ==0x8137) || (etherType == 0x80f3))
+        {
+            /* Bridge Tunnel */
+            snap[2] = 0xF800;
+        }
+        else
+        {
+            /* RFC 1042 */
+            snap[2] = 0x0000;
+        }
+        *snaplen = 6;
+
+        if ( etherType == 0x888E )
+        {
+            zfShowTxEAPOL(dev, buf, 14);
+        }
+    }
+    else
+    {
+        /* 802.3 format */
+        removed = 14;
+        *snaplen = 0;
+    }
+
+    return removed;
+}
+#endif
+
+u8_t zfIsVtxqEmpty(zdev_t* dev)
+{
+    u8_t isEmpty = TRUE;
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->vmmqHead != wd->vmmqTail)
+    {
+        isEmpty = FALSE;
+        goto check_done;
+    }
+
+    for(i=0; i < 4; i++)
+    {
+        if (wd->vtxqHead[i] != wd->vtxqTail[i])
+        {
+            isEmpty = FALSE;
+            goto check_done;
+        }
+    }
+
+check_done:
+    zmw_leave_critical_section(dev);
+    return isEmpty;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPutVtxq                   */
+/*      Put Tx buffer to virtual TxQ                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS or error code                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfPutVtxq(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t ac;
+    u8_t up;
+    u16_t fragOff;
+#ifdef ZM_AGG_TALLY
+    struct aggTally *agg_tal;
+#endif
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+	u16_t ret;
+    u16_t tid;
+    #endif
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+    if ( wd->zfcbClassifyTxPacket != NULL )
+    {
+        ac = wd->zfcbClassifyTxPacket(dev, buf);
+    }
+    else
+    {
+        ac = zcUpToAc[up&0x7] & 0x3;
+    }
+
+    /*
+     * add by honda
+     * main A-MPDU aggregation function
+     */
+#ifdef ZM_AGG_TALLY
+    agg_tal = &wd->agg_tal;
+    agg_tal->got_packets_sum++;
+
+#endif
+
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+    tid = up&0x7;
+    if(wd->enableAggregation==0)
+    {
+        if( (wd->wlanMode == ZM_MODE_AP) ||
+            (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+            (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+            // (infrastructure_mode && connect_to_11n_ap) || (ap_mode && is_11n_ap)
+            //ret = zfAggPutVtxq(dev, buf);
+
+
+            ret = zfAggTx(dev, buf, tid);
+            if (ZM_SUCCESS == ret)
+            {
+                //zfwBufFree(dev, buf, ZM_SUCCESS);
+
+                return ZM_SUCCESS;
+            }
+            if (ZM_ERR_EXCEED_PRIORITY_THRESHOLD == ret)
+            {
+                wd->commTally.txQosDropCount[ac]++;
+                zfwBufFree(dev, buf, ZM_SUCCESS);
+
+                zm_msg1_tx(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+
+                return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+            }
+            if (ZM_ERR_TX_BUFFER_UNAVAILABLE == ret)
+            {
+                /*
+                * do nothing
+                * continue following procession, put into VTXQ
+                * return ZM_SUCCESS;
+                */
+            }
+        }
+    }
+    #endif
+#endif
+    /*
+     * end of add by honda
+     */
+
+    /* First Ip frag */
+    if ((fragOff & 0xff3f) == 0x0020)
+    {
+        /* Don't let ip frag in if VTXQ unable to hold */
+        /* whole ip frag burst(assume 20 frag)         */
+        zmw_enter_critical_section(dev);
+        if (((wd->vtxqHead[ac] - wd->vtxqTail[ac])& ZM_VTXQ_SIZE_MASK)
+                > (ZM_VTXQ_SIZE-20))
+        {
+            wd->qosDropIpFrag[ac] = 1;
+        }
+        else
+        {
+            wd->qosDropIpFrag[ac] = 0;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (wd->qosDropIpFrag[ac] == 1)
+        {
+            //zm_debug_msg2("vtQ full, drop buf = ", buf);
+            wd->commTally.txQosDropCount[ac]++;
+            zfwBufFree(dev, buf, ZM_SUCCESS);
+            zm_msg1_tx(ZM_LV_1, "Packet discarded, first ip frag, ac=", ac);
+            //VTXQ[] can not hold whold ip frag burst(assume 20 frags)
+            return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+        }
+    }
+    else if ((fragOff & 0xff3f) == 0)
+    {
+        wd->qosDropIpFrag[ac] = 0;
+    }
+
+    if (((fragOff &= 0xff1f) != 0) && (wd->qosDropIpFrag[ac] == 1))
+    {
+        wd->commTally.txQosDropCount[ac]++;
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg1_tx(ZM_LV_1, "Packet discarded, ip frag, ac=", ac);
+        //Discard following ip frags
+        return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+    }
+
+    zmw_enter_critical_section(dev);
+    if (((wd->vtxqHead[ac] + 1) & ZM_VTXQ_SIZE_MASK) != wd->vtxqTail[ac])
+    {
+        wd->vtxq[ac][wd->vtxqHead[ac]] = buf;
+        wd->vtxqHead[ac] = ((wd->vtxqHead[ac] + 1) & ZM_VTXQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+
+        wd->commTally.txQosDropCount[ac]++;
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg1_tx(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+        return ZM_ERR_EXCEED_PRIORITY_THRESHOLD; //VTXQ[] Full
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetVtxq                   */
+/*      Get Tx buffer from virtual TxQ                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx buffer pointer                                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetVtxq(zdev_t* dev, u8_t ac)
+{
+    zbuf_t* buf;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ac &= 0x3;
+    zmw_enter_critical_section(dev);
+    if (wd->vtxqHead[ac] != wd->vtxqTail[ac])
+    {
+        buf = wd->vtxq[ac][wd->vtxqTail[ac]];
+        wd->vtxqTail[ac] = ((wd->vtxqTail[ac] + 1) & ZM_VTXQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return buf;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 0; //VTXQ[] empty
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPutVmmq                   */
+/*      Put Tx buffer to virtual MmQ                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS or error code                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfPutVmmq(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (((wd->vmmqHead + 1) & ZM_VMMQ_SIZE_MASK) != wd->vmmqTail)
+    {
+        wd->vmmq[wd->vmmqHead] = buf;
+        wd->vmmqHead = ((wd->vmmqHead + 1) & ZM_VMMQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg0_mm(ZM_LV_0, "Packet discarded, VMmQ full");
+        return ZM_ERR_VMMQ_FULL; //VTXQ[] Full
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetVmmq                   */
+/*      Get Tx buffer from virtual MmQ                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx buffer pointer                                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetVmmq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->vmmqHead != wd->vmmqTail)
+    {
+        buf = wd->vmmq[wd->vmmqTail];
+        wd->vmmqTail = ((wd->vmmqTail + 1) & ZM_VMMQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return buf;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 0; //VTXQ[] empty
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPushVtxq                  */
+/*      Service Virtual TxQ (weighted round robin)                      */
+/*      Get Tx buffer form virtual TxQ and put to hardware TxD queue    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfPushVtxq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t i;
+    u16_t txed;
+    u32_t freeTxd;
+    u16_t err;
+    u16_t skipFlag = 0;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+
+    //zm_debug_msg1("zfHpGetFreeTxdCount = ", zfHpGetFreeTxdCount(dev));
+
+    if (wd->halState == ZM_HAL_STATE_INIT)
+    {
+        if (!wd->modeMDKEnable)
+        {
+            zm_debug_msg0("HAL is not ready for Tx");
+        }
+        return;
+    }
+    else if (wd->sta.DFSDisableTx)
+    {
+        zm_debug_msg0("return because 802.11h DFS Disable Tx");
+        return;
+    }
+    else if (wd->sta.flagFreqChanging != 0)
+    {
+        //Hold until RF frequency changed
+        return;
+    }
+    else if (( wd->sta.flagKeyChanging ) && ( wd->wlanMode != ZM_MODE_AP ))
+    {
+        return;
+    }
+#ifdef ZM_ENABLE_POWER_SAVE
+    else if ( zfPowerSavingMgrIsSleeping(dev) )
+    {
+        //zm_debug_msg0("Packets queued since the MAC is in power-saving mode\n");
+        return;
+    }
+#endif
+
+    zmw_enter_critical_section(dev);
+    if (wd->vtxqPushing != 0)
+    {
+        skipFlag = 1;
+    }
+    else
+    {
+        wd->vtxqPushing = 1;
+    }
+    zmw_leave_critical_section(dev);
+
+    if (skipFlag == 1)
+    {
+        return;
+    }
+
+    while (1)
+    {
+        txed = 0;
+
+        /* 2006.12.20, Serve Management queue */
+        while( zfHpGetFreeTxdCount(dev) > 0 )
+        {
+            if ((buf = zfGetVmmq(dev)) != 0)
+            {
+                txed = 1;
+                //zm_debug_msg2("send buf = ", buf);
+                if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+                        ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+                {
+                    zfwBufFree(dev, buf, 0);
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+        if ((wd->sta.bScheduleScan) || ((wd->sta.bChannelScan == TRUE) && (zfStaIsConnected(dev))))
+        {
+            //Hold until Scan Stop
+            wd->vtxqPushing = 0;
+            return;
+        }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+        if( (wd->wlanMode == ZM_MODE_AP) ||
+            (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+            (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+
+            zfAggTxScheduler(dev, 0);
+
+            if (txed == 0) {
+                wd->vtxqPushing = 0;
+                return;
+            }
+            else {
+                continue;
+            }
+        }
+    #endif
+#endif
+
+        /* Service VTxQ[3] */
+        for (i=0; i<4; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= 3)
+            {
+                if ((buf = zfGetVtxq(dev, 3)) != 0)
+                {
+                    txed = 1;
+                    //zm_debug_msg2("send buf = ", buf);
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+
+        /* Service VTxQ[2] */
+        for (i=0; i<3; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*1/4))
+            {
+                if ((buf = zfGetVtxq(dev, 2)) != 0)
+                {
+                    txed = 1;
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+                if (wd->sta.ac0PriorityHigherThanAc2 == 1)
+                {
+                    if ((buf = zfGetVtxq(dev, 0)) != 0)
+                    {
+                        txed = 1;
+                        zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                        ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                    }
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+
+        /* Service VTxQ[0] */
+        for (i=0; i<2; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*2/4))
+            {
+                if ((buf = zfGetVtxq(dev, 0)) != 0)
+                {
+                    txed = 1;
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+            }
+            else
+            {
+                break;
+            }
+
+        }
+
+        /* Service VTxQ[1] */
+        if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*3/4))
+        {
+            if ((buf = zfGetVtxq(dev, 1)) != 0)
+            {
+                txed = 1;
+                zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+            }
+        }
+
+        /* All VTxQs are either empty or exceed their threshold */
+        if (txed == 0)
+        {
+            wd->vtxqPushing = 0;
+            return;
+        }
+    } //while (1)
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFlushVtxq                 */
+/*      Flush Virtual TxQ and MmQ                                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfFlushVtxq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u8_t i;
+    zmw_get_wlan_dev(dev);
+
+    /* Flush MmQ */
+    while ((buf = zfGetVmmq(dev)) != 0)
+    {
+        zfwBufFree(dev, buf, 0);
+        zm_debug_msg0("zfFlushVtxq: [Vmmq]");
+        wd->queueFlushed  |= 0x10;
+    }
+
+    /* Flush VTxQ */
+    for (i=0; i<4; i++)
+    {
+        while ((buf = zfGetVtxq(dev, i)) != 0)
+        {
+            zfwBufFree(dev, buf, 0);
+            zm_debug_msg1("zfFlushVtxq: [zfGetVtxq]- ", i);
+            wd->queueFlushed |= (1<<i);
+        }
+    }
+}
+
+void zf80211FrameSend(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t snapLen,
+                           u16_t* da, u16_t* sa, u8_t up, u16_t headerLen, u16_t* snap,
+                           u16_t* tail, u16_t tailLen, u16_t offset, u16_t bufType,
+                           u8_t ac, u8_t keyIdx)
+{
+    u16_t err;
+    u16_t fragLen;
+
+    zmw_get_wlan_dev(dev);
+
+    fragLen = zfwBufGetSize(dev, buf);
+    if ((da[0]&0x1) == 0)
+    {
+        wd->commTally.txUnicastFrm++;
+        wd->commTally.txUnicastOctets += (fragLen+snapLen);
+    }
+    else if (da[0] == 0xffff)
+    {
+        wd->commTally.txBroadcastFrm++;
+        wd->commTally.txBroadcastOctets += (fragLen+snapLen);
+    }
+    else
+    {
+        wd->commTally.txMulticastFrm++;
+        wd->commTally.txMulticastOctets += (fragLen+snapLen);
+    }
+    wd->ledStruct.txTraffic++;
+
+    if ((err = zfHpSend(dev, header, headerLen, snap, snapLen,
+                        tail, tailLen, buf, offset,
+                        bufType, ac, keyIdx)) != ZM_SUCCESS)
+    {
+        if (bufType == ZM_EXTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, buf, err);
+        }
+        else if (bufType == ZM_INTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, buf, 0);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    }
+}
+
+void zfCheckIsRIFSFrame(zdev_t* dev, zbuf_t* buf, u16_t frameSubtype)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* #2 Record the sequence number to determine whether the unicast frame is separated by RIFS or not */
+    if (frameSubtype & 0x80)
+    {   //QoS data frame
+        u16_t sequenceNum;
+        u16_t qosControlField;
+
+        sequenceNum = ( zmw_buf_readh(dev, buf, 22) >> 4 ); // Discard fragment number !
+        qosControlField = zmw_buf_readh(dev, buf, 24); // Don't consider WDS (Wireless Distribution System)
+        //DbgPrint("The QoS Control Field                              : %d", qosControlField);
+        //DbgPrint("The RIFS Count                                     : %d", wd->sta.rifsCount);
+
+        if( qosControlField & ZM_BIT_5 )
+        {// ACK policy is "No ACK"
+            /* RIFS-Like frame */
+            wd->sta.rifsLikeFrameSequence[wd->sta.rifsLikeFrameCnt]   = sequenceNum;
+
+            if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTING )
+            {
+                if( wd->sta.rifsLikeFrameSequence[2] != 0 )
+                {// RIFS-like Pattern collected
+                    if( ( wd->sta.rifsLikeFrameSequence[2] - wd->sta.rifsLikeFrameSequence[1] == 2 ) &&
+                        ( wd->sta.rifsLikeFrameSequence[1] - wd->sta.rifsLikeFrameSequence[0] == 2 ) )
+                    {
+                        /* RIFS pattern matched */
+
+                        /* #3 Enable RIFS function if the RIFS pattern matched  */
+                        zfHpEnableRifs(dev, ((wd->sta.currentFrequency<3000)?1:0), wd->sta.EnableHT, wd->sta.HT2040);
+
+                        // Set RIFS timer
+                        wd->sta.rifsTimer = wd->tick;
+
+                        wd->sta.rifsCount++;
+
+                        // Set state to be Detected
+                        wd->sta.rifsState = ZM_RIFS_STATE_DETECTED;
+                    }
+                }
+            }
+            else
+            {// state = Detected
+                // Reset RIFS timer
+                if( (wd->tick - wd->sta.rifsTimer) < ZM_RIFS_TIMER_TIMEOUT )
+                    wd->sta.rifsTimer = wd->tick;
+            }
+
+            //DbgPrint("SN1 = %d, SN2 = %d, SN3 = %d\n", wd->sta.rifsLikeFrameSequence[0],
+            //                                           wd->sta.rifsLikeFrameSequence[1],
+            //                                           wd->sta.rifsLikeFrameSequence[2]);
+
+            // Update RIFS-like sequence number
+            if( wd->sta.rifsLikeFrameSequence[2] != 0 )
+            {
+                wd->sta.rifsLikeFrameSequence[0] = wd->sta.rifsLikeFrameSequence[1];
+                wd->sta.rifsLikeFrameSequence[1] = wd->sta.rifsLikeFrameSequence[2];
+                wd->sta.rifsLikeFrameSequence[2] = 0;
+            }
+
+            // Only record three adjacent frame
+            if( wd->sta.rifsLikeFrameCnt < 2 )
+                wd->sta.rifsLikeFrameCnt++;
+        }
+    }
+
+    /* #4 Disable RIFS function if the timer TIMEOUT  */
+    if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+    {
+        if( ( wd->tick - wd->sta.rifsTimer ) > ZM_RIFS_TIMER_TIMEOUT )
+        {// TIMEOUT
+            // Disable RIFS
+            zfHpDisableRifs(dev);
+
+            // Reset RIFS-like sequence number FIFO
+            wd->sta.rifsLikeFrameSequence[0] = 0;
+            wd->sta.rifsLikeFrameSequence[1] = 0;
+            wd->sta.rifsLikeFrameSequence[2] = 0;
+            wd->sta.rifsLikeFrameCnt = 0;
+
+            // Set state to be Detecting
+            wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+        }
+    }
+}
diff --git a/drivers/staging/otus/80211core/cwep.c b/drivers/staging/otus/80211core/cwep.c
new file mode 100644
index 000000000000..ec31bb1ac283
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwep.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cwep.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u32_t crc32_tab[] =
+{
+       0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL, 0x076dc419L,
+       0x706af48fL, 0xe963a535L, 0x9e6495a3L, 0x0edb8832L, 0x79dcb8a4L,
+       0xe0d5e91eL, 0x97d2d988L, 0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L,
+       0x90bf1d91L, 0x1db71064L, 0x6ab020f2L, 0xf3b97148L, 0x84be41deL,
+       0x1adad47dL, 0x6ddde4ebL, 0xf4d4b551L, 0x83d385c7L, 0x136c9856L,
+       0x646ba8c0L, 0xfd62f97aL, 0x8a65c9ecL, 0x14015c4fL, 0x63066cd9L,
+       0xfa0f3d63L, 0x8d080df5L, 0x3b6e20c8L, 0x4c69105eL, 0xd56041e4L,
+       0xa2677172L, 0x3c03e4d1L, 0x4b04d447L, 0xd20d85fdL, 0xa50ab56bL,
+       0x35b5a8faL, 0x42b2986cL, 0xdbbbc9d6L, 0xacbcf940L, 0x32d86ce3L,
+       0x45df5c75L, 0xdcd60dcfL, 0xabd13d59L, 0x26d930acL, 0x51de003aL,
+       0xc8d75180L, 0xbfd06116L, 0x21b4f4b5L, 0x56b3c423L, 0xcfba9599L,
+       0xb8bda50fL, 0x2802b89eL, 0x5f058808L, 0xc60cd9b2L, 0xb10be924L,
+       0x2f6f7c87L, 0x58684c11L, 0xc1611dabL, 0xb6662d3dL, 0x76dc4190L,
+       0x01db7106L, 0x98d220bcL, 0xefd5102aL, 0x71b18589L, 0x06b6b51fL,
+       0x9fbfe4a5L, 0xe8b8d433L, 0x7807c9a2L, 0x0f00f934L, 0x9609a88eL,
+       0xe10e9818L, 0x7f6a0dbbL, 0x086d3d2dL, 0x91646c97L, 0xe6635c01L,
+       0x6b6b51f4L, 0x1c6c6162L, 0x856530d8L, 0xf262004eL, 0x6c0695edL,
+       0x1b01a57bL, 0x8208f4c1L, 0xf50fc457L, 0x65b0d9c6L, 0x12b7e950L,
+       0x8bbeb8eaL, 0xfcb9887cL, 0x62dd1ddfL, 0x15da2d49L, 0x8cd37cf3L,
+       0xfbd44c65L, 0x4db26158L, 0x3ab551ceL, 0xa3bc0074L, 0xd4bb30e2L,
+       0x4adfa541L, 0x3dd895d7L, 0xa4d1c46dL, 0xd3d6f4fbL, 0x4369e96aL,
+       0x346ed9fcL, 0xad678846L, 0xda60b8d0L, 0x44042d73L, 0x33031de5L,
+       0xaa0a4c5fL, 0xdd0d7cc9L, 0x5005713cL, 0x270241aaL, 0xbe0b1010L,
+       0xc90c2086L, 0x5768b525L, 0x206f85b3L, 0xb966d409L, 0xce61e49fL,
+       0x5edef90eL, 0x29d9c998L, 0xb0d09822L, 0xc7d7a8b4L, 0x59b33d17L,
+       0x2eb40d81L, 0xb7bd5c3bL, 0xc0ba6cadL, 0xedb88320L, 0x9abfb3b6L,
+       0x03b6e20cL, 0x74b1d29aL, 0xead54739L, 0x9dd277afL, 0x04db2615L,
+       0x73dc1683L, 0xe3630b12L, 0x94643b84L, 0x0d6d6a3eL, 0x7a6a5aa8L,
+       0xe40ecf0bL, 0x9309ff9dL, 0x0a00ae27L, 0x7d079eb1L, 0xf00f9344L,
+       0x8708a3d2L, 0x1e01f268L, 0x6906c2feL, 0xf762575dL, 0x806567cbL,
+       0x196c3671L, 0x6e6b06e7L, 0xfed41b76L, 0x89d32be0L, 0x10da7a5aL,
+       0x67dd4accL, 0xf9b9df6fL, 0x8ebeeff9L, 0x17b7be43L, 0x60b08ed5L,
+       0xd6d6a3e8L, 0xa1d1937eL, 0x38d8c2c4L, 0x4fdff252L, 0xd1bb67f1L,
+       0xa6bc5767L, 0x3fb506ddL, 0x48b2364bL, 0xd80d2bdaL, 0xaf0a1b4cL,
+       0x36034af6L, 0x41047a60L, 0xdf60efc3L, 0xa867df55L, 0x316e8eefL,
+       0x4669be79L, 0xcb61b38cL, 0xbc66831aL, 0x256fd2a0L, 0x5268e236L,
+       0xcc0c7795L, 0xbb0b4703L, 0x220216b9L, 0x5505262fL, 0xc5ba3bbeL,
+       0xb2bd0b28L, 0x2bb45a92L, 0x5cb36a04L, 0xc2d7ffa7L, 0xb5d0cf31L,
+       0x2cd99e8bL, 0x5bdeae1dL, 0x9b64c2b0L, 0xec63f226L, 0x756aa39cL,
+       0x026d930aL, 0x9c0906a9L, 0xeb0e363fL, 0x72076785L, 0x05005713L,
+       0x95bf4a82L, 0xe2b87a14L, 0x7bb12baeL, 0x0cb61b38L, 0x92d28e9bL,
+       0xe5d5be0dL, 0x7cdcefb7L, 0x0bdbdf21L, 0x86d3d2d4L, 0xf1d4e242L,
+       0x68ddb3f8L, 0x1fda836eL, 0x81be16cdL, 0xf6b9265bL, 0x6fb077e1L,
+       0x18b74777L, 0x88085ae6L, 0xff0f6a70L, 0x66063bcaL, 0x11010b5cL,
+       0x8f659effL, 0xf862ae69L, 0x616bffd3L, 0x166ccf45L, 0xa00ae278L,
+       0xd70dd2eeL, 0x4e048354L, 0x3903b3c2L, 0xa7672661L, 0xd06016f7L,
+       0x4969474dL, 0x3e6e77dbL, 0xaed16a4aL, 0xd9d65adcL, 0x40df0b66L,
+       0x37d83bf0L, 0xa9bcae53L, 0xdebb9ec5L, 0x47b2cf7fL, 0x30b5ffe9L,
+       0xbdbdf21cL, 0xcabac28aL, 0x53b39330L, 0x24b4a3a6L, 0xbad03605L,
+       0xcdd70693L, 0x54de5729L, 0x23d967bfL, 0xb3667a2eL, 0xc4614ab8L,
+       0x5d681b02L, 0x2a6f2b94L, 0xb40bbe37L, 0xc30c8ea1L, 0x5a05df1bL,
+       0x2d02ef8dL
+};
+
+void zfWEPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv)
+{
+    u8_t    S[256],S2[256];
+    u16_t   ui;
+    u16_t   i;
+    u16_t   j;
+    u8_t    temp;
+    u8_t    K;
+    u32_t   ltemp;
+    u16_t   len;
+    u32_t   icv;
+    u8_t    key[32];
+
+    key[0] = iv[0];
+    key[1] = iv[1];
+    key[2] = iv[2];
+
+    /* Append Wep Key after IV */
+    zfMemoryCopy(&key[3], WepKey, keyLen);
+
+    keyLen += 3;
+
+    for(i = 0; i < 256; i++)
+    {
+        S[i] = (u8_t)i;
+        S2[i] = key[i&(keyLen-1)];
+    }
+
+    j = 0;
+    for(i = 0; i < 256; i++)
+    {
+        j = (j + S[i] + S2[i]) ;
+        j&=255 ;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+    }
+
+    i = j = 0;
+    icv = -1;
+
+    /* For Snap Header */
+    for (ui = 0; ui < snapLen; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = snap[ui];
+        icv =  (icv>>8) ^ crc32_tab[(icv^In)&0xff];
+
+        snap[ui] = In ^ K;
+        //zmw_tx_buf_writeb(dev, buf, ui, In ^ K);
+    }
+
+    len = zfwBufGetSize(dev, buf);
+
+    for (ui = offset; ui < len; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = zmw_tx_buf_readb(dev, buf, ui);
+        icv =  (icv>>8) ^ crc32_tab[(icv^In)&0xff];
+
+        zmw_tx_buf_writeb(dev, buf, ui, In ^ K);
+    } //End of for (ui = 0; ui < Num_Bytes; ui++)
+
+    icv = ~(icv);
+    ltemp = (u32_t) icv;
+
+    for (ui = 0; ui < 4; ui++)
+    {
+        i ++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+        temp += S[i];
+        temp &= 255;
+        K = S[temp];        // Key used to Xor with input data
+
+        //*Out++ = (u8_t)(ltemp ^ K)&0xff;
+        zmw_tx_buf_writeb(dev, buf, len+ui, (u8_t)(ltemp ^ K)&0xff);
+        ltemp >>= 8;
+    }
+
+    zfwBufSetSize(dev, buf, len+4);
+}
+
+u16_t zfWEPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv)
+{
+    u8_t    S[256];
+    u8_t    S2[256];
+    u16_t   ui;
+    u16_t   i;
+    u16_t   j;
+    u32_t   icv_tmp;
+    u32_t   *icv;
+    u32_t   rxbuf_icv;
+    u8_t    temp;
+    u8_t    K;
+    u16_t   len;
+    u8_t    key[32];
+
+    /* Retrieve IV */
+    key[0] = iv[0];
+    key[1] = iv[1];
+    key[2] = iv[2];
+
+    /* Append Wep Key after IV */
+    zfMemoryCopy(&key[3], WepKey, keyLen);
+
+    keyLen += 3;
+
+    for(i = 0; i < 256; i++)
+    {
+        S[i] = (u8_t)i;
+        S2[i] = key[i&(keyLen-1)];
+    }
+
+    j = 0;
+    for(i = 0; i < 256; i++)
+    {
+        j = (j + S[i] + S2[i]);
+        j&=255 ;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+    }
+
+    i = j = 0;
+
+    len = zfwBufGetSize(dev, buf);
+
+    for (ui = offset; ui < len; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = zmw_rx_buf_readb(dev, buf, ui);
+
+        zmw_rx_buf_writeb(dev, buf, ui, In ^ K);
+    } //End of for (ui = 0; ui < Num_Bytes; ui++)
+
+    icv = &icv_tmp;
+    *icv = -1;
+
+    for (ui = offset; ui < len - 4; ui++)
+    {
+        u8_t In;
+
+        In = zmw_rx_buf_readb(dev, buf, ui);
+        *icv =  (*icv>>8) ^ crc32_tab[(*icv^In)&0xff];
+    }
+
+    *icv = ~*icv;
+
+    rxbuf_icv = (zmw_rx_buf_readb(dev, buf, len-4) |
+         zmw_rx_buf_readb(dev, buf, len-3) << 8 |
+         zmw_rx_buf_readb(dev, buf, len-2) << 16 |
+         zmw_rx_buf_readb(dev, buf, len-1) << 24);
+
+    if (*icv != rxbuf_icv)
+    {
+        return ZM_ICV_FAILURE;
+    }
+
+    return ZM_ICV_SUCCESS;
+}
diff --git a/drivers/staging/otus/80211core/cwm.c b/drivers/staging/otus/80211core/cwm.c
new file mode 100644
index 000000000000..80f1141bf910
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwm.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cwm.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains channel width related functions.           */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#include "cprecomp.h"
+
+
+
+void zfCwmInit(zdev_t* dev) {
+    //u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->wlanMode) {
+    case ZM_MODE_AP:
+        wd->cwm.cw_mode = CWM_MODE2040;
+        wd->cwm.cw_width = CWM_WIDTH40;
+        wd->cwm.cw_enable = 1;
+        break;
+    case ZM_MODE_INFRASTRUCTURE:
+    case ZM_MODE_PSEUDO:
+    case ZM_MODE_IBSS:
+    default:
+        wd->cwm.cw_mode = CWM_MODE2040;
+        wd->cwm.cw_width = CWM_WIDTH20;
+        wd->cwm.cw_enable = 1;
+        break;
+    }
+}
+
+
+void zfCoreCwmBusy(zdev_t* dev, u16_t busy)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_mm(ZM_LV_0, "CwmBusy=", busy);
+
+    if(wd->cwm.cw_mode == CWM_MODE20) {
+        wd->cwm.cw_width = CWM_WIDTH20;
+        return;
+    }
+
+    if(wd->cwm.cw_mode == CWM_MODE40) {
+        wd->cwm.cw_width = CWM_WIDTH40;
+        return;
+    }
+
+    if (busy) {
+        wd->cwm.cw_width = CWM_WIDTH20;
+        return;
+    }
+
+
+    if((wd->wlanMode == ZM_MODE_INFRASTRUCTURE || wd->wlanMode == ZM_MODE_PSEUDO ||
+        wd->wlanMode == ZM_MODE_IBSS)) {
+        if (wd->sta.ie.HtCap.HtCapInfo && HTCAP_SupChannelWidthSet != 0 &&
+            wd->sta.ie.HtInfo.ChannelInfo && ExtHtCap_RecomTxWidthSet != 0 &&
+            (wd->sta.ie.HtInfo.ChannelInfo && ExtHtCap_ExtChannelOffsetAbove) == 1) {
+
+            wd->cwm.cw_width = CWM_WIDTH40;
+        }
+        else {
+            wd->cwm.cw_width = CWM_WIDTH20;
+        }
+
+        return;
+    }
+
+    if(wd->wlanMode == ZM_MODE_AP) {
+        wd->cwm.cw_width = CWM_WIDTH40;
+    }
+
+}
+
+
+
+
+u16_t zfCwmIsExtChanBusy(u32_t ctlBusy, u32_t extBusy)
+{
+    u32_t busy; /* percentage */
+    u32_t cycleTime, ctlClear;
+
+    cycleTime = 1280000; //1.28 seconds
+
+    if (cycleTime > ctlBusy) {
+        ctlClear = cycleTime - ctlBusy;
+    }
+    else
+    {
+        ctlClear = 0;
+    }
+
+    /* Compute ratio of extension channel busy to control channel clear
+     * as an approximation to extension channel cleanliness.
+     *
+     * According to the hardware folks, ext rxclear is undefined
+     * if the ctrl rxclear is de-asserted (i.e. busy)
+     */
+    if (ctlClear) {
+        busy = (extBusy * 100) / ctlClear;
+    } else {
+        busy = 0;
+    }
+    if (busy > ATH_CWM_EXTCH_BUSY_THRESHOLD) {
+        return TRUE;
+    }
+
+    return FALSE;
+}
diff --git a/drivers/staging/otus/80211core/cwm.h b/drivers/staging/otus/80211core/cwm.h
new file mode 100644
index 000000000000..40c39fad5f44
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwm.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                          */
+/*  Module Name : cwm.h                                                     */
+/*                                                                          */
+/*  Abstract                                                                */
+/*      This module contains channel width relatived functions.             */
+/*                                                                          */
+/*  NOTES                                                                   */
+/*      None                                                                */
+/*                                                                          */
+/****************************************************************************/
+/*Revision History:                                                         */
+/*    Who         When        What                                          */
+/*    --------    --------    ----------------------------------------------*/
+/*                                                                          */
+/*    Honda       3-19-07     created                                       */
+/*                                                                          */
+/****************************************************************************/
+
+#ifndef _CWM_H
+#define _CWM_H
+
+#define ATH_CWM_EXTCH_BUSY_THRESHOLD  30  /* Extension Channel Busy Threshold (0-100%) */
+
+void zfCwmInit(zdev_t* dev);
+void zfCoreCwmBusy(zdev_t* dev, u16_t busy);
+u16_t zfCwmIsExtChanBusy(u32_t ctlBusy, u32_t extBusy);
+
+
+
+#endif /* #ifndef _CWM_H */
diff --git a/drivers/staging/otus/80211core/freqctrl.c b/drivers/staging/otus/80211core/freqctrl.c
new file mode 100644
index 000000000000..bab0df08d826
--- /dev/null
+++ b/drivers/staging/otus/80211core/freqctrl.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+/* zfAddFreqChangeReq should be called inside the critical section */
+static void zfAddFreqChangeReq(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb)
+{
+    zmw_get_wlan_dev(dev);
+
+//printk("zfAddFreqChangeReq  freqReqQueueTail%d\n", wd->freqCtrl.freqReqQueueTail);
+    wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueTail] = frequency;
+    wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueTail] = bw40;
+    wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueTail] = extOffset;
+    wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueTail] = cb;
+    wd->freqCtrl.freqReqQueueTail++;
+    if ( wd->freqCtrl.freqReqQueueTail >= ZM_MAX_FREQ_REQ_QUEUE )
+    {
+        wd->freqCtrl.freqReqQueueTail = 0;
+    }
+}
+
+void zfCoreSetFrequencyV2(zdev_t* dev, u16_t frequency, zfpFreqChangeCompleteCb cb)
+{
+    zfCoreSetFrequencyEx(dev, frequency, 0, 0, cb);
+}
+
+void zfCoreSetFrequencyExV2(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb, u8_t forceSetFreq)
+{
+    u8_t setFreqImmed = 0;
+    u8_t initRF = 0;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg1_scan(ZM_LV_1, "Freq=", frequency);
+
+    zmw_enter_critical_section(dev);
+    if ((wd->sta.currentFrequency == frequency)
+        && (wd->sta.currentBw40 == bw40)
+        && (wd->sta.currentExtOffset == extOffset))
+    {
+        if ( forceSetFreq == 0 && wd->sta.flagFreqChanging == 0 )
+        {
+            goto done;
+        }
+    }
+#ifdef ZM_FB50
+    /*if(frequency!=2437) {
+        zmw_leave_critical_section(dev);
+        return;
+    }*/
+#endif
+
+    zfAddFreqChangeReq(dev, frequency, bw40, extOffset, cb);
+
+//    zm_assert( wd->sta.flagFreqChanging == 0 );
+    //wd->sta.flagFreqChanging = 1;
+    if ( wd->sta.flagFreqChanging == 0 )
+    {
+        if ((wd->sta.currentBw40 != bw40) || (wd->sta.currentExtOffset != extOffset))
+        {
+            initRF = 1;
+        }
+        wd->sta.currentFrequency = frequency;
+        wd->sta.currentBw40 = bw40;
+        wd->sta.currentExtOffset = extOffset;
+        setFreqImmed = 1;
+    }
+    wd->sta.flagFreqChanging++;
+
+    zmw_leave_critical_section(dev);
+
+    if ( setFreqImmed )
+    {
+        //zfHpSetFrequency(dev, frequency, 0);
+        if ( forceSetFreq )
+        { // Cold reset to reset the frequency after scanning !
+            zm_debug_msg0("#6_1 20070917");
+            zm_debug_msg0("It is happen!!! No error message");
+            zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, 2);
+        }
+        else
+        {
+        zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, initRF);
+        }
+
+        if (    zfStaIsConnected(dev)
+             && (frequency == wd->frequency)) {
+            wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+        }
+    }
+    return;
+
+done:
+    zmw_leave_critical_section(dev);
+
+    if ( cb != NULL )
+    {
+        cb(dev);
+    }
+    zfPushVtxq(dev);
+    return;
+}
+
+void zfCoreSetFrequencyEx(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb)
+{
+    zfCoreSetFrequencyExV2(dev, frequency, bw40, extOffset, cb, 0);
+}
+
+void zfCoreSetFrequency(zdev_t* dev, u16_t frequency)
+{
+    zfCoreSetFrequencyV2(dev, frequency, NULL);
+}
+
+/* zfRemoveFreqChangeReq SHOULD NOT be called inside the critical section */
+static void zfRemoveFreqChangeReq(zdev_t* dev)
+{
+    zfpFreqChangeCompleteCb cb = NULL;
+    u16_t frequency;
+    u8_t bw40;
+    u8_t extOffset;
+    u16_t compFreq = 0;
+    u8_t compBw40 = 0;
+    u8_t compExtOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->freqCtrl.freqReqQueueHead != wd->freqCtrl.freqReqQueueTail)
+    {
+        zm_msg1_scan(ZM_LV_1, "Freq=",
+                wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead]);
+        compFreq = wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead];
+        compBw40 = wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueHead];
+        compExtOffset = wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueHead];
+
+        wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] = 0;
+        cb = wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueHead];
+        wd->freqCtrl.freqReqQueueHead++;
+        if ( wd->freqCtrl.freqReqQueueHead >= ZM_MAX_FREQ_REQ_QUEUE )
+        {
+            wd->freqCtrl.freqReqQueueHead = 0;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    if ( cb != NULL )
+    {
+        cb(dev);
+    }
+
+    zmw_enter_critical_section(dev);
+    while (wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] != 0)
+    {
+        frequency = wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead];
+        bw40 = wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueHead];
+        extOffset=wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueHead];
+        if ((compFreq == frequency)
+            && (compBw40 == bw40)
+            && (compExtOffset == extOffset))
+        {
+            /* Duplicated frequency command */
+            zm_msg1_scan(ZM_LV_1, "Duplicated Freq=", frequency);
+
+            cb = wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueHead];
+            wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] = 0;
+            wd->freqCtrl.freqReqQueueHead++;
+
+            if ( wd->freqCtrl.freqReqQueueHead >= ZM_MAX_FREQ_REQ_QUEUE )
+            {
+                wd->freqCtrl.freqReqQueueHead = 0;
+            }
+
+            if ( wd->sta.flagFreqChanging != 0 )
+            {
+                wd->sta.flagFreqChanging--;
+            }
+
+            zmw_leave_critical_section(dev);
+            if ( cb != NULL )
+            {
+                cb(dev);
+            }
+            zmw_enter_critical_section(dev);
+        }
+        else
+        {
+            u8_t    initRF = 0;
+            if ((wd->sta.currentBw40 != bw40) || (wd->sta.currentExtOffset != extOffset))
+            {
+                initRF = 1;
+            }
+            wd->sta.currentFrequency = frequency;
+            wd->sta.currentBw40 = bw40;
+            wd->sta.currentExtOffset = extOffset;
+            zmw_leave_critical_section(dev);
+
+            zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, initRF);
+            if (    zfStaIsConnected(dev)
+                && (frequency == wd->frequency)) {
+                wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+            }
+
+            return;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+void zfCoreSetFrequencyComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg1_scan(ZM_LV_1, "flagFreqChanging=", wd->sta.flagFreqChanging);
+
+    zmw_enter_critical_section(dev);
+    //wd->sta.flagFreqChanging = 0;
+    if ( wd->sta.flagFreqChanging != 0 )
+    {
+        wd->sta.flagFreqChanging--;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfRemoveFreqChangeReq(dev);
+
+    zfPushVtxq(dev);
+    return;
+}
+
+void zfReSetCurrentFrequency(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("It is happen!!! No error message");
+
+    zfCoreSetFrequencyExV2(dev, wd->frequency, 0, 0, NULL, 1);
+}
diff --git a/drivers/staging/otus/80211core/ledmgr.c b/drivers/staging/otus/80211core/ledmgr.c
new file mode 100644
index 000000000000..1e104a928ca4
--- /dev/null
+++ b/drivers/staging/otus/80211core/ledmgr.c
@@ -0,0 +1,557 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType1              */
+/*      Traditional single-LED state                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.6      */
+/*                                                                      */
+/************************************************************************/
+// bit 15-12 : Toff for Scan state
+//     11-8 : Ton for Scan state
+//     7 : Reserved
+//     6 : mode
+//--------------------------------------
+//     bit 6 = 0
+//     5-4 : Connect state
+//           00 => always off
+//           01 => always on
+//           10 => Idle off, acitve on
+//           11 => Idle on, active off
+//--------------------------------------
+//     bit 6 = 1
+//     5-4 : freq
+//           00 => 1Hz
+//           01 => 0.5Hz
+//           10 => 0.25Hz
+//           11 => 0.125Hz
+//--------------------------------------
+//     3 : Power save state
+//         0 => always off in power save state
+//         1 => works as connect state
+//     2 : Disable state
+//     1 : Reserved
+//     0 : Power-on state
+void zfLedCtrlType1(zdev_t* dev)
+{
+    u16_t i;
+    u32_t ton, toff, tmp, period;
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_LED_NUMBER; i++)
+    {
+        if (zfStaIsConnected(dev) != TRUE)
+        {
+            //Scan state
+            ton = ((wd->ledStruct.ledMode[i] & 0xf00) >> 8) * 5;
+            toff = ((wd->ledStruct.ledMode[i] & 0xf000) >> 12) * 5;
+
+            if ((ton + toff) != 0)
+            {
+                tmp = wd->ledStruct.counter / (ton+toff);
+                tmp = wd->ledStruct.counter - (tmp * (ton+toff));
+                if (tmp < ton)
+                {
+                    zfHpLedCtrl(dev, i, 1);
+                }
+                else
+                {
+                    zfHpLedCtrl(dev, i, 0);
+                }
+            }
+        }
+        else
+        {
+            if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[i] & 0x8) == 0))
+            {
+                zfHpLedCtrl(dev, i, 0);
+            }
+            else
+            {
+                //Connect state
+                if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+                {
+                    if ((wd->ledStruct.counter & 1) == 0)
+                    {
+                        zfHpLedCtrl(dev, i, (wd->ledStruct.ledMode[i] & 0x10) >> 4);
+                    }
+                    else
+                    {
+                        if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                        {
+                            wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                            if ((wd->ledStruct.ledMode[i] & 0x20) != 0)
+                            {
+                                zfHpLedCtrl(dev, i, ((wd->ledStruct.ledMode[i] & 0x10) >> 4)^1);
+                            }
+                        }
+                    }
+                }// if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+                else
+                {
+                    period = 5 * (1 << ((wd->ledStruct.ledMode[i] & 0x30) >> 4));
+                    tmp = wd->ledStruct.counter / (period*2);
+                    tmp = wd->ledStruct.counter - (tmp * (period*2));
+                    if (tmp < period)
+                    {
+                        if ((wd->ledStruct.counter & 1) == 0)
+                        {
+                            zfHpLedCtrl(dev, i, 0);
+                        }
+                        else
+                        {
+                            if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                            {
+                                wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                                zfHpLedCtrl(dev, i, 1);
+                            }
+                        }
+                    }
+                    else
+                    {
+                        if ((wd->ledStruct.counter & 1) == 0)
+                        {
+                            zfHpLedCtrl(dev, i, 1);
+                        }
+                        else
+                        {
+                            if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                            {
+                                wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                                zfHpLedCtrl(dev, i, 0);
+                            }
+                        }
+                    }
+                } //else, if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+            } //else, if (zfPowerSavingMgrIsSleeping(dev))
+        } //else : if (zfStaIsConnected(dev) != TRUE)
+    } //for (i=0; i<ZM_MAX_LED_NUMBER; i++)
+}
+
+/******************************************************************************/
+/*                                                                            */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType2                    */
+/*      Customize for Netgear Dual-LED state ((bug#31292))                    */
+/*                                                                            */
+/*      1. Status:  When dongle does not connect to 2.4G or 5G but in site    */
+/*                  survey/association                                        */
+/*         LED status: Slow blinking, Amber then Blue per 500ms               */
+/*      2. Status:	Connection at 2.4G in site survey/association             */
+/*         LED status: Slow blinking, Amber/off per 500ms                     */
+/*      3. Status:	Connection at 5G in site survey/association               */
+/*         LED status: Slow blinking, Blue/off per 500ms                      */
+/*      4. Status:	When transfer the packet                                  */
+/*         LED status: Blink per packet, including TX and RX                  */
+/*      5. Status:	When linking is established but no traffic                */
+/*         LED status: Always on                                              */
+/*      6. Status:	When linking is dropped but no re-connection              */
+/*         LED status: Always off                                             */
+/*      7. Status:	From one connection(2.4G or 5G) to change to another band */
+/*         LED status: Amber/Blue =>Slow blinking, Amber then Blue per 500ms  */
+/*                                                                            */
+/*    INPUTS                                                                  */
+/*      dev : device pointer                                                  */
+/*                                                                            */
+/*    OUTPUTS                                                                 */
+/*      None                                                                  */
+/*                                                                            */
+/*    AUTHOR                                                                  */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2007.11         */
+/*                                                                            */
+/******************************************************************************/
+void zfLedCtrlType2_scan(zdev_t* dev);
+
+void zfLedCtrlType2(zdev_t* dev)
+{
+    u32_t ton, toff, tmp, period;
+    u16_t OperateLED;
+    zmw_get_wlan_dev(dev);
+
+    if (zfStaIsConnected(dev) != TRUE)
+    {
+        // Disconnect state
+        if(wd->ledStruct.counter % 4 != 0)
+    	{
+      	    // Update LED each 400ms(4*100)
+      	    // Prevent this situation
+            //              _______         ___
+            // LED[0] ON   |       |       | x |
+            // ------ OFF->+-+-+-+-+-+-+-+-+-+-+-+->>>...
+            // LED[1] ON
+            //
+            return;
+        }
+
+        if (((wd->state == ZM_WLAN_STATE_DISABLED) && (wd->sta.bChannelScan))
+            || ((wd->state != ZM_WLAN_STATE_DISABLED) && (wd->sta.bAutoReconnect)))
+        {
+            // Scan/AutoReconnect state
+            zfLedCtrlType2_scan(dev);
+        }
+        else
+        {
+            // Neither Connected nor Scan
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+    else
+    {
+        if( wd->sta.bChannelScan )
+        {
+            // Scan state
+            if(wd->ledStruct.counter % 4 != 0)
+                return;
+            zfLedCtrlType2_scan(dev);
+            return;
+        }
+
+        if(wd->frequency < 3000)
+        {
+            OperateLED = 0;     // LED[0]: work on 2.4G (b/g band)
+            zfHpLedCtrl(dev, 1, 0);
+        }
+        else
+        {
+            OperateLED = 1;     // LED[1]: work on 5G (a band)
+            zfHpLedCtrl(dev, 0, 0);
+        }
+
+        if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[OperateLED] & 0x8) == 0))
+        {
+            // If Sleeping, turn OFF
+            zfHpLedCtrl(dev, OperateLED, 0);
+        }
+        else
+        {
+            //Connect state
+            if ((wd->ledStruct.counter & 1) == 0)   // even
+            {
+                // No traffic, always ON
+                zfHpLedCtrl(dev, OperateLED, 1);
+            }
+            else       // odd
+            {
+                if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                {
+                    // If have traffic, turn OFF
+		            //                   _____   _   _   _   _____
+		            // LED[Operate] ON        | | | | | | | |
+		            // ------------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+		            //
+                    wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                    zfHpLedCtrl(dev, OperateLED, 0);
+                }
+            }
+        }
+    }
+}
+
+void zfLedCtrlType2_scan(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    // When doing scan, blink(Amber/Blue) and off per 500ms (about 400ms in our driver)
+    //               _______                         _______
+    // LED[0] ON    |       |       8       12      |       |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    // LED[1] ON    0       4       |_______|       0       3
+    //
+
+    switch(wd->ledStruct.counter % 16)
+    {
+        case 0:   // case 0~3, LED[0] on
+            if(wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 0);
+            }
+            else
+            {
+                zfHpLedCtrl(dev, 1, 1);
+                zfHpLedCtrl(dev, 0, 0);
+            }
+            break;
+
+        case 8:   // case 8~11, LED[1] on
+            if(wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                zfHpLedCtrl(dev, 1, 1);
+                zfHpLedCtrl(dev, 0, 0);
+            }
+            else
+            {
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 0);
+            }
+            break;
+
+        default:  // others, all off
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+            break;
+    }
+}
+
+/**********************************************************************************/
+/*                                                                                */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType3                        */
+/*      Customize for Netgear Single-LED state ((bug#32243))                      */
+/*                                                                                */
+/*  ¡EOff: when the adapter is disabled or hasn't started to associate with AP    */
+/*         yet.                                                                          */
+/*  ¡EOn: Once adpater associate with AP successfully                             */
+/*  ¡ESlow blinking: whenever adapters do site-survey or try to associate with AP */
+/*    - If there is a connection already, and adapters do site-survey or          */
+/*      re-associate action, the LED should keep LED backgraoud as ON, thus       */
+/*      the blinking behavior SHOULD be OFF (200ms) - ON (800ms) and continue this*/
+/*      cycle.                                                                    */
+/*    - If there is no connection yet, and adapters start to do site-survey or    */
+/*      associate action, the LED should keep LED background as OFF, thus the     */
+/*      blinking behavior SHOULD be ON (200ms) - OFF (800ms) and continue this    */
+/*      cycle.                                                                    */
+/*    - For the case that associate fail, adpater should keep associating, and the*/
+/*      LED should also keep slow blinking.                                       */
+/*  ¡EQuick blinking: to blink OFF-ON cycle for each time that traffic packet is  */
+/*    received or is transmitted.                                                 */
+/*                                                                                */
+/*    INPUTS                                                                      */
+/*      dev : device pointer                                                      */
+/*                                                                                */
+/*    OUTPUTS                                                                     */
+/*      None                                                                      */
+/*                                                                                */
+/*    AUTHOR                                                                      */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2008.01             */
+/*                                                                                */
+/**********************************************************************************/
+void zfLedCtrlType3_scan(zdev_t* dev, u16_t isConnect);
+
+void zfLedCtrlType3(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (zfStaIsConnected(dev) != TRUE)
+    {
+        // Disconnect state
+        if(wd->ledStruct.counter % 2 != 0)
+    	{
+      	    // Update LED each 200ms(2*100)
+      	    // Prevent this situation
+            //              ___     _
+            // LED[0] ON   |   |   |x|
+            // ------ OFF->+-+-+-+-+-+-+->>>...
+            //
+            return;
+        }
+
+        if (((wd->state == ZM_WLAN_STATE_DISABLED) && (wd->sta.bChannelScan))
+            || ((wd->state != ZM_WLAN_STATE_DISABLED) && (wd->sta.bAutoReconnect)))
+        {
+            // Scan/AutoReconnect state
+            zfLedCtrlType3_scan(dev, 0);
+        }
+        else
+        {
+            // Neither Connected nor Scan
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+    else
+    {
+        if( wd->sta.bChannelScan )
+        {
+            // Scan state
+            if(wd->ledStruct.counter % 2 != 0)
+                return;
+            zfLedCtrlType3_scan(dev, 1);
+            return;
+        }
+
+        if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[0] & 0x8) == 0))
+        {
+            // If Sleeping, turn OFF
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+        else
+        {
+            //Connect state
+            if ((wd->ledStruct.counter & 1) == 0)   // even
+            {
+                // No traffic, always ON
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 1);
+            }
+            else       // odd
+            {
+                if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                {
+                    // If have traffic, turn OFF
+		            //                   _____   _   _   _   _____
+		            // LED[Operate] ON        | | | | | | | |
+		            // ------------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+		            //
+                    wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                    zfHpLedCtrl(dev, 0, 0);
+                    zfHpLedCtrl(dev, 1, 0);
+                }
+            }
+        }
+    }
+}
+
+void zfLedCtrlType3_scan(zdev_t* dev, u16_t isConnect)
+{
+    u32_t ton, toff, tmp;
+    zmw_get_wlan_dev(dev);
+
+    // Doing scan when :
+    // 1. Disconnected: ON (200ms) - OFF (800ms) (200ms-600ms in our driver)
+    //               ___             ___             ___
+    // LED[0] ON    |   |           |   |           |   |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    //              0   2   4   6   8  10  12  14  16
+    // 2. Connected:   ON (800ms) - OFF (200ms) (600ms-200ms in our driver)
+    //               ___________     ___________     ______
+    // LED[0] ON    |           |   |           |   |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    //              0   2   4   6   8  10  12  14  16
+
+    //Scan state
+    if(!isConnect)
+        ton = 2, toff = 6;
+    else
+        ton = 6, toff = 2;
+
+    if ((ton + toff) != 0)
+    {
+        tmp = wd->ledStruct.counter % (ton+toff);
+       if (tmp < ton)
+        {
+            zfHpLedCtrl(dev, 0, 1);
+            zfHpLedCtrl(dev, 1, 1);
+        }
+        else
+        {
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+}
+
+/******************************************************************************/
+/*                                                                            */
+/*    FUNCTION DESCRIPTION                  zfLedCtrl_BlinkWhenScan_Alpha     */
+/*      Customize for Alpha/DLink LED                                         */
+/*      - Blink LED 12 times within 3 seconds when doing Active Scan          */
+/*	                      ___   ___   ___   ___                               */
+/*	      LED[0] ON      |   | |   | |   | |   |                              */
+/*	      -------OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+--+-->>>...                   */
+/*                                                                            */
+/*    INPUTS                                                                  */
+/*      dev : device pointer                                                  */
+/*                                                                            */
+/*    OUTPUTS                                                                 */
+/*      None                                                                  */
+/*                                                                            */
+/*    AUTHOR                                                                  */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2007.11         */
+/*                                                                            */
+/******************************************************************************/
+void zfLedCtrl_BlinkWhenScan_Alpha(zdev_t* dev)
+{
+    static u32_t counter = 0;
+    zmw_get_wlan_dev(dev);
+
+    if(counter > 34)        // counter for 3 sec
+    {
+        wd->ledStruct.LEDCtrlFlag &= ~(u8_t)ZM_LED_CTRL_FLAG_ALPHA;
+        counter = 0;
+    }
+
+    if( (counter % 3) < 2)
+        zfHpLedCtrl(dev, 0, 1);
+    else
+        zfHpLedCtrl(dev, 0, 0);
+
+    counter++;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfLed100msCtrl              */
+/*      LED 100 milliseconds timer.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.6      */
+/*                                                                      */
+/************************************************************************/
+void zfLed100msCtrl(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ledStruct.counter++;
+
+    if(wd->ledStruct.LEDCtrlFlag)
+    {
+        switch(wd->ledStruct.LEDCtrlFlag) {
+        case ZM_LED_CTRL_FLAG_ALPHA:
+            zfLedCtrl_BlinkWhenScan_Alpha(dev);
+        break;
+        }
+    }
+    else
+    {
+        switch(wd->ledStruct.LEDCtrlType) {
+        case 1:			// Traditional 1 LED
+            zfLedCtrlType1(dev);
+        break;
+
+        case 2:			// Dual-LEDs for Netgear
+            zfLedCtrlType2(dev);
+        break;
+
+        case 3:			// Single-LED for Netgear (WN111v2)
+            zfLedCtrlType3(dev);
+        break;
+
+        default:
+            zfLedCtrlType1(dev);
+        break;
+        }
+    }
+}
+
diff --git a/drivers/staging/otus/80211core/performance.c b/drivers/staging/otus/80211core/performance.c
new file mode 100644
index 000000000000..51b42d54f653
--- /dev/null
+++ b/drivers/staging/otus/80211core/performance.c
@@ -0,0 +1,431 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : performance.c                                         */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module performance evaluation functions.                   */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+
+#define ZM_TP_SIZE 50
+struct zsSummary zm_summary;
+struct zsVariation zm_var;
+struct zsThroughput zm_tp;
+
+void zfiPerformanceInit(zdev_t* dev)
+{
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_summary.tick_base = wd->tick;
+    zm_summary.tx_msdu_count = 0;
+    zm_summary.tx_mpdu_count = 0;
+    zm_summary.rx_msdu_count = 0;
+    zm_summary.rx_mpdu_count = 0;
+    zm_summary.rx_broken_seq = 0;
+    zm_summary.rx_broken_sum = 0;
+    zm_summary.rx_seq_base = 0;
+    zm_summary.rx_broken_seq_dis = 0;
+    zm_summary.rx_duplicate_seq = 0;
+    zm_summary.rx_old_seq = 0;
+    zm_summary.reset_count = 0;
+    zm_summary.reset_sum = 0;
+    zm_summary.rx_lost_sum = 0;
+    zm_summary.rx_duplicate_error = 0;
+    zm_summary.rx_free = 0;
+    zm_summary.rx_amsdu_len = 0;
+    zm_summary.rx_flush = 0;
+    zm_summary.rx_clear = 0;
+    zm_summary.rx_reorder = 0;
+
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_tick[i] = zm_var.tx_mpdu_tick[i] = 0;
+        zm_var.rx_msdu_tick[i] = zm_var.rx_mpdu_tick[i] = 0;
+    }
+
+    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_PERFORMANCE, 100);
+
+    zm_tp.size = ZM_TP_SIZE;
+    zm_tp.head = zm_tp.size - 1;
+    zm_tp.tail = 0;
+    for (i=0; i<zm_tp.size; i++)
+    {
+        zm_tp.tx[i]=0;
+        zm_tp.rx[i]=0;
+    }
+}
+
+void zfiPerformanceGraph(zdev_t* dev)
+{
+    s16_t   i,j;
+    u8_t    s[ZM_TP_SIZE+5];
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<(zm_tp.size-1); i++)
+    {
+        zm_tp.tx[i] = zm_tp.tx[i+1];
+        zm_tp.rx[i] = zm_tp.rx[i+1];
+    }
+    zm_tp.tx[zm_tp.size-1] = zm_summary.tx_mpdu_count*1500*8/1000000;
+    zm_tp.rx[zm_tp.size-1] = zm_summary.rx_msdu_count*1500*8/1000000;
+
+    for (i=15; i>0; i--)
+    {
+        s[0] = (i/10) + '0';
+        s[1] = (i%10) + '0';
+        s[2] = '0';
+        s[3] = '|';
+        for (j=0; j<zm_tp.size; j++)
+        {
+            if ((zm_tp.tx[j]/10 == i) && (zm_tp.rx[j]/10 == i))
+            {
+                s[4+j] = 'X';
+            }
+            else if (zm_tp.tx[j]/10 == i)
+            {
+                s[4+j] = 'T';
+            }
+            else if (zm_tp.rx[j]/10 == i)
+            {
+                s[4+j] = 'R';
+            }
+            else
+            {
+                s[4+j] = ' ';
+            }
+        }
+        s[zm_tp.size+4] = '\0';
+        DbgPrint("%s",s);
+    }
+    DbgPrint("000|__________________________________________________");
+
+}
+
+
+void zfiPerformanceRefresh(zdev_t* dev)
+{
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfiDbgReadReg(dev, 0x11772c);
+
+    zm_var.tx_msdu_mean = zm_summary.tx_msdu_count / 100;
+    zm_var.tx_mpdu_mean = zm_summary.tx_mpdu_count / 100;
+    zm_var.rx_msdu_mean = zm_summary.rx_msdu_count / 100;
+    zm_var.rx_mpdu_mean = zm_summary.rx_mpdu_count / 100;
+
+    zm_var.tx_msdu_sum = zm_var.tx_mpdu_sum = 0;
+    zm_var.rx_msdu_sum = zm_var.rx_mpdu_sum = 0;
+    zm_summary.tx_idle_count = zm_summary.rx_idle_count = 0;
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_sum += (zm_var.tx_msdu_tick[i] * zm_var.tx_msdu_tick[i]);
+        zm_var.tx_mpdu_sum += (zm_var.tx_mpdu_tick[i] * zm_var.tx_mpdu_tick[i]);
+        zm_var.rx_msdu_sum += (zm_var.rx_msdu_tick[i] * zm_var.rx_msdu_tick[i]);
+        zm_var.rx_mpdu_sum += (zm_var.rx_mpdu_tick[i] * zm_var.rx_mpdu_tick[i]);
+
+        if (!zm_var.tx_mpdu_tick[i]) zm_summary.tx_idle_count++;
+        if (!zm_var.rx_mpdu_tick[i]) zm_summary.rx_idle_count++;
+    }
+    zm_var.tx_msdu_var = (zm_var.tx_msdu_sum / 100) - (zm_var.tx_msdu_mean * zm_var.tx_msdu_mean);
+    zm_var.tx_mpdu_var = (zm_var.tx_mpdu_sum / 100) - (zm_var.tx_mpdu_mean * zm_var.tx_mpdu_mean);
+    zm_var.rx_msdu_var = (zm_var.rx_msdu_sum / 100) - (zm_var.rx_msdu_mean * zm_var.rx_msdu_mean);
+    zm_var.rx_mpdu_var = (zm_var.rx_mpdu_sum / 100) - (zm_var.rx_mpdu_mean * zm_var.rx_mpdu_mean);
+
+    zm_summary.tick_base = wd->tick;
+    zm_summary.rx_broken_sum += zm_summary.rx_broken_seq;
+    zm_summary.rx_lost_sum += (zm_summary.rx_broken_seq - zm_summary.rx_duplicate_seq - zm_summary.rx_old_seq);
+
+    zfiPerformanceGraph(dev);
+
+    DbgPrint("******************************************************\n");
+    DbgPrint("* TX: MSDU=%5d, VAR=%5d; MPDU=%5d, VAR=%5d\n", zm_summary.tx_msdu_count,
+        zm_var.tx_msdu_var, zm_summary.tx_mpdu_count, zm_var.tx_mpdu_var);
+    DbgPrint("* TX: idle=%5d,TxRate=%3d,  PER=%5d\n", zm_summary.tx_idle_count,
+        wd->CurrentTxRateKbps/1000,
+        (u16_t)wd->PER[wd->sta.oppositeInfo[0].rcCell.currentRate]);
+    DbgPrint("* RX: MSDU=%5d, VAR=%5d; MPDU=%5d, VAR=%5d\n", zm_summary.rx_msdu_count,
+        zm_var.rx_msdu_var, zm_summary.rx_mpdu_count, zm_var.rx_mpdu_var);
+    DbgPrint("* RX: idle=%5d,RxRate=%3d,AMSDU=%5d\n", zm_summary.rx_idle_count,
+        wd->CurrentRxRateKbps/1000, zm_summary.rx_amsdu_len);
+    DbgPrint("* RX broken seq=%4d, distances=%4d, duplicates=%4d\n", zm_summary.rx_broken_seq,
+        zm_summary.rx_broken_seq_dis, zm_summary.rx_duplicate_seq);
+    DbgPrint("* RX    old seq=%4d,      lost=%4d, broken sum=%4d\n", zm_summary.rx_old_seq,
+        (zm_summary.rx_broken_seq - zm_summary.rx_duplicate_seq - zm_summary.rx_old_seq),
+        zm_summary.rx_broken_sum);
+    DbgPrint("* Rx   lost sum=%4d,dup. error=%4d, free count=%4d\n", zm_summary.rx_lost_sum,
+        zm_summary.rx_duplicate_error, zm_summary.rx_free);
+    DbgPrint("* Rx  flush sum=%4d, clear sum=%4d, reorder=%7d\n", zm_summary.rx_flush,
+        zm_summary.rx_clear, zm_summary.rx_reorder);
+    DbgPrint("* Firmware reset=%3d, reset sum=%4d\n", zm_summary.reset_count,
+        zm_summary.reset_sum);
+    DbgPrint("******************************************************\n\n");
+    //reset count 11772c
+    zm_summary.tx_msdu_count = 0;
+    zm_summary.tx_mpdu_count = 0;
+    zm_summary.rx_msdu_count = 0;
+    zm_summary.rx_mpdu_count = 0;
+    zm_summary.rx_broken_seq = 0;
+    zm_summary.rx_broken_seq_dis = 0;
+    zm_summary.rx_duplicate_seq = 0;
+    zm_summary.rx_old_seq = 0;
+    zm_summary.reset_count = 0;
+    zm_summary.rx_amsdu_len = 0;
+
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_tick[i] = zm_var.tx_mpdu_tick[i] = 0;
+        zm_var.rx_msdu_tick[i] = zm_var.rx_mpdu_tick[i] = 0;
+    }
+
+    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_PERFORMANCE, 100);
+}
+
+void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.tx_msdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.tx_msdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiRxPerformanceMSDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.rx_msdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.rx_msdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiTxPerformanceMPDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.tx_mpdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.tx_mpdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+#ifndef ZM_INT_USE_EP2_HEADER_SIZE
+#define ZM_INT_USE_EP2_HEADER_SIZE   12
+#endif
+void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf)
+{
+    u32_t   index;
+    u16_t   frameType;
+    u16_t   frameCtrl;
+    u8_t    mpduInd;
+    u16_t   plcpHdrLen;
+    u16_t   len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = zfwBufGetSize(dev, buf);
+    mpduInd = zmw_rx_buf_readb(dev, buf, len-1);
+    /* First MPDU or Single MPDU */
+    if(((mpduInd & 0x30) == 0x00) || ((mpduInd & 0x30) == 0x20))
+    //if ((mpduInd & 0x10) == 0x00)
+    {
+        plcpHdrLen = 12;        // PLCP header length
+    }
+    else
+    {
+        if (zmw_rx_buf_readh(dev, buf, 4) == wd->macAddr[0] &&
+            zmw_rx_buf_readh(dev, buf, 6) == wd->macAddr[1] &&
+            zmw_rx_buf_readh(dev, buf, 8) == wd->macAddr[2]) {
+            plcpHdrLen = 0;
+        }
+        else if (zmw_rx_buf_readh(dev, buf, 16) == wd->macAddr[0] &&
+                 zmw_rx_buf_readh(dev, buf, 18) == wd->macAddr[1] &&
+                 zmw_rx_buf_readh(dev, buf, 20) == wd->macAddr[2]){
+            plcpHdrLen = 12;
+        }
+        else {
+            plcpHdrLen = 0;
+        }
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, plcpHdrLen + 0);
+    frameType = frameCtrl & 0xf;
+
+    if (frameType != ZM_WLAN_DATA_FRAME)
+    {
+        return;
+    }
+
+    zm_summary.rx_mpdu_count++;
+
+    index = wd->tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.rx_mpdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiRxPerformanceSeq(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   seq_no;
+    u16_t   offset = 0;
+    u16_t   old_dis = zm_summary.rx_broken_seq_dis;
+    //sys_time = KeQueryPerformanceCounter(&freq);
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+
+    ZM_SEQ_DEBUG("Out   %5d\n", seq_no);
+
+    if (seq_no < zm_summary.rx_seq_base)
+    {
+        if (seq_no == 0)
+        {
+            if (zm_summary.rx_seq_base != 4095)
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(4096 - zm_summary.rx_seq_base);
+            }
+        }
+        else if ((seq_no < 300) && (zm_summary.rx_seq_base > 3800))
+        {
+            zm_summary.rx_broken_seq++;
+            ZM_SEQ_DEBUG("Broken seq");
+            zm_summary.rx_broken_seq_dis+=(4096 - zm_summary.rx_seq_base + seq_no);
+        }
+        else
+        {
+            zm_summary.rx_broken_seq++;
+            ZM_SEQ_DEBUG("Broken seq");
+            zm_summary.rx_broken_seq_dis+=(zm_summary.rx_seq_base - seq_no);
+            zm_summary.rx_old_seq++;
+        }
+    }
+    else
+    {
+        if (seq_no != (zm_summary.rx_seq_base + 1))
+        {
+            if ((seq_no > 3800) && (zm_summary.rx_seq_base < 300))
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(4096 - seq_no + zm_summary.rx_seq_base);
+                zm_summary.rx_old_seq++;
+            }
+            else
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(seq_no - zm_summary.rx_seq_base);
+            }
+        }
+    }
+    if (seq_no == zm_summary.rx_seq_base)
+    {
+        zm_summary.rx_duplicate_seq++;
+    }
+
+    if ((zm_summary.rx_broken_seq_dis - old_dis) > 100)
+    {
+        DbgPrint("* seq_no=%4d, base_seq=%4d, dis_diff=%4d", seq_no,
+            zm_summary.rx_seq_base, zm_summary.rx_broken_seq_dis - old_dis);
+    }
+    zm_summary.rx_seq_base = seq_no;
+}
+
+void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp)
+{
+    zm_summary.reset_count = (u16_t)rsp - zm_summary.reset_sum;
+    zm_summary.reset_sum = (u16_t)rsp;
+}
+
+void zfiRxPerformanceDup(zdev_t* dev, zbuf_t* buf1, zbuf_t* buf2)
+{
+    u16_t   seq_no1, seq_no2;
+
+    seq_no1 = zmw_rx_buf_readh(dev, buf1, 22) >> 4;
+    seq_no2 = zmw_rx_buf_readh(dev, buf2, 22) >> 4;
+    if (seq_no1 != seq_no2)
+    {
+        zm_summary.rx_duplicate_error++;
+    }
+}
+
+void zfiRxPerformanceFree(zdev_t* dev, zbuf_t* buf)
+{
+    zm_summary.rx_free++;
+}
+
+void zfiRxPerformanceAMSDU(zdev_t* dev, zbuf_t* buf, u16_t len)
+{
+    if (zm_summary.rx_amsdu_len < len)
+    {
+        zm_summary.rx_amsdu_len = len;
+    }
+}
+void zfiRxPerformanceFlush(zdev_t* dev)
+{
+    zm_summary.rx_flush++;
+}
+
+void zfiRxPerformanceClear(zdev_t* dev)
+{
+    zm_summary.rx_clear++;
+    ZM_SEQ_DEBUG("RxClear");
+}
+
+void zfiRxPerformanceReorder(zdev_t* dev)
+{
+    zm_summary.rx_reorder++;
+}
+#endif /* end of ZM_ENABLE_PERFORMANCE_EVALUATION */
diff --git a/drivers/staging/otus/80211core/performance.h b/drivers/staging/otus/80211core/performance.h
new file mode 100644
index 000000000000..29f658ae477c
--- /dev/null
+++ b/drivers/staging/otus/80211core/performance.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef _PERFORMANCE_H
+#define _PERFORMANCE_H
+
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+
+struct zsSummary
+{
+    u32_t tx_msdu_count;
+    u32_t tx_mpdu_count;
+    u32_t rx_msdu_count;
+    u32_t rx_mpdu_count;
+    u32_t tick_base;
+    u16_t rx_seq_base;
+    u16_t rx_broken_seq;
+    u16_t rx_broken_sum;
+    u16_t rx_broken_seq_dis;
+    u16_t rx_duplicate_seq;
+    u16_t rx_duplicate_error;
+    u16_t rx_old_seq;
+    u16_t rx_lost_sum;
+    u16_t tx_idle_count;
+    u16_t rx_idle_count;
+    u16_t reset_count;
+    u16_t reset_sum;
+    u16_t rx_free;
+    u16_t rx_amsdu_len;
+    u16_t rx_flush;
+    u16_t rx_clear;
+    u32_t rx_reorder;
+};
+
+struct zsVariation
+{
+    u32_t tx_msdu_tick[100];
+    u32_t tx_mpdu_tick[100];
+    u32_t rx_msdu_tick[100];
+    u32_t rx_mpdu_tick[100];
+
+    u32_t tx_msdu_mean;
+    u32_t tx_mpdu_mean;
+    u32_t rx_msdu_mean;
+    u32_t rx_mpdu_mean;
+
+    u32_t tx_msdu_sum;
+    u32_t tx_mpdu_sum;
+    u32_t rx_msdu_sum;
+    u32_t rx_mpdu_sum;
+
+    u32_t tx_msdu_var;
+    u32_t tx_mpdu_var;
+    u32_t rx_msdu_var;
+    u32_t rx_mpdu_var;
+};
+
+struct zsThroughput
+{
+    u32_t tx[50];
+    u32_t rx[50];
+    u16_t head;
+    u16_t tail;
+    u16_t size;
+    LARGE_INTEGER sys_time;
+    LARGE_INTEGER freq;
+};
+
+void zfiPerformanceInit(zdev_t* dev);
+void zfiPerformanceRefresh(zdev_t* dev);
+
+void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick);
+void zfiRxPerformanceMSDU(zdev_t* dev, u32_t tick);
+void zfiTxPerformanceMPDU(zdev_t* dev, u32_t tick);
+void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceSeq(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp);
+void zfiRxPerformanceDup(zdev_t* dev, zbuf_t* buf1, zbuf_t* buf2);
+void zfiRxPerformanceFree(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceAMSDU(zdev_t* dev, zbuf_t* buf, u16_t len);
+void zfiRxPerformanceFlush(zdev_t* dev);
+void zfiRxPerformanceClear(zdev_t* dev);
+void zfiRxPerformanceReorder(zdev_t* dev);
+#endif /* end of ZM_ENABLE_PERFORMANCE_EVALUATION */
+#endif /* end of _PERFORMANCE_H */
diff --git a/drivers/staging/otus/80211core/pub_usb.h b/drivers/staging/otus/80211core/pub_usb.h
new file mode 100644
index 000000000000..c4b4bd25e828
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_usb.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_USB_H
+#define _PUB_USB_H
+
+#include "../oal_dt.h"
+
+#define ZM_HAL_80211_MODE_AP              0
+#define ZM_HAL_80211_MODE_STA             1
+#define ZM_HAL_80211_MODE_IBSS_GENERAL    2
+#define ZM_HAL_80211_MODE_IBSS_WPA2PSK    3
+
+/* USB module description                                               */
+/* Queue Management                                                     */
+/* 80211core requires OAL to implement a transmission queue in OAL's    */
+/* USB module. Because there is only limited on-chip memory, so USB     */
+/* data transfer may be pending until on-chip memory is available.      */
+/* 80211core also requires OAL's USB module to provide two functions    */
+/* zfwUsbGetFreeTxQSize() and zfwUsbGetMaxTxQSize() for 80211core to    */
+/* query the status of this transmission queue. The main purpose of     */
+/* this queue is for QoS/WMM. Though there are hardware priority        */
+/* queues on the chip, and also software priority queues in the         */
+/* 80211core. There is still one and only one USB channel. So           */
+/* 80211core will use the information that zfwUsbGetFreeTxQSize()       */
+/* returned to schedule the traffic from the software priority          */
+/* queues to the hardware priority queues. For example, if 80211core    */
+/* found that USB transmission queue is going to be full, it will       */
+/* not allow packets with lower priority to enter the USB channel.      */
+
+
+/* Structure for USB call back functions */
+struct zfCbUsbFuncTbl {
+    void (*zfcbUsbRecv)(zdev_t *dev, zbuf_t *buf);
+    void (*zfcbUsbRegIn)(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+    void (*zfcbUsbOutComplete)(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr);
+    void (*zfcbUsbRegOutComplete)(zdev_t* dev);
+};
+
+/* Call back functions                                                  */
+/* Below are the functions that should be called by the OAL             */
+
+/* When data is available in endpoint 3, OAL shall embed the data in */
+/* zbuf_t and supply to 80211core by calling this function           */
+/* void (*zfcbUsbRecv)(zdev_t *dev, zbuf_t *buf); */
+
+/* When data is available in endpoint 2, OAL shall call this function */
+/* void (*zfcbUsbRegIn)(zdev_t* dev, u32_t* rsp, u16_t rspLen); */
+
+/* When USB data transfer completed in endpoint 1, OAL shall call this function */
+/* void (*zfcbUsbOutComplete)(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr); */
+
+
+/* Call out functions                                                   */
+/* Below are the functions that supply by the OAL for 80211core to      */
+/* manipulate the USB                                                   */
+
+/* Return OAL's USB TxQ size */
+extern u32_t zfwUsbGetMaxTxQSize(zdev_t* dev);
+
+/* Return OAL's TxQ available size */
+extern u32_t zfwUsbGetFreeTxQSize(zdev_t* dev);
+
+/* Register call back function */
+extern void zfwUsbRegisterCallBack(zdev_t* dev, struct zfCbUsbFuncTbl *zfUsbFunc);
+
+/* Enable USB interrupt endpoint */
+extern u32_t zfwUsbEnableIntEpt(zdev_t *dev, u8_t endpt);
+
+/* Enable USB Rx endpoint */
+extern int zfwUsbEnableRxEpt(zdev_t* dev, u8_t endpt);
+
+/* 80211core call this function to send a USB request over endpoint 0 */
+extern u32_t zfwUsbSubmitControl(zdev_t* dev, u8_t req, u16_t value,
+        u16_t index, void *data, u32_t size);
+extern u32_t zfwUsbSubmitControlIo(zdev_t* dev, u8_t req, u8_t reqtype,
+        u16_t value, u16_t index, void *data, u32_t size);
+
+/* 80211core call this function to transfer data out over endpoint 1 */
+extern void zfwUsbCmd(zdev_t* dev, u8_t endpt, u32_t* cmd, u16_t cmdLen);
+
+/* 80211core call this function to transfer data out over endpoint 4 */
+extern u32_t zfwUsbSend(zdev_t* dev, u8_t endpt, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+                u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset);
+
+/* 80211core call this function to set USB configuration */
+extern u32_t zfwUsbSetConfiguration(zdev_t *dev, u16_t value);
+
+#endif
diff --git a/drivers/staging/otus/80211core/pub_zfi.h b/drivers/staging/otus/80211core/pub_zfi.h
new file mode 100644
index 000000000000..a35bd5d41d2c
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_zfi.h
@@ -0,0 +1,821 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_DEFS_H
+#define _PUB_DEFS_H
+
+#include "../oal_dt.h"
+
+/***** Section 1 : Tunable Parameters *****/
+/* The defintions in this section are tunabel parameters */
+
+/* Maximum number of BSS that could be scaned */
+#define ZM_MAX_BSS                          128
+
+/* Maximum number of WPA2 PMKID that supported */
+#define ZM_PMKID_MAX_BSS_CNT               8
+
+/* Enable aggregation and deaggregation */
+#define ZM_ENABLE_AGGREGATION
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /* Enable BA failed retransmission in firmware */
+    #define ZM_ENABLE_FW_BA_RETRANSMISSION
+    #define ZM_BYPASS_AGGR_SCHEDULING
+    //#define ZM_AGGR_BIT_ON
+#endif
+
+
+#ifndef ZM_FB50
+//#define ZM_FB50
+#endif
+
+#ifndef ZM_AP_DEBUG
+//#define ZM_AP_DEBUG
+#endif
+
+//#define ZM_ENABLE_BA_RATECTRL
+
+/***** End of section 1 *****/
+
+
+/***** Section 2 : Public Definitions, data structures and prototypes *****/
+/* function return status */
+#define ZM_STATUS_SUCCESS                   0
+#define ZM_STATUS_FAILURE                   1
+
+// media connect status
+#define ZM_STATUS_MEDIA_CONNECT             0x00
+#define ZM_STATUS_MEDIA_DISCONNECT          0x01
+#define ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND    0x02
+#define ZM_STATUS_MEDIA_DISABLED            0x03
+#define ZM_STATUS_MEDIA_CONNECTION_DISABLED 0x04
+#define ZM_STATUS_MEDIA_CONNECTION_RESET    0x05
+#define ZM_STATUS_MEDIA_RESET               0x06
+#define ZM_STATUS_MEDIA_DISCONNECT_DEAUTH   0x07
+#define ZM_STATUS_MEDIA_DISCONNECT_DISASOC  0x08
+#define ZM_STATUS_MEDIA_DISCONNECT_TIMEOUT  0x09
+#define ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED  0x0a
+#define ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED  0x0b
+#define ZM_STATUS_MEDIA_DISCONNECT_MIC_FAIL   0x0c
+#define ZM_STATUS_MEDIA_DISCONNECT_UNREACHABLE 0x0d
+#define ZM_STATUS_MEDIA_DISCONNECT_BEACON_MISS  0x0e
+
+// Packet Filter
+#define ZM_PACKET_TYPE_DIRECTED             0x00000001
+#define ZM_PACKET_TYPE_MULTICAST            0x00000002
+#define ZM_PACKET_TYPE_ALL_MULTICAST        0x00000004
+#define ZM_PACKET_TYPE_BROADCAST            0x00000008
+#define ZM_PACKET_TYPE_PROMISCUOUS          0x00000020
+
+/* BSS mode definition */
+/* TODO : The definitions here are coupled with XP's NDIS OID. */
+/*        We can't be changed them freely, need to disarm this mine */
+#define ZM_MODE_IBSS                        0
+#define ZM_MODE_INFRASTRUCTURE              1
+#define ZM_MODE_UNKNOWN                     2
+#define ZM_MODE_INFRASTRUCTURE_MAX          3
+#define ZM_MODE_AP                          4
+#define ZM_MODE_PSEUDO                      5
+
+
+/* Authentication mode */
+#define ZM_AUTH_MODE_OPEN                   0
+#define ZM_AUTH_MODE_SHARED_KEY             1
+#define ZM_AUTH_MODE_AUTO                   2
+#define ZM_AUTH_MODE_WPA                    3
+#define ZM_AUTH_MODE_WPAPSK                 4
+#define ZM_AUTH_MODE_WPA_NONE               5
+#define ZM_AUTH_MODE_WPA2                   6
+#define ZM_AUTH_MODE_WPA2PSK                7
+#ifdef ZM_ENABLE_CENC
+#define ZM_AUTH_MODE_CENC                   8
+#endif //ZM_ENABLE_CENC
+#define ZM_AUTH_MODE_WPA_AUTO               9
+#define ZM_AUTH_MODE_WPAPSK_AUTO            10
+
+// Encryption mode
+#define ZM_NO_WEP                           0x0
+#define ZM_AES                              0x4
+#define ZM_TKIP                             0x2
+#define ZM_WEP64                            0x1
+#define ZM_WEP128                           0x5
+#define ZM_WEP256                           0x6
+#ifdef ZM_ENABLE_CENC
+#define ZM_CENC                             0x7
+#endif //ZM_ENABLE_CENC
+
+/* Encryption type for wep status */
+#define ZM_ENCRYPTION_WEP_DISABLED          0
+#define ZM_ENCRYPTION_WEP_ENABLED           1
+#define ZM_ENCRYPTION_WEP_KEY_ABSENT        2
+#define ZM_ENCRYPTION_NOT_SUPPORTED         3
+#define ZM_ENCRYPTION_TKIP                  4
+#define ZM_ENCRYPTION_TKIP_KEY_ABSENT       5
+#define ZM_ENCRYPTION_AES                   6
+#define ZM_ENCRYPTION_AES_KEY_ABSENT        7
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_ENCRYPTION_CENC                  8
+#endif //ZM_ENABLE_CENC
+
+/* security type */
+#define ZM_SECURITY_TYPE_NONE               0
+#define ZM_SECURITY_TYPE_WEP                1
+#define ZM_SECURITY_TYPE_WPA                2
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_SECURITY_TYPE_CENC               3
+#endif //ZM_ENABLE_CENC
+
+/* Encryption Exemption Action Type  */
+#define ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION   0
+#define ZM_ENCRYPTION_EXEMPT_ALWAYS         1
+
+/* MIC failure */
+#define ZM_MIC_PAIRWISE_ERROR               0x06
+#define ZM_MIC_GROUP_ERROR                  0x0E
+
+
+/* power save mode */
+#define ZM_STA_PS_NONE                    0
+#define ZM_STA_PS_MAX                     1
+#define ZM_STA_PS_FAST                    2
+#define ZM_STA_PS_LIGHT                   3
+
+/* WME AC Type */
+#define ZM_WME_AC_BK                        0       /* Background AC */
+#define ZM_WME_AC_BE                        1       /* Best-effort AC */
+#define ZM_WME_AC_VIDEO                     2       /* Video AC */
+#define ZM_WME_AC_VOICE                     3       /* Voice AC */
+
+/* Preamble type */
+#define ZM_PREAMBLE_TYPE_AUTO               0
+#define ZM_PREAMBLE_TYPE_LONG               1
+#define ZM_PREAMBLE_TYPE_SHORT              2
+
+/* wireless modes constants */
+#define ZM_WIRELESS_MODE_5_54        0x01   ///< 5 GHz 54 Mbps
+#define ZM_WIRELESS_MODE_5_108       0x02   ///< 5 GHz 108 Mbps
+#define ZM_WIRELESS_MODE_24_11       0x04   ///< 2.4 GHz 11 Mbps
+#define ZM_WIRELESS_MODE_24_54       0x08   ///< 2.4 GHz 54 Mbps
+#define ZM_WIRELESS_MODE_24_108      0x10   ///< 2.4 GHz 108 Mbps
+#define ZM_WIRELESS_MODE_49_13      0x100   ///< 4.9 GHz 13.5 Mbps, quarter rate chn-bandwidth = 5
+#define ZM_WIRELESS_MODE_49_27      0x200   ///< 4.9 GHz 27 Mbps, half rate chn-bandwidth = 10
+#define ZM_WIRELESS_MODE_49_54      0x400   ///< 4.9 GHz 54 Mbps, full rate chn-bandwidth = 20
+#define ZM_WIRELESS_MODE_5_300     0x1000   ///< 5 GHz 300 Mbps
+#define ZM_WIRELESS_MODE_24_300    0x2000   ///< 2.4 GHz 300 Mbps
+#define ZM_WIRELESS_MODE_5_130     0x4000   ///< 5 GHz 130 Mbps
+#define ZM_WIRELESS_MODE_24_130    0x8000   ///< 2.4 GHz 130 Mbps
+
+#define ZM_WIRELESS_MODE_24_N      (ZM_WIRELESS_MODE_24_130|ZM_WIRELESS_MODE_24_300)
+#define ZM_WIRELESS_MODE_5_N       (ZM_WIRELESS_MODE_5_130|ZM_WIRELESS_MODE_5_300)
+#define ZM_WIRELESS_MODE_24        (ZM_WIRELESS_MODE_24_11|ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)
+#define ZM_WIRELESS_MODE_5         (ZM_WIRELESS_MODE_5_54|ZM_WIRELESS_MODE_5_N)
+
+/* AdHoc Mode with different band */
+#define ZM_ADHOCBAND_A         1
+#define ZM_ADHOCBAND_B         2
+#define ZM_ADHOCBAND_G         3
+#define ZM_ADHOCBAND_BG        4
+#define ZM_ADHOCBAND_ABG       5
+
+/* Authentication algorithm in the field algNo of authentication frames */
+#define ZM_AUTH_ALGO_OPEN_SYSTEM      0x10000    /* Open system */
+#define ZM_AUTH_ALGO_SHARED_KEY       0x10001    /* Shared Key */
+#define ZM_AUTH_ALGO_LEAP             0x10080    /* Leap */
+
+struct zsScanResult
+{
+    u32_t reserved;
+};
+
+
+struct zsStastics
+{
+    u32_t reserved;
+};
+
+#define ZM_MAX_SUPP_RATES_IE_SIZE       12
+#define ZM_MAX_IE_SIZE                  50 //100
+#define ZM_MAX_WPS_IE_SIZE              150
+#define ZM_MAX_PROBE_FRAME_BODY_SIZE    512//300
+#define	ZM_MAX_COUNTRY_INFO_SIZE		20
+
+#define ZM_MAX_SSID_LENGTH          32
+struct zsBssInfo
+{
+    u8_t   macaddr[6];
+    u8_t   bssid[6];
+    u8_t   beaconInterval[2];
+    u8_t   capability[2];
+    u8_t   timeStamp[8];
+    u8_t   ssid[ZM_MAX_SSID_LENGTH + 2];   // EID(1) + Length(1) + SSID(32)
+    u8_t   supportedRates[ZM_MAX_SUPP_RATES_IE_SIZE + 2]; // EID(1) + Length(1) + supported rates [12]
+    u8_t   channel;
+    u16_t  frequency;
+    u16_t  atimWindow;
+    u8_t   erp;
+    u8_t   extSupportedRates[ZM_MAX_SUPP_RATES_IE_SIZE + 2]; // EID(1) + Length(1) + extended supported rates [12]
+    u8_t   wpaIe[ZM_MAX_IE_SIZE + 2];
+    u8_t   wscIe[ZM_MAX_WPS_IE_SIZE + 2];
+    u8_t   rsnIe[ZM_MAX_IE_SIZE + 2];
+#ifdef ZM_ENABLE_CENC
+    u8_t   cencIe[ZM_MAX_IE_SIZE + 2]; /* CENC */ /* half size because of memory exceed 64k boundary */
+#endif //ZM_ENABLE_CENC
+    u8_t   securityType;
+    u8_t   signalStrength;
+    u8_t   signalQuality;
+    u16_t  sortValue;
+    u8_t   wmeSupport;
+    u8_t   flag;
+    u8_t   EnableHT;
+    u8_t   enableHT40;
+    u8_t   SG40;
+    u8_t   extChOffset;
+    u8_t   apCap; // bit0:11N AP
+    u16_t  frameBodysize;
+    u8_t   frameBody[ZM_MAX_PROBE_FRAME_BODY_SIZE];
+    u8_t   countryInfo[ZM_MAX_COUNTRY_INFO_SIZE + 2];
+    u16_t  athOwlAp;
+    u16_t  marvelAp;
+    u16_t  broadcomHTAp;
+    u32_t  tick;
+    struct zsBssInfo* next;
+};
+
+struct zsBssList
+{
+    u8_t bssCount;
+    struct zsBssInfo* head;
+    struct zsBssInfo* tail;
+};
+
+struct zsBssListV1
+{
+    u8_t bssCount;
+    struct zsBssInfo bssInfo[ZM_MAX_BSS];
+};
+
+#define ZM_KEY_FLAG_GK                 0x0001
+#define ZM_KEY_FLAG_PK                 0X0002
+#define ZM_KEY_FLAG_AUTHENTICATOR      0x0004
+#define ZM_KEY_FLAG_INIT_IV            0x0008
+#define ZM_KEY_FLAG_DEFAULT_KEY        0x0010
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_KEY_FLAG_CENC               0x0020
+#endif //ZM_ENABLE_CENC
+
+// Comment: For TKIP, key[0]~key[15]  => TKIP key
+//                    key[16]~key[23] => Tx MIC key
+//                    key[24]~key[31] => Rx MIC key
+struct zsKeyInfo
+{
+    u8_t*   key;
+    u8_t    keyLength;
+    u8_t    keyIndex;
+    u8_t*   initIv;
+    u16_t   flag;
+    u8_t    vapId;
+    u16_t    vapAddr[3];
+    u16_t*   macAddr;
+};
+
+
+
+/*
+ * Channels are specified by frequency.
+ */
+typedef struct {
+	u16_t	channel;	/* setting in Mhz */
+	u32_t	channelFlags;	/* see below */
+	u8_t	privFlags;
+	s8_t	maxRegTxPower;	/* max regulatory tx power in dBm */
+	s8_t	maxTxPower;	/* max true tx power in 0.25 dBm */
+	s8_t	minTxPower;	/* min true tx power in 0.25 dBm */
+} ZM_HAL_CHANNEL;
+
+struct zsRegulationTable
+{
+    u16_t   regionCode;
+    u16_t   CurChIndex;
+    u16_t   allowChannelCnt;
+    ZM_HAL_CHANNEL   allowChannel[60];   /* 2.4GHz: 14 channels, 5 GHz: 31 channels */
+};
+
+struct zsPartnerNotifyEvent
+{
+    u8_t bssid[6];                      // The BSSID of IBSS
+    u8_t peerMacAddr[6];                // The MAC address of peer station
+};
+
+#define ZM_RC_TRAINED_BIT   0x1
+struct zsRcCell
+{
+    u32_t txCount;
+    u32_t failCount;
+    u8_t currentRate;
+    u8_t currentRateIndex;
+    u32_t probingTime;
+    u8_t operationRateSet[24];
+    u8_t operationRateCount;
+    u16_t rxRssi;
+    u8_t flag;
+    u32_t  lasttxCount;
+    u32_t  lastTime;
+};
+
+struct zsOppositeInfo
+{
+    u8_t            macAddr[6];
+    struct zsRcCell rcCell;
+    u8_t            valid;              // This indicate if this opposite is still valid
+    u8_t            aliveCounter;
+    u8_t            pkInstalled;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* For WPA2PSK ! */
+    u8_t 			wpaState;
+    u8_t            camIdx;
+    u8_t            encryMode;
+    u16_t           iv16;
+    u32_t           iv32;
+#endif
+};
+
+typedef void (*zfpIBSSIteratePeerStationCb)(
+    zdev_t* dev, struct zsOppositeInfo *peerInfo, void *ctx, u8_t index);
+
+typedef u16_t (*zfpStaRxSecurityCheckCb)(zdev_t* dev, zbuf_t* buf);
+
+
+/* Communication Tally data structure */
+struct zsCommTally
+{
+    u32_t txUnicastFrm;		    //  0 txUnicastFrames
+    u32_t txMulticastFrm;		//  1 txMulticastFrames
+    u32_t txUnicastOctets;	    //  2 txUniOctets  byte size
+    u32_t txMulticastOctets;	//  3 txMultiOctets  byte size
+    u32_t txFrmUpperNDIS;       //  4
+    u32_t txFrmDrvMgt;          //  5
+    u32_t RetryFailCnt;		    //  6
+    u32_t Hw_TotalTxFrm;		//  7 Hardware total Tx Frame
+    u32_t Hw_RetryCnt;		    //  8 txMultipleRetriesFrames
+    u32_t Hw_UnderrunCnt;       //  9
+
+    u32_t DriverRxFrmCnt;       // 10
+    u32_t rxUnicastFrm;		    // 11 rxUnicastFrames
+    u32_t rxMulticastFrm;	    // 12rxMulticastFrames
+
+    u32_t NotifyNDISRxFrmCnt;   // 14
+    u32_t rxUnicastOctets;	// 15 rxUniOctets  byte size
+    u32_t rxMulticastOctets;	    // 16 rxMultiOctets  byte size
+    u32_t DriverDiscardedFrm;       // 17 Discard by ValidateFrame
+    u32_t LessThanDataMinLen;       // 18
+    u32_t GreaterThanMaxLen;        // 19
+    u32_t DriverDiscardedFrmCauseByMulticastList;
+    u32_t DriverDiscardedFrmCauseByFrmCtrl;
+    u32_t rxNeedFrgFrm;		    // 22 need more frg frm
+    u32_t DriverRxMgtFrmCnt;
+    u32_t rxBroadcastFrm;	    // 24 Receive broadcast frame count
+    u32_t rxBroadcastOctets;	// 25 Receive broadcast frame byte size
+    u32_t rx11bDataFrame;		// 26 Measured quality 11b data frame count
+    u32_t rxOFDMDataFrame;	    // 27 Measured quality 11g data frame count
+
+
+    u32_t Hw_TotalRxFrm;        // 28
+    u32_t Hw_CRC16Cnt;		    // 29 rxPLCPCRCErrCnt
+    u32_t Hw_CRC32Cnt;		    // 30 rxCRC32ErrCnt
+    u32_t Hw_DecrypErr_UNI;     // 31
+    u32_t Hw_DecrypErr_Mul;     // 32
+
+    u32_t Hw_RxFIFOOverrun;     // 34
+    u32_t Hw_RxTimeOut;         // 35
+    u32_t LossAP;               // 36
+
+    u32_t Tx_MPDU;              // 37
+    u32_t BA_Fail;              // 38
+    u32_t Hw_Tx_AMPDU;          // 39
+    u32_t Hw_Tx_MPDU;           // 40
+
+    u32_t RateCtrlTxMPDU;
+    u32_t RateCtrlBAFail;
+
+    u32_t txQosDropCount[5];    //41 42 43 44 45
+
+	u32_t Hw_RxMPDU;            // 46
+	u32_t Hw_RxDropMPDU;        // 47
+	u32_t Hw_RxDelMPDU;         // 48
+
+	u32_t Hw_RxPhyMiscError;    // 49
+	u32_t Hw_RxPhyXRError;      // 50
+    u32_t Hw_RxPhyOFDMError;    // 51
+    u32_t Hw_RxPhyCCKError;     // 52
+    u32_t Hw_RxPhyHTError;      // 53
+    u32_t Hw_RxPhyTotalCount;   // 54
+
+    u32_t swRxFragmentCount;         // 55
+    u32_t swRxUnicastMicFailCount;   // 56
+    u32_t swRxMulticastMicFailCount; // 57
+    u32_t swRxDropUnencryptedCount;  // 58
+
+    u32_t txBroadcastFrm;
+    u32_t txBroadcastOctets;
+};
+
+/* Traffic Monitor Tally data structure */
+struct zsTrafTally
+{
+    u32_t rxDuplicate;
+    u32_t rxSrcIsOwnMac;
+    //u32_t rxDataFrameCount;
+    //u32_t rxDataByteCount;
+    //u32_t rxDataBytesIn1000ms;
+    //u32_t rxDataTmpFor1000ms;
+    //u32_t rxDataBytesIn2000ms;
+    //u32_t rxDataTmpFor2000ms;
+
+    //u32_t txDataFrameCount;
+    //u32_t txDataByteCount;
+    //u32_t txDataBytesIn1000ms;
+    //u32_t txDataTmpFor1000ms;
+    u32_t txDataBytesIn2000ms;
+    u32_t txDataTmpFor2000ms;
+};
+
+/* Hal rx packet moniter information */
+struct zsMonHalRxInfo
+{
+    u32_t currentRSSI[7];
+    u32_t currentRxEVM[14];
+    u32_t currentRxDataMT;
+    u32_t currentRxDataMCS;
+    u32_t currentRxDataBW;
+    u32_t currentRxDataSG;
+};
+
+struct zsTail
+{
+    u8_t SignalStrength1;
+    u8_t SignalStrength2;
+    u8_t SignalStrength3;
+    u8_t SignalQuality;
+    u8_t SAIndex;
+    u8_t DAIndex;
+    u8_t ErrorIndication;
+    u8_t RxMacStatus;
+};
+
+union zuTail
+{
+    struct zsTail Data;
+    u8_t Byte[8];
+};
+
+struct zsAdditionInfo
+{
+    u8_t PlcpHeader[12];
+    union zuTail   Tail;
+};
+
+
+struct zsPmkidBssidInfo
+{
+    u16_t      bssid[3];
+    u8_t       pmkid[16];
+};
+
+struct zsPmkidInfo
+{
+	   u32_t		bssidCount;
+	   struct zsPmkidBssidInfo	bssidInfo[ZM_PMKID_MAX_BSS_CNT];
+};
+
+
+struct zsCbFuncTbl
+{
+    u16_t (*zfcbAuthNotify)(zdev_t* dev, u16_t* macAddr);
+    u16_t (*zfcbAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+    u16_t (*zfcbDisAsocNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    u16_t (*zfcbApConnectNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    void (*zfcbConnectNotify)(zdev_t* dev, u16_t status, u16_t* bssid);
+    void (*zfcbScanNotify)(zdev_t* dev, struct zsScanResult* result);
+    void (*zfcbMicFailureNotify)(zdev_t* dev, u16_t* addr, u16_t status);
+    void (*zfcbApMicFailureNotify)(zdev_t* dev, u8_t* addr, zbuf_t* buf);
+    void (*zfcbIbssPartnerNotify)(zdev_t* dev, u16_t status,
+            struct zsPartnerNotifyEvent *event);
+    void (*zfcbMacAddressNotify)(zdev_t* dev, u8_t* addr);
+    void (*zfcbSendCompleteIndication)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbRecvEth)(zdev_t* dev, zbuf_t* buf, u16_t port);
+    void (*zfcbRecv80211)(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+    void (*zfcbRestoreBufData)(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+    u16_t (*zfcbCencAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+#endif //ZM_ENABLE_CENC
+    u8_t (*zfcbClassifyTxPacket)(zdev_t* dev, zbuf_t* buf);
+
+    void (*zfcbHwWatchDogNotify)(zdev_t* dev);
+};
+
+extern void zfZeroMemory(u8_t* va, u16_t length);
+#define ZM_INIT_CB_FUNC_TABLE(p)        zfZeroMemory((u8_t *)p, sizeof(struct zsCbFuncTbl));
+
+//extern struct zsWlanDev zgWlanDev;
+
+/* Initialize WLAN hardware and software, resource will be allocated */
+/* for WLAN operation, must be called first before other function.   */
+extern u16_t zfiWlanOpen(zdev_t* dev, struct zsCbFuncTbl* cbFuncTbl);
+
+/* WLAN hardware will be shutdown and all resource will be release */
+extern u16_t zfiWlanClose(zdev_t* dev);
+
+/* Enable/disable Wlan operation */
+extern u16_t zfiWlanEnable(zdev_t* dev);
+extern u16_t zfiWlanDisable(zdev_t* dev, u8_t ResetKeyCache);
+extern u16_t zfiWlanResume(zdev_t* dev, u8_t doReconn);
+extern u16_t zfiWlanSuspend(zdev_t* dev);
+
+/* Enable/disable ISR interrupt */
+extern u16_t zfiWlanInterruptEnable(zdev_t* dev);
+extern u16_t zfiWlanInterruptDisable(zdev_t* dev);
+
+/* Do WLAN site survey */
+extern u16_t zfiWlanScan(zdev_t* dev);
+
+/* Get WLAN stastics */
+extern u16_t zfiWlanGetStatistics(zdev_t* dev);
+
+/* Reset WLAN */
+extern u16_t zfiWlanReset(zdev_t* dev);
+
+/* Deauthenticate a STA */
+extern u16_t zfiWlanDeauth(zdev_t* dev, u16_t* macAddr, u16_t reason);
+
+extern u16_t zfiTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port);
+extern u8_t zfiIsTxQueueFull(zdev_t* dev);
+extern u16_t zfiTxSend80211Mgmt(zdev_t* dev, zbuf_t* buf, u16_t port);
+
+extern void zfiIsrPci(zdev_t* dev);
+
+extern u8_t zfiWlanIBSSGetPeerStationsCount(zdev_t* dev);
+extern u8_t zfiWlanIBSSIteratePeerStations(zdev_t* dev, u8_t numToIterate, zfpIBSSIteratePeerStationCb callback, void *ctx);
+extern void zfiWlanFlushAllQueuedBuffers(zdev_t* dev);
+
+/* coid.c */
+extern void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr);
+
+extern u16_t zfiGlobalDataSize(zdev_t* dev);
+
+extern void zfiHeartBeat(zdev_t* dev);
+
+extern void zfiWlanSetWlanMode(zdev_t* dev, u8_t wlanMode);
+extern void zfiWlanSetAuthenticationMode(zdev_t* dev, u8_t authMode);
+extern void zfiWlanSetWepStatus(zdev_t* dev, u8_t wepStatus);
+extern void zfiWlanSetSSID(zdev_t* dev, u8_t* ssid, u8_t ssidLength);
+extern void zfiWlanSetFragThreshold(zdev_t* dev, u16_t fragThreshold);
+extern void zfiWlanSetRtsThreshold(zdev_t* dev, u16_t rtsThreshold);
+extern void zfiWlanSetFrequency(zdev_t* dev, u32_t frequency, u8_t bImmediate);
+extern void zfiWlanSetBssid(zdev_t* dev, u8_t* bssid);
+extern void zfiWlanSetBeaconInterval(zdev_t* dev, u16_t beaconInterval,
+                              u8_t bImmediate);
+extern void zfiWlanSetDtimCount(zdev_t* dev, u8_t  dtim);
+extern void zfiWlanSetAtimWindow(zdev_t* dev, u16_t atimWindow, u8_t bImmediate);
+extern void zfiWlanSetEncryMode(zdev_t* dev, u8_t encryMode);
+extern u8_t zfiWlanSetKey(zdev_t* dev, struct zsKeyInfo keyInfo);
+extern u8_t zfiWlanPSEUDOSetKey(zdev_t* dev, struct zsKeyInfo keyInfo);
+extern void zfiWlanSetPowerSaveMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanQueryBssListV1(zdev_t* dev, struct zsBssListV1* bssListV1);
+extern void zfiWlanQueryBssList(zdev_t* dev, struct zsBssList* pBssList);
+extern void zfiWlanSetProtectionMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanFlushBssList(zdev_t* dev);
+
+void zfiWlanDisableDfsChannel(zdev_t* dev, u8_t disableFlag);
+
+extern u8_t zfiWlanQueryWlanMode(zdev_t* dev);
+extern u16_t zfiWlanChannelToFrequency(zdev_t* dev, u8_t channel);
+extern u8_t zfiWlanFrequencyToChannel(zdev_t* dev, u16_t freq);
+
+#define ZM_WLAN_STATE_OPENED        0
+#define ZM_WLAN_STATE_ENABLED       1
+#define ZM_WLAN_STATE_DISABLED      2
+#define ZM_WLAN_STATE_CLOSEDED      3
+extern u8_t zfiWlanQueryAdapterState(zdev_t* dev);
+extern u8_t zfiWlanQueryAuthenticationMode(zdev_t* dev, u8_t bWrapper);
+extern u8_t zfiWlanQueryWepStatus(zdev_t* dev, u8_t bWrapper);
+extern void zfiWlanQuerySSID(zdev_t* dev, u8_t* ssid, u8_t* pSsidLength);
+extern u16_t zfiWlanQueryFragThreshold(zdev_t* dev);
+extern u16_t zfiWlanQueryRtsThreshold(zdev_t* dev);
+extern u32_t zfiWlanQueryFrequency(zdev_t* dev);
+extern u32_t zfiWlanQueryCurrentFrequency(zdev_t* dev, u8_t qmode);
+extern u32_t zfiWlanQueryFrequencyAttribute(zdev_t* dev, u32_t frequency);
+extern void zfiWlanQueryFrequencyHT(zdev_t* dev, u32_t *bandWidth, u32_t *extOffset);
+extern u8_t zfiWlanQueryCWMode(zdev_t* dev);
+extern u32_t zfiWlanQueryCWEnable(zdev_t* dev);
+extern void zfiWlanQueryBssid(zdev_t* dev, u8_t* bssid);
+extern u16_t zfiWlanQueryBeaconInterval(zdev_t* dev);
+extern u32_t zfiWlanQueryRxBeaconTotal(zdev_t* dev);
+extern u16_t zfiWlanQueryAtimWindow(zdev_t* dev);
+extern u8_t zfiWlanQueryEncryMode(zdev_t* dev);
+extern u16_t zfiWlanQueryCapability(zdev_t* dev);
+extern u16_t zfiWlanQueryAid(zdev_t* dev);
+extern void zfiWlanQuerySupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength);
+extern void zfiWlanQueryExtSupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength);
+extern void zfiWlanQueryRsnIe(zdev_t* dev, u8_t* ie, u8_t* pLength);
+extern void zfiWlanQueryWpaIe(zdev_t* dev, u8_t* ie, u8_t* pLength);
+extern u8_t zfiWlanQueryHTMode(zdev_t* dev);
+extern u8_t zfiWlanQueryBandWidth40(zdev_t* dev);
+extern u8_t zfiWlanQueryMulticastCipherAlgo(zdev_t *dev);
+extern u16_t zfiWlanQueryRegionCode(zdev_t* dev);
+extern void zfiWlanSetWpaIe(zdev_t* dev, u8_t* ie, u8_t Length);
+extern void zfiWlanSetWpaSupport(zdev_t* dev, u8_t WpaSupport);
+extern void zfiWlanCheckStaWpaIe(zdev_t* dev);
+extern void zfiWlanSetBasicRate(zdev_t* dev, u8_t bRateSet, u8_t gRateSet,
+                         u32_t nRateSet);
+extern void zfiWlanSetBGMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetpreambleType(zdev_t* dev, u8_t type);
+extern u8_t zfiWlanQuerypreambleType(zdev_t* dev);
+extern u8_t zfiWlanQueryPowerSaveMode(zdev_t* dev);
+extern void zfiWlanSetMacAddress(zdev_t* dev, u16_t* mac);
+extern u16_t zfiWlanSetTxRate(zdev_t* dev, u16_t rate);
+extern u32_t zfiWlanQueryTxRate(zdev_t* dev);
+extern void zfWlanUpdateRxRate(zdev_t* dev, struct zsAdditionInfo* addInfo);
+extern u32_t zfiWlanQueryRxRate(zdev_t* dev);
+extern u8_t zfiWlanSetPmkidInfo(zdev_t* dev, u16_t* bssid, u8_t* pmkid);
+extern u32_t zfiWlanQueryPmkidInfo(zdev_t* dev, u8_t* buf, u32_t len);
+extern void zfiWlanSetAllMulticast(zdev_t* dev, u32_t setting);
+extern void zfiWlanSetHTCtrl(zdev_t* dev, u32_t *setting, u32_t forceTxTPC);
+extern void zfiWlanQueryHTCtrl(zdev_t* dev, u32_t *setting, u32_t *forceTxTPC);
+extern void zfiWlanDbg(zdev_t* dev, u8_t setting);
+
+extern void zfiWlanResetTally(zdev_t* dev);
+extern void zfiWlanQueryTally(zdev_t* dev, struct zsCommTally *tally);
+extern void zfiWlanQueryTrafTally(zdev_t* dev, struct zsTrafTally *tally);
+extern void zfiWlanQueryMonHalRxInfo(zdev_t* dev, struct zsMonHalRxInfo *halRxInfo);
+
+extern u32_t zfiFWConfig(zdev_t* dev, u32_t size);
+
+extern void zfiDKEnable(zdev_t* dev, u32_t enable);
+
+extern void zfiWlanSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList);
+extern void zfiWlanRemoveKey(zdev_t* dev, u8_t keyType, u8_t keyId);
+extern u8_t zfiWlanQueryIsPKInstalled(zdev_t *dev, u8_t *staMacAddr);
+extern u32_t zfiWlanQueryPacketTypePromiscuous(zdev_t* dev);
+extern void zfiWlanSetPacketTypePromiscuous(zdev_t* dev, u32_t setValue);
+extern void zfiSetChannelManagement(zdev_t* dev, u32_t setting);
+extern void zfiSetRifs(zdev_t* dev, u16_t setting);
+extern void zfiCheckRifs(zdev_t* dev);
+extern void zfiSetReorder(zdev_t* dev, u16_t value);
+extern void zfiSetSeqDebug(zdev_t* dev, u16_t value);
+
+extern u16_t zfiConfigWdsPort(zdev_t* dev, u8_t wdsPortId, u16_t flag, u16_t* wdsAddr,
+        u16_t encType, u32_t* wdsKey);
+extern void zfiWlanQueryRegulationTable(zdev_t* dev, struct zsRegulationTable* pEntry);
+extern void zfiWlanSetScanTimerPerChannel(zdev_t* dev, u16_t time);
+extern void zfiWlanSetAutoReconnect(zdev_t* dev, u8_t enable);
+extern u32_t zfiDebugCmd(zdev_t* dev, u32_t cmd, u32_t value);
+extern void zfiWlanSetProbingHiddenSsid(zdev_t* dev, u8_t* ssid, u8_t ssidLen,
+    u16_t entry);
+extern void zfiWlanSetDropUnencryptedPackets(zdev_t* dev, u8_t enable);
+extern void zfiWlanSetIBSSJoinOnly(zdev_t* dev, u8_t joinOnly);
+extern void zfiWlanSetDefaultKeyId(zdev_t* dev, u8_t keyId);
+extern void zfiWlanSetDisableProbingWithSsid(zdev_t* dev, u8_t mode);
+extern void zfiWlanQueryGSN(zdev_t* dev, u8_t *gsn, u16_t vapId);
+extern u16_t zfiStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+extern u8_t zfiWlanSetDot11DMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetDot11HDFSMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetDot11HTPCMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetAniMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetStaWme(zdev_t* dev, u8_t enable, u8_t uapsdInfo);
+extern void zfiWlanSetApWme(zdev_t* dev, u8_t enable);
+extern u8_t zfiWlanQuerywmeEnable(zdev_t* dev);
+#ifdef ZM_OS_LINUX_FUNC
+extern void zfiWlanShowTally(zdev_t* dev);
+#endif
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+extern u8_t zfiWlanSetCencPairwiseKey(zdev_t* dev, u8_t keyid, u32_t *txiv, u32_t *rxiv,
+        u8_t *key, u8_t *mic);
+extern u8_t zfiWlanSetCencGroupKey(zdev_t* dev, u8_t keyid, u32_t *rxiv,
+        u8_t *key, u8_t *mic);
+#endif //ZM_ENABLE_CENC
+extern void zfiWlanQuerySignalInfo(zdev_t* dev, u8_t *buffer);
+extern void zfiWlanQueryAdHocCreatedBssDesc(zdev_t* dev, struct zsBssInfo *pBssInfo);
+extern u8_t zfiWlanQueryAdHocIsCreator(zdev_t* dev);
+extern u32_t zfiWlanQuerySupportMode(zdev_t* dev);
+extern u32_t zfiWlanQueryTransmitPower(zdev_t* dev);
+extern void zfiWlanEnableLeapConfig(zdev_t* dev, u8_t leapEnabled);
+
+/* returned buffer allocated by driver core */
+extern void zfiRecvEthComplete(zdev_t* dev, zbuf_t* buf);
+
+extern void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+
+extern void zfiWlanSetMaxTxPower(zdev_t* dev, u8_t power2, u8_t power5);
+extern void zfiWlanQueryMaxTxPower(zdev_t* dev, u8_t *power2, u8_t *power5);
+extern void zfiWlanSetConnectMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetSupportMode(zdev_t* dev, u32_t mode);
+extern void zfiWlanSetAdhocMode(zdev_t* dev, u32_t mode);
+extern u32_t zfiWlanQueryAdhocMode(zdev_t* dev, u8_t bWrapper);
+extern u8_t zfiWlanSetCountryIsoName(zdev_t* dev, u8_t *countryIsoName, u8_t length);
+extern const char* zfiWlanQueryCountryIsoName(zdev_t* dev);
+extern u8_t zfiWlanQueryregulatoryDomain(zdev_t* dev);
+extern u8_t zfiWlanQueryCCS(zdev_t* dev);
+extern void zfiWlanSetCCS(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetRegulatory(zdev_t* dev, u8_t CCS, u16_t Code, u8_t bfirstChannel);
+extern const char* zfiHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode);
+extern void  zfiWlanSetLEDCtrlParam(zdev_t* dev, u8_t type, u8_t flag);
+extern u32_t zfiWlanQueryReceivedPacket(zdev_t* dev);
+extern void zfiWlanCheckSWEncryption(zdev_t* dev);
+extern u16_t zfiWlanQueryAllowChannels(zdev_t *dev, u16_t *channels);
+extern u16_t zfiWlanGetMulticastAddressCount(zdev_t* dev);
+extern void zfiWlanGetMulticastList(zdev_t* dev, u8_t* pMCList);
+extern void zfiWlanSetPacketFilter(zdev_t* dev, u32_t PacketFilter);
+extern u8_t zfiCompareWithMulticastListAddress(zdev_t* dev, u16_t* dstMacAddr);
+extern void zfiWlanSetSafeModeEnabled(zdev_t* dev, u8_t safeMode);
+extern void zfiWlanSetIBSSAdditionalIELength(zdev_t* dev, u32_t ibssAdditionalIESize,  u8_t* ibssAdditionalIE);
+extern void zfiWlanSetXLinkMode(zdev_t* dev, u32_t setValue);
+
+/* hprw.c */
+extern u32_t zfiDbgWriteFlash(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgWriteReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgReadReg(zdev_t* dev, u32_t addr);
+
+extern u32_t zfiDbgWriteEeprom(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgBlockWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf);
+extern u32_t zfiDbgBlockWriteEeprom_v2(zdev_t* dev, u32_t addr, u32_t* buf, u32_t wrlen);
+
+extern u16_t zfiDbgChipEraseFlash(zdev_t *dev);
+extern u16_t zfiDbgProgramFlash(zdev_t *dev, u32_t offset, u32_t len, u32_t *data);
+extern u32_t zfiDbgGetFlashCheckSum(zdev_t *dev, u32_t addr, u32_t len);
+extern u32_t zfiDbgReadFlash(zdev_t *dev, u32_t addr, u32_t len);
+extern u32_t zfiDownloadFwSet(zdev_t *dev);
+
+extern u32_t zfiDbgDelayWriteReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgFlushDelayWrite(zdev_t* dev);
+
+extern u32_t zfiDbgSetIFSynthesizer(zdev_t* dev, u32_t value);
+extern u32_t zfiDbgReadTally(zdev_t* dev);
+
+extern u32_t zfiDbgQueryHwTxBusy(zdev_t* dev);
+
+extern u8_t zfiWlanGetDestAddrFromBuf(zdev_t *dev, zbuf_t *buf, u16_t *macAddr);
+
+extern u32_t zfiWlanQueryHwCapability(zdev_t* dev);
+
+extern void zfiWlanSetDynamicSIFSParam(zdev_t* dev, u8_t val);
+
+/***** End of section 2 *****/
+
+/***** section 3 performace evaluation *****/
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+extern void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick);
+extern void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf);
+extern void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp);
+#define ZM_PERFORMANCE_INIT(dev)                zfiPerformanceInit(dev);
+#define ZM_PERFORMANCE_TX_MSDU(dev, tick)       zfiTxPerformanceMSDU(dev, tick);
+#define ZM_PERFORMANCE_RX_MSDU(dev, tick)       zfiRxPerformanceMSDU(dev, tick);
+#define ZM_PERFORMANCE_TX_MPDU(dev, tick)       zfiTxPerformanceMPDU(dev, tick);
+#define ZM_PERFORMANCE_RX_MPDU(dev, buf)        zfiRxPerformanceMPDU(dev, buf);
+#define ZM_PERFORMANCE_RX_SEQ(dev, buf)         zfiRxPerformanceSeq(dev, buf);
+#define ZM_PERFORMANCE_REG(dev, reg, rsp)    {if(cmd[1] == reg) zfiRxPerformanceReg(dev, reg, rsp);}
+#define ZM_PERFORMANCE_DUP(dev, buf1, buf2)     zfiRxPerformanceDup(dev, buf1, buf2);
+#define ZM_PERFORMANCE_FREE(dev, buf)           zfiRxPerformanceFree(dev, buf);
+#define ZM_PERFORMANCE_RX_AMSDU(dev, buf, len)  zfiRxPerformanceAMSDU(dev, buf, len);
+#define ZM_PERFORMANCE_RX_FLUSH(dev)            zfiRxPerformanceFlush(dev);
+#define ZM_PERFORMANCE_RX_CLEAR(dev)            zfiRxPerformanceClear(dev);
+#define ZM_SEQ_DEBUG                            if (wd->seq_debug) DbgPrint
+#define ZM_PERFORMANCE_RX_REORDER(dev)          zfiRxPerformanceReorder(dev);
+#else
+#define ZM_PERFORMANCE_INIT(dev)
+#define ZM_PERFORMANCE_TX_MSDU(dev, tick)
+#define ZM_PERFORMANCE_RX_MSDU(dev, tick)
+#define ZM_PERFORMANCE_TX_MPDU(dev, tick)
+#define ZM_PERFORMANCE_RX_MPDU(dev, buf)
+#define ZM_PERFORMANCE_RX_SEQ(dev, buf)
+#define ZM_PERFORMANCE_REG(dev, reg, rsp)
+#define ZM_PERFORMANCE_DUP(dev, buf1, buf2)
+#define ZM_PERFORMANCE_FREE(dev, buf)
+#define ZM_PERFORMANCE_RX_AMSDU(dev, buf, len)
+#define ZM_PERFORMANCE_RX_FLUSH(dev)
+#define ZM_PERFORMANCE_RX_CLEAR(dev)
+#define ZM_SEQ_DEBUG
+#define ZM_PERFORMANCE_RX_REORDER(dev)
+#endif
+/***** End of section 3 *****/
+#endif
diff --git a/drivers/staging/otus/80211core/pub_zfw.h b/drivers/staging/otus/80211core/pub_zfw.h
new file mode 100644
index 000000000000..2474bb7536e8
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_zfw.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_ZFW_H
+#define _PUB_ZFW_H
+
+#include "../oal_dt.h"
+
+
+/* Buffer management */
+#ifdef ZM_ENABLE_BUFFER_DEBUG
+extern zbuf_t* zfwBufAllocateWithContext(zdev_t* dev, u16_t len, u8_t *functionName, ULONG line);
+#define zfwBufAllocate(dev, len)  zfwBufAllocateWithContext(dev, len, (u8_t *)__func__, __LINE__)
+#else
+extern zbuf_t* zfwBufAllocate(zdev_t* dev, u16_t len);
+#endif
+extern void zfwBufFree(zdev_t* dev, zbuf_t* buf, u16_t errCode);
+extern u16_t zfwBufChain(zdev_t* dev, zbuf_t** head, zbuf_t* tail);
+extern u16_t zfwBufCopy(zdev_t* dev, zbuf_t* dst, zbuf_t* src);
+extern u16_t zfwBufSetSize(zdev_t* dev, zbuf_t* buf, u16_t size);
+extern u16_t zfwBufRemoveHead(zdev_t* dev, zbuf_t* buf, u16_t size);
+extern u16_t zfwBufGetSize(zdev_t* dev, zbuf_t* buf);
+extern void  zfwCopyBufContext(zdev_t* dev, zbuf_t* source, zbuf_t* dest);
+
+/* Memory management */
+extern void* zfwMemAllocate(zdev_t* dev, u32_t size);
+extern void zfwMemFree(zdev_t* dev, void* mem, u32_t size);
+extern void zfwMemoryCopy(u8_t* dst, u8_t* src, u16_t length);
+extern void zfwMemoryMove(u8_t* dst, u8_t* src, u16_t length);
+extern void zfwZeroMemory(u8_t* va, u16_t length);
+extern u8_t zfwMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length);
+
+/* Others */
+extern void zfwSleep(zdev_t* dev, u32_t ms);
+extern u16_t zfwGetVapId(zdev_t* dev);
+extern u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+extern u32_t zfwWaitForEvent(zdev_t *dev, u32_t event, u32_t timeout);
+extern void zfwSendEvent(zdev_t* dev);
+extern void zfwGetActiveScanDur(zdev_t* dev, u8_t* Dur );
+extern void zfwGetShowZeroLengthSSID(zdev_t* dev, u8_t* Dur );
+/* For debugging */
+extern void zfwDumpBuf(zdev_t* dev, zbuf_t* buf);
+extern void zfwDbgReadRegDone(zdev_t* dev, u32_t addr, u32_t val);
+/* For Evl */
+extern void zfwDbgDownloadFwInitDone(zdev_t* dev);
+extern void zfwDbgReadFlashDone(zdev_t* dev, u32_t addr, u32_t* rspdata, u32_t datalen);
+extern void zfwDbgGetFlashChkSumDone(zdev_t* dev, u32_t* rspdata);
+extern void zfwDbgProgrameFlashDone(zdev_t* dev);
+extern void zfwDbgProgrameFlashChkDone(zdev_t* dev);
+extern void zfwDbgWriteRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwDbgWriteEepromDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwDbgReadTallyDone(zdev_t* dev);
+extern void zfwWlanReadRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwWlanWriteRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwWlanReadTallyDone(zdev_t* dev);
+extern void zfwDbgQueryHwTxBusyDone(zdev_t* dev, u32_t val);
+extern u32_t zfwReadReg(zdev_t* dev, u32_t offset);
+extern u32_t zfwReadEeprom(zdev_t* dev, u32_t addr);
+
+/* Reserved for Vista, please return 0 */
+extern u8_t zfwGetPktEncExemptionActionType(zdev_t* dev, zbuf_t* buf);
+
+#ifdef ZM_ENABLE_CENC
+/* Reserved for CENC, please return 0 */
+extern u8_t zfwCencHandleBeaconProbrespon(zdev_t* dev, u8_t *pWIEc,
+        u8_t *pPeerSSIDc, u8_t *pPeerAddrc);
+#endif //ZM_ENABLE_CENC
+
+#ifdef ZM_HALPLUS_LOCK
+extern asmlinkage struct zsWlanDev *zfwGetWlanDev(zdev_t* dev);
+extern asmlinkage void zfwEnterCriticalSection(zdev_t* dev);
+extern asmlinkage void zfwLeaveCriticalSection(zdev_t* dev);
+extern asmlinkage u8_t zfwBufReadByte(zdev_t* dev, zbuf_t* buf, u16_t offset);
+extern asmlinkage u16_t zfwBufReadHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset);
+extern asmlinkage void zfwBufWriteByte(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t value);
+extern asmlinkage void zfwBufWriteHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t value);
+extern asmlinkage u8_t *zfwGetBuffer(zdev_t* dev, zbuf_t* buf);
+#endif
+
+#endif //_PUB_ZFW_H
diff --git a/drivers/staging/otus/80211core/queue.c b/drivers/staging/otus/80211core/queue.c
new file mode 100644
index 000000000000..d294831b5c36
--- /dev/null
+++ b/drivers/staging/otus/80211core/queue.c
@@ -0,0 +1,303 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : queue.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains queue management functions.                */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "queue.h"
+
+
+struct zsQueue* zfQueueCreate(zdev_t* dev, u16_t size)
+{
+    struct zsQueue* q;
+
+    if ((q = (struct zsQueue*)zfwMemAllocate(dev, sizeof(struct zsQueue)
+            + (sizeof(struct zsQueueCell)*(size-1)))) != NULL)
+    {
+        q->size = size;
+        q->sizeMask = size-1;
+        q->head = 0;
+        q->tail = 0;
+    }
+    return q;
+}
+
+void zfQueueDestroy(zdev_t* dev, struct zsQueue* q)
+{
+    u16_t size = sizeof(struct zsQueue) + (sizeof(struct zsQueueCell)*(q->size-1));
+
+    zfQueueFlush(dev, q);
+    zfwMemFree(dev, q, size);
+
+    return;
+}
+
+u16_t zfQueuePutNcs(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick)
+{
+    u16_t ret = ZM_ERR_QUEUE_FULL;
+
+    zm_msg0_mm(ZM_LV_1, "zfQueuePutNcs()");
+
+    if (((q->tail+1)&q->sizeMask) != q->head)
+    {
+        q->cell[q->tail].buf = buf;
+        q->cell[q->tail].tick = tick;
+        q->tail = (q->tail+1) & q->sizeMask;
+        ret = ZM_SUCCESS;
+    }
+
+    return ret;
+}
+
+u16_t zfQueuePut(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick)
+{
+    u16_t ret;
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    ret = zfQueuePutNcs(dev, q, buf, tick);
+
+    zmw_leave_critical_section(dev);
+
+    return ret;
+}
+
+zbuf_t* zfQueueGet(zdev_t* dev, struct zsQueue* q)
+{
+    zbuf_t* buf = NULL;
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (q->head != q->tail)
+    {
+        buf = q->cell[q->head].buf;
+        q->head = (q->head+1) & q->sizeMask;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return buf;
+}
+
+u16_t zfCompareDstwithBuf(zdev_t* dev, zbuf_t* buf, u8_t* addr)
+{
+    u16_t i;
+    u8_t dst[6];
+
+    for (i=0; i<6; i++)
+    {
+        dst[i] = zmw_buf_readb(dev, buf, i);
+        if (dst[i] != addr[i])
+        {
+            return 1+i;
+        }
+    }
+
+    return 0;
+}
+
+
+zbuf_t* zfQueueGetWithMac(zdev_t* dev, struct zsQueue* q, u8_t* addr, u8_t* mb)
+{
+    zbuf_t* buf;
+    zbuf_t* retBuf = NULL;
+    u16_t index, next;
+    zmw_declare_for_critical_section();
+
+    *mb = 0;
+
+    zmw_enter_critical_section(dev);
+
+    index = q->head;
+
+    while (1)
+    {
+        if (index != q->tail)
+        {
+            buf = q->cell[index].buf;
+
+            //if buf's detination address == input addr
+            if (zfCompareDstwithBuf(dev, buf, addr) == 0)
+            {
+                retBuf = buf;
+                //Get it, and trace the whole queue to calculate more bit
+                while ((next =((index+1)&q->sizeMask)) != q->tail)
+                {
+                    q->cell[index].buf = q->cell[next].buf;
+                    q->cell[index].tick = q->cell[next].tick;
+
+                    if ((*mb == 0) && (zfCompareDstwithBuf(dev,
+                            q->cell[next].buf, addr) == 0))
+                    {
+                        *mb = 1;
+                    }
+
+                    index = next;
+                }
+                q->tail = (q->tail-1) & q->sizeMask;
+
+                zmw_leave_critical_section(dev);
+                return retBuf;
+            }
+            index = (index + 1) & q->sizeMask;
+        } //if (index != q->tail)
+        else
+        {
+            break;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return retBuf;
+
+}
+
+void zfQueueFlush(zdev_t* dev, struct zsQueue* q)
+{
+    zbuf_t* buf;
+
+    while ((buf = zfQueueGet(dev, q)) != NULL)
+    {
+        zfwBufFree(dev, buf, 0);
+    }
+
+    return;
+}
+
+void zfQueueAge(zdev_t* dev, struct zsQueue* q, u32_t tick, u32_t msAge)
+{
+    zbuf_t* buf;
+    u32_t   buftick;
+    zmw_declare_for_critical_section();
+
+    while (1)
+    {
+        buf = NULL;
+        zmw_enter_critical_section(dev);
+
+        if (q->head != q->tail)
+        {
+            buftick = q->cell[q->head].tick;
+            if (((tick - buftick)*ZM_MS_PER_TICK) > msAge)
+            {
+                buf = q->cell[q->head].buf;
+                q->head = (q->head+1) & q->sizeMask;
+            }
+        }
+
+        zmw_leave_critical_section(dev);
+
+        if (buf != NULL)
+        {
+            zm_msg0_mm(ZM_LV_0, "Age frame in queue!");
+            zfwBufFree(dev, buf, 0);
+        }
+        else
+        {
+            break;
+        }
+    }
+    return;
+}
+
+
+u8_t zfQueueRemovewithIndex(zdev_t* dev, struct zsQueue* q, u16_t index, u8_t* addr)
+{
+    u16_t next;
+    u8_t mb = 0;
+
+    //trace the whole queue to calculate more bit
+    while ((next =((index+1)&q->sizeMask)) != q->tail)
+    {
+        q->cell[index].buf = q->cell[next].buf;
+        q->cell[index].tick = q->cell[next].tick;
+
+        if ((mb == 0) && (zfCompareDstwithBuf(dev,
+                q->cell[next].buf, addr) == 0))
+        {
+            mb = 1;
+        }
+
+        index = next;
+    }
+    q->tail = (q->tail-1) & q->sizeMask;
+
+    return mb;
+
+}
+
+void zfQueueGenerateUapsdTim(zdev_t* dev, struct zsQueue* q,
+        u8_t* uniBitMap, u16_t* highestByte)
+{
+    zbuf_t* psBuf;
+    u8_t dst[6];
+    u16_t id, aid, index, i;
+    u16_t bitPosition;
+    u16_t bytePosition;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    index = q->head;
+
+    while (index != q->tail)
+    {
+        psBuf = q->cell[index].buf;
+        for (i=0; i<6; i++)
+        {
+            dst[i] = zmw_buf_readb(dev, psBuf, i);
+        }
+        /* TODO : use u8_t* fot MAC address */
+        if (((id = zfApFindSta(dev, (u16_t*)dst)) != 0xffff)
+                && (wd->ap.staTable[id].psMode != 0))
+        {
+            /* Calculate PVB only when all AC are delivery-enabled */
+            if ((wd->ap.staTable[id].qosInfo & 0xf) == 0xf)
+            {
+                aid = id + 1;
+                bitPosition = (1 << (aid & 0x7));
+                bytePosition = (aid >> 3);
+                uniBitMap[bytePosition] |= bitPosition;
+
+                if (bytePosition>*highestByte)
+                {
+                    *highestByte = bytePosition;
+                }
+            }
+            index = (index+1) & q->sizeMask;
+        }
+        else
+        {
+            /* Free garbage UAPSD frame */
+            zfQueueRemovewithIndex(dev, q, index, dst);
+            zfwBufFree(dev, psBuf, 0);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/queue.h b/drivers/staging/otus/80211core/queue.h
new file mode 100644
index 000000000000..4526b882bd03
--- /dev/null
+++ b/drivers/staging/otus/80211core/queue.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _QUEUE_H
+#define _QUEUE_H
+
+#include "../oal_dt.h"
+
+struct zsQueueCell
+{
+    u32_t   tick;
+    zbuf_t* buf;
+};
+
+struct zsQueue
+{
+    u16_t   size;
+    u16_t   sizeMask;
+    u16_t   head;
+    u16_t   tail;
+    struct zsQueueCell cell[1];
+};
+
+#endif //#ifndef _QUEUE_H
diff --git a/drivers/staging/otus/80211core/ratectrl.c b/drivers/staging/otus/80211core/ratectrl.c
new file mode 100644
index 000000000000..a43104cd7f51
--- /dev/null
+++ b/drivers/staging/otus/80211core/ratectrl.c
@@ -0,0 +1,874 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+const u32_t zcRateToPhyCtrl[] =
+    {
+        /* 1M,        2M,         5M,        11M ,  0  1  2  3*/
+        0x00000,    0x10000,    0x20000,    0x30000,
+        /* 6M         9M          12M        18M ,  4  5  6  7*/
+        0xb0001,    0xf0001,    0xa0001,    0xe0001,
+        /* 24M         36M        48M        54M ,  8  9  10 11*/
+        0x90001,    0xd0001,    0x80001,    0xc0001,
+        /* MCS0      MCS1        MCS2        MCS3,  12 13 14 15*/
+        0x00002,    0x10002,    0x20002,    0x30002,
+        /* MCS4      MCS5        MCS6        MCS7,  16 17 18 19*/
+        0x40002,    0x50002,    0x60002,    0x70002,
+        /* MCS8      MCS9        MCS10       MCS11, 20 21 22 23*/
+        0x80002,    0x90002,    0xa0002,    0xb0002,
+        /* MCS12     MCS13       MCS14       MCS15, 24 25 26 27*/
+        0xc0002,    0xd0002,    0xe0002,    0xf0002,
+        /* MCS14SG,  MCS15SG     MCS7SG           , 28 29, 30*/
+        0x800e0002, 0x800f0002, 0x80070002
+    };
+
+
+const u8_t zcHtRateTable[15][4] =
+    { /*[5G 20MHz]  [5G 40MHz] [2.4G 20MHz]  [2.4G 40MHz]*/
+        {  4,          4,          0,          0},   /*OFDM6M OFDM6M  CCK1M  CCK1M  */
+        {  5,          5,          1,          1},   /*OFDM9M OFDM9M  CCK2M  CCK2M  */
+        {  13,         12,         2,          2},   /*MCS1   MCS0    CCK5M  CCK5M  */
+        {  14,         13,         3,          3},   /*MCS2   MCS1    CCK11M CCK11M */
+        {  15,         14,         13,         12},  /*MCS3   MCS2    MCS1   MCS0   */
+        {  16,         15,         14,         13},  /*MCS4   MCS3    MCS2   MCS1   */
+        {  23,         16,         15,         14},  /*MCS11  MCS4    MCS3   MCS2   */
+        {  24,         23,         16,         15},  /*MCS12  MCS11   MCS4   MCS3   */
+        {  25,         24,         23,         16},  /*MCS13  MCS12   MCS11  MCS4   */
+        {  26,         25,         24,         23},  /*MCS14  MCS13   MCS12  MCS11  */
+        {  27,         26,         25,         24},  /*MCS15  MCS14   MCS13  MCS12  */
+        {  0,          27,         26,         25},  /*0      MCS15   MCS14  MCS13  */
+        {  0,          29,         27,         26},  /*0      MCS15SG MCS15  MCS14  */
+        {  0,          0,          0,          28},  /*0      0       0      MCS14SG*/
+        {  0,          0,          0,          29}   /*0      0       0      MCS15SG*/
+    };
+
+const u8_t zcHtOneTxStreamRateTable[15][4] =
+    { /*[5G 20MHz]  [5G 40MHz] [2.4G 20MHz]  [2.4G 40MHz]*/
+        {  4,          4,          0,          0},   /*OFDM6M OFDM6M  CCK1M  CCK1M  */
+        {  5,          5,          1,          1},   /*OFDM9M OFDM9M  CCK2M  CCK2M  */
+        {  13,         12,         2,          2},   /*MCS1   MCS0    CCK5M  CCK5M  */
+        {  14,         13,         3,          3},   /*MCS2   MCS1    CCK11M CCK11M */
+        {  15,         14,         13,         12},  /*MCS3   MCS2    MCS1   MCS0   */
+        {  16,         15,         14,         13},  /*MCS4   MCS3    MCS2   MCS1   */
+        {  17,         16,         15,         14},  /*MCS5   MCS4    MCS3   MCS2   */
+        {  18,         17,         16,         15},  /*MCS6   MCS5    MCS4   MCS3   */
+        {  19,         18,         17,         16},  /*MCS7   MCS6    MCS5   MCS4   */
+        {  0,          19,         18,         17},  /*0      MCS7    MCS6   MCS5   */
+        {  0,          30,         19,         18},  /*0      MCS7SG  MCS7   MCS6   */
+        {  0,          0,          0,          19},  /*0      0       0      MCS7   */
+        {  0,          0,          0,          30},  /*0      0       0      MCS7SG */
+        {  0,          0,          0,          0 },  /*0      0       0      0      */
+        {  0,          0,          0,          0 }   /*0      0       0      0      */
+    };
+
+const u16_t zcRate[] =
+    {
+        1, 2, 5, 11,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6, 9, 12, 18,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24, 36, 48, 54,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        13, 27, 40, 54,               /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        81, 108, 121, 135,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        27, 54, 81, 108,              /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        162, 216, 243, 270,           /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        270, 300, 150                 /* MCS14SG, MCS15SG, MCS7SG , 28 29 30*/
+    };
+
+const u16_t PERThreshold[] =
+    {
+        100, 50, 50, 50,    /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        50, 50, 30, 30,     /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        25, 25, 25, 20,     /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        50, 50, 50, 40,    /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        30, 30, 30, 30,    /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        30, 30, 25, 25,    /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        25, 25, 15, 15,     /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        15, 15, 10          /* MCS14SG, MCS15SG         , 28 29*/
+    };
+
+const u16_t FailDiff[] =
+    {
+        40, 46, 40, 0,          /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        24, 17, 22, 16,         /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        19, 13, 5, 0,           /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        36, 22, 15, 19,         /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        12, 5, 4, 7,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        0, 0, 0, 0,             /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        9, 4, 3, 3,             /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        3, 0, 0                 /* MCS14SG, MCS15SG         , 28 29*/
+    };
+
+
+#ifdef ZM_ENABLE_BA_RATECTRL
+u32_t TxMPDU[29];
+u32_t BAFail[29];
+u32_t BAPER[29];
+const u16_t BADiff[] =
+    {
+        0, 0, 0, 0,
+        0, 0, 0, 0,
+        0, 0, 0, 0,
+        361, 220, 151, 187,
+        122, 48, 41, 65,
+        0, 0, 0, 0,
+        88, 33, 27, 25,
+        0
+    };
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlInitCell          */
+/*      Initialize rate control cell.                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      type : 0=>11b, 1=>11a/g, 2=>11n, 3=>11n one Tx stream           */
+/*      gBand : 1=>2.4G, 0=>5G                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlInitCell(zdev_t* dev, struct zsRcCell* rcCell, u8_t type,
+        u8_t gBand, u8_t SG40)
+{
+    u8_t i;
+    u8_t maxrate;
+    zmw_get_wlan_dev(dev);
+
+    if (SG40) SG40 = 1;
+
+    if (gBand != 0)
+    {
+        if (type == 1) //11g
+        {
+            for (i=0; i<4; i++) //1M 2M 5M 11M
+            {
+                rcCell->operationRateSet[i] = (u8_t)i;
+            }
+            for (i=4; i<10; i++) //12M 18M 24M 36M 48M 54M
+            {
+                rcCell->operationRateSet[i] = 2+i;
+            }
+            rcCell->operationRateCount = 10;
+            rcCell->currentRateIndex = 5; //18M
+        }
+        else if (type == 2) //11ng
+        {
+            if (wd->wlanMode == ZM_MODE_AP) //AP 11ng 40M
+            {
+                for (i=0; i<15; i++)
+                {
+                    rcCell->operationRateSet[i] = zcHtRateTable[i][3];
+                }
+                if(!SG40) rcCell->operationRateSet[13] = 27;
+                rcCell->operationRateCount = 14+SG40;
+                rcCell->currentRateIndex = 10;
+            }
+            else //STA
+            {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11ng 40M
+                {
+                    for (i=0; i<15; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][3];
+                    }
+                    if(!SG40) rcCell->operationRateSet[13] = 27;
+                    rcCell->operationRateCount = 14+SG40;
+                    rcCell->currentRateIndex = 10;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<13; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][2];
+                    }
+                    rcCell->operationRateCount = 13;
+                    rcCell->currentRateIndex = 9;
+                }
+            }
+        }
+        else if (type == 3) //11ng one Tx stream
+        {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11ng 40M one Tx stream
+                {
+                    if(SG40 != 0)
+                    {
+                        maxrate = 13;
+                    }
+                    else
+                    {
+                        maxrate = 12;
+                    }
+                    for (i=0; i<maxrate; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][3];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<11; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][2];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+        }
+        else //if (type == 0) //11b
+        {
+            for (i=0; i<4; i++)
+            {
+                rcCell->operationRateSet[i] = (u8_t)i;
+            }
+            rcCell->operationRateCount = 4;
+            rcCell->currentRateIndex = rcCell->operationRateCount-1;
+        }
+    }
+    else
+    {
+        if (type == 2) //11na
+        {
+            if (wd->wlanMode == ZM_MODE_AP) //AP 11na 40M
+            {
+                for (i=0; i<(12+SG40); i++)
+                {
+                    rcCell->operationRateSet[i] = zcHtRateTable[i][1];
+                }
+                rcCell->operationRateCount = 12+SG40;
+                rcCell->currentRateIndex = 8;
+            }
+            else //STA
+            {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11na 40M
+                {
+                    for (i=0; i<(12+SG40); i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][1];
+                    }
+                    rcCell->operationRateCount = 12+SG40;
+                    rcCell->currentRateIndex = 8;
+                }
+                else    //11na 20M
+                {
+                    for (i=0; i<11; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][0];
+                    }
+                    rcCell->operationRateCount = 11;
+                    rcCell->currentRateIndex = 7;
+                }
+            }
+        }
+        else if (type == 3) //11na one Tx stream
+        {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11na 40M one Tx stream
+                {
+                    if(SG40 != 0)
+                    {
+                        maxrate = 11;
+                    }
+                    else
+                    {
+                        maxrate = 10;
+                    }
+                    for (i=0; i<maxrate; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][1];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<9; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][0];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+        }
+        else //if (type == 1) //11a
+        {
+            for (i=0; i<8; i++) //6M 9M 12M 18M 24M 36M 48M 54M
+            {
+                rcCell->operationRateSet[i] = i+4;
+            }
+            rcCell->operationRateCount = 8;
+            rcCell->currentRateIndex = 4;  //24M
+        }
+    }
+
+    rcCell->flag = 0;
+    rcCell->txCount = 0;
+    rcCell->failCount = 0;
+    rcCell->currentRate = rcCell->operationRateSet[rcCell->currentRateIndex];
+    rcCell->lasttxCount = 0;
+    rcCell->lastTime    = wd->tick;
+    rcCell->probingTime = wd->tick;
+    for (i=0; i<ZM_RATE_TABLE_SIZE; i++) {
+        wd->PER[i] = 0;
+        wd->txMPDU[i] = wd->txFail[i] = 0;
+    }
+    wd->probeCount = 0;
+    wd->probeInterval = 0;
+#ifdef ZM_ENABLE_BA_RATECTRL
+    for (i=0; i<29; i++) {
+        TxMPDU[i]=0;
+        BAFail[i]=0;
+        BAPER[i]=0;
+    }
+#endif
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlGetHigherRate     */
+/*      Get a higher rate.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlGetHigherRate(struct zsRcCell* rcCell)
+{
+    u8_t rateIndex;
+
+    rateIndex = rcCell->currentRateIndex
+            + (((rcCell->currentRateIndex+1) < rcCell->operationRateCount)?1:0);
+    return rcCell->operationRateSet[rateIndex];
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlNextLowerRate     */
+/*      Get a lower rate.                                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlNextLowerRate(zdev_t* dev, struct zsRcCell* rcCell)
+{
+    zmw_get_wlan_dev(dev);
+    if (rcCell->currentRateIndex > 0)
+    {
+        rcCell->currentRateIndex--;
+        rcCell->currentRate = rcCell->operationRateSet[rcCell->currentRateIndex];
+    }
+    zm_msg1_tx(ZM_LV_0, "Lower Tx Rate=", rcCell->currentRate);
+    //DbgPrint("Lower Tx Rate=%d", rcCell->currentRate);
+    rcCell->failCount = rcCell->txCount = 0;
+    rcCell->lasttxCount = 0;
+    rcCell->lastTime  = wd->tick;
+    return rcCell->currentRate;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlRateDiff          */
+/*      Rate difference.                                                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      retryRate : retry rate                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate difference                                                 */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlRateDiff(struct zsRcCell* rcCell, u8_t retryRate)
+{
+    u16_t i;
+
+    /* Find retryRate in operationRateSet[] */
+    for (i=0; i<rcCell->operationRateCount; i++)
+    {
+        if (retryRate == rcCell->operationRateSet[i])
+        {
+            if (i < rcCell->currentRateIndex)
+            {
+                return ((rcCell->currentRateIndex - i)+1)>>1;
+            }
+            else if (i == rcCell->currentRateIndex == 0)
+            {
+                return 1;
+            }
+            else
+            {
+                return 0;
+            }
+        }
+    }
+    /* TODO : retry rate not in operation rate set */
+    zm_msg1_tx(ZM_LV_0, "Not in operation rate set:", retryRate);
+    return 1;
+
+}
+
+u32_t zfRateCtrlUDPTP(zdev_t* dev, u16_t Rate, u32_t PER) {
+    if ((PER < 100) && (Rate > 0) && PER)
+        return 1168000/(((12304/Rate)+197)*(100+100*PER/(100-PER)));
+    else
+        return 0;
+}
+
+u8_t zfRateCtrlFindMaxUDPTP(zdev_t* dev, struct zsRcCell* rcCell) {
+    u8_t i, maxIndex=0, rateIndex;
+    u32_t max=0, UDPThroughput;
+
+    zmw_get_wlan_dev(dev);
+
+    rateIndex = zm_agg_min(rcCell->currentRateIndex+3, rcCell->operationRateCount-1);
+    for (i=rcCell->currentRateIndex; i < rateIndex; i++) {
+        UDPThroughput = zfRateCtrlUDPTP(dev, zcRate[rcCell->operationRateSet[i]],
+            wd->PER[rcCell->operationRateSet[i]]);
+        if (max < UDPThroughput) {
+            max = UDPThroughput;
+            maxIndex = i;
+        }
+    }
+
+    return rcCell->operationRateSet[maxIndex];
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlGetTxRate         */
+/*      Get transmission rate.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      rcCell : rate control cell                                      */
+/*      probing : rate probing flag                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx rate                                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfRateCtrlGetTxRate(zdev_t* dev, struct zsRcCell* rcCell, u16_t* probing)
+{
+    u8_t newRate, highRate;
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_tx(ZM_LV_3, "txCount=", rcCell->txCount);
+    zm_msg1_tx(ZM_LV_3, "probingTime=", rcCell->probingTime);
+    zm_msg1_tx(ZM_LV_3, "tick=", wd->tick);
+    *probing = 0;
+    newRate = rcCell->currentRate;
+
+    if (wd->probeCount && (wd->probeCount < wd->success_probing))
+    {
+        if (wd->probeInterval < 50)
+        {
+            wd->probeInterval++;
+        }
+        else
+        {
+            wd->probeInterval++;
+            if (wd->probeInterval > 52) //probe 51, 52, 53 three packets every 50 packets
+            {
+                wd->probeInterval = 0;
+            }
+            newRate=zfRateCtrlGetHigherRate(rcCell);
+            *probing = 1;
+            wd->probeCount++;
+            rcCell->probingTime = wd->tick;
+        }
+    }
+    /* Accumulate at least 1000ms and 8 packets or Accumulate over 1K packets */
+    else if ((((wd->tick - rcCell->probingTime) > (ZM_RATE_CTRL_PROBING_INTERVAL_MS/ZM_MS_PER_TICK))
+                && (rcCell->txCount >= ZM_RATE_CTRL_MIN_PROBING_PACKET))
+        || (rcCell->txCount >= 1000))
+    {
+#ifndef ZM_DISABLE_RATE_CTRL
+        /* PER = fail/total */
+        wd->probeCount = 0;
+        wd->probeSuccessCount = 0;
+        if (wd->txMPDU[rcCell->currentRate] != 0) {
+            wd->PER[rcCell->currentRate] = zm_agg_min(100,
+                (wd->txFail[rcCell->currentRate]*100)/wd->txMPDU[rcCell->currentRate]);
+            if (!wd->PER[rcCell->currentRate]) wd->PER[rcCell->currentRate] ++;
+        }
+
+        /* if PER < threshold, do rate probing, return probing rate */
+        if ((wd->PER[rcCell->currentRate] <= (ZM_RATE_PROBING_THRESHOLD+15)) ||
+            ((rcCell->currentRate <= 16) &&
+            ((wd->PER[rcCell->currentRate]/2) <= ZM_RATE_PROBING_THRESHOLD)))
+        {
+            if ((newRate=zfRateCtrlGetHigherRate(rcCell)) != rcCell->currentRate)
+            {
+                *probing = 1;
+                wd->probeCount++;
+                wd->probeInterval = 0;
+                wd->success_probing =
+                    (rcCell->currentRate <= 16)? (ZM_RATE_SUCCESS_PROBING/2) : ZM_RATE_SUCCESS_PROBING;
+                //DbgPrint("Start Probing");
+                zm_msg1_tx(ZM_LV_0, "Probing Rate=", newRate);
+            }
+        }
+#endif
+
+        zm_msg0_tx(ZM_LV_1, "Diminish counter");
+        rcCell->failCount = rcCell->failCount>>1;
+        rcCell->txCount = rcCell->txCount>>1;
+        wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+        wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+
+        if (rcCell->currentRate > 15) {
+            highRate = zfRateCtrlGetHigherRate(rcCell);
+            if ((highRate != rcCell->currentRate) && wd->PER[highRate] &&
+                ((wd->PER[rcCell->currentRate] + FailDiff[rcCell->currentRate]) >
+                wd->PER[highRate])) {
+                //DbgPrint("PER compare force raise rate to %d", highRate);
+                wd->probeSuccessCount = wd->probeCount = ZM_RATE_SUCCESS_PROBING;
+                zfRateCtrlTxSuccessEvent(dev, rcCell, highRate);
+            }
+        }
+        else {
+            highRate = zfRateCtrlFindMaxUDPTP(dev, rcCell);
+            if (rcCell->currentRate < highRate) {
+                //DbgPrint("UDP Throughput compare force raise rate to %d", highRate);
+                wd->probeSuccessCount = wd->probeCount = ZM_RATE_SUCCESS_PROBING;
+                zfRateCtrlTxSuccessEvent(dev, rcCell, highRate);
+            }
+        }
+        rcCell->probingTime = wd->tick;
+    }
+
+    if( (wd->tick > 1000)
+        && ((wd->tick - rcCell->lastTime) > 3840) )
+    {
+        if (rcCell->lasttxCount < 70)
+        {
+            rcCell->failCount = rcCell->failCount>>1;
+            rcCell->txCount = rcCell->txCount>>1;
+            wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+            wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+            rcCell->failCount = (rcCell->failCount < rcCell->txCount)?
+                                rcCell->failCount : rcCell->txCount;
+            wd->txFail[rcCell->currentRate] = (wd->txFail[rcCell->currentRate] < wd->txMPDU[rcCell->currentRate])?
+                                              wd->txFail[rcCell->currentRate] : wd->txMPDU[rcCell->currentRate];
+        }
+
+        rcCell->lastTime    = wd->tick;
+        rcCell->lasttxCount = 0;
+    }
+
+    rcCell->txCount++;
+    rcCell->lasttxCount++;
+    wd->txMPDU[rcCell->currentRate]++;
+    zm_msg1_tx(ZM_LV_1, "Get Tx Rate=", newRate);
+    return newRate;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlTxFailEvent       */
+/*      Tx fail event. Calculate PER and lower Tx rate if under         */
+/*      PER under threshold.                                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      retryRate : retry rate                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlTxFailEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t aggRate, u32_t retryRate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+#ifndef ZM_DISABLE_RATE_CTRL
+    //DbgPrint("aggRate=%d, retryRate=%d", aggRate, retryRate);
+    if (aggRate && (aggRate != rcCell->currentRate)) {
+        wd->txFail[aggRate] += retryRate;
+        return;
+    }
+
+    if (!aggRate) {
+        retryRate = (zfRateCtrlRateDiff(rcCell, (u8_t)retryRate)+1)>>1;
+        if (rcCell->currentRate <12) //legacy rate
+        {
+            retryRate*=2;
+        }
+    }
+    rcCell->failCount += retryRate;
+    wd->txFail[rcCell->currentRate] += retryRate;
+
+    //DbgPrint("failCount=%d", rcCell->failCount);
+    if (rcCell->failCount > ZM_MIN_RATE_FAIL_COUNT)
+    {
+        if (wd->txMPDU[rcCell->currentRate] != 0) {
+            wd->PER[rcCell->currentRate] = zm_agg_min(100,
+                (wd->txFail[rcCell->currentRate]*100)/wd->txMPDU[rcCell->currentRate]);
+            if (!wd->PER[rcCell->currentRate]) wd->PER[rcCell->currentRate] ++;
+        }
+        //zm_msg1_tx(ZM_LV_1, "PER=", per);
+        //DbgPrint("PER=%d, txFail=%d, txMPDU=%d", wd->PER[rcCell->currentRate], wd->txFail[rcCell->currentRate], wd->txMPDU[rcCell->currentRate]);
+        if (wd->PER[rcCell->currentRate] > PERThreshold[rcCell->currentRate])
+        {
+            /* Lower Tx Rate if PER < THRESHOLD */
+            zfRateCtrlNextLowerRate(dev, rcCell);
+            rcCell->flag |= ZM_RC_TRAINED_BIT;
+
+            // Resolve compatibility problem with Marvell
+            if(rcCell->currentRate == 15)
+            {
+                zmw_leave_critical_section(dev);
+                zfHpSetAggPktNum(dev, 8);
+                zmw_enter_critical_section(dev);
+            }
+
+            wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+            wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+            wd->probeCount = wd->probeSuccessCount = 0;
+        }
+    }
+
+#endif
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlTxSuccessEvent    */
+/*      Tx success event. Raise Tx rate because rate probing success.   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      successRate : success rate                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlTxSuccessEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t successRate)
+{
+    /* Raise Tx Rate */
+    u16_t i, PERProbe;
+    u16_t pcount;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //DbgPrint("Probing successRate=%d", successRate);
+    /* Find successRate in operationRateSet[] */
+    wd->probeSuccessCount++;
+    if (wd->probeCount < wd->success_probing)
+    {
+        return;
+    }
+
+    pcount = wd->probeCount;
+    if (pcount != 0)
+    {
+        PERProbe = wd->probeSuccessCount * 100 / pcount;
+    }
+    else
+    {
+        PERProbe = 1;
+    }
+
+    if (PERProbe < ((rcCell->currentRate < 16)? 80:100))
+    {
+        return;
+    }
+    //DbgPrint("wd->probeCount=%d, wd->probeSuccessCount=%d", wd->probeCount, wd->probeSuccessCount);
+    wd->probeCount = wd->probeSuccessCount = 0;
+    for (i=0; i<rcCell->operationRateCount; i++)
+    {
+        if (successRate == rcCell->operationRateSet[i])
+        {
+            if (i > rcCell->currentRateIndex)
+            {
+                /* Raise current Tx rate */
+                zm_msg1_tx(ZM_LV_0, "Raise Tx Rate=", successRate);
+                //DbgPrint("Raise Tx Rate=%d", successRate);
+
+                // Resolve compatibility problem with Marvell
+                if((rcCell->currentRate <= 15) && (successRate > 15))
+                {
+                    zmw_leave_critical_section(dev);
+                    zfHpSetAggPktNum(dev, 16);
+                    zmw_enter_critical_section(dev);
+                }
+
+                rcCell->currentRate = successRate;
+                rcCell->currentRateIndex = (u8_t)i;
+                rcCell->failCount = rcCell->txCount = 0;
+                rcCell->lasttxCount = 0;
+                rcCell->lastTime  = wd->tick;
+                wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+                wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+            }
+        }
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlRxRssiEvent       */
+/*      Rx RSSI event. Calculate RSSI moving average, accelarate        */
+/*      rate probing if RSSI variation over threshold.                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      successRate : success rate                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlRxRssiEvent(struct zsRcCell* rcCell, u16_t rxRssi)
+{
+    /* if delta(rcCell->rxRssi, rxRssi) > ZM_RATE_CTRL_RSSI_VARIATION */
+    if ((rcCell->rxRssi - rxRssi) > ZM_RATE_CTRL_RSSI_VARIATION)
+    {
+        /* Accelerate rate probing via decreaing rcCell->probingTime */
+        rcCell->probingTime -= ZM_RATE_CTRL_PROBING_INTERVAL_MS/ZM_MS_PER_TICK;
+    }
+
+    /* Update RSSI moving average */
+    rcCell->rxRssi = (((rcCell->rxRssi*7) + rxRssi)+4) >> 3;
+    return;
+}
+
+
+#ifdef ZM_ENABLE_BA_RATECTRL
+u8_t HigherRate(u8_t Rate) {
+    if (Rate < 28) Rate++; //28=MCS15SG, 27=MCS15, 26=MCS14, 25=MCS13
+    if (Rate > 28) Rate = 28;
+    while ((Rate >= 20) && (Rate <= 23)) {
+        Rate ++;
+    }
+    return Rate;
+}
+
+u8_t LowerRate(u8_t Rate) {
+    if (Rate > 1) Rate--;
+    while ((Rate >= 20) && (Rate <= 23)) {
+        Rate --;
+    }
+    return Rate;
+}
+
+u8_t RateMapToRateIndex(u8_t Rate, struct zsRcCell* rcCell) {
+    u8_t i;
+    for (i=0; i<rcCell->operationRateCount; i++) {
+        if (Rate == rcCell->operationRateSet[i]) {
+            return i;
+        }
+    }
+    return 0;
+}
+
+void zfRateCtrlAggrSta(zdev_t* dev) {
+    u8_t RateIndex, Rate;
+    u8_t HRate;
+    u8_t LRate;
+    u32_t RateCtrlTxMPDU, RateCtrlBAFail;
+    zmw_get_wlan_dev(dev);
+
+    RateIndex = wd->sta.oppositeInfo[0].rcCell.currentRateIndex;
+    Rate = wd->sta.oppositeInfo[0].rcCell.operationRateSet[RateIndex];
+
+    TxMPDU[Rate] = (TxMPDU[Rate] / 5) + (wd->commTally.RateCtrlTxMPDU * 4 / 5);
+    BAFail[Rate] = (BAFail[Rate] / 5) + (wd->commTally.RateCtrlBAFail * 4 / 5);
+    RateCtrlTxMPDU = wd->commTally.RateCtrlTxMPDU;
+    RateCtrlBAFail = wd->commTally.RateCtrlBAFail;
+    wd->commTally.RateCtrlTxMPDU = 0;
+    wd->commTally.RateCtrlBAFail = 0;
+    if (TxMPDU[Rate] > 0) {
+        BAPER[Rate] = BAFail[Rate] * 1000 / TxMPDU[Rate]; //PER*1000
+        BAPER[Rate] = (BAPER[Rate]>0)? BAPER[Rate]:1;
+    }
+    else {
+        return;
+    }
+
+    HRate = HigherRate(Rate);
+    LRate = LowerRate(Rate);
+    if (BAPER[Rate]>200) {
+        if ((RateCtrlTxMPDU > 100) && (BAPER[Rate]<300) && (HRate != Rate) && BAPER[HRate] &&
+            (BAPER[HRate] < BAPER[Rate] + BADiff[Rate])) {
+            Rate = HRate;
+            //DbgPrint("Rate improved to %d", Rate);
+        }
+        else {
+            Rate = LRate;
+            //DbgPrint("Rate decreased to %d", Rate);
+        }
+    }
+    else if (BAPER[Rate] && BAPER[Rate]<100) {
+        if (RateCtrlTxMPDU > 100) {
+            Rate = HRate;
+            //DbgPrint("Rate improved to %d", Rate);
+        }
+    }
+    wd->sta.oppositeInfo[0].rcCell.currentRate = Rate;
+    wd->sta.oppositeInfo[0].rcCell.currentRateIndex = RateMapToRateIndex(Rate, &wd->sta.oppositeInfo[0].rcCell);
+}
+#endif
diff --git a/drivers/staging/otus/80211core/ratectrl.h b/drivers/staging/otus/80211core/ratectrl.h
new file mode 100644
index 000000000000..92411d725cd8
--- /dev/null
+++ b/drivers/staging/otus/80211core/ratectrl.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _RATECTRL_H
+#define _RATECTRL_H
+
+#define ZM_RATE_CTRL_PROBING_INTERVAL_MS    1000 //1000ms
+#define ZM_RATE_CTRL_MIN_PROBING_PACKET     8
+
+#define ZM_MIN_RATE_FAIL_COUNT              20
+
+#define ZM_RATE_PROBING_THRESHOLD           15  //6%
+#define ZM_RATE_SUCCESS_PROBING             10
+
+#define ZM_RATE_CTRL_RSSI_VARIATION         5  //TBD
+
+extern const u32_t zcRateToPhyCtrl[];
+
+extern void zfRateCtrlInitCell(zdev_t* dev, struct zsRcCell* rcCell, u8_t type, u8_t gBand, u8_t SG40);
+extern u16_t zfRateCtrlGetTxRate(zdev_t* dev, struct zsRcCell* rcCell, u16_t* probing);
+extern void zfRateCtrlTxFailEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t aggRate, u32_t retryRate);
+extern void zfRateCtrlTxSuccessEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t successRate);
+extern void zfRateCtrlAggrSta(zdev_t* dev);
+#endif
diff --git a/drivers/staging/otus/80211core/struct.h b/drivers/staging/otus/80211core/struct.h
new file mode 100644
index 000000000000..17b5ce37ebb5
--- /dev/null
+++ b/drivers/staging/otus/80211core/struct.h
@@ -0,0 +1,1315 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _STRUCT_H
+#define _STRUCT_H
+
+#include "../oal_marc.h"
+
+#define ZM_SW_LOOP_BACK                     0 /* 1=>enable, 0=>disable */
+#define ZM_PCI_LOOP_BACK                    0 /* 1=>enable, 0=>disable */
+#define ZM_PROTOCOL_RESPONSE_SIMULATION     0
+
+#define ZM_RX_FRAME_SIZE               1600
+
+extern const u8_t zg11bRateTbl[4];
+extern const u8_t zg11gRateTbl[8];
+
+#define ZM_DRIVER_CORE_MAJOR_VERSION        1
+#define ZM_DRIVER_CORE_MINOR_VERSION        1
+#define ZM_DRIVER_CORE_BRANCH_MAJOR_VERSION 3
+#define ZM_DRIVER_CORE_BRANCH_MINOR_VERSION 39
+
+#ifndef ZM_VTXQ_SIZE
+#define ZM_VTXQ_SIZE                        1024 //2^N
+#endif
+
+#define ZM_VTXQ_SIZE_MASK                   (ZM_VTXQ_SIZE-1)
+#define ZM_VMMQ_SIZE                        8 //2^N
+#define ZM_VMMQ_SIZE_MASK                   (ZM_VMMQ_SIZE-1)
+
+#include "cagg.h"
+
+#define ZM_AGG_POOL_SIZE                    20
+#define ZM_RATE_TABLE_SIZE                  32
+
+#define ZM_MAX_BUF_DISCRETE_NUMBER          5
+
+
+
+
+
+
+
+
+
+/**********************************************************************************/
+/* IBSS macros                                                     */
+/**********************************************************************************/
+#define ZM_IBSS_PEER_ALIVE_COUNTER     4
+
+/**********************************************************************************/
+/* BIT mapping related macros                                                     */
+/**********************************************************************************/
+
+#define ZM_BIT_0       0x1
+#define ZM_BIT_1       0x2
+#define ZM_BIT_2       0x4
+#define ZM_BIT_3       0x8
+#define ZM_BIT_4       0x10
+#define ZM_BIT_5       0x20
+#define ZM_BIT_6       0x40
+#define ZM_BIT_7       0x80
+#define ZM_BIT_8       0x100
+#define ZM_BIT_9       0x200
+#define ZM_BIT_10      0x400
+#define ZM_BIT_11      0x800
+#define ZM_BIT_12      0x1000
+#define ZM_BIT_13      0x2000
+#define ZM_BIT_14      0x4000
+#define ZM_BIT_15      0x8000
+#define ZM_BIT_16      0x10000
+#define ZM_BIT_17      0x20000
+#define ZM_BIT_18      0x40000
+#define ZM_BIT_19      0x80000
+#define ZM_BIT_20      0x100000
+#define ZM_BIT_21      0x200000
+#define ZM_BIT_22      0x400000
+#define ZM_BIT_23      0x800000
+#define ZM_BIT_24      0x1000000
+#define ZM_BIT_25      0x2000000
+#define ZM_BIT_26      0x4000000
+#define ZM_BIT_27      0x8000000
+#define ZM_BIT_28      0x10000000
+#define ZM_BIT_29      0x20000000   //WPA support
+#define ZM_BIT_30      0x40000000
+#define ZM_BIT_31      0x80000000
+
+
+/**********************************************************************************/
+/* MAC address related macros                                                     */
+/**********************************************************************************/
+#define ZM_MAC_BYTE_TO_WORD(macb, macw)   macw[0] = macb[0] + (macb[1] << 8); \
+                                          macw[1] = macb[2] + (macb[3] << 8); \
+                                          macw[2] = macb[4] + (macb[5] << 8);
+
+#define ZM_MAC_WORD_TO_BYTE(macw, macb)   macb[0] = (u8_t) (macw[0] & 0xff); \
+                                          macb[1] = (u8_t) (macw[0] >> 8);   \
+                                          macb[2] = (u8_t) (macw[1] & 0xff); \
+                                          macb[3] = (u8_t) (macw[1] >> 8);   \
+                                          macb[4] = (u8_t) (macw[2] & 0xff); \
+                                          macb[5] = (u8_t) (macw[2] >> 8);
+
+#define ZM_MAC_0(macw)   ((u8_t)(macw[0] & 0xff))
+#define ZM_MAC_1(macw)   ((u8_t)(macw[0] >> 8))
+#define ZM_MAC_2(macw)   ((u8_t)(macw[1] & 0xff))
+#define ZM_MAC_3(macw)   ((u8_t)(macw[1] >> 8))
+#define ZM_MAC_4(macw)   ((u8_t)(macw[2] & 0xff))
+#define ZM_MAC_5(macw)   ((u8_t)(macw[2] >> 8))
+
+#define ZM_IS_MULTICAST_OR_BROADCAST(mac) (mac[0] & 0x01)
+#define ZM_IS_MULTICAST(mac) ((mac[0] & 0x01) && (((u8_t)mac[0]) != 0xFF))
+
+#define ZM_MAC_EQUAL(mac1, mac2)   ((mac1[0]==mac2[0])&&(mac1[1]==mac2[1])&&(mac1[2]==mac2[2]))
+#define ZM_MAC_NOT_EQUAL(mac1, mac2)   ((mac1[0]!=mac2[0])||(mac1[1]!=mac2[1])||(mac1[2]!=mac2[2]))
+/**********************************************************************************/
+/* MAC address related mac'ros (end)                                               */
+/**********************************************************************************/
+#define ZM_BYTE_TO_WORD(A, B)   ((A<<8)+B)
+#define ZM_ROL32( A, n ) \
+        ( ((A) << (n)) | ( ((A)>>(32-(n)))  & ( (1UL << (n)) - 1 ) ) )
+#define ZM_ROR32( A, n ) ZM_ROL32( (A), 32-(n) )
+#define ZM_LO8(v16)  ((u8_t)((v16) & 0xFF))
+#define ZM_HI8(v16)  ((u8_t)(((v16)>>8)&0xFF))
+
+#ifdef ZM_ENABLE_BUFFER_TRACE
+extern void zfwBufTrace(zdev_t* dev, zbuf_t *buf, u8_t *functionName);
+#define ZM_BUFFER_TRACE(dev, buf)       zfwBufTrace(dev, buf, __func__);
+#else
+#define ZM_BUFFER_TRACE(dev, buf)
+#endif
+
+/* notification events to heart beat function */
+#define ZM_BSSID_LIST_SCAN         0x01
+
+/* CAM mode */
+#define ZM_CAM_AP                       0x1
+#define ZM_CAM_STA                      0x2
+#define ZM_CAM_HOST                     0x4
+
+/* finite state machine for adapter */
+#define ZM_STA_STATE_DISCONNECT           1
+#define ZM_STA_STATE_CONNECTING           2
+#define ZM_STA_STATE_CONNECTED            3
+
+/* Event definitions for  finite state machine */
+#define ZM_EVENT_TIMEOUT_SCAN             0x0000
+#define ZM_EVENT_TIMEOUT_BG_SCAN          0x0001
+#define ZN_EVENT_TIMEOUT_RECONNECT        0x0002
+#define ZM_EVENT_TIMEOUT_INIT_SCAN        0x0003
+#define ZM_EVENT_TIMEOUT_AUTH             0x0004
+#define ZM_EVENT_TIMEOUT_ASSO             0x0005
+#define ZM_EVENT_TIMEOUT_AUTO_SCAN        0x0006
+#define ZM_EVENT_TIMEOUT_MIC_FAIL         0x0007
+#define ZM_EVENT_TIMEOUT_CHECK_AP         0x0008
+#define ZM_EVENT_CONNECT                  0x0009
+#define ZM_EVENT_INIT_SCAN                0x000a
+#define ZM_EVENT_SCAN                     0x000b
+#define ZM_EVENT_BG_SCAN                  0x000c
+#define ZM_EVENT_DISCONNECT               0x000d
+#define ZM_EVENT_WPA_MIC_FAIL             0x000e
+#define ZM_EVENT_AP_ALIVE                 0x000f
+#define ZM_EVENT_CHANGE_TO_AP             0x0010
+#define ZM_EVENT_CHANGE_TO_STA            0x0011
+#define ZM_EVENT_IDLE                     0x0012
+#define ZM_EVENT_AUTH                     0x0013
+#define ZM_EVENT_ASSO_RSP                 0x0014
+#define ZM_EVENT_WPA_PK_OK                0x0015
+#define ZM_EVENT_WPA_GK_OK                0x0016
+#define ZM_EVENT_RCV_BEACON               0x0017
+#define ZM_EVENT_RCV_PROBE_RSP            0x0018
+#define ZM_EVENT_SEND_DATA                0x0019
+#define ZM_EVENT_AUTO_SCAN                0x001a
+#define ZM_EVENT_MIC_FAIL1                0x001d
+#define ZM_EVENT_MIC_FAIL2                0x001e
+#define ZM_EVENT_IBSS_MONITOR             0x001f
+#define ZM_EVENT_IN_SCAN                  0x0020
+#define ZM_EVENT_CM_TIMER                 0x0021
+#define ZM_EVENT_CM_DISCONNECT            0x0022
+#define ZM_EVENT_CM_BLOCK_TIMER           0x0023
+#define ZM_EVENT_TIMEOUT_ADDBA            0x0024
+#define ZM_EVENT_TIMEOUT_PERFORMANCE      0x0025
+#define ZM_EVENT_SKIP_COUNTERMEASURE	  0x0026
+#define ZM_EVENT_NONE                     0xffff
+
+/* Actions after call finite state machine */
+#define ZM_ACTION_NONE                    0x0000
+#define ZM_ACTION_QUEUE_DATA              0x0001
+#define ZM_ACTION_DROP_DATA               0x0002
+
+/* Timers for finite state machine */
+#define ZM_TICK_ZERO                      0
+#define ZM_TICK_INIT_SCAN_END             8
+#define ZM_TICK_NEXT_BG_SCAN              50
+#define ZM_TICK_BG_SCAN_END               8
+#define ZM_TICK_AUTH_TIMEOUT              4
+#define ZM_TICK_ASSO_TIMEOUT              4
+#define ZM_TICK_AUTO_SCAN                 300
+#define ZM_TICK_MIC_FAIL_TIMEOUT          6000
+#define ZM_TICK_CHECK_AP1                 150
+#define ZM_TICK_CHECK_AP2                 350
+#define ZM_TICK_CHECK_AP3                 250
+#define ZM_TICK_IBSS_MONITOR              160
+#define ZM_TICK_IN_SCAN                   4
+#define ZM_TICK_CM_TIMEOUT                6000
+#define ZM_TICK_CM_DISCONNECT             200
+#define ZM_TICK_CM_BLOCK_TIMEOUT          6000
+
+/* Fix bug#33338 Counter Measure Issur */
+#ifdef NDIS_CM_FOR_XP
+#define ZM_TICK_CM_TIMEOUT_OFFSET        2160
+#define ZM_TICK_CM_DISCONNECT_OFFSET     72
+#define ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET  2160
+#else
+#define ZM_TICK_CM_TIMEOUT_OFFSET        0
+#define ZM_TICK_CM_DISCONNECT_OFFSET     0
+#define ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET  0
+#endif
+
+#define ZM_TIME_ACTIVE_SCAN               30 //ms
+#define ZM_TIME_PASSIVE_SCAN              110 //ms
+
+/* finite state machine for BSS connect */
+#define ZM_STA_CONN_STATE_NONE            0
+#define ZM_STA_CONN_STATE_AUTH_OPEN       1
+#define ZM_STA_CONN_STATE_AUTH_SHARE_1    2
+#define ZM_STA_CONN_STATE_AUTH_SHARE_2    3
+#define ZM_STA_CONN_STATE_ASSOCIATE       4
+#define ZM_STA_CONN_STATE_SSID_NOT_FOUND  5
+#define ZM_STA_CONN_STATE_AUTH_COMPLETED  6
+
+/* finite state machine for WPA handshaking */
+#define ZM_STA_WPA_STATE_INIT             0
+#define ZM_STA_WPA_STATE_PK_OK            1
+#define ZM_STA_WPA_STATE_GK_OK            2
+
+/* various timers */
+#define ZM_INTERVAL_CONNECT_TIMEOUT          20   /* 200 milisecond */
+
+/* IBSS definitions */
+#define ZM_IBSS_PARTNER_LOST                 0
+#define ZM_IBSS_PARTNER_ALIVE                1
+#define ZM_IBSS_PARTNER_CHECK                2
+
+#define ZM_BCMC_ARRAY_SIZE                  16 /* Must be 2^N */
+#define ZM_UNI_ARRAY_SIZE                   16 /* Must be 2^N */
+
+#define ZM_MAX_DEFRAG_ENTRIES               4  /* 2^N */
+#define ZM_DEFRAG_AGING_TIME_SEC            5  /* 5 seconds */
+
+#define ZM_MAX_WPAIE_SIZE                   128
+/* WEP related definitions */
+#define ZM_USER_KEY_DEFAULT                 64
+#define ZM_USER_KEY_PK                      0                /* Pairwise Key */
+#define ZM_USER_KEY_GK                      1                /* Group Key */
+/* AP WLAN Type */
+#define ZM_WLAN_TYPE_PURE_B                 2
+#define ZM_WLAN_TYPE_PURE_G                 1
+#define ZM_WLAN_TYPE_MIXED                  0
+
+/* HAL State */
+#define ZM_HAL_STATE_INIT                   0
+#define ZM_HAL_STATE_RUNNING                1
+
+/* AP Capability */
+#define ZM_All11N_AP                        0x01
+#define ZM_XR_AP                            0x02
+#define ZM_SuperG_AP                        0x04
+
+/* MPDU Density */
+#define ZM_MPDU_DENSITY_NONE                0
+#define ZM_MPDU_DENSITY_1_8US               1
+#define ZM_MPDU_DENSITY_1_4US               2
+#define ZM_MPDU_DENSITY_1_2US               3
+#define ZM_MPDU_DENSITY_1US                 4
+#define ZM_MPDU_DENSITY_2US                 5
+#define ZM_MPDU_DENSITY_4US                 6
+#define ZM_MPDU_DENSITY_8US                 7
+
+/* Software Encryption */
+#define ZM_SW_TKIP_ENCRY_EN                0x01
+#define ZM_SW_TKIP_DECRY_EN                0x02
+#define ZM_SW_WEP_ENCRY_EN                 0x04
+#define ZM_SW_WEP_DECRY_EN                 0x08
+
+/* Default Support Rate */
+#define ZM_DEFAULT_SUPPORT_RATE_ZERO       0x0
+#define ZM_DEFAULT_SUPPORT_RATE_DISCONNECT 0x1
+#define ZM_DEFAULT_SUPPORT_RATE_IBSS_B     0x2
+#define ZM_DEFAULT_SUPPORT_RATE_IBSS_AG    0x3
+
+/* security related definitions */
+struct zsTkipSeed
+{
+    u8_t   tk[32];     /* key */
+    u8_t   ta[6];
+    u16_t  ttak[5];
+    u16_t  ppk[6];
+    u16_t  iv16,iv16tmp;
+    u32_t  iv32,iv32tmp;
+};
+
+struct zsMicVar
+{
+    u32_t  k0, k1;        // Key
+    u32_t  left, right;   // Current state
+    u32_t  m;             // Message accumulator (single word)
+    u16_t  nBytes;        // # bytes in M
+};
+
+struct zsDefragEntry
+{
+    u8_t    fragCount;
+    u8_t    addr[6];
+    u16_t   seqNum;
+    zbuf_t* fragment[8];
+    u32_t   tick;
+};
+
+struct zsDefragList
+{
+    struct zsDefragEntry   defragEntry[ZM_MAX_DEFRAG_ENTRIES];
+    u8_t                   replaceNum;
+};
+
+#define ZM_MAX_OPPOSITE_COUNT      16
+#define ZM_MAX_TX_SAMPLES          15
+#define ZM_TX_RATE_DOWN_CRITERIA   80
+#define ZM_TX_RATE_UP_CRITERIA    200
+
+
+#define ZM_MAX_PROBE_HIDDEN_SSID_SIZE 2
+struct zsSsidList
+{
+    u8_t            ssid[32];
+    u8_t            ssidLen;
+};
+
+struct zsWrapperSetting
+{
+    u8_t            bDesiredBssid;
+    u8_t            desiredBssid[6];
+    u16_t           bssid[3];
+    u8_t            ssid[32];
+    u8_t            ssidLen;
+    u8_t            authMode;
+    u8_t            wepStatus;
+    u8_t            encryMode;
+    u8_t            wlanMode;
+    u16_t           frequency;
+    u16_t           beaconInterval;
+    u8_t            dtim;
+    u8_t            preambleType;
+    u16_t           atimWindow;
+
+    struct zsSsidList probingSsidList[ZM_MAX_PROBE_HIDDEN_SSID_SIZE];
+
+    u8_t            dropUnencryptedPkts;
+    u8_t            ibssJoinOnly;
+    u32_t           adhocMode;
+    u8_t            countryIsoName[4];
+    u16_t           autoSetFrequency;
+
+    /* AP */
+    u8_t            bRateBasic;
+    u8_t            gRateBasic;
+    u32_t           nRateBasic;
+    u8_t            bgMode;
+
+    /* Common */
+    u8_t            staWmeEnabled;
+    u8_t            staWmeQosInfo;
+    u8_t            apWmeEnabled;
+
+
+    /* rate information: added in the future */
+};
+
+struct zsWrapperFeatureCtrl
+{
+    u8_t           bIbssGMode;
+};
+
+#define  ZM_MAX_PS_STA            16
+#define  ZM_PS_QUEUE_SIZE         32
+
+struct zsStaPSEntity
+{
+    u8_t           bUsed;
+    u8_t           macAddr[6];
+    u8_t           bDataQueued;
+};
+
+struct zsStaPSList
+{
+    u8_t           count;
+    struct zsStaPSEntity    entity[ZM_MAX_PS_STA];
+};
+
+#define ZM_MAX_TIMER_COUNT   32
+
+/* double linked list */
+struct zsTimerEntry
+{
+    u16_t   event;
+    u32_t   timer;
+    struct zsTimerEntry *pre;
+    struct zsTimerEntry *next;
+};
+
+struct zsTimerList
+{
+    u8_t   freeCount;
+    struct zsTimerEntry list[ZM_MAX_TIMER_COUNT];
+    struct zsTimerEntry *head;
+    struct zsTimerEntry *tail;
+};
+
+/* Multicast list */
+#define ZM_MAX_MULTICAST_LIST_SIZE     64
+
+struct zsMulticastAddr
+{
+    u8_t addr[6];
+};
+
+struct zsMulticastList
+{
+    u8_t   size;
+    struct zsMulticastAddr macAddr[ZM_MAX_MULTICAST_LIST_SIZE];
+};
+
+enum ieee80211_cwm_mode {
+    CWM_MODE20,
+    CWM_MODE2040,
+    CWM_MODE40,
+    CWM_MODEMAX
+
+};
+
+enum ieee80211_cwm_extprotspacing {
+    CWM_EXTPROTSPACING20,
+    CWM_EXTPROTSPACING25,
+    CWM_EXTPROTSPACINGMAX
+};
+
+enum ieee80211_cwm_width {
+    CWM_WIDTH20,
+    CWM_WIDTH40
+};
+
+enum ieee80211_cwm_extprotmode {
+    CWM_EXTPROTNONE,  /* no protection */
+    CWM_EXTPROTCTSONLY,   /* CTS to self */
+    CWM_EXTPROTRTSCTS,    /* RTS-CTS */
+    CWM_EXTPROTMAX
+};
+
+struct ieee80211_cwm {
+
+    /* Configuration */
+    enum ieee80211_cwm_mode         cw_mode;            /* CWM mode */
+    u8_t                            cw_extoffset;       /* CWM Extension Channel Offset */
+    enum ieee80211_cwm_extprotmode  cw_extprotmode;     /* CWM Extension Channel Protection Mode */
+    enum ieee80211_cwm_extprotspacing cw_extprotspacing;/* CWM Extension Channel Protection Spacing */
+    u32_t                           cw_enable;          /* CWM State Machine Enabled */
+    u32_t                           cw_extbusythreshold;/* CWM Extension Channel Busy Threshold */
+
+    /* State */
+    enum ieee80211_cwm_width        cw_width;           /* CWM channel width */
+};
+
+
+/* AP : STA database structure */
+struct zsStaTable
+{
+    u32_t time;     /* tick time */
+    //u32_t phyCtrl;   /* Tx PHY CTRL */
+    u16_t addr[3];  /* STA MAC address */
+    u16_t state;    /* aut/asoc */
+    //u16_t retry;    /* Retry count */
+    struct zsRcCell rcCell;
+
+    u8_t valid;     /* Valid flag : 1=>valid */
+    u8_t psMode;    /* STA power saving mode */
+    u8_t staType;   /* 0=>11b, 1=>11g, 2=>11n */
+    u8_t qosType;   /* 0=>Legacy, 1=>WME */
+    u8_t qosInfo;   /* WME QoS info */
+    u8_t vap;       /* Virtual AP ID */
+    u8_t encryMode; /* Encryption type for this STA */
+    u8_t keyIdx;
+    struct zsMicVar     txMicKey;
+    struct zsMicVar     rxMicKey;
+    u16_t iv16;
+    u32_t iv32;
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u8_t cencKeyIdx;
+    u32_t txiv[4];
+    u32_t rxiv[4];
+#endif //ZM_ENABLE_CENC
+};
+
+struct zdStructWds
+{
+    u8_t    wdsBitmap;                      /* Set bit-N to 1 to enable WDS */
+    u8_t    encryMode[ZM_MAX_WDS_SUPPORT];  /* WDS encryption mode */
+    u16_t   macAddr[ZM_MAX_WDS_SUPPORT][3]; /* WDS neighbor MAC address */
+};
+
+    // htcapinfo 16bits
+#define HTCAP_AdvCodingCap          0x0001
+#define HTCAP_SupChannelWidthSet    0x0002
+#define HTCAP_DynamicSMPS           0x0004
+#define HTCAP_SMEnabled             0x000C
+#define HTCAP_GreenField            0x0010
+#define HTCAP_ShortGIfor20MHz       0x0020
+#define HTCAP_ShortGIfor40MHz       0x0040
+#define HTCAP_TxSTBC                0x0080
+#define HTCAP_RxOneStream           0x0100
+#define HTCAP_RxTwoStream           0x0200
+#define HTCAP_RxThreeStream         0x0300
+#define HTCAP_DelayedBlockACK       0x0400
+#define HTCAP_MaxAMSDULength        0x0800
+#define HTCAP_DSSSandCCKin40MHz     0x1000
+#define HTCAP_PSMPSup               0x2000
+#define HTCAP_STBCControlFrameSup   0x4000
+#define HTCAP_LSIGTXOPProtectionSUP 0x8000
+    // Ampdu HT Parameter Info 8bits
+#define HTCAP_MaxRxAMPDU0           0x00
+#define HTCAP_MaxRxAMPDU1           0x01
+#define HTCAP_MaxRxAMPDU2           0x02
+#define HTCAP_MaxRxAMPDU3           0x03
+    // PCO 8bits
+#define HTCAP_PCO                   0x01
+#define HTCAP_TransmissionTime1     0x02
+#define HTCAP_TransmissionTime2     0x04
+#define HTCAP_TransmissionTime3     0x06
+    // MCS FeedBack 8bits
+#define HTCAP_PlusHTCSupport        0x04
+#define HTCAP_RDResponder           0x08
+    // TX Beamforming 0 8bits
+#define HTCAP_TxBFCapable           0x01
+#define HTCAP_RxStaggeredSoundCap   0x02
+#define HTCAP_TxStaggeredSoundCap   0x04
+#define HTCAP_RxZLFCapable          0x08
+#define HTCAP_TxZLFCapable          0x10
+#define HTCAP_ImplicitTxBFCapable   0x20
+    // Tx Beamforming 1 8bits
+#define HTCAP_ExplicitCSITxBFCap    0x01
+#define HTCAP_ExpUncompSteerMatrCap 0x02
+    // Antenna Selection Capabilities 8bits
+#define HTCAP_AntennaSelectionCap       0x01
+#define HTCAP_ExplicitCSITxASCap        0x02
+#define HTCAP_AntennaIndFeeTxASCap      0x04
+#define HTCAP_ExplicitCSIFeedbackCap    0x08
+#define HTCAP_AntennaIndFeedbackCap     0x10
+#define HTCAP_RxASCap                   0x20
+#define HTCAP_TxSoundPPDUsCap           0x40
+
+
+
+struct zsHTCapability
+{
+    u8_t ElementID;
+    u8_t Length;
+    // HT Capability Info
+    u16_t HtCapInfo;
+    u8_t AMPDUParam;
+    u8_t MCSSet[16];    //16 bytes
+    // Extended HT Capability Info
+    u8_t PCO;
+    u8_t MCSFeedBack;
+
+    u8_t TxBFCap[4];
+    u8_t AselCap;
+};
+
+union zuHTCapability
+{
+    struct zsHTCapability Data;
+    u8_t Byte[28];
+};
+
+    //channelinfo 8bits
+#define ExtHtCap_ExtChannelOffsetAbove  0x01
+#define ExtHtCap_ExtChannelOffsetBelow  0x03
+#define ExtHtCap_RecomTxWidthSet        0x04
+#define ExtHtCap_RIFSMode               0x08
+#define ExtHtCap_ControlAccessOnly      0x10
+    //operatinginfo 16bits
+#define ExtHtCap_NonGFDevicePresent     0x0004
+    //beaconinfo 16bits
+#define ExtHtCap_DualBeacon             0x0040
+#define ExtHtCap_DualSTBCProtection     0x0080
+#define ExtHtCap_SecondaryBeacon        0x0100
+#define ExtHtCap_LSIGTXOPProtectFullSup 0x0200
+#define ExtHtCap_PCOActive              0x0400
+#define ExtHtCap_PCOPhase               0x0800
+
+
+struct zsExtHTCapability
+{
+    u8_t    ElementID;
+    u8_t    Length;
+    u8_t    ControlChannel;
+    u8_t    ChannelInfo;
+    u16_t   OperatingInfo;
+    u16_t   BeaconInfo;
+    // Supported MCS Set
+    u8_t    MCSSet[16];
+};
+
+union zuExtHTCapability
+{
+    struct zsExtHTCapability Data;
+    u8_t Byte[24];
+};
+
+struct InformationElementSta {
+    struct zsHTCapability       HtCap;
+    struct zsExtHTCapability    HtInfo;
+};
+
+struct InformationElementAp {
+    struct zsHTCapability       HtCap;
+};
+
+#define ZM_MAX_FREQ_REQ_QUEUE  32
+typedef void (*zfpFreqChangeCompleteCb)(zdev_t* dev);
+
+struct zsWlanDevFreqControl
+{
+    u16_t                     freqReqQueue[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                     freqReqBw40[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                     freqReqExtOffset[ZM_MAX_FREQ_REQ_QUEUE];
+    zfpFreqChangeCompleteCb   freqChangeCompCb[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                      freqReqQueueHead;
+    u8_t                      freqReqQueueTail;
+};
+
+struct zsWlanDevAp
+{
+    u16_t   protectedObss;    /* protected overlap BSS */
+    u16_t   staAgingTimeSec;  /* in second, STA will be deathed if it does not */
+                              /* active for this long time                     */
+    u16_t   staProbingTimeSec;/* in second, STA will be probed if it does not  */
+                              /* active for this long time                     */
+    u8_t    authSharing;      /* authentication on going*/
+    u8_t    bStaAssociated;   /* 11b STA associated */
+    u8_t    gStaAssociated;   /* 11g STA associated */
+    u8_t    nStaAssociated;   /* 11n STA associated */
+    u16_t   protectionMode;   /* AP protection mode flag */
+    u16_t   staPowerSaving;   /* Set associated power saving STA count */
+
+
+
+    zbuf_t*  uniArray[ZM_UNI_ARRAY_SIZE]; /* array to store unicast frames */
+    u16_t   uniHead;
+    u16_t   uniTail;
+
+    /* HT Capability Info */
+    union zuHTCapability HTCap; //CWYang(+)
+
+    /* Extended HT Capability Info */
+    union zuExtHTCapability ExtHTCap; //CWYang(+)
+
+    /* STA table */
+    struct zsStaTable staTable[ZM_MAX_STA_SUPPORT];
+
+    /* WDS */
+    struct zdStructWds wds;
+    /* WPA */
+    u8_t wpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_WPAIE_SIZE];
+    u8_t wpaLen[ZM_MAX_AP_SUPPORT];
+    u8_t stawpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_WPAIE_SIZE];
+    u8_t stawpaLen[ZM_MAX_AP_SUPPORT];
+    u8_t wpaSupport[ZM_MAX_AP_SUPPORT];
+
+    //struct zsTkipSeed   bcSeed;
+    u8_t bcKeyIndex[ZM_MAX_AP_SUPPORT];
+    u8_t bcHalKeyIdx[ZM_MAX_AP_SUPPORT];
+    struct zsMicVar     bcMicKey[ZM_MAX_AP_SUPPORT];
+    u16_t iv16[ZM_MAX_AP_SUPPORT];
+    u32_t iv32[ZM_MAX_AP_SUPPORT];
+
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u32_t txiv[ZM_MAX_AP_SUPPORT][4];
+#endif //ZM_ENABLE_CENC
+
+    /* Virtual AP */
+    u8_t    beaconCounter;
+    u8_t    vapNumber;
+    u8_t    apBitmap;                         /* Set bit-N to 1 to enable VAP */
+    u8_t    hideSsid[ZM_MAX_AP_SUPPORT];
+    u8_t    authAlgo[ZM_MAX_AP_SUPPORT];
+    u8_t    ssid[ZM_MAX_AP_SUPPORT][32];      /* SSID */
+    u8_t    ssidLen[ZM_MAX_AP_SUPPORT];       /* SSID length */
+    u8_t    encryMode[ZM_MAX_AP_SUPPORT];
+    u8_t    wepStatus[ZM_MAX_AP_SUPPORT];
+    u16_t   capab[ZM_MAX_AP_SUPPORT];         /* Capability */
+    u8_t    timBcmcBit[ZM_MAX_AP_SUPPORT];    /* BMCM bit of TIM */
+    u8_t    wlanType[ZM_MAX_AP_SUPPORT];
+
+    /* Array to store BC or MC frames */
+    zbuf_t*  bcmcArray[ZM_MAX_AP_SUPPORT][ZM_BCMC_ARRAY_SIZE];
+    u16_t   bcmcHead[ZM_MAX_AP_SUPPORT];
+    u16_t   bcmcTail[ZM_MAX_AP_SUPPORT];
+
+    u8_t                    qosMode;                          /* 1=>WME */
+    u8_t                    uapsdEnabled;
+    struct zsQueue*         uapsdQ;
+
+    u8_t                    challengeText[128];
+
+    struct InformationElementAp ie[ZM_MAX_STA_SUPPORT];
+
+
+};
+
+#define ZM_MAX_BLOCKING_AP_LIST_SIZE    4 /* 2^N */
+struct zsBlockingAp
+{
+    u8_t addr[6];
+    u8_t weight;
+};
+
+#define ZM_SCAN_MGR_SCAN_NONE           0
+#define ZM_SCAN_MGR_SCAN_INTERNAL       1
+#define ZM_SCAN_MGR_SCAN_EXTERNAL       2
+
+struct zsWlanDevStaScanMgr
+{
+    u8_t                    scanReqs[2];
+    u8_t                    currScanType;
+    u8_t                    scanStartDelay;
+};
+
+#define ZM_PS_MSG_STATE_ACTIVE          0
+#define ZM_PS_MSG_STATE_SLEEP           1
+#define ZM_PS_MSG_STATE_T1              2
+#define ZM_PS_MSG_STATE_T2              3
+#define ZM_PS_MSG_STATE_S1              4
+
+#define ZM_PS_MAX_SLEEP_PERIODS         3       // The number of beacon periods
+
+struct zsWlanDevStaPSMgr
+{
+    u8_t                    state;
+    u8_t                    isSleepAllowed;
+    u8_t                    maxSleepPeriods;
+    u8_t                    ticks;
+    u32_t                   lastTxUnicastFrm;
+    u32_t                   lastTxMulticastFrm;
+    u32_t                   lastTxBroadcastFrm;
+    u8_t                    tempWakeUp; /*enable when wake up but still in ps mode */
+    u16_t                   sleepAllowedtick;
+};
+
+struct zsWlanDevSta
+{
+    u32_t                   beaconTxCnt;  /* Transmitted beacon counter (in IBSS) */
+    u8_t                    txBeaconInd;  /* In IBSS mode, true means that we just transmit a beacon during
+                                             last beacon period.
+                                           */
+    u16_t                   beaconCnt;    /* receive beacon count, will be perodically reset */
+    u16_t                   bssid[3];     /* BSSID of connected AP */
+    u8_t                    ssid[32];     /* SSID */
+    u8_t                    ssidLen;      /* SSID length */
+    u8_t                    mTxRate;      /* Tx rate for multicast */
+    u8_t                    uTxRate;      /* Tx rate for unicast */
+    u8_t                    mmTxRate;     /* Tx rate for management frame */
+    u8_t                    bChannelScan;
+    u8_t                    bScheduleScan;
+
+    u8_t                    InternalScanReq;
+    u16_t                   activescanTickPerChannel;
+    u16_t                   passiveScanTickPerChannel;
+    u16_t                   scanFrequency;
+    u32_t                   connPowerInHalfDbm;
+
+    u16_t                   currentFrequency;
+    u16_t                   currentBw40;
+    u16_t                   currentExtOffset;
+
+    u8_t                    bPassiveScan;
+
+    struct zsBlockingAp     blockingApList[ZM_MAX_BLOCKING_AP_LIST_SIZE];
+
+    //struct zsBssInfo        bssInfoPool[ZM_MAX_BSS];
+    struct zsBssInfo*       bssInfoArray[ZM_MAX_BSS];
+    struct zsBssList        bssList;
+    u8_t                    bssInfoArrayHead;
+    u8_t                    bssInfoArrayTail;
+    u8_t                    bssInfoFreeCount;
+
+    u8_t                    authMode;
+    u8_t                    currentAuthMode;
+    u8_t                    wepStatus;
+    u8_t                    encryMode;
+    u8_t                    keyId;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t                    ibssWpa2Psk;
+#endif
+#ifdef ZM_ENABLE_CENC
+    u8_t                    cencKeyId; //CENC
+#endif //ZM_ENABLE_CENC
+    u8_t                    dropUnencryptedPkts;
+    u8_t                    ibssJoinOnly;
+    u8_t                    adapterState;
+    u8_t                    oldAdapterState;
+    u8_t                    connectState;
+    u8_t                    connectRetry;
+    u8_t                    wpaState;
+    u8_t                    wpaIe[ZM_MAX_IE_SIZE + 2];
+    u8_t                    rsnIe[ZM_MAX_IE_SIZE + 2];
+    u8_t                    challengeText[255+2];
+    u8_t                    capability[2];
+    //u8_t                    connectingHiddenAP;
+    //u8_t                    scanWithSSID;
+    u16_t                   aid;
+    u32_t                   mgtFrameCount;
+    u8_t                    bProtectionMode;
+	u32_t					NonNAPcount;
+	u8_t					RTSInAGGMode;
+    u32_t                   connectTimer;
+    u16_t                   atimWindow;
+    u8_t                    desiredBssid[6];
+    u8_t                    bDesiredBssid;
+    struct zsTkipSeed       txSeed;
+    struct zsTkipSeed       rxSeed[4];
+    struct zsMicVar         txMicKey;
+    struct zsMicVar         rxMicKey[4];
+    u16_t                   iv16;
+    u32_t                   iv32;
+    struct zsOppositeInfo   oppositeInfo[ZM_MAX_OPPOSITE_COUNT];
+    u8_t                    oppositeCount;
+    u8_t                    bssNotFoundCount;     /* sitesurvey for search desired ISBB threshold */
+    u16_t                   rxBeaconCount;
+    u8_t                    beaconMissState;
+    u32_t                   rxBeaconTotal;
+    u8_t                    bIsSharedKey;
+    u8_t                    connectTimeoutCount;
+
+    u8_t                    recvAtim;
+
+    /* ScanMgr Control block */
+    struct zsWlanDevStaScanMgr scanMgr;
+    struct zsWlanDevStaPSMgr   psMgr;
+
+    // The callback would be called if receiving an unencrypted packets but
+    // the station is in encrypted mode. The wrapper could decide whether
+    // to drop the packet by its OS setting.
+    zfpStaRxSecurityCheckCb pStaRxSecurityCheckCb;
+
+    /* WME */
+    u8_t                    apWmeCapability; //bit-0 => a WME AP
+                                             //bit-7 => a UAPSD AP
+    u8_t                    wmeParameterSetCount;
+
+    u8_t                    wmeEnabled;
+    #define ZM_STA_WME_ENABLE_BIT       0x1
+    #define ZM_STA_UAPSD_ENABLE_BIT     0x2
+    u8_t                    wmeQosInfo;
+
+    u8_t                    wmeConnected;
+    u8_t                    qosInfo;
+    struct zsQueue*         uapsdQ;
+
+    /* countermeasures */
+    u8_t                    cmMicFailureCount;
+    u8_t                    cmDisallowSsidLength;
+    u8_t                    cmDisallowSsid[32];
+
+    /* power-saving mode */
+    u8_t                    powerSaveMode;
+    zbuf_t*                 staPSDataQueue[ZM_PS_QUEUE_SIZE];
+    u8_t                    staPSDataCount;
+
+    /* IBSS power-saving mode */
+    /* record the STA which has entered the PS mode */
+    struct zsStaPSList      staPSList;
+    /* queue the data of the PS STAs */
+    zbuf_t*                  ibssPSDataQueue[ZM_PS_QUEUE_SIZE];
+    u8_t                    ibssPSDataCount;
+    u8_t                    ibssPrevPSDataCount;
+    u8_t                    bIbssPSEnable;
+    /* BIT_15: ON/OFF, BIT_0~14: Atim Timer */
+    u16_t                   ibssAtimTimer;
+
+    /* WPA2 */
+    struct zsPmkidInfo      pmkidInfo;
+
+    /* Multicast list related objects */
+    struct zsMulticastList  multicastList;
+
+    /* XP packet filter feature : */
+    /* 1=>enable: All multicast address packets, not just the ones enumerated in the multicast address list. */
+    /* 0=>disable */
+    u8_t                    bAllMulticast;
+
+    /* reassociation flag */
+    u8_t                    connectByReasso;
+    u8_t                    failCntOfReasso;
+
+	/* for HT configure control setting */
+    u8_t                    preambleTypeHT;  /* HT: 0 Mixed mode    1 Green field */
+	u8_t                    htCtrlBandwidth;
+	u8_t                    htCtrlSTBC;
+	u8_t                    htCtrlSG;
+    u8_t                    defaultTA;
+
+    u8_t                    connection_11b;
+
+    u8_t                    EnableHT;
+    u8_t                    SG40;
+    u8_t                    HT2040;
+    /* for WPA setting */
+    u8_t                    wpaSupport;
+    u8_t                    wpaLen;
+
+    /* IBSS related objects */
+    u8_t                    ibssDelayedInd;
+    struct zsPartnerNotifyEvent ibssDelayedIndEvent;
+    u8_t                    ibssPartnerStatus;
+
+    u8_t                    bAutoReconnect;
+
+    u8_t                    flagFreqChanging;
+    u8_t                    flagKeyChanging;
+    struct zsBssInfo        ibssBssDesc;
+    u8_t                    ibssBssIsCreator;
+    u16_t                   ibssReceiveBeaconCount;
+    u8_t                    ibssSiteSurveyStatus;
+
+    u8_t                    disableProbingWithSsid;
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u8_t                    cencIe[ZM_MAX_IE_SIZE + 2];
+#endif //ZM_ENABLE_CENC
+    u32_t txiv[4];  //Tx PN Sequence
+    u32_t rxiv[4];  //Rx PN Sequence
+    u32_t rxivGK[4];//Broadcast Rx PN Sequence
+    u8_t  wepKey[4][32];    // For Software WEP
+	u8_t  SWEncryMode[4];
+
+    /* 802.11d */
+    u8_t b802_11D;
+
+    /* 802.11h */
+    u8_t TPCEnable;
+    u8_t DFSEnable;
+    u8_t DFSDisableTx;
+
+    /* Owl AP */
+    u8_t athOwlAp;
+
+    /* Enable BA response in driver */
+    u8_t enableDrvBA;
+
+    /* HT Capability Info */
+    union zuHTCapability HTCap; //CWYang(+)
+
+    /* Extended HT Capability Info */
+    union zuExtHTCapability ExtHTCap; //CWYang(+)
+
+    struct InformationElementSta   ie;
+
+#define ZM_CACHED_FRAMEBODY_SIZE   200
+    u8_t                    asocReqFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   asocReqFrameBodySize;
+    u8_t                    asocRspFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   asocRspFrameBodySize;
+    u8_t                    beaconFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   beaconFrameBodySize;
+
+    u8_t                    ac0PriorityHigherThanAc2;
+    u8_t                    SWEncryptEnable;
+
+    u8_t                    leapEnabled;
+
+    u32_t                    TotalNumberOfReceivePackets;
+    u32_t                    TotalNumberOfReceiveBytes;
+    u32_t                    avgSizeOfReceivePackets;
+
+    u32_t                    ReceivedPacketRateCounter;
+    u32_t                    ReceivedPktRatePerSecond;
+
+    /* #2 Record the sequence number to determine whether the unicast frame is separated by RIFS or not */
+#define    ZM_RIFS_STATE_DETECTING    0
+#define    ZM_RIFS_STATE_DETECTED     1
+#define    ZM_RIFS_TIMER_TIMEOUT      4480          // <Driver time>4480ms <Real time>7s
+    u8_t                    rifsState;
+    u8_t                    rifsLikeFrameCnt;
+    u16_t                   rifsLikeFrameSequence[3];
+    u32_t                   rifsTimer;
+    u32_t                   rifsCount;
+
+    /* RX filter desired by upper layers.  Note this contains some bits which must be filtered
+       by sw since the hw supports only a subset of possible filter actions.= */
+    u32_t  osRxFilter;
+
+    u8_t   bSafeMode;
+
+    u32_t  ibssAdditionalIESize;
+    u8_t   ibssAdditionalIE[256];
+}; //struct zsWlanDevSta
+
+#define ZM_CMD_QUEUE_SIZE                   256  //Roger Check, test 64 when ready
+
+#define ZM_OID_READ                         1
+#define ZM_OID_WRITE                        2
+#define ZM_OID_INTERNAL_WRITE               3
+#define ZM_CMD_SET_FREQUENCY                4
+#define ZM_CMD_SET_KEY                      5
+#define ZM_CWM_READ                         6
+#define ZM_MAC_READ                         7
+#define ZM_ANI_READ                         8
+#define ZM_EEPROM_READ                      9
+#define ZM_EEPROM_WRITE                     0x0A
+#define ZM_OID_CHAN							0x30
+#define ZM_OID_SYNTH						0x32
+#define ZM_OID_TALLY						0x81
+#define ZM_OID_TALLY_APD					0x82
+
+#define ZM_OID_DKTX_STATUS                  0x92
+#define ZM_OID_FLASH_CHKSUM                 0xD0
+#define ZM_OID_FLASH_READ                   0xD1
+#define ZM_OID_FLASH_PROGRAM                0xD2
+#define ZM_OID_FW_DL_INIT                   0xD3
+
+/* Driver to Firmware OID */
+#define ZM_CMD_ECHO         0x80
+#define ZM_CMD_TALLY        0x81
+#define ZM_CMD_TALLY_APD    0x82
+#define ZM_CMD_CONFIG       0x83
+#define ZM_CMD_RREG         0x00
+#define ZM_CMD_WREG         0x01
+#define ZM_CMD_RMEM         0x02
+#define ZM_CMD_WMEM         0x03
+#define ZM_CMD_BITAND       0x04
+#define ZM_CMD_BITOR        0x05
+#define ZM_CMD_EKEY         0x28
+#define ZM_CMD_DKEY         0x29
+#define ZM_CMD_FREQUENCY    0x30
+#define ZM_CMD_RF_INIT      0x31
+#define ZM_CMD_SYNTH        0x32
+#define ZM_CMD_FREQ_STRAT   0x33
+#define ZM_CMD_RESET        0x90
+#define ZM_CMD_DKRESET      0x91
+#define ZM_CMD_DKTX_STATUS  0x92
+#define ZM_CMD_FDC          0xA0
+#define ZM_CMD_WREEPROM     0xB0
+#define ZM_CMD_WFLASH       0xB0
+#define ZM_CMD_FLASH_ERASE  0xB1
+#define ZM_CMD_FLASH_PROG   0xB2
+#define ZM_CMD_FLASH_CHKSUM 0xB3
+#define ZM_CMD_FLASH_READ   0xB4
+#define ZM_CMD_FW_DL_INIT   0xB5
+#define ZM_CMD_MEM_WREEPROM 0xBB
+
+
+/* duplicate filter table column */
+#define ZM_FILTER_TABLE_COL                 2 /* 2^n */
+/* duplicate filter table Row */
+#define ZM_FILTER_TABLE_ROW                 8 /* 2^n */
+
+/* duplicate filter table structure */
+struct zsRxFilter
+{
+    u16_t addr[3];
+    u16_t seq;
+    u8_t up;
+};
+
+struct zsWlanDev
+{
+    /* AP global variables */
+    struct zsWlanDevAp ap;
+    /* STA global variables */
+    struct zsWlanDevSta sta;
+    /* save wrapper setting */
+    struct zsWrapperSetting ws;
+    /* features determined by wrapper (vendor) */
+    struct zsWrapperFeatureCtrl wfc;
+    /* Traffic Monitor tally */
+    struct zsTrafTally trafTally;
+    /* Communication tally */
+    struct zsCommTally commTally;
+    /* Duplicate frame filter table */
+    struct zsRxFilter rxFilterTbl[ZM_FILTER_TABLE_COL][ZM_FILTER_TABLE_ROW];
+    /* Regulatory table */
+    struct zsRegulationTable  regulationTable;
+
+    /* */
+    struct zsWlanDevFreqControl freqCtrl;
+
+    enum devState state;
+
+    u8_t  halState;
+    u8_t  wlanMode;         /* AP/INFRASTRUCTURE/IBSS/PSEUDO */
+    u16_t macAddr[3];       /* MAC address */
+    u16_t beaconInterval;   /* beacon Interval */
+    u8_t dtim;              /* DTIM period */
+    u8_t            CurrentDtimCount;
+    u8_t  preambleType;
+    u8_t  preambleTypeInUsed;
+    u8_t  maxTxPower2;	    /* 2.4 GHz Max Tx power (Unit: 0.5 dBm) */
+    u8_t  maxTxPower5;	    /* 5 GHz Max Tx power (Unit: 0.5 dBm) */
+    u8_t  connectMode;
+    u32_t supportMode;
+
+    u8_t bRate;             /* 11b Support Rate bit map */
+    u8_t bRateBasic;        /* 11b Basic Rate bit map */
+    u8_t gRate;             /* 11g Support Rate bit map */
+    u8_t gRateBasic;        /* 11g Basic Rate bit map */
+    /* channel index point to the item in regulation table */
+    u8_t channelIndex;
+
+    /* channel management */
+    u8_t    BandWidth40;
+    u8_t    ExtOffset;      //1 above, 3 below, 0 not present
+    u16_t   frequency;      /* operation frequency */
+
+    u8_t erpElement;        /* ERP information element data */
+
+    u8_t disableSelfCts;    /* set to 1 to disable Self-CTS */
+    u8_t bgMode;
+
+	/* private test flag */
+    u32_t enableProtectionMode;   /* force enable/disable self cts */
+	u32_t checksumTest;     /* OTUS checksum test 1=>zero checksum 0=>normal */
+	u32_t rxPacketDump;     /* rx packet dump */
+
+    u8_t enableAggregation; /* force enable/disable A-MSPU */
+    u8_t enableWDS;         /* force enable/disable WDS testing */
+	u8_t enableTxPathMode;  /* OTUS special testing mode 1=>diable, 0=>enable: ZM_SYSTEM_TEST_MODE */
+    u8_t enableHALDbgInfo;  /*  */
+
+    u32_t forceTxTPC;       /* force tx packet send TPC */
+
+    u16_t seq[4];
+    u16_t mmseq;
+
+    /* driver core time tick */
+    u32_t tick;
+    u16_t tickIbssSendBeacon;
+    u16_t tickIbssReceiveBeacon;
+
+    /* RTS threshold */
+    u16_t rtsThreshold;
+
+    /* fragmentation threshold,  256 <= value <= 2346, 0=disabled */
+    u16_t fragThreshold;
+
+    /* Tx Rate */
+    u16_t txMCS;
+    u16_t txMT;
+    u32_t CurrentTxRateKbps; //CWYang(+)
+    /* Rx Rate */
+    u32_t CurrentRxRateKbps; //Janet(+)
+    u8_t CurrentRxRateUpdated;
+    u8_t modulationType;
+    u8_t rxInfo;
+    u16_t rateField;
+
+    /* timer related objects */
+    struct zsTimerList  timerList;
+    u8_t                bTimerReady;
+
+    /* for defragmentation */
+    struct zsDefragList defragTable;
+
+    /* Data struct for Interface Dependent Layer */
+    //struct zsIdlStruct idlStruct;
+
+    /* Signal Strength/Quality Related Parameters */
+    u8_t SignalStrength; //CWYang(+)
+    u8_t SignalQuality;  //CWYang(+)
+
+
+
+    /* QoS */
+    zbuf_t* vtxq[4][ZM_VTXQ_SIZE];
+    u16_t vtxqHead[4];
+    u16_t vtxqTail[4];
+    u16_t qosDropIpFrag[4];
+
+    /* Management Tx queue */
+    zbuf_t* vmmq[ZM_VMMQ_SIZE];
+    u16_t vmmqHead;
+    u16_t vmmqTail;
+
+    u8_t                vtxqPushing;
+
+    /*
+     * add by honda
+     * 1. Aggregate queues
+     * 2. STA's associated information and queue number
+     * 3. rx aggregation re-ordering queue
+     */
+    struct aggQueue    *aggQPool[ZM_AGG_POOL_SIZE];
+    u8_t                aggInitiated;
+    u8_t                addbaComplete;
+    u8_t                addbaCount;
+    u8_t                aggState;
+    u8_t                destLock;
+    struct aggSta       aggSta[ZM_MAX_STA_SUPPORT];
+    struct agg_tid_rx  *tid_rx[ZM_AGG_POOL_SIZE];
+    struct aggTally     agg_tal;
+    struct destQ        destQ;
+    struct baw_enabler *baw_enabler;
+    struct ieee80211_cwm    cwm;
+    u16_t               reorder;
+    u16_t               seq_debug;
+    /* rate control */
+    u32_t txMPDU[ZM_RATE_TABLE_SIZE];
+    u32_t txFail[ZM_RATE_TABLE_SIZE];
+    u32_t PER[ZM_RATE_TABLE_SIZE];
+    u16_t probeCount;
+    u16_t probeSuccessCount;
+    u16_t probeInterval;
+    u16_t success_probing;
+    /*
+     * end of add by honda
+     */
+
+    /* airopeek sniffer mode for upper sw */
+    u32_t               swSniffer;   /* window: airoPeek */
+    u32_t               XLinkMode;
+
+    /* MDK mode */
+    /* init by 0=>normal driver 1=>MDK driver */
+    u32_t               modeMDKEnable;
+
+    u32_t               heartBeatNotification;
+
+    /* pointer for HAL Plus private memory */
+    void* hpPrivate;
+
+    /* for WPA setting */
+    //u8_t                    wpaSupport[ZM_MAX_AP_SUPPORT];
+    //u8_t                    wpaLen[ZM_MAX_AP_SUPPORT];
+    //u8_t                    wpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_IE_SIZE];
+
+    struct zsLedStruct      ledStruct;
+
+    /* ani flag */
+    u8_t aniEnable;
+    u16_t txq_threshold;
+
+	//Skip Mic Error Check
+	u8_t	TKIP_Group_KeyChanging;
+
+	u8_t    dynamicSIFSEnable;
+
+	u8_t    queueFlushed;
+
+    u16_t (*zfcbAuthNotify)(zdev_t* dev, u16_t* macAddr);
+    u16_t (*zfcbAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port);
+    u16_t (*zfcbDisAsocNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    u16_t (*zfcbApConnectNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    void (*zfcbConnectNotify)(zdev_t* dev, u16_t status, u16_t* bssid);
+    void (*zfcbScanNotify)(zdev_t* dev, struct zsScanResult* result);
+    void (*zfcbMicFailureNotify)(zdev_t* dev, u16_t* addr, u16_t status);
+    void (*zfcbApMicFailureNotify)(zdev_t* dev, u8_t* addr, zbuf_t* buf);
+    void (*zfcbIbssPartnerNotify)(zdev_t* dev, u16_t status, struct zsPartnerNotifyEvent *event);
+    void (*zfcbMacAddressNotify)(zdev_t* dev, u8_t* addr);
+    void (*zfcbSendCompleteIndication)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbRecvEth)(zdev_t* dev, zbuf_t* buf, u16_t port);
+    void (*zfcbRecv80211)(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+    void (*zfcbRestoreBufData)(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+    u16_t (*zfcbCencAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+#endif //ZM_ENABLE_CENC
+    u8_t (*zfcbClassifyTxPacket)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbHwWatchDogNotify)(zdev_t* dev);
+};
+
+
+struct zsWlanKey
+{
+    u8_t key;
+};
+
+
+/* These macros are defined here for backward compatibility */
+/* Please leave them alone */
+/* For Tx packet allocated in upper layer layer */
+#define zmw_tx_buf_readb(dev, buf, offset) zmw_buf_readb(dev, buf, offset)
+#define zmw_tx_buf_readh(dev, buf, offset) zmw_buf_readh(dev, buf, offset)
+#define zmw_tx_buf_writeb(dev, buf, offset, value) zmw_buf_writeb(dev, buf, offset, value)
+#define zmw_tx_buf_writeh(dev, buf, offset, value) zmw_buf_writeh(dev, buf, offset, value)
+
+/* For Rx packet allocated in driver */
+#define zmw_rx_buf_readb(dev, buf, offset) zmw_buf_readb(dev, buf, offset)
+#define zmw_rx_buf_readh(dev, buf, offset) zmw_buf_readh(dev, buf, offset)
+#define zmw_rx_buf_writeb(dev, buf, offset, value) zmw_buf_writeb(dev, buf, offset, value)
+#define zmw_rx_buf_writeh(dev, buf, offset, value) zmw_buf_writeh(dev, buf, offset, value)
+
+#endif /* #ifndef _STRUCT_H */
diff --git a/drivers/staging/otus/80211core/wlan.h b/drivers/staging/otus/80211core/wlan.h
new file mode 100644
index 000000000000..26c18b837cfc
--- /dev/null
+++ b/drivers/staging/otus/80211core/wlan.h
@@ -0,0 +1,595 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wlan_defs.h                                           */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains WLAN definitions.                          */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _WLAN_H
+#define _WLAN_H
+
+
+#define ZM_EXTERNAL_ALLOC_BUF               0
+#define ZM_INTERNAL_ALLOC_BUF               1
+
+#define ZM_SIZE_OF_CTRL_SET                 8
+#define ZM_SIZE_OF_IV                       4
+#define ZM_SIZE_OF_EXT_IV                   4
+#define ZM_SIZE_OF_MIC                      8
+#define ZM_SIZE_OF_CCX_MIC                  8
+#define ZM_SIZE_OF_WLAN_DATA_HEADER         24
+#define ZM_SIZE_OF_QOS_CTRL                 2
+
+/* Header definition */
+#define ZM_SIZE_OF_WLAN_WDS_HEADER          32
+#define ZM_SIZE_OF_SNAP_HEADER              8
+
+#define ZM_WLAN_HEADER_A1_OFFSET            4
+#define ZM_WLAN_HEADER_A2_OFFSET            10
+#define ZM_WLAN_HEADER_A3_OFFSET            16
+#define ZM_WLAN_HEADER_A4_OFFSET            24
+#define ZM_WLAN_HEADER_IV_OFFSET            24
+#define ZM_SIZE_OF_WLAN_DATA_HEADER         24
+
+/* Port definition */
+#define ZM_PORT_DISABLED                    0
+#define ZM_PORT_ENABLED                     1
+
+/* Frame Type */
+#define ZM_WLAN_MANAGEMENT_FRAME            0x0
+#define ZM_WLAN_CONTROL_FRAME               0x4
+#define ZM_WLAN_DATA_FRAME                  0x8
+
+/* Frame Subtype */
+#define ZM_WLAN_FRAME_TYPE_ASOCREQ          0x00
+#define ZM_WLAN_FRAME_TYPE_ASOCRSP          0x10
+#define ZM_WLAN_FRAME_TYPE_REASOCREQ        0x20
+#define ZM_WLAN_FRAME_TYPE_REASOCRSP        0x30
+#define ZM_WLAN_FRAME_TYPE_PROBEREQ         0x40
+#define ZM_WLAN_FRAME_TYPE_PROBERSP         0x50
+/* 0x60, 0x70 => Reserved */
+#define ZM_WLAN_FRAME_TYPE_BEACON           0x80
+#define ZM_WLAN_FRAME_TYPE_ATIM             0x90
+#define ZM_WLAN_FRAME_TYPE_DISASOC          0xA0
+#define ZM_WLAN_FRAME_TYPE_AUTH             0xB0
+#define ZM_WLAN_FRAME_TYPE_DEAUTH           0xC0
+#define ZM_WLAN_FRAME_TYPE_ACTION			0xD0
+
+/* Frame type and subtype */
+#define ZM_WLAN_FRAME_TYPE_NULL             0x48
+#define ZM_WLAN_FRAME_TYPE_BAR              0x84
+#define ZM_WLAN_FRAME_TYPE_BA               0x94
+#define ZM_WLAN_FRAME_TYPE_PSPOLL           0xA4
+#define ZM_WLAN_FRAME_TYPE_RTS              0xB4
+#define ZM_WLAN_FRAME_TYPE_CTS              0xC4
+#define ZM_WLAN_FRAME_TYPE_QOS_NULL         0xC8
+
+/* action frame */
+#define ZM_WLAN_SPECTRUM_MANAGEMENT_ACTION_FRAME    0
+#define ZM_WLAN_QOS_ACTION_FRAME            1
+#define ZM_WLAN_DLS_ACTION_FRAME            2
+#define ZM_WLAN_BLOCK_ACK_ACTION_FRAME      3
+/* block ack action frame*/
+#define ZM_WLAN_ADDBA_REQUEST_FRAME         0
+#define ZM_WLAN_ADDBA_RESPONSE_FRAME        1
+#define ZM_WLAN_DELBA_FRAME                 2
+
+/* Element ID */
+#define ZM_WLAN_EID_SSID                    0
+#define ZM_WLAN_EID_SUPPORT_RATE            1
+#define ZM_WLAN_EID_FH                      2
+#define ZM_WLAN_EID_DS                      3
+#define ZM_WLAN_EID_CFS                     4
+#define ZM_WLAN_EID_TIM                     5
+#define ZM_WLAN_EID_IBSS                    6
+#define ZM_WLAN_EID_COUNTRY                 7
+/* reserved 8-15 */
+#define ZM_WLAN_EID_CHALLENGE               16
+/* reserved 17-31 */
+#define ZM_WLAN_EID_POWER_CONSTRAINT        32
+#define ZM_WLAN_EID_POWER_CAPABILITY        33
+#define ZM_WLAN_EID_TPC_REQUEST             34
+#define ZM_WLAN_EID_TPC_REPORT              35
+#define ZM_WLAN_EID_SUPPORTED_CHANNELS      36
+#define ZM_WLAN_EID_CHANNEL_SWITCH_ANNOUNCE 37
+#define ZM_WLAN_EID_MEASUREMENT_REQUEST     38
+#define ZM_WLAN_EID_MEASUREMENT_REPORT      39
+#define ZM_WLAN_EID_QUIET                   40
+#define ZM_WLAN_EID_IBSS_DFS                41
+#define ZM_WLAN_EID_ERP                     42
+#define ZM_WLAN_PREN2_EID_HTCAPABILITY      45
+#define ZM_WLAN_EID_RSN_IE                  48
+#define ZM_WLAN_EID_EXTENDED_RATE           50
+#define ZM_WLAN_EID_HT_CAPABILITY           51
+#define ZM_WLAN_EID_EXTENDED_HT_CAPABILITY  52
+#define ZM_WLAN_EID_NEW_EXT_CHANNEL_OFFSET  53
+#define ZM_WLAN_PREN2_EID_HTINFORMATION     61
+#define ZM_WLAN_PREN2_EID_SECONDCHOFFSET    62
+#ifdef ZM_ENABLE_CENC
+#define ZM_WLAN_EID_CENC_IE                 68
+#endif //ZM_ENABLE_CENC
+#define ZM_WLAN_EID_VENDOR_PRIVATE          221     /* Vendor private space; must demux OUI */
+#define ZM_WLAN_EID_WPA_IE                  221
+#define ZM_WLAN_EID_WPS_IE                  221
+#define ZM_WLAN_EID_WIFI_IE                 221
+
+/* ERP information element */
+#define ZM_WLAN_NON_ERP_PRESENT_BIT         0x1
+#define ZM_WLAN_USE_PROTECTION_BIT          0x2
+#define ZM_WLAN_BARKER_PREAMBLE_MODE_BIT    0x4
+
+/* Channel frequency, in MHz */
+#define ZM_CH_G_1                          2412
+#define ZM_CH_G_2                          2417
+#define ZM_CH_G_3                          2422
+#define ZM_CH_G_4                          2427
+#define ZM_CH_G_5                          2432
+#define ZM_CH_G_6                          2437
+#define ZM_CH_G_7                          2442
+#define ZM_CH_G_8                          2447
+#define ZM_CH_G_9                          2452
+#define ZM_CH_G_10                         2457
+#define ZM_CH_G_11                         2462
+#define ZM_CH_G_12                         2467
+#define ZM_CH_G_13                         2472
+#define ZM_CH_G_14                         2484
+#define ZM_CH_A_184                        4920
+#define ZM_CH_A_188                        4940
+#define ZM_CH_A_192                        4960
+#define ZM_CH_A_196                        4980
+#define ZM_CH_A_8                          5040
+#define ZM_CH_A_12                         5060
+#define ZM_CH_A_16                         5080
+#define ZM_CH_A_36                         5180
+#define ZM_CH_A_40                         5200
+#define ZM_CH_A_44                         5220
+#define ZM_CH_A_48                         5240
+#define ZM_CH_A_52                         5260
+#define ZM_CH_A_56                         5280
+#define ZM_CH_A_60                         5300
+#define ZM_CH_A_64                         5320
+#define ZM_CH_A_100                        5500
+#define ZM_CH_A_104                        5520
+#define ZM_CH_A_108                        5540
+#define ZM_CH_A_112                        5560
+#define ZM_CH_A_116                        5580
+#define ZM_CH_A_120                        5600
+#define ZM_CH_A_124                        5620
+#define ZM_CH_A_128                        5640
+#define ZM_CH_A_132                        5660
+#define ZM_CH_A_136                        5680
+#define ZM_CH_A_140                        5700
+#define ZM_CH_A_149                        5745
+#define ZM_CH_A_153                        5765
+#define ZM_CH_A_157                        5785
+#define ZM_CH_A_161                        5805
+#define ZM_CH_A_165                        5825
+
+
+/* AP : STA table => STA Type */
+#define ZM_11B_STA                          0x0
+#define ZM_11G_STA                          0x2
+#define ZM_11N_STA                          0x4
+
+/* AP : timeout */
+#define ZM_MS_PER_TICK                      10
+#define ZM_TICK_PER_SECOND                  (1000/ZM_MS_PER_TICK)
+#define ZM_TICK_PER_MINUTE                  (60*1000/ZM_MS_PER_TICK)
+#define ZM_PREAUTH_TIMEOUT_MS               1000 /* 1 sec */
+#define ZM_AUTH_TIMEOUT_MS                  1000 /* 1 sec */
+
+/* Error code */
+#define ZM_SUCCESS                          0
+#define ZM_ERR_TX_PORT_DISABLED             1
+#define ZM_ERR_BUFFER_DMA_ADDR              2
+#define ZM_ERR_FREE_TXD_EXHAUSTED           3
+#define ZM_ERR_TX_BUFFER_UNAVAILABLE        4
+#define ZM_ERR_BCMC_PS_BUFFER_UNAVAILABLE   5
+#define ZM_ERR_UNI_PS_BUFFER_UNAVAILABLE    6
+#define ZM_ERR_EXCEED_PRIORITY_THRESHOLD    7
+#define ZM_ERR_VMMQ_FULL                    8
+#define ZM_ERR_FLUSH_PS_QUEUE               9
+#define ZM_ERR_CMD_INT_MISSED               15 /* Polling cmd int timeout*/
+/* Rx */
+#define ZM_ERR_RX_FRAME_TYPE                20
+#define ZM_ERR_MIN_RX_ENCRYPT_FRAME_LENGTH  21
+#define ZM_ERR_MIN_RX_FRAME_LENGTH          22
+#define ZM_ERR_MAX_RX_FRAME_LENGTH          23
+#define ZM_ERR_RX_DUPLICATE                 24
+#define ZM_ERR_RX_SRC_ADDR_IS_OWN_MAC       25
+#define ZM_ERR_MIN_RX_PROTOCOL_VERSION      26
+#define ZM_ERR_WPA_GK_NOT_INSTALLED         27
+#define ZM_ERR_STA_NOT_ASSOCIATED           28
+#define ZM_ERR_DATA_BEFORE_CONNECTED        29
+#define ZM_ERR_DATA_NOT_ENCRYPTED           30
+#define ZM_ERR_DATA_BSSID_NOT_MATCHED       31
+#define ZM_ERR_RX_BAR_FRAME                 32
+#define ZM_ERR_OUT_OF_ORDER_NULL_DATA       33
+
+/* ZFI */
+#define ZM_ERR_INVALID_TX_RATE              40
+#define ZM_ERR_WDS_PORT_ID                  41
+
+/* QUEUE */
+#define ZM_ERR_QUEUE_FULL                   50
+#define ZM_ERR_STA_UAPSD_QUEUE_FULL         51
+#define ZM_ERR_AP_UAPSD_QUEUE_FULL          52
+
+/* Maximum Rx frame length */
+#if ZM_LARGEPAYLOAD_TEST == 1
+#define ZM_WLAN_MAX_RX_SIZE                 16384
+#else
+#define ZM_WLAN_MAX_RX_SIZE                 8192
+#endif
+
+/* PCI DMA test error code */
+#define ZM_ERR_INTERRUPT_MISSED             100
+#define ZM_ERR_OWN_BIT_NOT_CLEARED          101
+#define ZM_ERR_RX_SEQ_NUMBER                102
+#define ZM_ERR_RX_LENGTH                    103
+#define ZM_ERR_RX_DATA                      104
+#define ZM_ERR_RX_DESCRIPTOR_NUM            105
+/* Common register test error code */
+#define ZM_ERR_REGISTER_ACCESS              110 /* Register R/W test fail*/
+#define ZM_ERR_CLEAR_INTERRUPT_FLAG         111
+#define ZM_ERR_COMMAND_RESPONSE             112
+#define ZM_ERR_INTERRUPT_GENERATE           113
+#define ZM_ERR_INTERRUPT_ACK                114
+#define ZM_ERR_SCRATCH_ACCESS               115
+#define ZM_ERR_INTERRUPT_MASK_ACCESS        116
+#define ZM_ERR_SHARE_MEMORY_PCI_ACCESS      117
+#define ZM_ERR_SHARE_MEMORY_FW_ACCESS       118
+#define ZM_ERR_SHARE_MEMORY_DISABLE         119
+#define ZM_ERR_SHARE_MEMORY_TEST_RESPONSE   120
+
+/* Firmware Download error code */
+#define ZM_ERR_FIRMWARE_DOWNLOAD_TIMEOUT    150
+#define ZM_ERR_FIRMWARE_DOWNLOAD_INT_FLAG   151
+#define ZM_ERR_FIRMWARE_READY_TIMEOUT       152
+#define ZM_ERR_FIRMWARE_WRONG_TYPE          153
+
+/* Debug */
+#define ZM_LV_0     0//Debug level 0, Disable debug message
+#define ZM_LV_1     1//Debug level 1, Show minimum information
+#define ZM_LV_2     2//Debug level 2, Show medium message
+#define ZM_LV_3     3//Debug level 3, Show all
+
+#define ZM_SCANMSG_LEV  ZM_LV_1
+#define ZM_TXMSG_LEV    ZM_LV_0//ZM_LV_0
+#define ZM_RXMSG_LEV    ZM_LV_0
+#define ZM_MMMSG_LEV    ZM_LV_0
+#define ZM_DESMSG_LEV   ZM_LV_0//ZM_LV_0
+#define ZM_BUFMSG_LEV   ZM_LV_0//ZM_LV_1
+#define ZM_INITMSG_LEV  ZM_LV_0
+
+#define zm_msg0_scan(lv, msg) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_scan(lv, msg, val) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_scan(lv, msg, val) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_tx(lv, msg) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_tx(lv, msg, val) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_tx(lv, msg, val) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_rx(lv, msg) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_rx(lv, msg, val) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_rx(lv, msg, val) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_mm(lv, msg) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_mm(lv, msg, val) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_mm(lv, msg, val) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_des(lv, msg) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_des(lv, msg, val) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_des(lv, msg, val) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_buf(lv, msg) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_buf(lv, msg, val) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_buf(lv, msg, val) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_init(lv, msg) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_init(lv, msg, val) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_init(lv, msg, val) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define ZM_MAX_AP_SUPPORT                   2  /* Must <= 8 */
+#define ZM_MAX_WDS_SUPPORT                  6  /* Must <= 6 */
+#define ZM_MAX_STA_SUPPORT                  16 /* Must <= 64 */
+
+/* STA table state */
+#define ZM_STATE_AUTH                       1
+#define ZM_STATE_PREAUTH                    2
+#define ZM_STATE_ASOC                       3
+
+/* Rate set */
+#define ZM_RATE_SET_CCK                     0
+#define ZM_RATE_SET_OFDM                    1
+
+/* HT PT */
+#define ZM_PREAMBLE_TYPE_MIXED_MODE         0
+#define ZM_PREAMBLE_TYPE_GREEN_FIELD        1
+
+/* HT bandwidth */
+#define ZM_BANDWIDTH_20MHZ                  0
+#define ZM_BANDWIDTH_40MHZ                  1
+
+/* MIC status */
+#define ZM_MIC_SUCCESS                      0
+#define ZM_MIC_FAILURE                      1
+
+/* ICV status */
+#define ZM_ICV_SUCCESS                      0
+#define ZM_ICV_FAILURE                      1
+
+/* definition check */
+#if (ZM_MAX_AP_SUPPORT > 8)
+definition error, ZM_MAX_AP_SUPPORT > 8
+#endif
+#if (ZM_MAX_AP_SUPPORT > 64)
+definition error, ZM_MAX_STA_SUPPORT > 64
+#endif
+
+/*  Transmission Rate information */
+
+/* WLAN frame format */
+#define ZM_PLCP_HEADER_SIZE          5
+#define ZM_ETHERNET_ADDRESS_LENGTH   6
+#define ZM_TIMESTAMP_OFFSET          0
+#define ZM_BEACON_INTERVAL_OFFSET    8
+#define ZM_CAPABILITY_OFFSET        10
+
+/* Reason Code */
+/* An unsolicited notification management frame of       */
+/* type Disassocation or Deauthentication was generated. */
+#ifdef ZM_REASON_CODE
+#define ZM_WLAN_REASON_CODE_UNSPECIFIED   1
+#define ZM_WLAN_FRAME_DISASOC_DEAUTH_REASON_CODE  24
+#endif
+
+struct zsWlanManagementFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        da[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        sa[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        bssid[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        body[1];
+};
+
+struct zsWlanProbeRspFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        da[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        sa[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        bssid[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        timeStamp[8];
+    u8_t        beaconInterval[2];
+    u8_t        capability[2];
+    u8_t        ssid[ZM_MAX_SSID_LENGTH + 2];   // EID(1) + Length(1) + SSID(32)
+} ;
+
+#define zsWlanBeaconFrameHeader zsWlanProbeRspFrameHeader
+
+struct zsWlanAuthFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        address1[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address2[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address3[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u16_t       algo;
+    u16_t       seq;
+    u16_t       status;
+    u8_t        challengeText[255]; // the first 2 bytes are information ID, length
+};
+
+struct zsWlanAssoFrameHeader
+{
+    //u8_t      plcpHdr[PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        address1[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address2[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address3[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        capability[2];
+    u16_t       status;
+    u16_t       aid;
+    //u8_t        supportedRates[10];
+};
+
+struct zsFrag
+{
+    zbuf_t* buf[16];
+    u16_t bufType[16];
+    u16_t seq[16];
+    u8_t flag[16];
+
+};
+
+//================================
+// Hardware related definitions
+//================================
+#define ZM_MAC_REG_BASE                         0x1c3000
+
+#define ZM_MAC_REG_ATIM_WINDOW                  (ZM_MAC_REG_BASE + 0x51C)
+#define ZM_MAC_REG_BCN_PERIOD                   (ZM_MAC_REG_BASE + 0x520)
+#define ZM_MAC_REG_PRETBTT                      (ZM_MAC_REG_BASE + 0x524)
+
+#define ZM_MAC_REG_MAC_ADDR_L                   (ZM_MAC_REG_BASE + 0x610)
+#define ZM_MAC_REG_MAC_ADDR_H                   (ZM_MAC_REG_BASE + 0x614)
+
+#define ZM_MAC_REG_GROUP_HASH_TBL_L             (ZM_MAC_REG_BASE + 0x624)
+#define ZM_MAC_REG_GROUP_HASH_TBL_H             (ZM_MAC_REG_BASE + 0x628)
+
+#define ZM_MAC_REG_BASIC_RATE                   (ZM_MAC_REG_BASE + 0x630)
+#define ZM_MAC_REG_MANDATORY_RATE               (ZM_MAC_REG_BASE + 0x634)
+#define ZM_MAC_REG_RTS_CTS_RATE                 (ZM_MAC_REG_BASE + 0x638)
+#define ZM_MAC_REG_BACKOFF_PROTECT              (ZM_MAC_REG_BASE + 0x63c)
+#define ZM_MAC_REG_RX_THRESHOLD                 (ZM_MAC_REG_BASE + 0x640)
+#define ZM_MAC_REG_RX_PE_DELAY                  (ZM_MAC_REG_BASE + 0x64C)
+
+#define ZM_MAC_REG_DYNAMIC_SIFS_ACK             (ZM_MAC_REG_BASE + 0x658)
+#define ZM_MAC_REG_SNIFFER                      (ZM_MAC_REG_BASE + 0x674)
+#define ZM_MAC_REG_TX_UNDERRUN		            (ZM_MAC_REG_BASE + 0x688)
+#define ZM_MAC_REG_RX_TOTAL			            (ZM_MAC_REG_BASE + 0x6A0)
+#define ZM_MAC_REG_RX_CRC32			            (ZM_MAC_REG_BASE + 0x6A4)
+#define ZM_MAC_REG_RX_CRC16			            (ZM_MAC_REG_BASE + 0x6A8)
+#define ZM_MAC_REG_RX_ERR_UNI		            (ZM_MAC_REG_BASE + 0x6AC)
+#define ZM_MAC_REG_RX_OVERRUN		            (ZM_MAC_REG_BASE + 0x6B0)
+#define ZM_MAC_REG_RX_ERR_MUL		            (ZM_MAC_REG_BASE + 0x6BC)
+#define ZM_MAC_REG_TX_RETRY			            (ZM_MAC_REG_BASE + 0x6CC)
+#define ZM_MAC_REG_TX_TOTAL			            (ZM_MAC_REG_BASE + 0x6F4)
+
+
+#define ZM_MAC_REG_ACK_EXTENSION                (ZM_MAC_REG_BASE + 0x690)
+#define ZM_MAC_REG_EIFS_AND_SIFS                (ZM_MAC_REG_BASE + 0x698)
+
+#define ZM_MAC_REG_SLOT_TIME                    (ZM_MAC_REG_BASE + 0x6F0)
+
+#define ZM_MAC_REG_ROLL_CALL_TBL_L              (ZM_MAC_REG_BASE + 0x704)
+#define ZM_MAC_REG_ROLL_CALL_TBL_H              (ZM_MAC_REG_BASE + 0x708)
+
+#define ZM_MAC_REG_AC0_CW                       (ZM_MAC_REG_BASE + 0xB00)
+#define ZM_MAC_REG_AC1_CW                       (ZM_MAC_REG_BASE + 0xB04)
+#define ZM_MAC_REG_AC2_CW                       (ZM_MAC_REG_BASE + 0xB08)
+#define ZM_MAC_REG_AC3_CW                       (ZM_MAC_REG_BASE + 0xB0C)
+#define ZM_MAC_REG_AC4_CW                       (ZM_MAC_REG_BASE + 0xB10)
+#define ZM_MAC_REG_AC1_AC0_AIFS                 (ZM_MAC_REG_BASE + 0xB14)
+#define ZM_MAC_REG_AC3_AC2_AIFS                 (ZM_MAC_REG_BASE + 0xB18)
+
+#define ZM_MAC_REG_RETRY_MAX                    (ZM_MAC_REG_BASE + 0xB28)
+
+#define ZM_MAC_REG_TXOP_NOT_ENOUGH_INDICATION   (ZM_MAC_REG_BASE + 0xB30)
+
+#define ZM_MAC_REG_AC1_AC0_TXOP                 (ZM_MAC_REG_BASE + 0xB44)
+#define ZM_MAC_REG_AC3_AC2_TXOP                 (ZM_MAC_REG_BASE + 0xB48)
+
+#define ZM_MAC_REG_ACK_TABLE                    (ZM_MAC_REG_BASE + 0xC00)
+
+#define ZM_MAC_REG_BCN_ADDR                     (ZM_MAC_REG_BASE + 0xD84)
+#define ZM_MAC_REG_BCN_LENGTH                   (ZM_MAC_REG_BASE + 0xD88)
+
+#define ZM_MAC_REG_BCN_PLCP                     (ZM_MAC_REG_BASE + 0xD90)
+#define ZM_MAC_REG_BCN_CTRL                     (ZM_MAC_REG_BASE + 0xD94)
+
+#define ZM_MAC_REG_BCN_HT1                      (ZM_MAC_REG_BASE + 0xDA0)
+#define ZM_MAC_REG_BCN_HT2                      (ZM_MAC_REG_BASE + 0xDA4)
+
+
+#define ZM_RX_STATUS_IS_MIC_FAIL(rxStatus) rxStatus->Tail.Data.ErrorIndication & ZM_BIT_6
+
+//================================
+//================================
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+#define ZM_80211_FRAME_HEADER_LEN           24
+#define ZM_80211_FRAME_TYPE_OFFSET          30    // ZM_80211_FRAME_HEADER_LEN + SNAP
+#define ZM_80211_FRAME_IP_OFFSET            32    // ZM_80211_FRAME_HEADER_LEN + SNAP + TYPE
+#else
+#define ZM_80211_FRAME_HEADER_LEN           14
+#define ZM_80211_FRAME_TYPE_OFFSET          12    // ZM_80211_FRAME_HEADER_LEN + SNAP
+#define ZM_80211_FRAME_IP_OFFSET            14    // ZM_80211_FRAME_HEADER_LEN + SNAP + TYPE
+#endif
+
+#define ZM_BSS_INFO_VALID_BIT      0x01
+#define ZM_BSS_INFO_UPDATED_BIT    0x02
+
+
+
+
+
+#define ZM_ERROR_INDICATION_RX_TIMEOUT      0x01
+#define ZM_ERROR_INDICATION_OVERRUN         0x02
+#define ZM_ERROR_INDICATION_DECRYPT_ERROR   0x04
+#define ZM_ERROR_INDICATION_CRC32_ERROR     0x08
+#define ZM_ERROR_INDICATION_ADDR_NOT_MATCH  0x10
+#define ZM_ERROR_INDICATION_CRC16_ERROR     0x20
+#define ZM_ERROR_INDICATION_MIC_ERROR       0x40
+
+#define ZM_RXMAC_STATUS_MOD_TYPE_CCK        0x00
+#define ZM_RXMAC_STATUS_MOD_TYPE_OFDM       0x01
+#define ZM_RXMAC_STATUS_MOD_TYPE_HT_OFDM    0x02
+#define ZM_RXMAC_STATUS_MOD_TYPE_DL_OFDM    0x03
+#define ZM_RXMAC_STATUS_TOTAL_ERROR         0x80
+
+
+
+
+
+#define ZM_MAX_LED_NUMBER       2
+
+#define ZM_LED_DISABLE_MODE     0x0
+#define ZM_LED_LINK_MODE        0x1
+#define ZM_LED_LINK_TR_MODE     0x2
+#define ZM_LED_TR_ON_MODE       0x3
+#define ZM_LED_TR_OFF_MODE      0x4
+
+#define ZM_LED_CTRL_FLAG_ALPHA      0x1
+
+struct zsLedStruct
+{
+    u32_t   counter;
+    u32_t   counter100ms;
+    u16_t   ledLinkState;
+    u16_t   ledMode[ZM_MAX_LED_NUMBER];
+    u32_t   txTraffic;
+    u32_t   rxTraffic;
+    u8_t    LEDCtrlType;
+    u8_t    LEDCtrlFlag;         // Control Flag for vendors
+    u8_t    LEDCtrlFlagFromReg;  // Control Flag for vendors in registry
+};
+
+
+//HAL+ capability bits definition
+#define ZM_HP_CAP_11N                   0x1
+#define ZM_HP_CAP_11N_ONE_TX_STREAM     0x2
+#define ZM_HP_CAP_2G                    0x4
+#define ZM_HP_CAP_5G                    0x8
+
+#endif /* #ifndef _WLAN_H */