drivers/net/wireless/rockchip_wlan/rtl8723au/core/rtw_xmit.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  17  *
  18  *
  19  ******************************************************************************/
  20 #define _RTW_XMIT_C_
  21
  22 #include <drv_conf.h>
  23 #include <osdep_service.h>
  24 #include <drv_types.h>
  25 #include <rtw_byteorder.h>
  26 #include <wifi.h>
  27 #include <osdep_intf.h>
  28 #include <circ_buf.h>
  29 #include <ip.h>
  30
  31 #if defined (PLATFORM_LINUX) && defined (PLATFORM_WINDOWS)
  32 #error "Shall be Linux or Windows, but not both!\n"
  33 #endif
  34
  35 #ifdef PLATFORM_WINDOWS
  36 #include <if_ether.h>
  37 #endif
  38
  39 #ifdef CONFIG_USB_HCI
  40 #include <usb_ops.h>
  41 #endif
  42
  43
  44 static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
  45 static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
  46
  47 static void _init_txservq(struct tx_servq *ptxservq)
  48 {
  49 _func_enter_;
  50         _rtw_init_listhead(&ptxservq->tx_pending);
  51         _rtw_init_queue(&ptxservq->sta_pending);
  52         ptxservq->qcnt = 0;
  53 _func_exit_;
  54 }
  55
  56
  57 void    _rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
  58 {
  59
  60 _func_enter_;
  61
  62         _rtw_memset((unsigned char *)psta_xmitpriv, 0, sizeof (struct sta_xmit_priv));
  63
  64         _rtw_spinlock_init(&psta_xmitpriv->lock);
  65
  66         //for(i = 0 ; i < MAX_NUMBLKS; i++)
  67         //      _init_txservq(&(psta_xmitpriv->blk_q[i]));
  68
  69         _init_txservq(&psta_xmitpriv->be_q);
  70         _init_txservq(&psta_xmitpriv->bk_q);
  71         _init_txservq(&psta_xmitpriv->vi_q);
  72         _init_txservq(&psta_xmitpriv->vo_q);
  73         _rtw_init_listhead(&psta_xmitpriv->legacy_dz);
  74         _rtw_init_listhead(&psta_xmitpriv->apsd);
  75
  76 _func_exit_;
  77
  78 }
  79
  80 s32     _rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, _adapter *padapter)
  81 {
  82         int i;
  83         struct xmit_buf *pxmitbuf;
  84         struct xmit_frame *pxframe;
  85         sint    res=_SUCCESS;
  86         u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
  87         u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
  88
  89 #if defined(CONFIG_MP_INCLUDED) && defined(CONFIG_RTL8723A)
  90                 if (padapter->registrypriv.mp_mode) {
  91                         max_xmit_extbuf_size = 6000;
  92                         num_xmit_extbuf = 8;
  93                 }
  94 #endif
  95
  96 _func_enter_;
  97
  98         // We don't need to memset padapter->XXX to zero, because adapter is allocated by rtw_zvmalloc().
  99         //_rtw_memset((unsigned char *)pxmitpriv, 0, sizeof(struct xmit_priv));
 100
 101         _rtw_spinlock_init(&pxmitpriv->lock);
 102         _rtw_spinlock_init(&pxmitpriv->lock_sctx);
 103         _rtw_init_sema(&pxmitpriv->xmit_sema, 0);
 104         _rtw_init_sema(&pxmitpriv->terminate_xmitthread_sema, 0);
 105
 106         /*
 107         Please insert all the queue initializaiton using _rtw_init_queue below
 108         */
 109
 110         pxmitpriv->adapter = padapter;
 111
 112         //for(i = 0 ; i < MAX_NUMBLKS; i++)
 113         //      _rtw_init_queue(&pxmitpriv->blk_strms[i]);
 114
 115         _rtw_init_queue(&pxmitpriv->be_pending);
 116         _rtw_init_queue(&pxmitpriv->bk_pending);
 117         _rtw_init_queue(&pxmitpriv->vi_pending);
 118         _rtw_init_queue(&pxmitpriv->vo_pending);
 119         _rtw_init_queue(&pxmitpriv->bm_pending);
 120
 121         //_rtw_init_queue(&pxmitpriv->legacy_dz_queue);
 122         //_rtw_init_queue(&pxmitpriv->apsd_queue);
 123
 124         _rtw_init_queue(&pxmitpriv->free_xmit_queue);
 125
 126         /*
 127         Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
 128         and initialize free_xmit_frame below.
 129         Please also apply  free_txobj to link_up all the xmit_frames...
 130         */
 131
 132         pxmitpriv->pallocated_frame_buf = rtw_zvmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
 133
 134         if (pxmitpriv->pallocated_frame_buf  == NULL){
 135                 pxmitpriv->pxmit_frame_buf =NULL;
 136                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xmit_frame fail!\n"));
 137                 res= _FAIL;
 138                 goto exit;
 139         }
 140         pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4);
 141         //pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 -
 142         //                                              ((SIZE_PTR) (pxmitpriv->pallocated_frame_buf) &3);
 143
 144         pxframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
 145
 146         for (i = 0; i < NR_XMITFRAME; i++)
 147         {
 148                 _rtw_init_listhead(&(pxframe->list));
 149
 150                 pxframe->padapter = padapter;
 151                 pxframe->frame_tag = NULL_FRAMETAG;
 152
 153                 pxframe->pkt = NULL;
 154
 155                 pxframe->buf_addr = NULL;
 156                 pxframe->pxmitbuf = NULL;
 157
 158                 rtw_list_insert_tail(&(pxframe->list), &(pxmitpriv->free_xmit_queue.queue));
 159
 160                 pxframe++;
 161         }
 162
 163         pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
 164
 165         pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
 166
 167
 168         //init xmit_buf
 169         _rtw_init_queue(&pxmitpriv->free_xmitbuf_queue);
 170         _rtw_init_queue(&pxmitpriv->pending_xmitbuf_queue);
 171
 172         pxmitpriv->pallocated_xmitbuf = rtw_zvmalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
 173
 174         if (pxmitpriv->pallocated_xmitbuf  == NULL){
 175                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xmit_buf fail!\n"));
 176                 res= _FAIL;
 177                 goto exit;
 178         }
 179
 180         pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4);
 181         //pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 -
 182         //                                              ((SIZE_PTR) (pxmitpriv->pallocated_xmitbuf) &3);
 183
 184         pxmitbuf = (struct xmit_buf*)pxmitpriv->pxmitbuf;
 185
 186         for (i = 0; i < NR_XMITBUFF; i++)
 187         {
 188                 _rtw_init_listhead(&pxmitbuf->list);
 189
 190                 pxmitbuf->priv_data = NULL;
 191                 pxmitbuf->padapter = padapter;
 192                 pxmitbuf->ext_tag = _FALSE;
 193
 194 /*
 195                 pxmitbuf->pallocated_buf = rtw_zmalloc(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ);
 196                 if (pxmitbuf->pallocated_buf == NULL)
 197                 {
 198                         res = _FAIL;
 199                         goto exit;
 200                 }
 201
 202                 pxmitbuf->pbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitbuf->pallocated_buf), XMITBUF_ALIGN_SZ);
 203                 //pxmitbuf->pbuf = pxmitbuf->pallocated_buf + XMITBUF_ALIGN_SZ -((SIZE_PTR) (pxmitbuf->pallocated_buf) &(XMITBUF_ALIGN_SZ-1));
 204 */
 205                 /* Tx buf allocation may fail sometimes, so sleep and retry. */
 206                 if((res=rtw_os_xmit_resource_alloc(padapter, pxmitbuf,(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ))) == _FAIL) {
 207                         rtw_msleep_os(10);
 208                         res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf,(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
 209                         if (res == _FAIL) {
 210                                 goto exit;
 211                         }
 212                 }
 213
 214 #if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
 215                 pxmitbuf->phead = pxmitbuf->pbuf;
 216                 pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMITBUF_SZ;
 217                 pxmitbuf->len = 0;
 218                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
 219 #endif
 220
 221                 pxmitbuf->flags = XMIT_VO_QUEUE;
 222
 223                 rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmitbuf_queue.queue));
 224                 #ifdef DBG_XMIT_BUF
 225                 pxmitbuf->no=i;
 226                 #endif
 227
 228                 pxmitbuf++;
 229
 230         }
 231
 232         pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
 233
 234         /* init xframe_ext queue,  the same count as extbuf  */
 235         _rtw_init_queue(&pxmitpriv->free_xframe_ext_queue);
 236
 237         pxmitpriv->xframe_ext_alloc_addr = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_frame) + 4);
 238
 239         if (pxmitpriv->xframe_ext_alloc_addr  == NULL){
 240                 pxmitpriv->xframe_ext = NULL;
 241                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xframe_ext fail!\n"));
 242                 res= _FAIL;
 243                 goto exit;
 244         }
 245         pxmitpriv->xframe_ext = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->xframe_ext_alloc_addr), 4);
 246         pxframe = (struct xmit_frame*)pxmitpriv->xframe_ext;
 247
 248         for (i = 0; i < num_xmit_extbuf; i++) {
 249                 _rtw_init_listhead(&(pxframe->list));
 250
 251                 pxframe->padapter = padapter;
 252                 pxframe->frame_tag = NULL_FRAMETAG;
 253
 254                 pxframe->pkt = NULL;
 255
 256                 pxframe->buf_addr = NULL;
 257                 pxframe->pxmitbuf = NULL;
 258
 259                 pxframe->ext_tag = 1;
 260
 261                 rtw_list_insert_tail(&(pxframe->list), &(pxmitpriv->free_xframe_ext_queue.queue));
 262
 263                 pxframe++;
 264         }
 265         pxmitpriv->free_xframe_ext_cnt = num_xmit_extbuf;
 266
 267         // Init xmit extension buff
 268         _rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
 269
 270         pxmitpriv->pallocated_xmit_extbuf = rtw_zvmalloc(num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
 271
 272         if (pxmitpriv->pallocated_xmit_extbuf  == NULL){
 273                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("alloc xmit_extbuf fail!\n"));
 274                 res= _FAIL;
 275                 goto exit;
 276         }
 277
 278         pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4);
 279
 280         pxmitbuf = (struct xmit_buf*)pxmitpriv->pxmit_extbuf;
 281
 282         for (i = 0; i < num_xmit_extbuf; i++)
 283         {
 284                 _rtw_init_listhead(&pxmitbuf->list);
 285
 286                 pxmitbuf->priv_data = NULL;
 287                 pxmitbuf->padapter = padapter;
 288                 pxmitbuf->ext_tag = _TRUE;
 289
 290 /*
 291                 pxmitbuf->pallocated_buf = rtw_zmalloc(max_xmit_extbuf_size);
 292                 if (pxmitbuf->pallocated_buf == NULL)
 293                 {
 294                         res = _FAIL;
 295                         goto exit;
 296                 }
 297
 298                 pxmitbuf->pbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitbuf->pallocated_buf), 4);
 299 */
 300
 301                 if((res=rtw_os_xmit_resource_alloc(padapter, pxmitbuf,max_xmit_extbuf_size + XMITBUF_ALIGN_SZ)) == _FAIL) {
 302                         res= _FAIL;
 303                         goto exit;
 304                 }
 305
 306 #if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
 307                 pxmitbuf->phead = pxmitbuf->pbuf;
 308                 pxmitbuf->pend = pxmitbuf->pbuf + max_xmit_extbuf_size;
 309                 pxmitbuf->len = 0;
 310                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
 311 #endif
 312
 313                 rtw_list_insert_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue));
 314                 #ifdef DBG_XMIT_BUF_EXT
 315                 pxmitbuf->no=i;
 316                 #endif
 317                 pxmitbuf++;
 318
 319         }
 320
 321         pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf;
 322
 323         rtw_alloc_hwxmits(padapter);
 324         rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
 325
 326         for (i = 0; i < 4; i ++)
 327         {
 328                 pxmitpriv->wmm_para_seq[i] = i;
 329         }
 330
 331 #ifdef CONFIG_USB_HCI
 332         pxmitpriv->txirp_cnt=1;
 333
 334         _rtw_init_sema(&(pxmitpriv->tx_retevt), 0);
 335
 336         //per AC pending irp
 337         pxmitpriv->beq_cnt = 0;
 338         pxmitpriv->bkq_cnt = 0;
 339         pxmitpriv->viq_cnt = 0;
 340         pxmitpriv->voq_cnt = 0;
 341 #endif
 342
 343
 344 #ifdef CONFIG_XMIT_ACK
 345         pxmitpriv->ack_tx = _FALSE;
 346         _rtw_mutex_init(&pxmitpriv->ack_tx_mutex);
 347         rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0);
 348 #endif
 349
 350         rtw_hal_init_xmit_priv(padapter);
 351
 352 exit:
 353
 354 _func_exit_;
 355
 356         return res;
 357 }
 358
 359 void  rtw_mfree_xmit_priv_lock (struct xmit_priv *pxmitpriv);
 360 void  rtw_mfree_xmit_priv_lock (struct xmit_priv *pxmitpriv)
 361 {
 362         _rtw_spinlock_free(&pxmitpriv->lock);
 363         _rtw_free_sema(&pxmitpriv->xmit_sema);
 364         _rtw_free_sema(&pxmitpriv->terminate_xmitthread_sema);
 365
 366         _rtw_spinlock_free(&pxmitpriv->be_pending.lock);
 367         _rtw_spinlock_free(&pxmitpriv->bk_pending.lock);
 368         _rtw_spinlock_free(&pxmitpriv->vi_pending.lock);
 369         _rtw_spinlock_free(&pxmitpriv->vo_pending.lock);
 370         _rtw_spinlock_free(&pxmitpriv->bm_pending.lock);
 371
 372         //_rtw_spinlock_free(&pxmitpriv->legacy_dz_queue.lock);
 373         //_rtw_spinlock_free(&pxmitpriv->apsd_queue.lock);
 374
 375         _rtw_spinlock_free(&pxmitpriv->free_xmit_queue.lock);
 376         _rtw_spinlock_free(&pxmitpriv->free_xmitbuf_queue.lock);
 377         _rtw_spinlock_free(&pxmitpriv->pending_xmitbuf_queue.lock);
 378 }
 379
 380
 381 void _rtw_free_xmit_priv (struct xmit_priv *pxmitpriv)
 382 {
 383        int i;
 384       _adapter *padapter = pxmitpriv->adapter;
 385         struct xmit_frame       *pxmitframe = (struct xmit_frame*) pxmitpriv->pxmit_frame_buf;
 386         struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
 387         u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
 388         u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
 389
 390 #if defined(CONFIG_MP_INCLUDED) && defined(CONFIG_RTL8723A)
 391         if (padapter->registrypriv.mp_mode) {
 392                         max_xmit_extbuf_size = 6000;
 393                         num_xmit_extbuf = 8;
 394         }
 395 #endif
 396
 397  _func_enter_;
 398
 399         rtw_hal_free_xmit_priv(padapter);
 400
 401         rtw_mfree_xmit_priv_lock(pxmitpriv);
 402
 403         if(pxmitpriv->pxmit_frame_buf==NULL)
 404                 goto out;
 405
 406         for(i=0; i<NR_XMITFRAME; i++)
 407         {
 408                 rtw_os_xmit_complete(padapter, pxmitframe);
 409
 410                 pxmitframe++;
 411         }
 412
 413         for(i=0; i<NR_XMITBUFF; i++)
 414         {
 415                 rtw_os_xmit_resource_free(padapter, pxmitbuf,(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
 416
 417                 //if(pxmitbuf->pallocated_buf)
 418                 //      rtw_mfree(pxmitbuf->pallocated_buf, MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ);
 419
 420                 pxmitbuf++;
 421         }
 422
 423         if(pxmitpriv->pallocated_frame_buf) {
 424                 rtw_vmfree(pxmitpriv->pallocated_frame_buf, NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
 425         }
 426
 427
 428         if(pxmitpriv->pallocated_xmitbuf) {
 429                 rtw_vmfree(pxmitpriv->pallocated_xmitbuf, NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
 430         }
 431
 432         /* free xframe_ext queue,  the same count as extbuf  */
 433         if ((pxmitframe = (struct xmit_frame*)pxmitpriv->xframe_ext)) {
 434                 for (i=0; i<num_xmit_extbuf; i++) {
 435                         rtw_os_xmit_complete(padapter, pxmitframe);
 436                         pxmitframe++;
 437                 }
 438         }
 439         if (pxmitpriv->xframe_ext_alloc_addr)
 440                 rtw_vmfree(pxmitpriv->xframe_ext_alloc_addr, num_xmit_extbuf * sizeof(struct xmit_frame) + 4);
 441         _rtw_spinlock_free(&pxmitpriv->free_xframe_ext_queue.lock);
 442
 443         // free xmit extension buff
 444         _rtw_spinlock_free(&pxmitpriv->free_xmit_extbuf_queue.lock);
 445
 446         pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
 447         for(i=0; i<num_xmit_extbuf; i++)
 448         {
 449                 rtw_os_xmit_resource_free(padapter, pxmitbuf,(max_xmit_extbuf_size + XMITBUF_ALIGN_SZ));
 450
 451                 //if(pxmitbuf->pallocated_buf)
 452                 //      rtw_mfree(pxmitbuf->pallocated_buf, max_xmit_extbuf_size);
 453
 454                 pxmitbuf++;
 455         }
 456
 457         if(pxmitpriv->pallocated_xmit_extbuf) {
 458                 rtw_vmfree(pxmitpriv->pallocated_xmit_extbuf, num_xmit_extbuf * sizeof(struct xmit_buf) + 4);
 459         }
 460
 461         rtw_free_hwxmits(padapter);
 462
 463 #ifdef CONFIG_XMIT_ACK
 464         _rtw_mutex_free(&pxmitpriv->ack_tx_mutex);
 465 #endif
 466
 467 out:
 468
 469 _func_exit_;
 470
 471 }
 472
 473 static void update_attrib_vcs_info(_adapter *padapter, struct xmit_frame *pxmitframe)
 474 {
 475         u32     sz;
 476         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
 477         struct sta_info *psta = pattrib->psta;
 478         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
 479         struct mlme_ext_info    *pmlmeinfo = &(pmlmeext->mlmext_info);
 480
 481         if(pattrib->psta)
 482         {
 483                 psta = pattrib->psta;
 484         }
 485         else
 486         {
 487                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
 488                 psta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0] );
 489         }
 490
 491         if(psta==NULL)
 492         {
 493                 DBG_871X("%s, psta==NUL\n", __func__);
 494                 return;
 495         }
 496
 497         if(!(psta->state &_FW_LINKED))
 498         {
 499                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
 500                 return;
 501         }
 502
 503         if (pattrib->nr_frags != 1)
 504         {
 505                 sz = padapter->xmitpriv.frag_len;
 506         }
 507         else //no frag
 508         {
 509                 sz = pattrib->last_txcmdsz;
 510         }
 511
 512         // (1) RTS_Threshold is compared to the MPDU, not MSDU.
 513         // (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame.
 514         //              Other fragments are protected by previous fragment.
 515         //              So we only need to check the length of first fragment.
 516         if(pmlmeext->cur_wireless_mode < WIRELESS_11_24N  || padapter->registrypriv.wifi_spec)
 517         {
 518                 if(sz > padapter->registrypriv.rts_thresh)
 519                 {
 520                         pattrib->vcs_mode = RTS_CTS;
 521                 }
 522                 else
 523                 {
 524                         if(psta->rtsen)
 525                                 pattrib->vcs_mode = RTS_CTS;
 526                         else if(psta->cts2self)
 527                                 pattrib->vcs_mode = CTS_TO_SELF;
 528                         else
 529                                 pattrib->vcs_mode = NONE_VCS;
 530                 }
 531         }
 532         else
 533         {
 534                 while (_TRUE)
 535                 {
 536 #if 0 //Todo
 537                         //check IOT action
 538                         if(pHTInfo->IOTAction & HT_IOT_ACT_FORCED_CTS2SELF)
 539                         {
 540                                 pattrib->vcs_mode = CTS_TO_SELF;
 541                                 pattrib->rts_rate = MGN_24M;
 542                                 break;
 543                         }
 544                         else if(pHTInfo->IOTAction & (HT_IOT_ACT_FORCED_RTS|HT_IOT_ACT_PURE_N_MODE))
 545                         {
 546                                 pattrib->vcs_mode = RTS_CTS;
 547                                 pattrib->rts_rate = MGN_24M;
 548                                 break;
 549                         }
 550 #endif
 551
 552                         //IOT action
 553                         if((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) && (pattrib->ampdu_en==_TRUE) &&
 554                                 (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_ ))
 555                         {
 556                                 pattrib->vcs_mode = CTS_TO_SELF;
 557                                 break;
 558                         }
 559
 560
 561                         //check ERP protection
 562                         if(psta->rtsen || psta->cts2self)
 563                         {
 564                                 if(psta->rtsen)
 565                                         pattrib->vcs_mode = RTS_CTS;
 566                                 else if(psta->cts2self)
 567                                         pattrib->vcs_mode = CTS_TO_SELF;
 568
 569                                 break;
 570                         }
 571
 572                         //check HT op mode
 573                         if(pattrib->ht_en)
 574                         {
 575                                 u8 HTOpMode = pmlmeinfo->HT_protection;
 576                                 if((pmlmeext->cur_bwmode && (HTOpMode == 2 || HTOpMode == 3)) ||
 577                                         (!pmlmeext->cur_bwmode && HTOpMode == 3) )
 578                                 {
 579                                         pattrib->vcs_mode = RTS_CTS;
 580                                         break;
 581                                 }
 582                         }
 583
 584                         //check rts
 585                         if(sz > padapter->registrypriv.rts_thresh)
 586                         {
 587                                 pattrib->vcs_mode = RTS_CTS;
 588                                 break;
 589                         }
 590
 591                         //to do list: check MIMO power save condition.
 592
 593                         //check AMPDU aggregation for TXOP
 594                         if(pattrib->ampdu_en==_TRUE)
 595                         {
 596                                 pattrib->vcs_mode = RTS_CTS;
 597                                 break;
 598                         }
 599
 600                         pattrib->vcs_mode = NONE_VCS;
 601                         break;
 602                 }
 603         }
 604 }
 605
 606 static void update_attrib_phy_info(_adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
 607 {
 608         struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
 609
 610         /*if(psta->rtsen)
 611                 pattrib->vcs_mode = RTS_CTS;
 612         else if(psta->cts2self)
 613                 pattrib->vcs_mode = CTS_TO_SELF;
 614         else
 615                 pattrib->vcs_mode = NONE_VCS;*/
 616
 617         pattrib->mdata = 0;
 618         pattrib->eosp = 0;
 619         pattrib->triggered=0;
 620
 621         //qos_en, ht_en, init rate, ,bw, ch_offset, sgi
 622         pattrib->qos_en = psta->qos_option;
 623
 624         pattrib->raid = psta->raid;
 625 #ifdef CONFIG_80211N_HT
 626         pattrib->ht_en = psta->htpriv.ht_option;
 627         if (mlmeext->cur_bwmode < psta->htpriv.bwmode)
 628                 pattrib->bwmode = mlmeext->cur_bwmode;
 629         else
 630                 pattrib->bwmode = psta->htpriv.bwmode;
 631         pattrib->ch_offset = psta->htpriv.ch_offset;
 632         pattrib->sgi= psta->htpriv.sgi;
 633         pattrib->ampdu_en = _FALSE;
 634 #endif //CONFIG_80211N_HT
 635         //if(pattrib->ht_en && psta->htpriv.ampdu_enable)
 636         //{
 637         //      if(psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
 638         //              pattrib->ampdu_en = _TRUE;
 639         //}
 640
 641
 642         pattrib->retry_ctrl = _FALSE;
 643
 644 }
 645
 646 u8      qos_acm(u8 acm_mask, u8 priority)
 647 {
 648         u8      change_priority = priority;
 649
 650         switch (priority)
 651         {
 652                 case 0:
 653                 case 3:
 654                         if(acm_mask & BIT(1))
 655                                 change_priority = 1;
 656                         break;
 657                 case 1:
 658                 case 2:
 659                         break;
 660                 case 4:
 661                 case 5:
 662                         if(acm_mask & BIT(2))
 663                                 change_priority = 0;
 664                         break;
 665                 case 6:
 666                 case 7:
 667                         if(acm_mask & BIT(3))
 668                                 change_priority = 5;
 669                         break;
 670                 default:
 671                         DBG_871X("qos_acm(): invalid pattrib->priority: %d!!!\n", priority);
 672                         break;
 673         }
 674
 675         return change_priority;
 676 }
 677
 678 static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
 679 {
 680         struct ethhdr etherhdr;
 681         struct iphdr ip_hdr;
 682         s32 UserPriority = 0;
 683
 684
 685         _rtw_open_pktfile(ppktfile->pkt, ppktfile);
 686         _rtw_pktfile_read(ppktfile, (unsigned char*)&etherhdr, ETH_HLEN);
 687
 688         // get UserPriority from IP hdr
 689         if (pattrib->ether_type == 0x0800) {
 690                 _rtw_pktfile_read(ppktfile, (u8*)&ip_hdr, sizeof(ip_hdr));
 691 //              UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3;
 692                 UserPriority = ip_hdr.tos >> 5;
 693         } else if (pattrib->ether_type == 0x888e) {
 694                 // "When priority processing of data frames is supported,
 695                 // a STA's SME should send EAPOL-Key frames at the highest priority."
 696                 UserPriority = 7;
 697         }
 698
 699         pattrib->priority = UserPriority;
 700         pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
 701         pattrib->subtype = WIFI_QOS_DATA_TYPE;
 702 }
 703
 704 static s32 update_attrib(_adapter *padapter, _pkt *pkt, struct pkt_attrib *pattrib)
 705 {
 706         uint i;
 707         struct pkt_file pktfile;
 708         struct sta_info *psta = NULL;
 709         struct ethhdr etherhdr;
 710
 711         sint bmcast;
 712         struct sta_priv         *pstapriv = &padapter->stapriv;
 713         struct security_priv    *psecuritypriv = &padapter->securitypriv;
 714         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
 715         struct qos_priv         *pqospriv= &pmlmepriv->qospriv;
 716         sint res = _SUCCESS;
 717
 718  _func_enter_;
 719
 720         DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib);
 721
 722         _rtw_open_pktfile(pkt, &pktfile);
 723         i = _rtw_pktfile_read(&pktfile, (u8*)&etherhdr, ETH_HLEN);
 724
 725         pattrib->ether_type = ntohs(etherhdr.h_proto);
 726
 727
 728         _rtw_memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
 729         _rtw_memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
 730
 731         pattrib->pctrl = 0;
 732
 733         if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) ||
 734                 (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) {
 735                 _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
 736                 _rtw_memcpy(pattrib->ta, myid(&padapter->eeprompriv), ETH_ALEN);
 737                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_adhoc);
 738         }
 739         else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 740                 _rtw_memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
 741                 _rtw_memcpy(pattrib->ta, myid(&padapter->eeprompriv), ETH_ALEN);
 742                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_sta);
 743         }
 744         else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 745                 _rtw_memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
 746                 _rtw_memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
 747                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_ap);
 748         }
 749         else
 750                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_unknown);
 751
 752         pattrib->pktlen = pktfile.pkt_len;
 753
 754         if (ETH_P_IP == pattrib->ether_type)
 755         {
 756                 // The following is for DHCP and ARP packet, we use cck1M to tx these packets and let LPS awake some time
 757                 // to prevent DHCP protocol fail
 758                 u8 tmp[24];
 759                 _rtw_pktfile_read(&pktfile, &tmp[0], 24);
 760                 pattrib->dhcp_pkt = 0;
 761                 if (pktfile.pkt_len > 282) {//MINIMUM_DHCP_PACKET_SIZE) {
 762                         if (ETH_P_IP == pattrib->ether_type) {// IP header
 763                                 if (((tmp[21] == 68) && (tmp[23] == 67)) ||
 764                                         ((tmp[21] == 67) && (tmp[23] == 68))) {
 765                                         // 68 : UDP BOOTP client
 766                                         // 67 : UDP BOOTP server
 767                                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("======================update_attrib: get DHCP Packet \n"));
 768                                         // Use low rate to send DHCP packet.
 769                                         //if(pMgntInfo->IOTAction & HT_IOT_ACT_WA_IOT_Broadcom)
 770                                         //{
 771                                         //      tcb_desc->DataRate = MgntQuery_TxRateExcludeCCKRates(ieee);//0xc;//ofdm 6m
 772                                         //      tcb_desc->bTxDisableRateFallBack = false;
 773                                         //}
 774                                         //else
 775                                         //      pTcb->DataRate = Adapter->MgntInfo.LowestBasicRate;
 776                                         //RTPRINT(FDM, WA_IOT, ("DHCP TranslateHeader(), pTcb->DataRate = 0x%x\n", pTcb->DataRate));
 777                                         pattrib->dhcp_pkt = 1;
 778                                         DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_dhcp);
 779                                 }
 780                         }
 781                 }
 782         } else if (0x888e == pattrib->ether_type) {
 783                 DBG_871X_LEVEL(_drv_always_, "send eapol packet\n");
 784         }
 785
 786         if ( (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1) )
 787         {
 788                 rtw_set_scan_deny(padapter, 3000);
 789         }
 790
 791 #ifdef CONFIG_LPS
 792         // If EAPOL , ARP , OR DHCP packet, driver must be in active mode.
 793 #ifdef CONFIG_WAPI_SUPPORT
 794         if ( (pattrib->ether_type == 0x88B4) || (pattrib->ether_type == 0x0806) || (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1) )
 795 #else
 796         if ( (pattrib->ether_type == 0x0806) || (pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1) )
 797 #endif
 798         {
 799                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_active);
 800                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
 801         }
 802 #endif
 803
 804         bmcast = IS_MCAST(pattrib->ra);
 805
 806         // get sta_info
 807         if (bmcast) {
 808                 psta = rtw_get_bcmc_stainfo(padapter);
 809         } else {
 810                 psta = rtw_get_stainfo(pstapriv, pattrib->ra);
 811                 if (psta == NULL)       { // if we cannot get psta => drrp the pkt
 812                         DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_sta);
 813                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT"\n", MAC_ARG(pattrib->ra)));
 814                         #ifdef DBG_TX_DROP_FRAME
 815                         DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __FUNCTION__, MAC_ARG(pattrib->ra));
 816                         #endif
 817                         res =_FAIL;
 818                         goto exit;
 819                 }
 820                 else if((check_fwstate(pmlmepriv, WIFI_AP_STATE)==_TRUE)&&(!(psta->state & _FW_LINKED)))
 821                 {
 822                         DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_ap_link);
 823                         res =_FAIL;
 824                         goto exit;
 825                 }
 826         }
 827
 828         if (psta)
 829         {
 830                 pattrib->mac_id = psta->mac_id;
 831                 //DBG_8192C("%s ==> mac_id(%d)\n",__FUNCTION__,pattrib->mac_id );
 832                 pattrib->psta = psta;
 833         }
 834         else
 835         {
 836                 // if we cannot get psta => drop the pkt
 837                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sta);
 838                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT "\n", MAC_ARG(pattrib->ra)));
 839                 #ifdef DBG_TX_DROP_FRAME
 840                 DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __FUNCTION__, MAC_ARG(pattrib->ra));
 841                 #endif
 842                 res = _FAIL;
 843                 goto exit;
 844         }
 845
 846         pattrib->ack_policy = 0;
 847         // get ether_hdr_len
 848         pattrib->pkt_hdrlen = ETH_HLEN;//(pattrib->ether_type == 0x8100) ? (14 + 4 ): 14; //vlan tag
 849
 850         pattrib->hdrlen = WLAN_HDR_A3_LEN;
 851         pattrib->subtype = WIFI_DATA_TYPE;
 852         pattrib->priority = 0;
 853
 854         if (check_fwstate(pmlmepriv, WIFI_AP_STATE|WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE))
 855         {
 856                 if(psta->qos_option)
 857                         set_qos(&pktfile, pattrib);
 858         }
 859         else
 860         {
 861                 if(pqospriv->qos_option)
 862                 {
 863                         set_qos(&pktfile, pattrib);
 864
 865                         if(pmlmepriv->acm_mask != 0)
 866                         {
 867                                 pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority);
 868                         }
 869                 }
 870         }
 871
 872         //pattrib->priority = 5; //force to used VI queue, for testing
 873
 874         if (psta->ieee8021x_blocked == _TRUE)
 875         {
 876                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("\n psta->ieee8021x_blocked == _TRUE \n"));
 877
 878                 pattrib->encrypt = 0;
 879
 880                 if((pattrib->ether_type != 0x888e) && (check_fwstate(pmlmepriv, WIFI_MP_STATE) == _FALSE))
 881                 {
 882                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("\npsta->ieee8021x_blocked == _TRUE,  pattrib->ether_type(%.4x) != 0x888e\n",pattrib->ether_type));
 883                         #ifdef DBG_TX_DROP_FRAME
 884                         DBG_871X("DBG_TX_DROP_FRAME %s psta->ieee8021x_blocked == _TRUE,  pattrib->ether_type(%04x) != 0x888e\n", __FUNCTION__,pattrib->ether_type);
 885                         #endif
 886                         res = _FAIL;
 887                         goto exit;
 888                 }
 889         }
 890         else
 891         {
 892                 GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
 893
 894 #ifdef CONFIG_WAPI_SUPPORT
 895                 if(pattrib->ether_type == 0x88B4)
 896                         pattrib->encrypt=_NO_PRIVACY_;
 897 #endif
 898
 899                 switch(psecuritypriv->dot11AuthAlgrthm)
 900                 {
 901                         case dot11AuthAlgrthm_Open:
 902                         case dot11AuthAlgrthm_Shared:
 903                         case dot11AuthAlgrthm_Auto:
 904                                 pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;
 905                                 break;
 906                         case dot11AuthAlgrthm_8021X:
 907                                 if(bmcast)
 908                                         pattrib->key_idx = (u8)psecuritypriv->dot118021XGrpKeyid;
 909                                 else
 910                                         pattrib->key_idx = 0;
 911                                 break;
 912                         default:
 913                                 pattrib->key_idx = 0;
 914                                 break;
 915                 }
 916
 917
 918         }
 919
 920         switch (pattrib->encrypt)
 921         {
 922                 case _WEP40_:
 923                 case _WEP104_:
 924                         pattrib->iv_len = 4;
 925                         pattrib->icv_len = 4;
 926                         break;
 927
 928                 case _TKIP_:
 929                         pattrib->iv_len = 8;
 930                         pattrib->icv_len = 4;
 931
 932                         if(padapter->securitypriv.busetkipkey==_FAIL)
 933                         {
 934                                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("\npadapter->securitypriv.busetkipkey(%d)==_FAIL drop packet\n", padapter->securitypriv.busetkipkey));
 935                                 #ifdef DBG_TX_DROP_FRAME
 936                                 DBG_871X("DBG_TX_DROP_FRAME %s padapter->securitypriv.busetkipkey(%d)==_FAIL drop packet\n", __FUNCTION__, padapter->securitypriv.busetkipkey);
 937                                 #endif
 938                                 res =_FAIL;
 939                                 goto exit;
 940                         }
 941
 942                         break;
 943                 case _AES_:
 944                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("pattrib->encrypt=%d (_AES_)\n",pattrib->encrypt));
 945                         pattrib->iv_len = 8;
 946                         pattrib->icv_len = 8;
 947                         break;
 948
 949 #ifdef CONFIG_WAPI_SUPPORT
 950                 case _SMS4_:
 951                         pattrib->iv_len = 18;
 952                         pattrib->icv_len = 16;
 953                         break;
 954 #endif
 955
 956                 default:
 957                         pattrib->iv_len = 0;
 958                         pattrib->icv_len = 0;
 959                         break;
 960         }
 961
 962         RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 963                  ("update_attrib: encrypt=%d  securitypriv.sw_encrypt=%d\n",
 964                   pattrib->encrypt, padapter->securitypriv.sw_encrypt));
 965
 966         if (pattrib->encrypt &&
 967             ((padapter->securitypriv.sw_encrypt == _TRUE) || (psecuritypriv->hw_decrypted == _FALSE)))
 968         {
 969                 pattrib->bswenc = _TRUE;
 970                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,
 971                          ("update_attrib: encrypt=%d securitypriv.hw_decrypted=%d bswenc=_TRUE\n",
 972                           pattrib->encrypt, padapter->securitypriv.sw_encrypt));
 973         } else {
 974                 pattrib->bswenc = _FALSE;
 975                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("update_attrib: bswenc=_FALSE\n"));
 976         }
 977
 978 #if defined(CONFIG_CONCURRENT_MODE) && !defined(DYNAMIC_CAMID_ALLOC)
 979         if((pattrib->encrypt && bmcast) || (pattrib->encrypt ==_WEP40_) || (pattrib->encrypt ==_WEP104_))
 980         {
 981                 pattrib->bswenc = _TRUE;//force using sw enc.
 982         }
 983 #endif
 984
 985 #ifdef CONFIG_WAPI_SUPPORT
 986         if(pattrib->encrypt == _SMS4_)
 987                 pattrib->bswenc = _FALSE;
 988 #endif
 989
 990         rtw_set_tx_chksum_offload(pkt, pattrib);
 991
 992         update_attrib_phy_info(padapter, pattrib, psta);
 993
 994 exit:
 995
 996 _func_exit_;
 997
 998         return res;
 999 }
1000
1001 static s32 xmitframe_addmic(_adapter *padapter, struct xmit_frame *pxmitframe){
1002         sint                    curfragnum,length;
1003         u8      *pframe, *payload,mic[8];
1004         struct  mic_data                micdata;
1005         struct  sta_info                *stainfo;
1006         struct  qos_priv   *pqospriv= &(padapter->mlmepriv.qospriv);
1007         struct  pkt_attrib       *pattrib = &pxmitframe->attrib;
1008         struct  security_priv   *psecuritypriv=&padapter->securitypriv;
1009         struct  xmit_priv               *pxmitpriv=&padapter->xmitpriv;
1010         u8 priority[4]={0x0,0x0,0x0,0x0};
1011         u8 hw_hdr_offset = 0;
1012         sint bmcst = IS_MCAST(pattrib->ra);
1013
1014         if(pattrib->psta)
1015         {
1016                 stainfo = pattrib->psta;
1017         }
1018         else
1019         {
1020                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
1021                 stainfo=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
1022         }
1023
1024         if(stainfo==NULL)
1025         {
1026                 DBG_871X("%s, psta==NUL\n", __func__);
1027                 return _FAIL;
1028         }
1029
1030         if(!(stainfo->state &_FW_LINKED))
1031         {
1032                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
1033                 return _FAIL;
1034         }
1035
1036 _func_enter_;
1037
1038 #ifdef CONFIG_USB_TX_AGGREGATION
1039         hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);;
1040 #else
1041         #ifdef CONFIG_TX_EARLY_MODE
1042         hw_hdr_offset = TXDESC_OFFSET+ EARLY_MODE_INFO_SIZE;
1043         #else
1044         hw_hdr_offset = TXDESC_OFFSET;
1045         #endif
1046 #endif
1047
1048         if(pattrib->encrypt ==_TKIP_)//if(psecuritypriv->dot11PrivacyAlgrthm==_TKIP_PRIVACY_)
1049         {
1050                 //encode mic code
1051                 if(stainfo!= NULL){
1052                         u8 null_key[16]={0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0,0x0};
1053
1054                         pframe = pxmitframe->buf_addr + hw_hdr_offset;
1055
1056                         if(bmcst)
1057                         {
1058                                 if(_rtw_memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16)==_TRUE){
1059                                         //DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey==0\n");
1060                                         //rtw_msleep_os(10);
1061                                         return _FAIL;
1062                                 }
1063                                 //start to calculate the mic code
1064                                 rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey);
1065                         }
1066                         else
1067                         {
1068                                 if(_rtw_memcmp(&stainfo->dot11tkiptxmickey.skey[0],null_key, 16)==_TRUE){
1069                                         //DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey==0\n");
1070                                         //rtw_msleep_os(10);
1071                                         return _FAIL;
1072                                 }
1073                                 //start to calculate the mic code
1074                                 rtw_secmicsetkey(&micdata, &stainfo->dot11tkiptxmickey.skey[0]);
1075                         }
1076
1077                         if(pframe[1]&1){   //ToDS==1
1078                                 rtw_secmicappend(&micdata, &pframe[16], 6);  //DA
1079                                 if(pframe[1]&2)  //From Ds==1
1080                                         rtw_secmicappend(&micdata, &pframe[24], 6);
1081                                 else
1082                                 rtw_secmicappend(&micdata, &pframe[10], 6);
1083                         }
1084                         else{   //ToDS==0
1085                                 rtw_secmicappend(&micdata, &pframe[4], 6);   //DA
1086                                 if(pframe[1]&2)  //From Ds==1
1087                                         rtw_secmicappend(&micdata, &pframe[16], 6);
1088                                 else
1089                                         rtw_secmicappend(&micdata, &pframe[10], 6);
1090
1091                         }
1092
1093                     //if(pqospriv->qos_option==1)
1094                     if(pattrib->qos_en)
1095                                 priority[0]=(u8)pxmitframe->attrib.priority;
1096
1097
1098                         rtw_secmicappend(&micdata, &priority[0], 4);
1099
1100                         payload=pframe;
1101
1102                         for(curfragnum=0;curfragnum<pattrib->nr_frags;curfragnum++){
1103                                 payload=(u8 *)RND4((SIZE_PTR)(payload));
1104                                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("===curfragnum=%d, pframe= 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
1105                                         curfragnum,*payload, *(payload+1),*(payload+2),*(payload+3),*(payload+4),*(payload+5),*(payload+6),*(payload+7)));
1106
1107                                 payload=payload+pattrib->hdrlen+pattrib->iv_len;
1108                                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("curfragnum=%d pattrib->hdrlen=%d pattrib->iv_len=%d",curfragnum,pattrib->hdrlen,pattrib->iv_len));
1109                                 if((curfragnum+1)==pattrib->nr_frags){
1110                                         length=pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-( (pattrib->bswenc) ? pattrib->icv_len : 0);
1111                                         rtw_secmicappend(&micdata, payload,length);
1112                                         payload=payload+length;
1113                                 }
1114                                 else{
1115                                         length=pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-( (pattrib->bswenc) ? pattrib->icv_len : 0);
1116                                         rtw_secmicappend(&micdata, payload, length);
1117                                         payload=payload+length+pattrib->icv_len;
1118                                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("curfragnum=%d length=%d pattrib->icv_len=%d",curfragnum,length,pattrib->icv_len));
1119                                 }
1120                         }
1121                         rtw_secgetmic(&micdata,&(mic[0]));
1122                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("xmitframe_addmic: before add mic code!!!\n"));
1123                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("xmitframe_addmic: pattrib->last_txcmdsz=%d!!!\n",pattrib->last_txcmdsz));
1124                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("xmitframe_addmic: mic[0]=0x%.2x ,mic[1]=0x%.2x ,mic[2]=0x%.2x ,mic[3]=0x%.2x \n\
1125   mic[4]=0x%.2x ,mic[5]=0x%.2x ,mic[6]=0x%.2x ,mic[7]=0x%.2x !!!!\n",
1126                                 mic[0],mic[1],mic[2],mic[3],mic[4],mic[5],mic[6],mic[7]));
1127                         //add mic code  and add the mic code length in last_txcmdsz
1128
1129                         _rtw_memcpy(payload, &(mic[0]),8);
1130                         pattrib->last_txcmdsz+=8;
1131
1132                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("\n ========last pkt========\n"));
1133                         payload=payload-pattrib->last_txcmdsz+8;
1134                         for(curfragnum=0;curfragnum<pattrib->last_txcmdsz;curfragnum=curfragnum+8)
1135                                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,(" %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x ",
1136                                         *(payload+curfragnum), *(payload+curfragnum+1), *(payload+curfragnum+2),*(payload+curfragnum+3),
1137                                         *(payload+curfragnum+4),*(payload+curfragnum+5),*(payload+curfragnum+6),*(payload+curfragnum+7)));
1138                         }
1139                         else{
1140                                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("xmitframe_addmic: rtw_get_stainfo==NULL!!!\n"));
1141                         }
1142         }
1143
1144 _func_exit_;
1145
1146         return _SUCCESS;
1147 }
1148
1149 static s32 xmitframe_swencrypt(_adapter *padapter, struct xmit_frame *pxmitframe){
1150
1151         struct  pkt_attrib       *pattrib = &pxmitframe->attrib;
1152         //struct        security_priv   *psecuritypriv=&padapter->securitypriv;
1153
1154 _func_enter_;
1155
1156         //if((psecuritypriv->sw_encrypt)||(pattrib->bswenc))
1157         if(pattrib->bswenc)
1158         {
1159                 //DBG_871X("start xmitframe_swencrypt\n");
1160                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_alert_,("### xmitframe_swencrypt\n"));
1161                 switch(pattrib->encrypt){
1162                 case _WEP40_:
1163                 case _WEP104_:
1164                         rtw_wep_encrypt(padapter, (u8 *)pxmitframe);
1165                         break;
1166                 case _TKIP_:
1167                         rtw_tkip_encrypt(padapter, (u8 *)pxmitframe);
1168                         break;
1169                 case _AES_:
1170                         rtw_aes_encrypt(padapter, (u8 * )pxmitframe);
1171                         break;
1172 #ifdef CONFIG_WAPI_SUPPORT
1173                 case _SMS4_:
1174                         rtw_sms4_encrypt(padapter, (u8 * )pxmitframe);
1175 #endif
1176                 default:
1177                                 break;
1178                 }
1179
1180         } else {
1181                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_notice_,("### xmitframe_hwencrypt\n"));
1182         }
1183
1184 _func_exit_;
1185
1186         return _SUCCESS;
1187 }
1188
1189 s32 rtw_make_wlanhdr (_adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib)
1190 {
1191         u16 *qc;
1192
1193         struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr;
1194         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1195         struct qos_priv *pqospriv = &pmlmepriv->qospriv;
1196         u8 qos_option = _FALSE;
1197 #ifdef CONFIG_TDLS
1198         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
1199         struct sta_priv         *pstapriv = &padapter->stapriv;
1200         struct sta_info *ptdls_sta=NULL, *psta_backup=NULL;
1201         u8 direct_link=0;
1202 #endif //CONFIG_TDLS
1203
1204         sint res = _SUCCESS;
1205         u16 *fctrl = &pwlanhdr->frame_ctl;
1206
1207         struct sta_info *psta;
1208
1209         sint bmcst = IS_MCAST(pattrib->ra);
1210
1211 _func_enter_;
1212
1213         if (pattrib->psta) {
1214                 psta = pattrib->psta;
1215         } else {
1216                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
1217                 if(bmcst) {
1218                         psta = rtw_get_bcmc_stainfo(padapter);
1219                 } else {
1220                         psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
1221                 }
1222         }
1223
1224         if(psta==NULL)
1225         {
1226                 DBG_871X("%s, psta==NUL\n", __func__);
1227                 return _FAIL;
1228         }
1229
1230         if(!(psta->state &_FW_LINKED))
1231         {
1232                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
1233                 return _FAIL;
1234         }
1235
1236         _rtw_memset(hdr, 0, WLANHDR_OFFSET);
1237
1238         SetFrameSubType(fctrl, pattrib->subtype);
1239
1240         if (pattrib->subtype & WIFI_DATA_TYPE)
1241         {
1242                 if ((check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == _TRUE)) {
1243 #ifdef CONFIG_TDLS
1244                         if((ptdlsinfo->setup_state == TDLS_LINKED_STATE)){
1245                                 ptdls_sta = rtw_get_stainfo(pstapriv, pattrib->dst);
1246                                 if((ptdls_sta!=NULL)&&(ptdls_sta->tdls_sta_state & TDLS_LINKED_STATE)&&(pattrib->ether_type!=0x0806)){
1247                                         //TDLS data transfer, ToDS=0, FrDs=0
1248                                         _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
1249                                         _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
1250                                         _rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
1251                                         direct_link=1;
1252                                 }else{
1253                                         // 1.Data transfer to AP
1254                                         // 2.Arp pkt will relayed by AP
1255                                         SetToDs(fctrl);
1256                                         _rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
1257                                         _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
1258                                         _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
1259                                 }
1260                         }else
1261 #endif //CONFIG_TDLS
1262                         {
1263                                 //to_ds = 1, fr_ds = 0;
1264                                 //Data transfer to AP
1265                                 SetToDs(fctrl);
1266                                 _rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
1267                                 _rtw_memcpy(pwlanhdr->addr2, pattrib->ta, ETH_ALEN);
1268                                 _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
1269                         }
1270
1271                         if (pqospriv->qos_option)
1272                                 qos_option = _TRUE;
1273
1274                 }
1275                 else if ((check_fwstate(pmlmepriv,  WIFI_AP_STATE) == _TRUE) ) {
1276                         //to_ds = 0, fr_ds = 1;
1277                         SetFrDs(fctrl);
1278                         _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
1279                         _rtw_memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
1280                         _rtw_memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
1281
1282                         if(psta->qos_option)
1283                                 qos_option = _TRUE;
1284                 }
1285                 else if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE) ||
1286                 (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE)) {
1287                         _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
1288                         _rtw_memcpy(pwlanhdr->addr2, pattrib->ta, ETH_ALEN);
1289                         _rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
1290
1291                         if(psta->qos_option)
1292                                 qos_option = _TRUE;
1293                 }
1294                 else {
1295                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("fw_state:%x is not allowed to xmit frame\n", get_fwstate(pmlmepriv)));
1296                         res = _FAIL;
1297                         goto exit;
1298                 }
1299
1300                 if(pattrib->mdata)
1301                         SetMData(fctrl);
1302
1303                 if (pattrib->encrypt)
1304                         SetPrivacy(fctrl);
1305
1306                 if (qos_option)
1307                 {
1308                         qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
1309
1310                         if (pattrib->priority)
1311                                 SetPriority(qc, pattrib->priority);
1312
1313                         SetEOSP(qc, pattrib->eosp);
1314
1315                         SetAckpolicy(qc, pattrib->ack_policy);
1316                 }
1317
1318                 //TODO: fill HT Control Field
1319
1320                 //Update Seq Num will be handled by f/w
1321                 {
1322                         if(psta){
1323 #ifdef CONFIG_TDLS
1324                                 if(direct_link==1)
1325                                 {
1326                                         psta_backup = psta;
1327                                         psta = ptdls_sta;
1328                                 }
1329 #endif //CONFIG_TDLS
1330
1331                                 psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
1332                                 psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
1333
1334                                 pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
1335
1336                                 SetSeqNum(hdr, pattrib->seqnum);
1337
1338 #ifdef CONFIG_80211N_HT
1339                                 //check if enable ampdu
1340                                 if(pattrib->ht_en && psta->htpriv.ampdu_enable)
1341                                 {
1342                                         if(psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
1343                                         pattrib->ampdu_en = _TRUE;
1344                                 }
1345
1346                                 //re-check if enable ampdu by BA_starting_seqctrl
1347                                 if(pattrib->ampdu_en == _TRUE)
1348                                 {
1349                                         u16 tx_seq;
1350
1351                                         tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
1352
1353                                         //check BA_starting_seqctrl
1354                                         if(SN_LESS(pattrib->seqnum, tx_seq))
1355                                         {
1356                                                 //DBG_871X("tx ampdu seqnum(%d) < tx_seq(%d)\n", pattrib->seqnum, tx_seq);
1357                                                 pattrib->ampdu_en = _FALSE;//AGG BK
1358                                         }
1359                                         else if(SN_EQUAL(pattrib->seqnum, tx_seq))
1360                                         {
1361                                                 psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq+1)&0xfff;
1362
1363                                                 pattrib->ampdu_en = _TRUE;//AGG EN
1364                                         }
1365                                         else
1366                                         {
1367                                                 //DBG_871X("tx ampdu over run\n");
1368                                                 psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff;
1369                                                 pattrib->ampdu_en = _TRUE;//AGG EN
1370                                         }
1371
1372                                 }
1373 #endif //CONFIG_80211N_HT
1374 #ifdef CONFIG_TDLS
1375                                 if(direct_link==1)
1376                                 {
1377                                         if (pattrib->encrypt){
1378                                                 pattrib->encrypt= _AES_;
1379                                                 pattrib->iv_len=8;
1380                                                 pattrib->icv_len=8;
1381                                         }
1382
1383                                         //qos_en, ht_en, init rate, ,bw, ch_offset, sgi
1384                                         //pattrib->qos_en = ptdls_sta->qos_option;
1385
1386                                         pattrib->raid = ptdls_sta->raid;
1387 #ifdef CONFIG_80211N_HT
1388                                         pattrib->bwmode = ptdls_sta->htpriv.bwmode;
1389                                         pattrib->ht_en = ptdls_sta->htpriv.ht_option;
1390                                         pattrib->ch_offset = ptdls_sta->htpriv.ch_offset;
1391                                         pattrib->sgi= ptdls_sta->htpriv.sgi;
1392 #endif //CONFIG_80211N_HT
1393                                         pattrib->mac_id = ptdls_sta->mac_id;
1394
1395                                         psta = psta_backup;
1396                                 }
1397 #endif //CONFIG_TDLS
1398
1399                         }
1400                 }
1401
1402         }
1403         else
1404         {
1405
1406         }
1407
1408 exit:
1409
1410 _func_exit_;
1411
1412         return res;
1413 }
1414
1415 s32 rtw_txframes_pending(_adapter *padapter)
1416 {
1417         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1418
1419         return ((_rtw_queue_empty(&pxmitpriv->be_pending) == _FALSE) ||
1420                          (_rtw_queue_empty(&pxmitpriv->bk_pending) == _FALSE) ||
1421                          (_rtw_queue_empty(&pxmitpriv->vi_pending) == _FALSE) ||
1422                          (_rtw_queue_empty(&pxmitpriv->vo_pending) == _FALSE));
1423 }
1424
1425 s32 rtw_txframes_sta_ac_pending(_adapter *padapter, struct pkt_attrib *pattrib)
1426 {
1427         struct sta_info *psta;
1428         struct tx_servq *ptxservq;
1429         int priority = pattrib->priority;
1430
1431         if(pattrib->psta)
1432         {
1433                 psta = pattrib->psta;
1434         }
1435         else
1436         {
1437                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
1438                 psta=rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
1439         }
1440
1441         if(psta==NULL)
1442         {
1443                 DBG_871X("%s, psta==NUL\n", __func__);
1444                 return 0;
1445         }
1446
1447         if(!(psta->state &_FW_LINKED))
1448         {
1449                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
1450                 return 0;
1451         }
1452
1453         switch(priority)
1454         {
1455                         case 1:
1456                         case 2:
1457                                 ptxservq = &(psta->sta_xmitpriv.bk_q);
1458                                 break;
1459                         case 4:
1460                         case 5:
1461                                 ptxservq = &(psta->sta_xmitpriv.vi_q);
1462                                 break;
1463                         case 6:
1464                         case 7:
1465                                 ptxservq = &(psta->sta_xmitpriv.vo_q);
1466                                 break;
1467                         case 0:
1468                         case 3:
1469                         default:
1470                                 ptxservq = &(psta->sta_xmitpriv.be_q);
1471                         break;
1472
1473         }
1474
1475         return ptxservq->qcnt;
1476 }
1477
1478 #ifdef CONFIG_TDLS
1479
1480 int rtw_build_tdls_ies(_adapter * padapter, struct xmit_frame * pxmitframe, u8 *pframe, u8 action)
1481 {
1482         int res=_SUCCESS;
1483
1484         switch(action){
1485                 case TDLS_SETUP_REQUEST:
1486                         rtw_build_tdls_setup_req_ies(padapter, pxmitframe, pframe);
1487                         break;
1488                 case TDLS_SETUP_RESPONSE:
1489                         rtw_build_tdls_setup_rsp_ies(padapter, pxmitframe, pframe);
1490                         break;
1491                 case TDLS_SETUP_CONFIRM:
1492                         rtw_build_tdls_setup_cfm_ies(padapter, pxmitframe, pframe);
1493                         break;
1494                 case TDLS_TEARDOWN:
1495                         rtw_build_tdls_teardown_ies(padapter, pxmitframe, pframe);
1496                         break;
1497                 case TDLS_DISCOVERY_REQUEST:
1498                         rtw_build_tdls_dis_req_ies(padapter, pxmitframe, pframe);
1499                         break;
1500                 case TDLS_PEER_TRAFFIC_INDICATION:
1501                         rtw_build_tdls_peer_traffic_indication_ies(padapter, pxmitframe, pframe);
1502                         break;
1503                 case TDLS_CHANNEL_SWITCH_REQUEST:
1504                         rtw_build_tdls_ch_switch_req_ies(padapter, pxmitframe, pframe);
1505                         break;
1506                 case TDLS_CHANNEL_SWITCH_RESPONSE:
1507                         rtw_build_tdls_ch_switch_rsp_ies(padapter, pxmitframe, pframe);
1508                         break;
1509 #ifdef CONFIG_WFD
1510                 case TUNNELED_PROBE_REQ:
1511                         rtw_build_tunneled_probe_req_ies(padapter, pxmitframe, pframe);
1512                         break;
1513                 case TUNNELED_PROBE_RSP:
1514                         rtw_build_tunneled_probe_rsp_ies(padapter, pxmitframe, pframe);
1515                         break;
1516 #endif //CONFIG_WFD
1517                 default:
1518                         res=_FAIL;
1519                         break;
1520         }
1521
1522         return res;
1523 }
1524
1525 s32 rtw_make_tdls_wlanhdr (_adapter *padapter , u8 *hdr, struct pkt_attrib *pattrib, u8 action)
1526 {
1527         u16 *qc;
1528         struct rtw_ieee80211_hdr *pwlanhdr = (struct rtw_ieee80211_hdr *)hdr;
1529         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1530         struct qos_priv *pqospriv = &pmlmepriv->qospriv;
1531         struct sta_priv         *pstapriv = &padapter->stapriv;
1532         struct sta_info *psta=NULL, *ptdls_sta=NULL;
1533         u8 tdls_seq=0, baddr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
1534
1535         sint res = _SUCCESS;
1536         u16 *fctrl = &pwlanhdr->frame_ctl;
1537
1538 _func_enter_;
1539
1540         _rtw_memset(hdr, 0, WLANHDR_OFFSET);
1541
1542         SetFrameSubType(fctrl, pattrib->subtype);
1543
1544         switch(action){
1545                 case TDLS_SETUP_REQUEST:
1546                 case TDLS_SETUP_RESPONSE:
1547                 case TDLS_SETUP_CONFIRM:
1548                 case TDLS_TEARDOWN:     //directly to peer STA or via AP
1549                 case TDLS_PEER_TRAFFIC_INDICATION:
1550                 case TDLS_PEER_PSM_REQUEST:     //directly to peer STA or via AP
1551                 case TUNNELED_PROBE_REQ:
1552                 case TUNNELED_PROBE_RSP:
1553                         SetToDs(fctrl);
1554                         _rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
1555                         _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
1556                         _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
1557                         break;
1558                 case TDLS_CHANNEL_SWITCH_REQUEST:
1559                 case TDLS_CHANNEL_SWITCH_RESPONSE:
1560                 case TDLS_PEER_PSM_RESPONSE:
1561                 case TDLS_PEER_TRAFFIC_RESPONSE:
1562                         _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
1563                         _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
1564                         _rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
1565                         tdls_seq=1;
1566                         break;
1567                 case TDLS_DISCOVERY_REQUEST:    //unicast: directly to peer sta, Bcast: via AP
1568                         if(_rtw_memcmp(pattrib->dst, baddr, ETH_ALEN) )
1569                         {
1570                                 SetToDs(fctrl);
1571                                 _rtw_memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
1572                                 _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
1573                                 _rtw_memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
1574                         }
1575                         else
1576                         {
1577                                 _rtw_memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
1578                                 _rtw_memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
1579                                 _rtw_memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
1580                                 tdls_seq=1;
1581                         }
1582                         break;
1583         }
1584
1585         if (pattrib->encrypt)
1586                 SetPrivacy(fctrl);
1587
1588         if (pqospriv->qos_option)
1589         {
1590                 qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
1591                 if (pattrib->priority)
1592                         SetPriority(qc, pattrib->priority);
1593                 SetAckpolicy(qc, pattrib->ack_policy);
1594         }
1595
1596         psta = pattrib->psta;
1597
1598         //  1. update seq_num per link by sta_info
1599         //  2. rewrite encrypt to _AES_, also rewrite iv_len, icv_len
1600         if(tdls_seq==1){
1601                 ptdls_sta=rtw_get_stainfo(pstapriv, pattrib->dst);
1602                 if(ptdls_sta){
1603                         ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
1604                         ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
1605                         pattrib->seqnum = ptdls_sta->sta_xmitpriv.txseq_tid[pattrib->priority];
1606                         SetSeqNum(hdr, pattrib->seqnum);
1607
1608                         if (pattrib->encrypt){
1609                                 pattrib->encrypt= _AES_;
1610                                 pattrib->iv_len=8;
1611                                 pattrib->icv_len=8;
1612                         }
1613                 }else{
1614                         res=_FAIL;
1615                         goto exit;
1616                 }
1617         }else if(psta){
1618                 psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
1619                 psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
1620                 pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
1621                 SetSeqNum(hdr, pattrib->seqnum);
1622         }
1623
1624
1625 exit:
1626
1627 _func_exit_;
1628
1629         return res;
1630 }
1631
1632 s32 rtw_xmit_tdls_coalesce(_adapter * padapter, struct xmit_frame * pxmitframe, u8 action)
1633 {
1634         s32 llc_sz;
1635
1636         u8 *pframe, *mem_start;
1637
1638         struct sta_info         *psta;
1639         struct sta_priv         *pstapriv = &padapter->stapriv;
1640         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
1641         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
1642         u8 *pbuf_start;
1643         s32 bmcst = IS_MCAST(pattrib->ra);
1644         s32 res = _SUCCESS;
1645
1646 _func_enter_;
1647
1648         if (pattrib->psta) {
1649                 psta = pattrib->psta;
1650         } else {
1651                 if(bmcst) {
1652                         psta = rtw_get_bcmc_stainfo(padapter);
1653                 } else {
1654                         psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
1655                 }
1656         }
1657
1658         if(psta==NULL)
1659                 return _FAIL;
1660
1661         if (pxmitframe->buf_addr == NULL)
1662                 return _FAIL;
1663
1664         pbuf_start = pxmitframe->buf_addr;
1665         mem_start = pbuf_start + TXDESC_OFFSET;
1666
1667         if (rtw_make_tdls_wlanhdr(padapter, mem_start, pattrib, action) == _FAIL) {
1668                 res = _FAIL;
1669                 goto exit;
1670         }
1671
1672         pframe = mem_start;
1673         pframe += pattrib->hdrlen;
1674
1675         //adding icv, if necessary...
1676         if (pattrib->iv_len)
1677         {
1678                 if (psta != NULL)
1679                 {
1680                         switch(pattrib->encrypt)
1681                         {
1682                                 case _WEP40_:
1683                                 case _WEP104_:
1684                                                 WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
1685                                         break;
1686                                 case _TKIP_:
1687                                         if(bmcst)
1688                                                 TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
1689                                         else
1690                                                 TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
1691                                         break;
1692                                 case _AES_:
1693                                         if(bmcst)
1694                                                 AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
1695                                         else
1696                                                 AES_IV(pattrib->iv, psta->dot11txpn, 0);
1697                                         break;
1698                         }
1699                 }
1700
1701                 _rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len);
1702                 pframe += pattrib->iv_len;
1703
1704         }
1705
1706         llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
1707         pframe += llc_sz;
1708
1709         //pattrib->pktlen will be counted in rtw_build_tdls_ies
1710         pattrib->pktlen = 0;
1711
1712         rtw_build_tdls_ies(padapter, pxmitframe, pframe, action);
1713
1714         if ((pattrib->icv_len >0 )&& (pattrib->bswenc)) {
1715                 pframe += pattrib->pktlen;
1716                 _rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len);
1717                 pframe += pattrib->icv_len;
1718         }
1719
1720         pattrib->nr_frags = 1;
1721         pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + llc_sz +
1722                         ((pattrib->bswenc) ? pattrib->icv_len : 0) + pattrib->pktlen;
1723
1724         if (xmitframe_addmic(padapter, pxmitframe) == _FAIL)
1725         {
1726                 goto exit;
1727         }
1728
1729         xmitframe_swencrypt(padapter, pxmitframe);
1730
1731         update_attrib_vcs_info(padapter, pxmitframe);
1732
1733 exit:
1734
1735 _func_exit_;
1736
1737         return res;
1738 }
1739 #endif //CONFIG_TDLS
1740
1741 /*
1742  * Calculate wlan 802.11 packet MAX size from pkt_attrib
1743  * This function doesn't consider fragment case
1744  */
1745 u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib)
1746 {
1747         u32     len = 0;
1748
1749         len = pattrib->hdrlen + pattrib->iv_len; // WLAN Header and IV
1750         len += SNAP_SIZE + sizeof(u16); // LLC
1751         len += pattrib->pktlen;
1752         if (pattrib->encrypt == _TKIP_) len += 8; // MIC
1753         len += ((pattrib->bswenc) ? pattrib->icv_len : 0); // ICV
1754
1755         return len;
1756 }
1757
1758 /*
1759
1760 This sub-routine will perform all the following:
1761
1762 1. remove 802.3 header.
1763 2. create wlan_header, based on the info in pxmitframe
1764 3. append sta's iv/ext-iv
1765 4. append LLC
1766 5. move frag chunk from pframe to pxmitframe->mem
1767 6. apply sw-encrypt, if necessary.
1768
1769 */
1770 s32 rtw_xmitframe_coalesce(_adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
1771 {
1772         struct pkt_file pktfile;
1773
1774         s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
1775
1776         SIZE_PTR addr;
1777
1778         u8 *pframe, *mem_start;
1779         u8 hw_hdr_offset;
1780
1781         struct sta_info         *psta;
1782         //struct sta_priv               *pstapriv = &padapter->stapriv;
1783         //struct mlme_priv      *pmlmepriv = &padapter->mlmepriv;
1784         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
1785
1786         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
1787
1788         u8 *pbuf_start;
1789
1790         s32 bmcst = IS_MCAST(pattrib->ra);
1791         s32 res = _SUCCESS;
1792
1793 _func_enter_;
1794
1795         if (pattrib->psta)
1796         {
1797                 psta = pattrib->psta;
1798         } else
1799         {
1800                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
1801                 psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
1802         }
1803
1804         if(psta==NULL)
1805         {
1806
1807                 DBG_871X("%s, psta==NUL\n", __func__);
1808                 return _FAIL;
1809         }
1810
1811
1812         if(!(psta->state &_FW_LINKED))
1813         {
1814                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
1815                 return _FAIL;
1816         }
1817
1818         if (pxmitframe->buf_addr == NULL){
1819                 DBG_8192C("==> %s buf_addr==NULL \n",__FUNCTION__);
1820                 return _FAIL;
1821         }
1822
1823         pbuf_start = pxmitframe->buf_addr;
1824
1825 #ifdef CONFIG_USB_TX_AGGREGATION
1826         hw_hdr_offset =  TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
1827 #else
1828         #ifdef CONFIG_TX_EARLY_MODE //for SDIO && Tx Agg
1829         hw_hdr_offset = TXDESC_OFFSET + EARLY_MODE_INFO_SIZE;
1830         #else
1831         hw_hdr_offset = TXDESC_OFFSET;
1832         #endif
1833 #endif
1834
1835         mem_start = pbuf_start +        hw_hdr_offset;
1836
1837         if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
1838                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"));
1839                 DBG_8192C("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n");
1840                 res = _FAIL;
1841                 goto exit;
1842         }
1843
1844         _rtw_open_pktfile(pkt, &pktfile);
1845         _rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
1846
1847         frg_inx = 0;
1848         frg_len = pxmitpriv->frag_len - 4;//2346-4 = 2342
1849
1850         while (1)
1851         {
1852                 llc_sz = 0;
1853
1854                 mpdu_len = frg_len;
1855
1856                 pframe = mem_start;
1857
1858                 SetMFrag(mem_start);
1859
1860                 pframe += pattrib->hdrlen;
1861                 mpdu_len -= pattrib->hdrlen;
1862
1863                 //adding icv, if necessary...
1864                 if (pattrib->iv_len)
1865                 {
1866                         //if (check_fwstate(pmlmepriv, WIFI_MP_STATE))
1867                         //      psta = rtw_get_stainfo(pstapriv, get_bssid(pmlmepriv));
1868                         //else
1869                         //      psta = rtw_get_stainfo(pstapriv, pattrib->ra);
1870
1871                         if (psta != NULL)
1872                         {
1873                                 switch(pattrib->encrypt)
1874                                 {
1875                                         case _WEP40_:
1876                                         case _WEP104_:
1877                                                         WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
1878                                                 break;
1879                                         case _TKIP_:
1880                                                 if(bmcst)
1881                                                         TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
1882                                                 else
1883                                                         TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
1884                                                 break;
1885                                         case _AES_:
1886                                                 if(bmcst)
1887                                                         AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
1888                                                 else
1889                                                         AES_IV(pattrib->iv, psta->dot11txpn, 0);
1890                                                 break;
1891 #ifdef CONFIG_WAPI_SUPPORT
1892                                         case _SMS4_:
1893                                                 rtw_wapi_get_iv(padapter,pattrib->ra,pattrib->iv);
1894                                                 break;
1895 #endif
1896                                 }
1897                         }
1898
1899                         _rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len);
1900
1901                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
1902                                  ("rtw_xmitframe_coalesce: keyid=%d pattrib->iv[3]=%.2x pframe=%.2x %.2x %.2x %.2x\n",
1903                                   padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe+1), *(pframe+2), *(pframe+3)));
1904
1905                         pframe += pattrib->iv_len;
1906
1907                         mpdu_len -= pattrib->iv_len;
1908                 }
1909
1910                 if (frg_inx == 0) {
1911                         llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
1912                         pframe += llc_sz;
1913                         mpdu_len -= llc_sz;
1914                 }
1915
1916                 if ((pattrib->icv_len >0) && (pattrib->bswenc)) {
1917                         mpdu_len -= pattrib->icv_len;
1918                 }
1919
1920
1921                 if (bmcst) {
1922                         // don't do fragment to broadcat/multicast packets
1923                         mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
1924                 } else {
1925                         mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len);
1926                 }
1927
1928                 pframe += mem_sz;
1929
1930                 if ((pattrib->icv_len >0 )&& (pattrib->bswenc)) {
1931                         _rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len);
1932                         pframe += pattrib->icv_len;
1933                 }
1934
1935                 frg_inx++;
1936
1937                 if (bmcst || (rtw_endofpktfile(&pktfile) == _TRUE))
1938                 {
1939                         pattrib->nr_frags = frg_inx;
1940
1941                         pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags==1)? llc_sz:0) +
1942                                         ((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz;
1943
1944                         ClearMFrag(mem_start);
1945
1946                         break;
1947                 } else {
1948                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __FUNCTION__));
1949                 }
1950
1951                 addr = (SIZE_PTR)(pframe);
1952
1953                 mem_start = (unsigned char *)RND4(addr) + hw_hdr_offset;
1954                 _rtw_memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
1955
1956         }
1957
1958         if (xmitframe_addmic(padapter, pxmitframe) == _FAIL)
1959         {
1960                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n"));
1961                 DBG_8192C("xmitframe_addmic(padapter, pxmitframe)==_FAIL\n");
1962                 res = _FAIL;
1963                 goto exit;
1964         }
1965
1966         xmitframe_swencrypt(padapter, pxmitframe);
1967
1968         if(bmcst == _FALSE)
1969                 update_attrib_vcs_info(padapter, pxmitframe);
1970         else
1971                 pattrib->vcs_mode = NONE_VCS;
1972
1973 exit:
1974
1975 _func_exit_;
1976
1977         return res;
1978 }
1979
1980 #ifdef CONFIG_IEEE80211W
1981 //broadcast or multicast management pkt use BIP, unicast management pkt use CCMP encryption
1982 s32 rtw_mgmt_xmitframe_coalesce(_adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
1983 {
1984         struct pkt_file pktfile;
1985         s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
1986         SIZE_PTR addr;
1987         u8 *pframe, *mem_start = NULL, *tmp_buf=NULL;
1988         u8 hw_hdr_offset, subtype ;
1989         struct sta_info         *psta = NULL;
1990         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
1991         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
1992         u8 *pbuf_start;
1993         s32 bmcst = IS_MCAST(pattrib->ra);
1994         s32 res = _FAIL;
1995         u8 *BIP_AAD;
1996         u8 *MGMT_body=NULL;
1997
1998         struct mlme_ext_priv    *pmlmeext = &padapter->mlmeextpriv;
1999         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
2000         struct rtw_ieee80211_hdr        *pwlanhdr;
2001         u8 MME[_MME_IE_LENGTH_];
2002
2003         _irqL irqL;
2004         u32     ori_len;
2005         mem_start = pframe = (u8 *)(pxmitframe->buf_addr) + TXDESC_OFFSET;
2006         pwlanhdr = (struct rtw_ieee80211_hdr *)pframe;
2007         tmp_buf = BIP_AAD;
2008
2009 _func_enter_;
2010         ori_len = BIP_AAD_SIZE+pattrib->pktlen;
2011         tmp_buf = BIP_AAD = rtw_zmalloc(ori_len);
2012         subtype = GetFrameSubType(pframe); //bit(7)~bit(2)
2013
2014         if(BIP_AAD == NULL)
2015                 return _FAIL;
2016
2017         _enter_critical_bh(&padapter->security_key_mutex, &irqL);
2018
2019         //only support station mode
2020         if(!check_fwstate(pmlmepriv, WIFI_STATION_STATE) || !check_fwstate(pmlmepriv, _FW_LINKED))
2021                 goto xmitframe_coalesce_success;
2022
2023         //IGTK key is not install, it may not support 802.11w
2024         if(padapter->securitypriv.binstallBIPkey != _TRUE)
2025         {
2026                 DBG_871X("no instll BIP key\n");
2027                 goto xmitframe_coalesce_success;
2028         }
2029         //station mode doesn't need TX BIP, just ready the code
2030         if(bmcst)
2031         {
2032                 int frame_body_len;
2033                 u8 mic[16];
2034
2035                 _rtw_memset(MME, 0, 18);
2036
2037                 //other types doesn't need the BIP
2038                 if(GetFrameSubType(pframe) != WIFI_DEAUTH && GetFrameSubType(pframe) != WIFI_DISASSOC)
2039                         goto xmitframe_coalesce_fail;
2040
2041                 MGMT_body = pframe + sizeof(struct rtw_ieee80211_hdr_3addr);
2042                 pframe += pattrib->pktlen;
2043
2044                 //octent 0 and 1 is key index ,BIP keyid is 4 or 5, LSB only need octent 0
2045                 MME[0]=padapter->securitypriv.dot11wBIPKeyid;
2046                 //copy packet number
2047                 _rtw_memcpy(&MME[2], &pmlmeext->mgnt_80211w_IPN, 6);
2048                 //increase the packet number
2049                 pmlmeext->mgnt_80211w_IPN++;
2050
2051                 //add MME IE with MIC all zero, MME string doesn't include element id and length
2052                 pframe = rtw_set_ie(pframe, _MME_IE_ , 16 , MME, &(pattrib->pktlen));
2053                 pattrib->last_txcmdsz = pattrib->pktlen;
2054                 // total frame length - header length
2055                 frame_body_len = pattrib->pktlen - sizeof(struct rtw_ieee80211_hdr_3addr);
2056
2057                 //conscruct AAD, copy frame control field
2058                 _rtw_memcpy(BIP_AAD, &pwlanhdr->frame_ctl, 2);
2059                 ClearRetry(BIP_AAD);
2060                 ClearPwrMgt(BIP_AAD);
2061                 ClearMData(BIP_AAD);
2062                 //conscruct AAD, copy address 1 to address 3
2063                 _rtw_memcpy(BIP_AAD+2, pwlanhdr->addr1, 18);
2064                 //copy management fram body
2065                 _rtw_memcpy(BIP_AAD+BIP_AAD_SIZE, MGMT_body, frame_body_len);
2066                 /*//dump total packet include MME with zero MIC
2067                 {
2068                         int i;
2069                         printk("Total packet: ");
2070                         for(i=0; i < BIP_AAD_SIZE+frame_body_len; i++)
2071                                 printk(" %02x ", BIP_AAD[i]);
2072                         printk("\n");
2073                 }*/
2074                 //calculate mic
2075                 if(omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey
2076                         , BIP_AAD, BIP_AAD_SIZE+frame_body_len, mic))
2077                         goto xmitframe_coalesce_fail;
2078
2079                 /*//dump calculated mic result
2080                 {
2081                         int i;
2082                         printk("Calculated mic result: ");
2083                         for(i=0; i<16; i++)
2084                                 printk(" %02x ", mic[i]);
2085                         printk("\n");
2086                 }*/
2087                 //copy right BIP mic value, total is 128bits, we use the 0~63 bits
2088                 _rtw_memcpy(pframe-8, mic, 8);
2089                 /*/dump all packet after mic ok
2090                 {
2091                         int pp;
2092                         printk("pattrib->pktlen = %d \n", pattrib->pktlen);
2093                         for(pp=0;pp< pattrib->pktlen; pp++)
2094                                 printk(" %02x ", mem_start[pp]);
2095                         printk("\n");
2096                 }*/
2097         }
2098         else //unicast mgmt frame TX
2099         {
2100                 //start to encrypt mgmt frame
2101                 if(subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC ||
2102                         subtype == WIFI_REASSOCREQ || subtype == WIFI_ACTION)
2103                 {
2104                         if (pattrib->psta)
2105                                 psta = pattrib->psta;
2106                         else
2107                         {
2108                                 psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
2109                         }
2110
2111                         if(psta==NULL)
2112                     {
2113
2114                                 DBG_871X("%s, psta==NUL\n", __func__);
2115                                 goto xmitframe_coalesce_fail;
2116                         }
2117
2118                         if(!(psta->state & _FW_LINKED) || pxmitframe->buf_addr==NULL)
2119                         {
2120                                 DBG_871X("%s, not _FW_LINKED or addr null\n", __func__);
2121                                 goto xmitframe_coalesce_fail;
2122                         }
2123
2124                         //DBG_871X("%s, action frame category=%d \n", __func__, pframe[WLAN_HDR_A3_LEN]);
2125                         //according 802.11-2012 standard, these five types are not robust types
2126                         if(subtype == WIFI_ACTION &&
2127                         (pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_PUBLIC ||
2128                         pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_HT ||
2129                         pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_UNPROTECTED_WNM ||
2130                         pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_SELF_PROTECTED  ||
2131                         pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_P2P))
2132                                 goto xmitframe_coalesce_fail;
2133                         /*//before encrypt dump the management packet content
2134                         {
2135                                 int i;
2136                                 printk("Management pkt: ");
2137                                 for(i=0; i<pattrib->pktlen; i++)
2138                                 printk(" %02x ", pframe[i]);
2139                                 printk("=======\n");
2140                         }*/
2141
2142                         //bakeup original management packet
2143                         _rtw_memcpy(tmp_buf, pframe, pattrib->pktlen);
2144                         //move to data portion
2145                         pframe += pattrib->hdrlen;
2146
2147                         //802.11w unicast management packet must be _AES_
2148                         pattrib->iv_len = 8;
2149                         //it's MIC of AES
2150                         pattrib->icv_len = 8;
2151
2152                         switch(pattrib->encrypt)
2153                         {
2154                                 case _AES_:
2155                                                 //set AES IV header
2156                                                 AES_IV(pattrib->iv, psta->dot11wtxpn, 0);
2157                                         break;
2158                                 default:
2159                                         goto xmitframe_coalesce_fail;
2160                         }
2161                         //insert iv header into management frame
2162                         _rtw_memcpy(pframe, pattrib->iv, pattrib->iv_len);
2163                         pframe += pattrib->iv_len;
2164                         //copy mgmt data portion after CCMP header
2165                         _rtw_memcpy(pframe, tmp_buf+pattrib->hdrlen, pattrib->pktlen-pattrib->hdrlen);
2166                         //move pframe to end of mgmt pkt
2167                         pframe += pattrib->pktlen-pattrib->hdrlen;
2168                         //add 8 bytes CCMP IV header to length
2169                         pattrib->pktlen += pattrib->iv_len;
2170                         /*//dump management packet include AES IV header
2171                         {
2172                                 int i;
2173                                 printk("Management pkt + IV: ");
2174                                 //for(i=0; i<pattrib->pktlen; i++)
2175                                 //printk(" %02x ", mem_start[i]);
2176                                 printk("@@@@@@@@@@@@@\n");
2177                         }*/
2178
2179                         if ((pattrib->icv_len >0 )&& (pattrib->bswenc)) {
2180                                 _rtw_memcpy(pframe, pattrib->icv, pattrib->icv_len);
2181                                 pframe += pattrib->icv_len;
2182                         }
2183                         //add 8 bytes MIC
2184                         pattrib->pktlen += pattrib->icv_len;
2185                         //set final tx command size
2186                         pattrib->last_txcmdsz = pattrib->pktlen;
2187
2188                         //set protected bit must be beofre SW encrypt
2189                         SetPrivacy(mem_start);
2190                         /*//dump management packet include AES header
2191                         {
2192                                 int i;
2193                                 printk("prepare to enc Management pkt + IV: ");
2194                                 for(i=0; i<pattrib->pktlen; i++)
2195                                 printk(" %02x ", mem_start[i]);
2196                                 printk("@@@@@@@@@@@@@\n");
2197                         }*/
2198                         //software encrypt
2199                         xmitframe_swencrypt(padapter, pxmitframe);
2200                 }
2201         }
2202
2203 xmitframe_coalesce_success:
2204         _exit_critical_bh(&padapter->security_key_mutex, &irqL);
2205         rtw_mfree(BIP_AAD, ori_len);
2206 _func_exit_;
2207         return _SUCCESS;
2208
2209 xmitframe_coalesce_fail:
2210         _exit_critical_bh(&padapter->security_key_mutex, &irqL);
2211         rtw_mfree(BIP_AAD, ori_len);
2212 _func_exit_;
2213
2214         return _FAIL;
2215 }
2216 #endif //CONFIG_IEEE80211W
2217
2218 /* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
2219  * IEEE LLC/SNAP header contains 8 octets
2220  * First 3 octets comprise the LLC portion
2221  * SNAP portion, 5 octets, is divided into two fields:
2222  *      Organizationally Unique Identifier(OUI), 3 octets,
2223  *      type, defined by that organization, 2 octets.
2224  */
2225 s32 rtw_put_snap(u8 *data, u16 h_proto)
2226 {
2227         struct ieee80211_snap_hdr *snap;
2228         u8 *oui;
2229
2230 _func_enter_;
2231
2232         snap = (struct ieee80211_snap_hdr *)data;
2233         snap->dsap = 0xaa;
2234         snap->ssap = 0xaa;
2235         snap->ctrl = 0x03;
2236
2237         if (h_proto == 0x8137 || h_proto == 0x80f3)
2238                 oui = P802_1H_OUI;
2239         else
2240                 oui = RFC1042_OUI;
2241
2242         snap->oui[0] = oui[0];
2243         snap->oui[1] = oui[1];
2244         snap->oui[2] = oui[2];
2245
2246         *(u16 *)(data + SNAP_SIZE) = htons(h_proto);
2247
2248 _func_exit_;
2249
2250         return SNAP_SIZE + sizeof(u16);
2251 }
2252
2253 void rtw_update_protection(_adapter *padapter, u8 *ie, uint ie_len)
2254 {
2255
2256         uint    protection;
2257         u8      *perp;
2258         sint     erp_len;
2259         struct  xmit_priv *pxmitpriv = &padapter->xmitpriv;
2260         struct  registry_priv *pregistrypriv = &padapter->registrypriv;
2261
2262 _func_enter_;
2263
2264         switch(pxmitpriv->vcs_setting)
2265         {
2266                 case DISABLE_VCS:
2267                         pxmitpriv->vcs = NONE_VCS;
2268                         break;
2269
2270                 case ENABLE_VCS:
2271                         break;
2272
2273                 case AUTO_VCS:
2274                 default:
2275                         perp = rtw_get_ie(ie, _ERPINFO_IE_, &erp_len, ie_len);
2276                         if(perp == NULL)
2277                         {
2278                                 pxmitpriv->vcs = NONE_VCS;
2279                         }
2280                         else
2281                         {
2282                                 protection = (*(perp + 2)) & BIT(1);
2283                                 if (protection)
2284                                 {
2285                                         if(pregistrypriv->vcs_type == RTS_CTS)
2286                                                 pxmitpriv->vcs = RTS_CTS;
2287                                         else
2288                                                 pxmitpriv->vcs = CTS_TO_SELF;
2289                                 }
2290                                 else
2291                                         pxmitpriv->vcs = NONE_VCS;
2292                         }
2293
2294                         break;
2295
2296         }
2297
2298 _func_exit_;
2299
2300 }
2301
2302 void rtw_count_tx_stats(PADAPTER padapter, struct xmit_frame *pxmitframe, int sz)
2303 {
2304         struct sta_info *psta = NULL;
2305         struct stainfo_stats *pstats = NULL;
2306         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
2307         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
2308
2309         if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG)
2310         {
2311                 pxmitpriv->tx_bytes += sz;
2312 #if defined(CONFIG_USB_TX_AGGREGATION) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
2313                 pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pxmitframe->agg_num;
2314 #else
2315                 pmlmepriv->LinkDetectInfo.NumTxOkInPeriod++;
2316 #endif
2317
2318                 psta = pxmitframe->attrib.psta;
2319                 if (psta)
2320                 {
2321                         pstats = &psta->sta_stats;
2322 #if defined(CONFIG_USB_TX_AGGREGATION) || defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
2323                         pstats->tx_pkts += pxmitframe->agg_num;
2324 #else
2325                         pstats->tx_pkts++;
2326 #endif
2327                         pstats->tx_bytes += sz;
2328                 }
2329         }
2330 }
2331
2332 struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv)
2333 {
2334         _irqL irqL;
2335         struct xmit_buf *pxmitbuf =  NULL;
2336         _list *plist, *phead;
2337         _queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
2338
2339 _func_enter_;
2340
2341         _enter_critical(&pfree_queue->lock, &irqL);
2342
2343         if(_rtw_queue_empty(pfree_queue) == _TRUE) {
2344                 pxmitbuf = NULL;
2345         } else {
2346
2347                 phead = get_list_head(pfree_queue);
2348
2349                 plist = get_next(phead);
2350
2351                 pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
2352
2353                 rtw_list_delete(&(pxmitbuf->list));
2354         }
2355
2356         if (pxmitbuf !=  NULL)
2357         {
2358                 pxmitpriv->free_xmit_extbuf_cnt--;
2359                 #ifdef DBG_XMIT_BUF_EXT
2360                 DBG_871X("DBG_XMIT_BUF_EXT ALLOC no=%d,  free_xmit_extbuf_cnt=%d\n",pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
2361                 #endif
2362
2363
2364                 pxmitbuf->priv_data = NULL;
2365                 //pxmitbuf->ext_tag = _TRUE;
2366
2367 #if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
2368                 pxmitbuf->len = 0;
2369                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
2370 #endif
2371 #ifdef CONFIG_PCI_HCI
2372                 pxmitbuf->len = 0;
2373 #endif
2374
2375                 if (pxmitbuf->sctx) {
2376                         DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
2377                         rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
2378                 }
2379
2380         }
2381
2382         _exit_critical(&pfree_queue->lock, &irqL);
2383
2384 _func_exit_;
2385
2386         return pxmitbuf;
2387 }
2388
2389 s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
2390 {
2391         _irqL irqL;
2392         _queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
2393
2394 _func_enter_;
2395
2396         if(pxmitbuf==NULL)
2397         {
2398                 return _FAIL;
2399         }
2400
2401         _enter_critical(&pfree_queue->lock, &irqL);
2402
2403         rtw_list_delete(&pxmitbuf->list);
2404
2405         rtw_list_insert_tail(&(pxmitbuf->list), get_list_head(pfree_queue));
2406         pxmitpriv->free_xmit_extbuf_cnt++;
2407         #ifdef DBG_XMIT_BUF_EXT
2408         DBG_871X("DBG_XMIT_BUF_EXT FREE no=%d, free_xmit_extbuf_cnt=%d\n",pxmitbuf->no ,pxmitpriv->free_xmit_extbuf_cnt);
2409         #endif
2410
2411         _exit_critical(&pfree_queue->lock, &irqL);
2412
2413 _func_exit_;
2414
2415         return _SUCCESS;
2416 }
2417
2418 struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
2419 {
2420         _irqL irqL;
2421         struct xmit_buf *pxmitbuf =  NULL;
2422         _list *plist, *phead;
2423         _queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
2424
2425 _func_enter_;
2426
2427         //DBG_871X("+rtw_alloc_xmitbuf\n");
2428
2429         _enter_critical(&pfree_xmitbuf_queue->lock, &irqL);
2430
2431         if(_rtw_queue_empty(pfree_xmitbuf_queue) == _TRUE) {
2432                 pxmitbuf = NULL;
2433         } else {
2434
2435                 phead = get_list_head(pfree_xmitbuf_queue);
2436
2437                 plist = get_next(phead);
2438
2439                 pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
2440
2441                 rtw_list_delete(&(pxmitbuf->list));
2442         }
2443
2444         if (pxmitbuf !=  NULL)
2445         {
2446                 pxmitpriv->free_xmitbuf_cnt--;
2447                 #ifdef DBG_XMIT_BUF
2448                 DBG_871X("DBG_XMIT_BUF ALLOC no=%d,  free_xmitbuf_cnt=%d\n",pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
2449                 #endif
2450                 //DBG_871X("alloc, free_xmitbuf_cnt=%d\n", pxmitpriv->free_xmitbuf_cnt);
2451
2452                 pxmitbuf->priv_data = NULL;
2453                 //pxmitbuf->ext_tag = _FALSE;
2454 #if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
2455                 pxmitbuf->len = 0;
2456                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
2457                 pxmitbuf->agg_num = 0;
2458                 pxmitbuf->pg_num = 0;
2459 #endif
2460 #ifdef CONFIG_PCI_HCI
2461                 pxmitbuf->len = 0;
2462 #endif
2463
2464                 if (pxmitbuf->sctx) {
2465                         DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
2466                         rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
2467                 }
2468         }
2469         #ifdef DBG_XMIT_BUF
2470         else
2471         {
2472                 DBG_871X("DBG_XMIT_BUF rtw_alloc_xmitbuf return NULL\n");
2473         }
2474         #endif
2475
2476         _exit_critical(&pfree_xmitbuf_queue->lock, &irqL);
2477
2478 _func_exit_;
2479
2480         return pxmitbuf;
2481 }
2482
2483 s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
2484 {
2485         _irqL irqL;
2486         _queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
2487
2488 _func_enter_;
2489
2490         //DBG_871X("+rtw_free_xmitbuf\n");
2491
2492         if(pxmitbuf==NULL)
2493         {
2494                 return _FAIL;
2495         }
2496
2497         if (pxmitbuf->sctx) {
2498                 DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
2499                 rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
2500         }
2501
2502         if(pxmitbuf->ext_tag)
2503         {
2504                 rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf);
2505         }
2506         else
2507         {
2508                 _enter_critical(&pfree_xmitbuf_queue->lock, &irqL);
2509
2510                 rtw_list_delete(&pxmitbuf->list);
2511
2512                 rtw_list_insert_tail(&(pxmitbuf->list), get_list_head(pfree_xmitbuf_queue));
2513
2514                 pxmitpriv->free_xmitbuf_cnt++;
2515                 //DBG_871X("FREE, free_xmitbuf_cnt=%d\n", pxmitpriv->free_xmitbuf_cnt);
2516                 #ifdef DBG_XMIT_BUF
2517                 DBG_871X("DBG_XMIT_BUF FREE no=%d, free_xmitbuf_cnt=%d\n",pxmitbuf->no ,pxmitpriv->free_xmitbuf_cnt);
2518                 #endif
2519                 _exit_critical(&pfree_xmitbuf_queue->lock, &irqL);
2520         }
2521
2522 _func_exit_;
2523
2524         return _SUCCESS;
2525 }
2526
2527 void rtw_init_xmitframe(struct xmit_frame *pxframe)
2528 {
2529         if (pxframe !=  NULL)//default value setting
2530         {
2531                 pxframe->buf_addr = NULL;
2532                 pxframe->pxmitbuf = NULL;
2533
2534                 _rtw_memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
2535                 //pxframe->attrib.psta = NULL;
2536
2537                 pxframe->frame_tag = DATA_FRAMETAG;
2538
2539 #ifdef CONFIG_USB_HCI
2540                 pxframe->pkt = NULL;
2541 #ifdef USB_PACKET_OFFSET_SZ
2542                 pxframe->pkt_offset = (PACKET_OFFSET_SZ/8);
2543 #else
2544                 pxframe->pkt_offset = 1;//default use pkt_offset to fill tx desc
2545 #endif
2546
2547 #ifdef CONFIG_USB_TX_AGGREGATION
2548                 pxframe->agg_num = 1;
2549 #endif
2550
2551 #endif //#ifdef CONFIG_USB_HCI
2552
2553 #if defined(CONFIG_SDIO_HCI) || defined(CONFIG_GSPI_HCI)
2554                 pxframe->pg_num = 1;
2555                 pxframe->agg_num = 1;
2556 #endif
2557
2558 #ifdef CONFIG_XMIT_ACK
2559                 pxframe->ack_report = 0;
2560 #endif
2561
2562         }
2563 }
2564
2565 /*
2566 Calling context:
2567 1. OS_TXENTRY
2568 2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
2569
2570 If we turn on USE_RXTHREAD, then, no need for critical section.
2571 Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
2572
2573 Must be very very cautious...
2574
2575 */
2576 struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)//(_queue *pfree_xmit_queue)
2577 {
2578         /*
2579                 Please remember to use all the osdep_service api,
2580                 and lock/unlock or _enter/_exit critical to protect
2581                 pfree_xmit_queue
2582         */
2583
2584         _irqL irqL;
2585         struct xmit_frame *pxframe = NULL;
2586         _list *plist, *phead;
2587         _queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
2588 #ifdef PLATFORM_LINUX
2589         _adapter *padapter = pxmitpriv->adapter;
2590 #endif //PLATFORM_LINUX
2591
2592 _func_enter_;
2593
2594         _enter_critical_bh(&pfree_xmit_queue->lock, &irqL);
2595
2596         if (_rtw_queue_empty(pfree_xmit_queue) == _TRUE) {
2597                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_alloc_xmitframe:%d\n", pxmitpriv->free_xmitframe_cnt));
2598                 pxframe =  NULL;
2599         } else {
2600                 phead = get_list_head(pfree_xmit_queue);
2601
2602                 plist = get_next(phead);
2603
2604                 pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
2605
2606                 rtw_list_delete(&(pxframe->list));
2607                 pxmitpriv->free_xmitframe_cnt--;
2608                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe():free_xmitframe_cnt=%d\n", pxmitpriv->free_xmitframe_cnt));
2609         }
2610
2611         _exit_critical_bh(&pfree_xmit_queue->lock, &irqL);
2612
2613         rtw_init_xmitframe(pxframe);
2614
2615 _func_exit_;
2616
2617         return pxframe;
2618 }
2619
2620 struct xmit_frame *rtw_alloc_xmitframe_ext(struct xmit_priv *pxmitpriv)
2621 {
2622         _irqL irqL;
2623         struct xmit_frame *pxframe = NULL;
2624         _list *plist, *phead;
2625         _queue *queue = &pxmitpriv->free_xframe_ext_queue;
2626
2627 _func_enter_;
2628
2629         _enter_critical_bh(&queue->lock, &irqL);
2630
2631         if (_rtw_queue_empty(queue) == _TRUE) {
2632                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_alloc_xmitframe_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
2633                 pxframe =  NULL;
2634         } else {
2635                 phead = get_list_head(queue);
2636                 plist = get_next(phead);
2637                 pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
2638
2639                 rtw_list_delete(&(pxframe->list));
2640                 pxmitpriv->free_xframe_ext_cnt--;
2641                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext():free_xmitframe_cnt=%d\n", pxmitpriv->free_xframe_ext_cnt));
2642         }
2643
2644         _exit_critical_bh(&queue->lock, &irqL);
2645
2646         rtw_init_xmitframe(pxframe);
2647
2648 _func_exit_;
2649
2650         return pxframe;
2651 }
2652
2653 struct xmit_frame *rtw_alloc_xmitframe_once(struct xmit_priv *pxmitpriv)
2654 {
2655         struct xmit_frame *pxframe = NULL;
2656         u8 *alloc_addr;
2657
2658         alloc_addr = rtw_zmalloc(sizeof(struct xmit_frame) + 4);
2659
2660         if (alloc_addr == NULL)
2661                 goto exit;
2662
2663         pxframe = (struct xmit_frame *)N_BYTE_ALIGMENT((SIZE_PTR)(alloc_addr), 4);
2664         pxframe->alloc_addr = alloc_addr;
2665
2666         pxframe->padapter = pxmitpriv->adapter;
2667         pxframe->frame_tag = NULL_FRAMETAG;
2668
2669         pxframe->pkt = NULL;
2670
2671         pxframe->buf_addr = NULL;
2672         pxframe->pxmitbuf = NULL;
2673
2674         rtw_init_xmitframe(pxframe);
2675
2676         DBG_871X("################## %s ##################\n", __func__);
2677
2678 exit:
2679         return pxframe;
2680 }
2681
2682 s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
2683 {
2684         _irqL irqL;
2685         _queue *queue;
2686         _adapter *padapter = pxmitpriv->adapter;
2687         _pkt *pndis_pkt = NULL;
2688
2689 _func_enter_;
2690
2691         if (pxmitframe == NULL) {
2692                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======rtw_free_xmitframe():pxmitframe==NULL!!!!!!!!!!\n"));
2693                 goto exit;
2694         }
2695
2696         if (pxmitframe->pkt){
2697                 pndis_pkt = pxmitframe->pkt;
2698                 pxmitframe->pkt = NULL;
2699         }
2700
2701         if (pxmitframe->alloc_addr) {
2702                 DBG_871X("################## %s with alloc_addr ##################\n", __func__);
2703                 rtw_mfree(pxmitframe->alloc_addr, sizeof(struct xmit_frame) + 4);
2704                 goto check_pkt_complete;
2705         }
2706
2707         if (pxmitframe->ext_tag == 0)
2708                 queue = &pxmitpriv->free_xmit_queue;
2709         else if(pxmitframe->ext_tag == 1)
2710                 queue = &pxmitpriv->free_xframe_ext_queue;
2711         else
2712         {}
2713
2714         _enter_critical_bh(&queue->lock, &irqL);
2715
2716         rtw_list_delete(&pxmitframe->list);
2717         rtw_list_insert_tail(&pxmitframe->list, get_list_head(queue));
2718         if (pxmitframe->ext_tag == 0) {
2719                 pxmitpriv->free_xmitframe_cnt++;
2720                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xmitframe_cnt=%d\n", pxmitpriv->free_xmitframe_cnt));
2721         } else if(pxmitframe->ext_tag == 1) {
2722                 pxmitpriv->free_xframe_ext_cnt++;
2723                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xframe_ext_cnt=%d\n", pxmitpriv->free_xframe_ext_cnt));
2724         } else {
2725         }
2726
2727         _exit_critical_bh(&queue->lock, &irqL);
2728
2729 check_pkt_complete:
2730
2731         if(pndis_pkt)
2732                 rtw_os_pkt_complete(padapter, pndis_pkt);
2733
2734 exit:
2735
2736 _func_exit_;
2737
2738         return _SUCCESS;
2739 }
2740
2741 void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, _queue *pframequeue)
2742 {
2743         _irqL irqL;
2744         _list   *plist, *phead;
2745         struct  xmit_frame      *pxmitframe;
2746
2747 _func_enter_;
2748
2749         _enter_critical_bh(&(pframequeue->lock), &irqL);
2750
2751         phead = get_list_head(pframequeue);
2752         plist = get_next(phead);
2753
2754         while (rtw_end_of_queue_search(phead, plist) == _FALSE)
2755         {
2756
2757                 pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
2758
2759                 plist = get_next(plist);
2760
2761                 rtw_free_xmitframe(pxmitpriv,pxmitframe);
2762
2763         }
2764         _exit_critical_bh(&(pframequeue->lock), &irqL);
2765
2766 _func_exit_;
2767 }
2768
2769 s32 rtw_xmitframe_enqueue(_adapter *padapter, struct xmit_frame *pxmitframe)
2770 {
2771         DBG_COUNTER(padapter->tx_logs.core_tx_enqueue);
2772         if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL)
2773         {
2774                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
2775                          ("rtw_xmitframe_enqueue: drop xmit pkt for classifier fail\n"));
2776 //              pxmitframe->pkt = NULL;
2777                 return _FAIL;
2778         }
2779
2780         return _SUCCESS;
2781 }
2782
2783 static struct xmit_frame *dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit, struct tx_servq *ptxservq, _queue *pframe_queue)
2784 {
2785         _list   *xmitframe_plist, *xmitframe_phead;
2786         struct  xmit_frame      *pxmitframe=NULL;
2787
2788         xmitframe_phead = get_list_head(pframe_queue);
2789         xmitframe_plist = get_next(xmitframe_phead);
2790
2791         while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
2792         {
2793                 pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
2794
2795                 xmitframe_plist = get_next(xmitframe_plist);
2796
2797 /*#ifdef RTK_DMP_PLATFORM
2798 #ifdef CONFIG_USB_TX_AGGREGATION
2799                 if((ptxservq->qcnt>0) && (ptxservq->qcnt<=2))
2800                 {
2801                         pxmitframe = NULL;
2802
2803                         tasklet_schedule(&pxmitpriv->xmit_tasklet);
2804
2805                         break;
2806                 }
2807 #endif
2808 #endif*/
2809                 rtw_list_delete(&pxmitframe->list);
2810
2811                 ptxservq->qcnt--;
2812
2813                 //rtw_list_insert_tail(&pxmitframe->list, &phwxmit->pending);
2814
2815                 //ptxservq->qcnt--;
2816
2817                 break;
2818
2819                 pxmitframe = NULL;
2820
2821         }
2822
2823         return pxmitframe;
2824 }
2825
2826 struct xmit_frame* rtw_dequeue_xframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i, sint entry)
2827 {
2828         _irqL irqL0;
2829         _list *sta_plist, *sta_phead;
2830         struct hw_xmit *phwxmit;
2831         struct tx_servq *ptxservq = NULL;
2832         _queue *pframe_queue = NULL;
2833         struct xmit_frame *pxmitframe = NULL;
2834         _adapter *padapter = pxmitpriv->adapter;
2835         struct registry_priv    *pregpriv = &padapter->registrypriv;
2836         int i, inx[4];
2837 #ifdef CONFIG_USB_HCI
2838 //      int j, tmp, acirp_cnt[4];
2839 #endif
2840
2841 _func_enter_;
2842
2843         inx[0] = 0; inx[1] = 1; inx[2] = 2; inx[3] = 3;
2844
2845         if(pregpriv->wifi_spec==1)
2846         {
2847                 int j, tmp, acirp_cnt[4];
2848 #if 0
2849                 if(flags<XMIT_QUEUE_ENTRY)
2850                 {
2851                         //priority exchange according to the completed xmitbuf flags.
2852                         inx[flags] = 0;
2853                         inx[0] = flags;
2854                 }
2855 #endif
2856
2857 #if defined(CONFIG_USB_HCI) || defined(CONFIG_SDIO_HCI)
2858                 for(j=0; j<4; j++)
2859                         inx[j] = pxmitpriv->wmm_para_seq[j];
2860 #endif
2861         }
2862
2863         _enter_critical_bh(&pxmitpriv->lock, &irqL0);
2864
2865         for(i = 0; i < entry; i++)
2866         {
2867                 phwxmit = phwxmit_i + inx[i];
2868
2869                 //_enter_critical_ex(&phwxmit->sta_queue->lock, &irqL0);
2870
2871                 sta_phead = get_list_head(phwxmit->sta_queue);
2872                 sta_plist = get_next(sta_phead);
2873
2874                 while ((rtw_end_of_queue_search(sta_phead, sta_plist)) == _FALSE)
2875                 {
2876
2877                         ptxservq= LIST_CONTAINOR(sta_plist, struct tx_servq, tx_pending);
2878
2879                         pframe_queue = &ptxservq->sta_pending;
2880
2881                         pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue);
2882
2883                         if(pxmitframe)
2884                         {
2885                                 phwxmit->accnt--;
2886
2887                                 //Remove sta node when there is no pending packets.
2888                                 if(_rtw_queue_empty(pframe_queue)) //must be done after get_next and before break
2889                                         rtw_list_delete(&ptxservq->tx_pending);
2890
2891                                 //_exit_critical_ex(&phwxmit->sta_queue->lock, &irqL0);
2892
2893                                 goto exit;
2894                         }
2895
2896                         sta_plist = get_next(sta_plist);
2897
2898                 }
2899
2900                 //_exit_critical_ex(&phwxmit->sta_queue->lock, &irqL0);
2901
2902         }
2903
2904 exit:
2905
2906         _exit_critical_bh(&pxmitpriv->lock, &irqL0);
2907
2908 _func_exit_;
2909
2910         return pxmitframe;
2911 }
2912
2913 #if 1
2914 struct tx_servq *rtw_get_sta_pending(_adapter *padapter, struct sta_info *psta, sint up, u8 *ac)
2915 {
2916         struct tx_servq *ptxservq=NULL;
2917
2918 _func_enter_;
2919
2920         switch (up)
2921         {
2922                 case 1:
2923                 case 2:
2924                         ptxservq = &(psta->sta_xmitpriv.bk_q);
2925                         *(ac) = 3;
2926                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : BK \n"));
2927                         break;
2928
2929                 case 4:
2930                 case 5:
2931                         ptxservq = &(psta->sta_xmitpriv.vi_q);
2932                         *(ac) = 1;
2933                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : VI\n"));
2934                         break;
2935
2936                 case 6:
2937                 case 7:
2938                         ptxservq = &(psta->sta_xmitpriv.vo_q);
2939                         *(ac) = 0;
2940                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : VO \n"));
2941                         break;
2942
2943                 case 0:
2944                 case 3:
2945                 default:
2946                         ptxservq = &(psta->sta_xmitpriv.be_q);
2947                         *(ac) = 2;
2948                         RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : BE \n"));
2949                 break;
2950
2951         }
2952
2953 _func_exit_;
2954
2955         return ptxservq;
2956 }
2957 #else
2958 __inline static struct tx_servq *rtw_get_sta_pending
2959         (_adapter *padapter, _queue **ppstapending, struct sta_info *psta, sint up)
2960 {
2961         struct tx_servq *ptxservq;
2962         struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
2963
2964 _func_enter_;
2965
2966 #ifdef CONFIG_RTL8711
2967
2968         if(IS_MCAST(psta->hwaddr))
2969         {
2970                 ptxservq = &(psta->sta_xmitpriv.be_q); // we will use be_q to queue bc/mc frames in BCMC_stainfo
2971                 *ppstapending = &padapter->xmitpriv.bm_pending;
2972         }
2973         else
2974 #endif
2975         {
2976                 switch (up)
2977                 {
2978                         case 1:
2979                         case 2:
2980                                 ptxservq = &(psta->sta_xmitpriv.bk_q);
2981                                 *ppstapending = &padapter->xmitpriv.bk_pending;
2982                                 (phwxmits+3)->accnt++;
2983                                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : BK \n"));
2984                                 break;
2985
2986                         case 4:
2987                         case 5:
2988                                 ptxservq = &(psta->sta_xmitpriv.vi_q);
2989                                 *ppstapending = &padapter->xmitpriv.vi_pending;
2990                                 (phwxmits+1)->accnt++;
2991                                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : VI\n"));
2992                                 break;
2993
2994                         case 6:
2995                         case 7:
2996                                 ptxservq = &(psta->sta_xmitpriv.vo_q);
2997                                 *ppstapending = &padapter->xmitpriv.vo_pending;
2998                                 (phwxmits+0)->accnt++;
2999                                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : VO \n"));
3000                                 break;
3001
3002                         case 0:
3003                         case 3:
3004                         default:
3005                                 ptxservq = &(psta->sta_xmitpriv.be_q);
3006                                 *ppstapending = &padapter->xmitpriv.be_pending;
3007                                 (phwxmits+2)->accnt++;
3008                                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("rtw_get_sta_pending : BE \n"));
3009                         break;
3010
3011                 }
3012
3013         }
3014
3015 _func_exit_;
3016
3017         return ptxservq;
3018 }
3019 #endif
3020
3021 /*
3022  * Will enqueue pxmitframe to the proper queue,
3023  * and indicate it to xx_pending list.....
3024  */
3025 s32 rtw_xmit_classifier(_adapter *padapter, struct xmit_frame *pxmitframe)
3026 {
3027         //_irqL irqL0;
3028         u8      ac_index;
3029         struct sta_info *psta;
3030         struct tx_servq *ptxservq;
3031         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
3032         struct sta_priv *pstapriv = &padapter->stapriv;
3033         struct hw_xmit  *phwxmits =  padapter->xmitpriv.hwxmits;
3034         sint res = _SUCCESS;
3035
3036 _func_enter_;
3037
3038         DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class);
3039         if (pattrib->psta) {
3040                 psta = pattrib->psta;
3041         } else {
3042                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
3043                 psta = rtw_get_stainfo(pstapriv, pattrib->ra);
3044         }
3045
3046         if (psta == NULL) {
3047                 DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_nosta);
3048                 res = _FAIL;
3049                 DBG_8192C("rtw_xmit_classifier: psta == NULL\n");
3050                 RT_TRACE(_module_rtl871x_xmit_c_,_drv_err_,("rtw_xmit_classifier: psta == NULL\n"));
3051                 goto exit;
3052         }
3053
3054         if(!(psta->state &_FW_LINKED))
3055         {
3056                 DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_fwlink);
3057                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
3058                 return _FAIL;
3059         }
3060
3061         ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
3062
3063         //_enter_critical(&pstapending->lock, &irqL0);
3064
3065         if (rtw_is_list_empty(&ptxservq->tx_pending)) {
3066                 rtw_list_insert_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
3067         }
3068
3069         //_enter_critical(&ptxservq->sta_pending.lock, &irqL1);
3070
3071         rtw_list_insert_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
3072         ptxservq->qcnt++;
3073         phwxmits[ac_index].accnt++;
3074
3075         //_exit_critical(&ptxservq->sta_pending.lock, &irqL1);
3076
3077         //_exit_critical(&pstapending->lock, &irqL0);
3078
3079 exit:
3080
3081 _func_exit_;
3082
3083         return res;
3084 }
3085
3086 void rtw_alloc_hwxmits(_adapter *padapter)
3087 {
3088         struct hw_xmit *hwxmits;
3089         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3090
3091         pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
3092
3093         pxmitpriv->hwxmits = (struct hw_xmit *)rtw_zmalloc(sizeof (struct hw_xmit) * pxmitpriv->hwxmit_entry);
3094
3095         hwxmits = pxmitpriv->hwxmits;
3096
3097         if(pxmitpriv->hwxmit_entry == 5)
3098         {
3099                 //pxmitpriv->bmc_txqueue.head = 0;
3100                 //hwxmits[0] .phwtxqueue = &pxmitpriv->bmc_txqueue;
3101                 hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
3102
3103                 //pxmitpriv->vo_txqueue.head = 0;
3104                 //hwxmits[1] .phwtxqueue = &pxmitpriv->vo_txqueue;
3105                 hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
3106
3107                 //pxmitpriv->vi_txqueue.head = 0;
3108                 //hwxmits[2] .phwtxqueue = &pxmitpriv->vi_txqueue;
3109                 hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
3110
3111                 //pxmitpriv->bk_txqueue.head = 0;
3112                 //hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue;
3113                 hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
3114
3115                 //pxmitpriv->be_txqueue.head = 0;
3116                 //hwxmits[4] .phwtxqueue = &pxmitpriv->be_txqueue;
3117                 hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
3118
3119         }
3120         else if(pxmitpriv->hwxmit_entry == 4)
3121         {
3122
3123                 //pxmitpriv->vo_txqueue.head = 0;
3124                 //hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue;
3125                 hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
3126
3127                 //pxmitpriv->vi_txqueue.head = 0;
3128                 //hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue;
3129                 hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
3130
3131                 //pxmitpriv->be_txqueue.head = 0;
3132                 //hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue;
3133                 hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
3134
3135                 //pxmitpriv->bk_txqueue.head = 0;
3136                 //hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue;
3137                 hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
3138         }
3139         else
3140         {
3141
3142
3143         }
3144
3145
3146 }
3147
3148 void rtw_free_hwxmits(_adapter *padapter)
3149 {
3150         struct hw_xmit *hwxmits;
3151         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3152
3153         hwxmits = pxmitpriv->hwxmits;
3154         if(hwxmits)
3155                 rtw_mfree((u8 *)hwxmits, (sizeof (struct hw_xmit) * pxmitpriv->hwxmit_entry));
3156 }
3157
3158 void rtw_init_hwxmits(struct hw_xmit *phwxmit, sint entry)
3159 {
3160         sint i;
3161 _func_enter_;
3162         for(i = 0; i < entry; i++, phwxmit++)
3163         {
3164                 //_rtw_spinlock_init(&phwxmit->xmit_lock);
3165                 //_rtw_init_listhead(&phwxmit->pending);
3166                 //phwxmit->txcmdcnt = 0;
3167                 phwxmit->accnt = 0;
3168         }
3169 _func_exit_;
3170 }
3171
3172 #ifdef CONFIG_BR_EXT
3173 int rtw_br_client_tx(_adapter *padapter, struct sk_buff **pskb)
3174 {
3175         struct sk_buff *skb = *pskb;
3176         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3177         _irqL irqL;
3178         //if(check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == _TRUE)
3179         {
3180                 void dhcp_flag_bcast(_adapter *priv, struct sk_buff *skb);
3181                 int res, is_vlan_tag=0, i, do_nat25=1;
3182                 unsigned short vlan_hdr=0;
3183                 void *br_port = NULL;
3184
3185                 //mac_clone_handle_frame(priv, skb);
3186
3187 #if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
3188                 br_port = padapter->pnetdev->br_port;
3189 #else   // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
3190                 rcu_read_lock();
3191                 br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
3192                 rcu_read_unlock();
3193 #endif  // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
3194                 _enter_critical_bh(&padapter->br_ext_lock, &irqL);
3195                 if (    !(skb->data[0] & 1) &&
3196                                 br_port &&
3197                                 memcmp(skb->data+MACADDRLEN, padapter->br_mac, MACADDRLEN) &&
3198                                 *((unsigned short *)(skb->data+MACADDRLEN*2)) != __constant_htons(ETH_P_8021Q) &&
3199                                 *((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP) &&
3200                                 !memcmp(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN) && padapter->scdb_entry) {
3201                         memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
3202                         padapter->scdb_entry->ageing_timer = jiffies;
3203                         _exit_critical_bh(&padapter->br_ext_lock, &irqL);
3204                 }
3205                 else
3206                 //if (!priv->pmib->ethBrExtInfo.nat25_disable)
3207                 {
3208 //                      if (priv->dev->br_port &&
3209 //                               !memcmp(skb->data+MACADDRLEN, priv->br_mac, MACADDRLEN)) {
3210 #if 1
3211                         if (*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_8021Q)) {
3212                                 is_vlan_tag = 1;
3213                                 vlan_hdr = *((unsigned short *)(skb->data+MACADDRLEN*2+2));
3214                                 for (i=0; i<6; i++)
3215                                         *((unsigned short *)(skb->data+MACADDRLEN*2+2-i*2)) = *((unsigned short *)(skb->data+MACADDRLEN*2-2-i*2));
3216                                 skb_pull(skb, 4);
3217                         }
3218                         //if SA == br_mac && skb== IP  => copy SIP to br_ip ?? why
3219                         if (!memcmp(skb->data+MACADDRLEN, padapter->br_mac, MACADDRLEN) &&
3220                                 (*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP)))
3221                                 memcpy(padapter->br_ip, skb->data+WLAN_ETHHDR_LEN+12, 4);
3222
3223                         if (*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_IP)) {
3224                                 if (memcmp(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN)) {
3225                                         void *scdb_findEntry(_adapter *priv, unsigned char *macAddr, unsigned char *ipAddr);
3226
3227                                         if ((padapter->scdb_entry = (struct nat25_network_db_entry *)scdb_findEntry(padapter,
3228                                                                 skb->data+MACADDRLEN, skb->data+WLAN_ETHHDR_LEN+12)) != NULL) {
3229                                                 memcpy(padapter->scdb_mac, skb->data+MACADDRLEN, MACADDRLEN);
3230                                                 memcpy(padapter->scdb_ip, skb->data+WLAN_ETHHDR_LEN+12, 4);
3231                                                 padapter->scdb_entry->ageing_timer = jiffies;
3232                                                 do_nat25 = 0;
3233                                         }
3234                                 }
3235                                 else {
3236                                         if (padapter->scdb_entry) {
3237                                                 padapter->scdb_entry->ageing_timer = jiffies;
3238                                                 do_nat25 = 0;
3239                                         }
3240                                         else {
3241                                                 memset(padapter->scdb_mac, 0, MACADDRLEN);
3242                                                 memset(padapter->scdb_ip, 0, 4);
3243                                         }
3244                                 }
3245                         }
3246                         _exit_critical_bh(&padapter->br_ext_lock, &irqL);
3247 #endif // 1
3248                         if (do_nat25)
3249                         {
3250                                 int nat25_db_handle(_adapter *priv, struct sk_buff *skb, int method);
3251                                 if (nat25_db_handle(padapter, skb, NAT25_CHECK) == 0) {
3252                                         struct sk_buff *newskb;
3253
3254                                         if (is_vlan_tag) {
3255                                                 skb_push(skb, 4);
3256                                                 for (i=0; i<6; i++)
3257                                                         *((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
3258                                                 *((unsigned short *)(skb->data+MACADDRLEN*2)) = __constant_htons(ETH_P_8021Q);
3259                                                 *((unsigned short *)(skb->data+MACADDRLEN*2+2)) = vlan_hdr;
3260                                         }
3261
3262                                         newskb = rtw_skb_copy(skb);
3263                                         if (newskb == NULL) {
3264                                                 //priv->ext_stats.tx_drops++;
3265                                                 DEBUG_ERR("TX DROP: rtw_skb_copy fail!\n");
3266                                                 //goto stop_proc;
3267                                                 return -1;
3268                                         }
3269                                         rtw_skb_free(skb);
3270
3271                                         *pskb = skb = newskb;
3272                                         if (is_vlan_tag) {
3273                                                 vlan_hdr = *((unsigned short *)(skb->data+MACADDRLEN*2+2));
3274                                                 for (i=0; i<6; i++)
3275                                                         *((unsigned short *)(skb->data+MACADDRLEN*2+2-i*2)) = *((unsigned short *)(skb->data+MACADDRLEN*2-2-i*2));
3276                                                 skb_pull(skb, 4);
3277                                         }
3278                                 }
3279
3280                                 if (skb_is_nonlinear(skb))
3281                                         DEBUG_ERR("%s(): skb_is_nonlinear!!\n", __FUNCTION__);
3282
3283
3284 #if (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
3285                                 res = skb_linearize(skb, GFP_ATOMIC);
3286 #else   // (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
3287                                 res = skb_linearize(skb);
3288 #endif  // (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 18))
3289                                 if (res < 0) {
3290                                                 DEBUG_ERR("TX DROP: skb_linearize fail!\n");
3291                                                 //goto free_and_stop;
3292                                                 return -1;
3293                                 }
3294
3295                                 res = nat25_db_handle(padapter, skb, NAT25_INSERT);
3296                                 if (res < 0) {
3297                                         if (res == -2) {
3298                                                 //priv->ext_stats.tx_drops++;
3299                                                 DEBUG_ERR("TX DROP: nat25_db_handle fail!\n");
3300                                                 //goto free_and_stop;
3301                                                 return -1;
3302
3303                                         }
3304                                         // we just print warning message and let it go
3305                                         //DEBUG_WARN("%s()-%d: nat25_db_handle INSERT Warning!\n", __FUNCTION__, __LINE__);
3306                                         //return -1; // return -1 will cause system crash on 2011/08/30!
3307                                         return 0;
3308                                 }
3309                         }
3310
3311                         memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
3312
3313                         dhcp_flag_bcast(padapter, skb);
3314
3315                         if (is_vlan_tag) {
3316                                 skb_push(skb, 4);
3317                                 for (i=0; i<6; i++)
3318                                         *((unsigned short *)(skb->data+i*2)) = *((unsigned short *)(skb->data+4+i*2));
3319                                 *((unsigned short *)(skb->data+MACADDRLEN*2)) = __constant_htons(ETH_P_8021Q);
3320                                 *((unsigned short *)(skb->data+MACADDRLEN*2+2)) = vlan_hdr;
3321                         }
3322                 }
3323 #if 0
3324                 else{
3325                         if (*((unsigned short *)(skb->data+MACADDRLEN*2)) == __constant_htons(ETH_P_8021Q)) {
3326                                 is_vlan_tag = 1;
3327                         }
3328
3329                         if(is_vlan_tag){
3330                                 if(ICMPV6_MCAST_MAC(skb->data) && ICMPV6_PROTO1A_VALN(skb->data)){
3331                                         memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
3332                                 }
3333                         }else
3334                         {
3335                                 if(ICMPV6_MCAST_MAC(skb->data) && ICMPV6_PROTO1A(skb->data)){
3336                                         memcpy(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN);
3337                                 }
3338                         }
3339                 }
3340 #endif  // 0
3341
3342                 // check if SA is equal to our MAC
3343                 if (memcmp(skb->data+MACADDRLEN, GET_MY_HWADDR(padapter), MACADDRLEN)) {
3344                         //priv->ext_stats.tx_drops++;
3345                         DEBUG_ERR("TX DROP: untransformed frame SA:%02X%02X%02X%02X%02X%02X!\n",
3346                                 skb->data[6],skb->data[7],skb->data[8],skb->data[9],skb->data[10],skb->data[11]);
3347                         //goto free_and_stop;
3348                         return -1;
3349                 }
3350         }
3351         return 0;
3352 }
3353 #endif  // CONFIG_BR_EXT
3354
3355 u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
3356 {
3357         u32 addr;
3358         struct pkt_attrib *pattrib = &pxmitframe->attrib;
3359
3360         switch(pattrib->qsel)
3361         {
3362                 case 0:
3363                 case 3:
3364                         addr = BE_QUEUE_INX;
3365                         break;
3366                 case 1:
3367                 case 2:
3368                         addr = BK_QUEUE_INX;
3369                         break;
3370                 case 4:
3371                 case 5:
3372                         addr = VI_QUEUE_INX;
3373                         break;
3374                 case 6:
3375                 case 7:
3376                         addr = VO_QUEUE_INX;
3377                         break;
3378                 case 0x10:
3379                         addr = BCN_QUEUE_INX;
3380                         break;
3381                 case 0x11://BC/MC in PS (HIQ)
3382                         addr = HIGH_QUEUE_INX;
3383                         break;
3384                 case 0x12:
3385                 default:
3386                         addr = MGT_QUEUE_INX;
3387                         break;
3388
3389         }
3390
3391         return addr;
3392
3393 }
3394
3395 static void do_queue_select(_adapter    *padapter, struct pkt_attrib *pattrib)
3396 {
3397         u8 qsel;
3398
3399         qsel = pattrib->priority;
3400         RT_TRACE(_module_rtl871x_xmit_c_,_drv_info_,("### do_queue_select priority=%d ,qsel = %d\n",pattrib->priority ,qsel));
3401
3402 #ifdef CONFIG_CONCURRENT_MODE
3403 //      if (check_fwstate(&padapter->mlmepriv, WIFI_AP_STATE) == _TRUE)
3404 //              qsel = 7;//
3405 #endif
3406
3407         pattrib->qsel = qsel;
3408 }
3409
3410 /*
3411  * The main transmit(tx) entry
3412  *
3413  * Return
3414  *      1       enqueue
3415  *      0       success, hardware will handle this xmit frame(packet)
3416  *      <0      fail
3417  */
3418 s32 rtw_xmit(_adapter *padapter, _pkt **ppkt)
3419 {
3420         static u32 start = 0;
3421         static u32 drop_cnt = 0;
3422 #ifdef CONFIG_AP_MODE
3423         _irqL irqL0;
3424 #endif
3425         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3426         struct xmit_frame *pxmitframe = NULL;
3427 #ifdef CONFIG_BR_EXT
3428         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
3429         void *br_port = NULL;
3430 #endif  // CONFIG_BR_EXT
3431
3432         s32 res;
3433
3434         DBG_COUNTER(padapter->tx_logs.core_tx);
3435
3436         if (start == 0)
3437                 start = rtw_get_current_time();
3438
3439         pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
3440
3441         if (rtw_get_passing_time_ms(start) > 2000) {
3442                 if (drop_cnt)
3443                         DBG_871X("DBG_TX_DROP_FRAME %s no more pxmitframe, drop_cnt:%u\n", __FUNCTION__, drop_cnt);
3444                 start = rtw_get_current_time();
3445                 drop_cnt = 0;
3446         }
3447
3448         if (pxmitframe == NULL) {
3449                 drop_cnt ++;
3450                 RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: no more pxmitframe\n"));
3451                 DBG_COUNTER(padapter->tx_logs.core_tx_err_pxmitframe);
3452                 return -1;
3453         }
3454
3455 #ifdef CONFIG_BR_EXT
3456
3457 #if (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
3458         br_port = padapter->pnetdev->br_port;
3459 #else   // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
3460         rcu_read_lock();
3461         br_port = rcu_dereference(padapter->pnetdev->rx_handler_data);
3462         rcu_read_unlock();
3463 #endif  // (LINUX_VERSION_CODE <= KERNEL_VERSION(2, 6, 35))
3464
3465         if( br_port && check_fwstate(pmlmepriv, WIFI_STATION_STATE|WIFI_ADHOC_STATE) == _TRUE)
3466         {
3467                 res = rtw_br_client_tx(padapter, ppkt);
3468                 if (res == -1)
3469                 {
3470                         rtw_free_xmitframe(pxmitpriv, pxmitframe);
3471                         DBG_COUNTER(padapter->tx_logs.core_tx_err_brtx);
3472                         return -1;
3473                 }
3474         }
3475
3476 #endif  // CONFIG_BR_EXT
3477
3478         res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
3479
3480 #ifdef CONFIG_WAPI_SUPPORT
3481         if(pxmitframe->attrib.ether_type != 0x88B4)
3482         {
3483                 if(rtw_wapi_drop_for_key_absent(padapter, pxmitframe->attrib.ra))
3484                 {
3485                         WAPI_TRACE(WAPI_RX,"drop for key absend when tx \n");
3486                         res = _FAIL;
3487                 }
3488         }
3489 #endif
3490         if (res == _FAIL) {
3491                 RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: update attrib fail\n"));
3492                 #ifdef DBG_TX_DROP_FRAME
3493                 DBG_871X("DBG_TX_DROP_FRAME %s update attrib fail\n", __FUNCTION__);
3494                 #endif
3495                 rtw_free_xmitframe(pxmitpriv, pxmitframe);
3496                 return -1;
3497         }
3498         pxmitframe->pkt = *ppkt;
3499
3500         rtw_led_control(padapter, LED_CTL_TX);
3501
3502         do_queue_select(padapter, &pxmitframe->attrib);
3503
3504 #ifdef CONFIG_AP_MODE
3505         _enter_critical_bh(&pxmitpriv->lock, &irqL0);
3506         if(xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe) == _TRUE)
3507         {
3508                 _exit_critical_bh(&pxmitpriv->lock, &irqL0);
3509                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue);
3510                 return 1;
3511         }
3512         _exit_critical_bh(&pxmitpriv->lock, &irqL0);
3513 #endif
3514
3515         if (rtw_hal_xmit(padapter, pxmitframe) == _FALSE)
3516                 return 1;
3517
3518         return 0;
3519 }
3520
3521 #ifdef CONFIG_TDLS
3522 sint xmitframe_enqueue_for_tdls_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe)
3523 {
3524         sint ret=_FALSE;
3525
3526         _irqL irqL;
3527         struct sta_info *ptdls_sta=NULL;
3528         struct sta_priv *pstapriv = &padapter->stapriv;
3529         struct pkt_attrib *pattrib = &pxmitframe->attrib;
3530         struct mlme_ext_priv    *pmlmeext = &(padapter->mlmeextpriv);
3531         int i;
3532
3533         ptdls_sta=rtw_get_stainfo(pstapriv, pattrib->dst);
3534         if(ptdls_sta==NULL){
3535                 return ret;
3536         }else if(ptdls_sta->tdls_sta_state&TDLS_LINKED_STATE){
3537
3538                 if(pattrib->triggered==1)
3539                 {
3540                         ret = _TRUE;
3541                         return ret;
3542                         }
3543
3544                 _enter_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);
3545
3546                 if(ptdls_sta->state&WIFI_SLEEP_STATE)
3547                 {
3548                         rtw_list_delete(&pxmitframe->list);
3549
3550                         //_enter_critical_bh(&psta->sleep_q.lock, &irqL);
3551
3552                         rtw_list_insert_tail(&pxmitframe->list, get_list_head(&ptdls_sta->sleep_q));
3553
3554                         ptdls_sta->sleepq_len++;
3555                         ptdls_sta->sleepq_ac_len++;
3556
3557                         //indicate 4-AC queue bit in TDLS peer traffic indication
3558                         switch(pattrib->priority)
3559                         {
3560                                 case 1:
3561                                 case 2:
3562                                         ptdls_sta->uapsd_bk = ptdls_sta->uapsd_bk | BIT(1);
3563                                         break;
3564                                 case 4:
3565                                 case 5:
3566                                         ptdls_sta->uapsd_vi = ptdls_sta->uapsd_vi | BIT(1);
3567                                         break;
3568                                 case 6:
3569                                 case 7:
3570                                         ptdls_sta->uapsd_vo = ptdls_sta->uapsd_vo | BIT(1);
3571                                         break;
3572                                 case 0:
3573                                 case 3:
3574                                 default:
3575                                         ptdls_sta->uapsd_be = ptdls_sta->uapsd_be | BIT(1);
3576                                         break;
3577                         }
3578
3579                         if(ptdls_sta->sleepq_len==1)
3580                         {
3581                                 //transmit TDLS PTI via AP
3582                                 rtw_tdls_cmd(padapter, ptdls_sta->hwaddr, TDLS_SD_PTI);
3583                         }
3584                         ret = _TRUE;
3585
3586                 }
3587
3588                 _exit_critical_bh(&ptdls_sta->sleep_q.lock, &irqL);
3589         }
3590
3591         return ret;
3592
3593 }
3594 #endif //CONFIG_TDLS
3595
3596 #define RTW_HIQ_FILTER_ALLOW_ALL 0
3597 #define RTW_HIQ_FILTER_ALLOW_SPECIAL 1
3598 #define RTW_HIQ_FILTER_DENY_ALL 2
3599
3600 inline bool xmitframe_hiq_filter(struct xmit_frame *xmitframe)
3601 {
3602         bool allow = _FALSE;
3603         _adapter *adapter = xmitframe->padapter;
3604         struct registry_priv *registry = &adapter->registrypriv;
3605
3606 if (adapter->interface_type != RTW_PCIE) {
3607
3608         if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_SPECIAL) {
3609
3610                 struct pkt_attrib *attrib = &xmitframe->attrib;
3611
3612                 if (attrib->ether_type == 0x0806
3613                         || attrib->ether_type == 0x888e
3614                         #ifdef CONFIG_WAPI_SUPPORT
3615                         || attrib->ether_type == 0x88B4
3616                         #endif
3617                         || attrib->dhcp_pkt
3618                 ) {
3619                         if (0)
3620                         DBG_871X(FUNC_ADPT_FMT" ether_type:0x%04x%s\n", FUNC_ADPT_ARG(xmitframe->padapter)
3621                                 , attrib->ether_type, attrib->dhcp_pkt?" DHCP":"");
3622                         allow = _TRUE;
3623                 }
3624         }
3625         else if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_ALL) {
3626                 allow = _TRUE;
3627         }
3628         else if (registry->hiq_filter == RTW_HIQ_FILTER_DENY_ALL) {
3629         }
3630         else {
3631                 rtw_warn_on(1);
3632         }
3633 }
3634         return allow;
3635 }
3636
3637 #if defined(CONFIG_AP_MODE) || defined(CONFIG_TDLS)
3638
3639 sint xmitframe_enqueue_for_sleeping_sta(_adapter *padapter, struct xmit_frame *pxmitframe)
3640 {
3641         _irqL irqL;
3642         sint ret=_FALSE;
3643         struct sta_info *psta=NULL;
3644         struct sta_priv *pstapriv = &padapter->stapriv;
3645         struct pkt_attrib *pattrib = &pxmitframe->attrib;
3646         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
3647         sint bmcst = IS_MCAST(pattrib->ra);
3648         bool update_tim = _FALSE;
3649 #ifdef CONFIG_TDLS
3650         struct tdls_info *ptdlsinfo = &padapter->tdlsinfo;
3651
3652         if( ptdlsinfo->setup_state == TDLS_LINKED_STATE )
3653         {
3654                 ret = xmitframe_enqueue_for_tdls_sleeping_sta(padapter, pxmitframe);
3655                 return ret;
3656         }
3657 #endif //CONFIG_TDLS
3658
3659         if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == _FALSE)
3660         {
3661                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_fwstate);
3662             return ret;
3663         }
3664
3665         if(pattrib->psta)
3666         {
3667                 psta = pattrib->psta;
3668         }
3669         else
3670         {
3671                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
3672                 psta=rtw_get_stainfo(pstapriv, pattrib->ra);
3673         }
3674
3675         if(psta==NULL)
3676         {
3677                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_nosta);
3678                 DBG_871X("%s, psta==NUL\n", __func__);
3679                 return _FALSE;
3680         }
3681
3682         if(!(psta->state &_FW_LINKED))
3683         {
3684                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_link);
3685                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
3686                 return _FALSE;
3687         }
3688
3689         if(pattrib->triggered==1)
3690         {
3691                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_trigger);
3692                 //DBG_871X("directly xmit pspoll_triggered packet\n");
3693
3694                 //pattrib->triggered=0;
3695                 if (bmcst && xmitframe_hiq_filter(pxmitframe) == _TRUE)
3696                         pattrib->qsel = 0x11;//HIQ
3697
3698                 return ret;
3699         }
3700
3701
3702         if(bmcst)
3703         {
3704                 _enter_critical_bh(&psta->sleep_q.lock, &irqL);
3705
3706                 if(pstapriv->sta_dz_bitmap)//if anyone sta is in ps mode
3707                 {
3708                         //pattrib->qsel = 0x11;//HIQ
3709
3710                         rtw_list_delete(&pxmitframe->list);
3711
3712                         //_enter_critical_bh(&psta->sleep_q.lock, &irqL);
3713
3714                         rtw_list_insert_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
3715
3716                         psta->sleepq_len++;
3717
3718                         if (!(pstapriv->tim_bitmap & BIT(0)))
3719                                 update_tim = _TRUE;
3720
3721                         pstapriv->tim_bitmap |= BIT(0);//
3722                         pstapriv->sta_dz_bitmap |= BIT(0);
3723
3724                         //DBG_871X("enqueue, sq_len=%d, tim=%x\n", psta->sleepq_len, pstapriv->tim_bitmap);
3725
3726                         if (update_tim == _TRUE) {
3727                                 if (is_broadcast_mac_addr(pattrib->ra))
3728                                         _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, "buffer BC");
3729                                 else
3730                                         _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, "buffer MC");
3731                         } else {
3732                                 chk_bmc_sleepq_cmd(padapter);
3733                         }
3734
3735                         //_exit_critical_bh(&psta->sleep_q.lock, &irqL);
3736
3737                         ret = _TRUE;
3738
3739                         DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_mcast);
3740
3741                 }
3742
3743                 _exit_critical_bh(&psta->sleep_q.lock, &irqL);
3744
3745                 return ret;
3746
3747         }
3748
3749
3750         _enter_critical_bh(&psta->sleep_q.lock, &irqL);
3751
3752         if(psta->state&WIFI_SLEEP_STATE)
3753         {
3754                 u8 wmmps_ac=0;
3755
3756                 if(pstapriv->sta_dz_bitmap&BIT(psta->aid))
3757                 {
3758                         rtw_list_delete(&pxmitframe->list);
3759
3760                         //_enter_critical_bh(&psta->sleep_q.lock, &irqL);
3761
3762                         rtw_list_insert_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
3763
3764                         psta->sleepq_len++;
3765
3766                         switch(pattrib->priority)
3767                         {
3768                                 case 1:
3769                                 case 2:
3770                                         wmmps_ac = psta->uapsd_bk&BIT(0);
3771                                         break;
3772                                 case 4:
3773                                 case 5:
3774                                         wmmps_ac = psta->uapsd_vi&BIT(0);
3775                                         break;
3776                                 case 6:
3777                                 case 7:
3778                                         wmmps_ac = psta->uapsd_vo&BIT(0);
3779                                         break;
3780                                 case 0:
3781                                 case 3:
3782                                 default:
3783                                         wmmps_ac = psta->uapsd_be&BIT(0);
3784                                         break;
3785                         }
3786
3787                         if(wmmps_ac)
3788                                 psta->sleepq_ac_len++;
3789
3790                         if(((psta->has_legacy_ac) && (!wmmps_ac)) ||((!psta->has_legacy_ac)&&(wmmps_ac)))
3791                         {
3792                                 if (!(pstapriv->tim_bitmap & BIT(psta->aid)))
3793                                         update_tim = _TRUE;
3794
3795                                 pstapriv->tim_bitmap |= BIT(psta->aid);
3796
3797                                 //DBG_871X("enqueue, sq_len=%d, tim=%x\n", psta->sleepq_len, pstapriv->tim_bitmap);
3798
3799                                 if(update_tim == _TRUE)
3800                                 {
3801                                         //DBG_871X("sleepq_len==1, update BCNTIM\n");
3802                                         //upate BCN for TIM IE
3803                                         _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, "buffer UC");
3804                                 }
3805                         }
3806
3807                         //_exit_critical_bh(&psta->sleep_q.lock, &irqL);
3808
3809                         //if(psta->sleepq_len > (NR_XMITFRAME>>3))
3810                         //{
3811                         //      wakeup_sta_to_xmit(padapter, psta);
3812                         //}
3813
3814                         ret = _TRUE;
3815
3816                         DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_ucast);
3817                 }
3818
3819         }
3820
3821         _exit_critical_bh(&psta->sleep_q.lock, &irqL);
3822
3823         return ret;
3824
3825 }
3826
3827 static void dequeue_xmitframes_to_sleeping_queue(_adapter *padapter, struct sta_info *psta, _queue *pframequeue)
3828 {
3829         sint ret;
3830         _list   *plist, *phead;
3831         u8      ac_index;
3832         struct tx_servq *ptxservq;
3833         struct pkt_attrib       *pattrib;
3834         struct xmit_frame       *pxmitframe;
3835         struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
3836
3837         phead = get_list_head(pframequeue);
3838         plist = get_next(phead);
3839
3840         while (rtw_end_of_queue_search(phead, plist) == _FALSE)
3841         {
3842                 pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
3843
3844                 plist = get_next(plist);
3845
3846                 pattrib = &pxmitframe->attrib;
3847
3848                 pattrib->triggered = 0;
3849
3850                 ret = xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe);
3851
3852                 if(_TRUE == ret)
3853                 {
3854                         ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
3855
3856                         ptxservq->qcnt--;
3857                         phwxmits[ac_index].accnt--;
3858                 }
3859                 else
3860                 {
3861                         //DBG_871X("xmitframe_enqueue_for_sleeping_sta return _FALSE\n");
3862                 }
3863
3864         }
3865
3866 }
3867
3868 void stop_sta_xmit(_adapter *padapter, struct sta_info *psta)
3869 {
3870         _irqL irqL0;
3871         struct sta_info *psta_bmc;
3872         struct sta_xmit_priv *pstaxmitpriv;
3873         struct sta_priv *pstapriv = &padapter->stapriv;
3874         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3875
3876         pstaxmitpriv = &psta->sta_xmitpriv;
3877
3878         //for BC/MC Frames
3879         psta_bmc = rtw_get_bcmc_stainfo(padapter);
3880
3881
3882         _enter_critical_bh(&pxmitpriv->lock, &irqL0);
3883
3884         psta->state |= WIFI_SLEEP_STATE;
3885
3886 #ifdef CONFIG_TDLS
3887         if( !(psta->tdls_sta_state & TDLS_LINKED_STATE) )
3888 #endif //CONFIG_TDLS
3889         pstapriv->sta_dz_bitmap |= BIT(psta->aid);
3890
3891
3892
3893         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
3894         rtw_list_delete(&(pstaxmitpriv->vo_q.tx_pending));
3895
3896
3897         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
3898         rtw_list_delete(&(pstaxmitpriv->vi_q.tx_pending));
3899
3900
3901         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending);
3902         rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending));
3903
3904
3905         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending);
3906         rtw_list_delete(&(pstaxmitpriv->bk_q.tx_pending));
3907
3908 #ifdef CONFIG_TDLS
3909         if( !(psta->tdls_sta_state & TDLS_LINKED_STATE) )
3910         {
3911                 if( psta_bmc != NULL )
3912                 {
3913 #endif //CONFIG_TDLS
3914
3915
3916         //for BC/MC Frames
3917         pstaxmitpriv = &psta_bmc->sta_xmitpriv;
3918         dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending);
3919         rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending));
3920
3921
3922 #ifdef CONFIG_TDLS
3923                 }
3924         }
3925 #endif //CONFIG_TDLS
3926         _exit_critical_bh(&pxmitpriv->lock, &irqL0);
3927
3928
3929 }
3930
3931 void wakeup_sta_to_xmit(_adapter *padapter, struct sta_info *psta)
3932 {
3933         _irqL irqL;
3934         u8 update_mask=0, wmmps_ac=0;
3935         struct sta_info *psta_bmc;
3936         _list   *xmitframe_plist, *xmitframe_phead;
3937         struct xmit_frame *pxmitframe=NULL;
3938         struct sta_priv *pstapriv = &padapter->stapriv;
3939         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
3940
3941         psta_bmc = rtw_get_bcmc_stainfo(padapter);
3942
3943
3944         //_enter_critical_bh(&psta->sleep_q.lock, &irqL);
3945         _enter_critical_bh(&pxmitpriv->lock, &irqL);
3946
3947         xmitframe_phead = get_list_head(&psta->sleep_q);
3948         xmitframe_plist = get_next(xmitframe_phead);
3949
3950         while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
3951         {
3952                 pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
3953
3954                 xmitframe_plist = get_next(xmitframe_plist);
3955
3956                 rtw_list_delete(&pxmitframe->list);
3957
3958                 switch(pxmitframe->attrib.priority)
3959                 {
3960                         case 1:
3961                         case 2:
3962                                 wmmps_ac = psta->uapsd_bk&BIT(1);
3963                                 break;
3964                         case 4:
3965                         case 5:
3966                                 wmmps_ac = psta->uapsd_vi&BIT(1);
3967                                 break;
3968                         case 6:
3969                         case 7:
3970                                 wmmps_ac = psta->uapsd_vo&BIT(1);
3971                                 break;
3972                         case 0:
3973                         case 3:
3974                         default:
3975                                 wmmps_ac = psta->uapsd_be&BIT(1);
3976                                 break;
3977                 }
3978
3979                 psta->sleepq_len--;
3980                 if(psta->sleepq_len>0)
3981                         pxmitframe->attrib.mdata = 1;
3982                 else
3983                         pxmitframe->attrib.mdata = 0;
3984
3985                 if(wmmps_ac)
3986                 {
3987                         psta->sleepq_ac_len--;
3988                         if(psta->sleepq_ac_len>0)
3989                         {
3990                                 pxmitframe->attrib.mdata = 1;
3991                                 pxmitframe->attrib.eosp = 0;
3992                         }
3993                         else
3994                         {
3995                                 pxmitframe->attrib.mdata = 0;
3996                                 pxmitframe->attrib.eosp = 1;
3997                         }
3998                 }
3999
4000                 pxmitframe->attrib.triggered = 1;
4001
4002 /*
4003                 _exit_critical_bh(&psta->sleep_q.lock, &irqL);
4004                 if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
4005                 {
4006                         rtw_os_xmit_complete(padapter, pxmitframe);
4007                 }
4008                 _enter_critical_bh(&psta->sleep_q.lock, &irqL);
4009 */
4010                 rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
4011
4012         }
4013
4014         if(psta->sleepq_len==0)
4015         {
4016 #ifdef CONFIG_TDLS
4017                 if( psta->tdls_sta_state & TDLS_LINKED_STATE )
4018                 {
4019                         if(psta->state&WIFI_SLEEP_STATE)
4020                                 psta->state ^= WIFI_SLEEP_STATE;
4021
4022                         goto _exit;
4023                 }
4024 #endif //CONFIG_TDLS
4025
4026                 if (pstapriv->tim_bitmap & BIT(psta->aid)) {
4027                         //DBG_871X("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap);
4028                         //upate BCN for TIM IE
4029                         //update_BCNTIM(padapter);
4030                         update_mask = BIT(0);
4031                 }
4032
4033                 pstapriv->tim_bitmap &= ~BIT(psta->aid);
4034
4035                 if(psta->state&WIFI_SLEEP_STATE)
4036                         psta->state ^= WIFI_SLEEP_STATE;
4037
4038                 if(psta->state & WIFI_STA_ALIVE_CHK_STATE)
4039                 {
4040                         psta->expire_to = pstapriv->expire_to;
4041                         psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
4042                 }
4043
4044                 pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
4045         }
4046
4047         //for BC/MC Frames
4048         if(!psta_bmc)
4049                 goto _exit;
4050
4051         if((pstapriv->sta_dz_bitmap&0xfffe) == 0x0)//no any sta in ps mode
4052         {
4053                 xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
4054                 xmitframe_plist = get_next(xmitframe_phead);
4055
4056                 while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
4057                 {
4058                         pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
4059
4060                         xmitframe_plist = get_next(xmitframe_plist);
4061
4062                         rtw_list_delete(&pxmitframe->list);
4063
4064                         psta_bmc->sleepq_len--;
4065                         if(psta_bmc->sleepq_len>0)
4066                                 pxmitframe->attrib.mdata = 1;
4067                         else
4068                                 pxmitframe->attrib.mdata = 0;
4069
4070
4071                         pxmitframe->attrib.triggered = 1;
4072 /*
4073                         _exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
4074                         if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
4075                         {
4076                                 rtw_os_xmit_complete(padapter, pxmitframe);
4077                         }
4078                         _enter_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
4079
4080 */
4081                         rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
4082
4083                 }
4084
4085                 if(psta_bmc->sleepq_len==0)
4086                 {
4087                         if (pstapriv->tim_bitmap & BIT(0)) {
4088                                 //DBG_871X("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap);
4089                                 //upate BCN for TIM IE
4090                                 //update_BCNTIM(padapter);
4091                                 update_mask |= BIT(1);
4092                         }
4093
4094                         pstapriv->tim_bitmap &= ~BIT(0);
4095                         pstapriv->sta_dz_bitmap &= ~BIT(0);
4096                 }
4097         }
4098
4099 _exit:
4100
4101         //_exit_critical_bh(&psta_bmc->sleep_q.lock, &irqL);
4102         _exit_critical_bh(&pxmitpriv->lock, &irqL);
4103
4104         if(update_mask)
4105         {
4106                 //update_BCNTIM(padapter);
4107                 if ((update_mask & (BIT(0)|BIT(1))) == (BIT(0)|BIT(1)))
4108                         _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, "clear UC&BMC");
4109                 else if ((update_mask & BIT(1)) == BIT(1))
4110                         _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, "clear BMC");
4111                 else
4112                         _update_beacon(padapter, _TIM_IE_, NULL, _TRUE, "clear UC");
4113         }
4114
4115 }
4116
4117 void xmit_delivery_enabled_frames(_adapter *padapter, struct sta_info *psta)
4118 {
4119         _irqL irqL;
4120         u8 wmmps_ac=0;
4121         _list   *xmitframe_plist, *xmitframe_phead;
4122         struct xmit_frame *pxmitframe=NULL;
4123         struct sta_priv *pstapriv = &padapter->stapriv;
4124         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
4125
4126
4127         //_enter_critical_bh(&psta->sleep_q.lock, &irqL);
4128         _enter_critical_bh(&pxmitpriv->lock, &irqL);
4129
4130         xmitframe_phead = get_list_head(&psta->sleep_q);
4131         xmitframe_plist = get_next(xmitframe_phead);
4132
4133         while ((rtw_end_of_queue_search(xmitframe_phead, xmitframe_plist)) == _FALSE)
4134         {
4135                 pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
4136
4137                 xmitframe_plist = get_next(xmitframe_plist);
4138
4139                 switch(pxmitframe->attrib.priority)
4140                 {
4141                         case 1:
4142                         case 2:
4143                                 wmmps_ac = psta->uapsd_bk&BIT(1);
4144                                 break;
4145                         case 4:
4146                         case 5:
4147                                 wmmps_ac = psta->uapsd_vi&BIT(1);
4148                                 break;
4149                         case 6:
4150                         case 7:
4151                                 wmmps_ac = psta->uapsd_vo&BIT(1);
4152                                 break;
4153                         case 0:
4154                         case 3:
4155                         default:
4156                                 wmmps_ac = psta->uapsd_be&BIT(1);
4157                                 break;
4158                 }
4159
4160                 if(!wmmps_ac)
4161                         continue;
4162
4163                 rtw_list_delete(&pxmitframe->list);
4164
4165                 psta->sleepq_len--;
4166                 psta->sleepq_ac_len--;
4167
4168                 if(psta->sleepq_ac_len>0)
4169                 {
4170                         pxmitframe->attrib.mdata = 1;
4171                         pxmitframe->attrib.eosp = 0;
4172                 }
4173                 else
4174                 {
4175                         pxmitframe->attrib.mdata = 0;
4176                         pxmitframe->attrib.eosp = 1;
4177                 }
4178
4179                 pxmitframe->attrib.triggered = 1;
4180
4181 /*
4182                 if(rtw_hal_xmit(padapter, pxmitframe) == _TRUE)
4183                 {
4184                         rtw_os_xmit_complete(padapter, pxmitframe);
4185                 }
4186 */
4187                 rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
4188
4189                 if((psta->sleepq_ac_len==0) && (!psta->has_legacy_ac) && (wmmps_ac))
4190                 {
4191 #ifdef CONFIG_TDLS
4192                         if(psta->tdls_sta_state & TDLS_LINKED_STATE )
4193                         {
4194                                 //_exit_critical_bh(&psta->sleep_q.lock, &irqL);
4195                                 _exit_critical_bh(&pxmitpriv->lock, &irqL);
4196                                 return;
4197                         }
4198 #endif //CONFIG_TDLS
4199                         pstapriv->tim_bitmap &= ~BIT(psta->aid);
4200
4201                         //DBG_871X("wakeup to xmit, qlen==0, update_BCNTIM, tim=%x\n", pstapriv->tim_bitmap);
4202                         //upate BCN for TIM IE
4203                         //update_BCNTIM(padapter);
4204                         update_beacon(padapter, _TIM_IE_, NULL, _TRUE);
4205                         //update_mask = BIT(0);
4206                 }
4207
4208         }
4209
4210         //_exit_critical_bh(&psta->sleep_q.lock, &irqL);
4211         _exit_critical_bh(&pxmitpriv->lock, &irqL);
4212
4213 }
4214
4215 #endif
4216
4217 #ifdef CONFIG_XMIT_THREAD_MODE
4218 void enqueue_pending_xmitbuf(
4219         struct xmit_priv *pxmitpriv,
4220         struct xmit_buf *pxmitbuf)
4221 {
4222         _irqL irql;
4223         _queue *pqueue;
4224         _adapter *pri_adapter = pxmitpriv->adapter;
4225
4226         pqueue = &pxmitpriv->pending_xmitbuf_queue;
4227
4228         _enter_critical_bh(&pqueue->lock, &irql);
4229         rtw_list_delete(&pxmitbuf->list);
4230         rtw_list_insert_tail(&pxmitbuf->list, get_list_head(pqueue));
4231         _exit_critical_bh(&pqueue->lock, &irql);
4232
4233
4234
4235 #if defined(CONFIG_SDIO_HCI) && defined(CONFIG_CONCURRENT_MODE)
4236         if (pri_adapter->adapter_type > PRIMARY_ADAPTER)
4237                 pri_adapter = pri_adapter->pbuddy_adapter;
4238 #endif  //SDIO_HCI + CONCURRENT
4239         _rtw_up_sema(&(pri_adapter->xmitpriv.xmit_sema));
4240
4241 }
4242
4243 void enqueue_pending_xmitbuf_to_head(
4244         struct xmit_priv *pxmitpriv,
4245         struct xmit_buf *pxmitbuf)
4246 {
4247         _irqL irql;
4248         _queue *pqueue;
4249         _adapter *pri_adapter = pxmitpriv->adapter;
4250
4251         pqueue = &pxmitpriv->pending_xmitbuf_queue;
4252
4253         _enter_critical_bh(&pqueue->lock, &irql);
4254         rtw_list_delete(&pxmitbuf->list);
4255         rtw_list_insert_head(&pxmitbuf->list, get_list_head(pqueue));
4256         _exit_critical_bh(&pqueue->lock, &irql);
4257 }
4258
4259 struct xmit_buf* dequeue_pending_xmitbuf(
4260         struct xmit_priv *pxmitpriv)
4261 {
4262         _irqL irql;
4263         struct xmit_buf *pxmitbuf;
4264         _queue *pqueue;
4265
4266
4267         pxmitbuf = NULL;
4268         pqueue = &pxmitpriv->pending_xmitbuf_queue;
4269
4270         _enter_critical_bh(&pqueue->lock, &irql);
4271
4272         if (_rtw_queue_empty(pqueue) == _FALSE)
4273         {
4274                 _list *plist, *phead;
4275
4276                 phead = get_list_head(pqueue);
4277                 plist = get_next(phead);
4278                 pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
4279                 rtw_list_delete(&pxmitbuf->list);
4280         }
4281
4282         _exit_critical_bh(&pqueue->lock, &irql);
4283
4284         return pxmitbuf;
4285 }
4286
4287 struct xmit_buf* dequeue_pending_xmitbuf_under_survey(
4288         struct xmit_priv *pxmitpriv)
4289 {
4290         _irqL irql;
4291         struct xmit_buf *pxmitbuf;
4292 #ifdef CONFIG_USB_HCI
4293         struct xmit_frame *pxmitframe;
4294 #endif
4295         _queue *pqueue;
4296
4297
4298         pxmitbuf = NULL;
4299         pqueue = &pxmitpriv->pending_xmitbuf_queue;
4300
4301         _enter_critical_bh(&pqueue->lock, &irql);
4302
4303         if (_rtw_queue_empty(pqueue) == _FALSE)
4304         {
4305                 _list *plist, *phead;
4306                 u8 type;
4307
4308                 phead = get_list_head(pqueue);
4309                 plist = phead;
4310                 do {
4311                         plist = get_next(plist);
4312                                 if (plist == phead) break;
4313
4314                         pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
4315
4316 #ifdef CONFIG_USB_HCI
4317                         pxmitframe = (struct xmit_frame*)pxmitbuf->priv_data;
4318                         if(pxmitframe)
4319                         {
4320                                 type = GetFrameSubType(pxmitbuf->pbuf + TXDESC_SIZE + pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
4321                         }
4322                         else
4323                         {
4324                                 DBG_871X("%s, !!!ERROR!!! For USB, TODO ITEM \n", __FUNCTION__);
4325                         }
4326 #else
4327                         type = GetFrameSubType(pxmitbuf->pbuf + TXDESC_OFFSET);
4328 #endif
4329
4330                         if ((type == WIFI_PROBEREQ) ||
4331                                 (type == WIFI_DATA_NULL) ||
4332                                 (type == WIFI_QOS_DATA_NULL))
4333                         {
4334                                 rtw_list_delete(&pxmitbuf->list);
4335                                 break;
4336                         }
4337                         pxmitbuf = NULL;
4338                 } while (1);
4339         }
4340
4341         _exit_critical_bh(&pqueue->lock, &irql);
4342
4343         return pxmitbuf;
4344 }
4345
4346 sint check_pending_xmitbuf(
4347         struct xmit_priv *pxmitpriv)
4348 {
4349         _irqL irql;
4350         _queue *pqueue;
4351         sint    ret = _FALSE;
4352
4353         pqueue = &pxmitpriv->pending_xmitbuf_queue;
4354
4355         _enter_critical_bh(&pqueue->lock, &irql);
4356
4357         if(_rtw_queue_empty(pqueue) == _FALSE)
4358                 ret = _TRUE;
4359
4360         _exit_critical_bh(&pqueue->lock, &irql);
4361
4362         return ret;
4363 }
4364
4365 thread_return rtw_xmit_thread(thread_context context)
4366 {
4367         s32 err;
4368         PADAPTER padapter;
4369
4370
4371         err = _SUCCESS;
4372         padapter = (PADAPTER)context;
4373
4374         thread_enter("RTW_XMIT_THREAD");
4375
4376         do {
4377                 err = rtw_hal_xmit_thread_handler(padapter);
4378                 flush_signals_thread();
4379         } while (_SUCCESS == err);
4380
4381         _rtw_up_sema(&padapter->xmitpriv.terminate_xmitthread_sema);
4382
4383         thread_exit();
4384 }
4385 #endif
4386
4387 void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms)
4388 {
4389         sctx->timeout_ms = timeout_ms;
4390         sctx->submit_time= rtw_get_current_time();
4391 #ifdef PLATFORM_LINUX /* TODO: add condition wating interface for other os */
4392         init_completion(&sctx->done);
4393 #endif
4394         sctx->status = RTW_SCTX_SUBMITTED;
4395 }
4396
4397 int rtw_sctx_wait(struct submit_ctx *sctx)
4398 {
4399         int ret = _FAIL;
4400         unsigned long expire;
4401         int status = 0;
4402
4403 #ifdef PLATFORM_LINUX
4404         expire= sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) : MAX_SCHEDULE_TIMEOUT;
4405         if (!wait_for_completion_timeout(&sctx->done, expire)) {
4406                 /* timeout, do something?? */
4407                 status = RTW_SCTX_DONE_TIMEOUT;
4408                 DBG_871X("%s timeout\n", __func__);
4409         } else {
4410                 status = sctx->status;
4411         }
4412 #endif
4413
4414         if (status == RTW_SCTX_DONE_SUCCESS) {
4415                 ret = _SUCCESS;
4416         }
4417
4418         return ret;
4419 }
4420
4421 bool rtw_sctx_chk_waring_status(int status)
4422 {
4423         switch(status) {
4424         case RTW_SCTX_DONE_UNKNOWN:
4425         case RTW_SCTX_DONE_BUF_ALLOC:
4426         case RTW_SCTX_DONE_BUF_FREE:
4427
4428         case RTW_SCTX_DONE_DRV_STOP:
4429         case RTW_SCTX_DONE_DEV_REMOVE:
4430                 return _TRUE;
4431         default:
4432                 return _FALSE;
4433         }
4434 }
4435
4436 void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
4437 {
4438         if (*sctx) {
4439                 if (rtw_sctx_chk_waring_status(status))
4440                         DBG_871X("%s status:%d\n", __func__, status);
4441                 (*sctx)->status = status;
4442                 #ifdef PLATFORM_LINUX
4443                 complete(&((*sctx)->done));
4444                 #endif
4445                 *sctx = NULL;
4446         }
4447 }
4448
4449 void rtw_sctx_done(struct submit_ctx **sctx)
4450 {
4451         rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
4452 }
4453
4454 #ifdef CONFIG_XMIT_ACK
4455
4456 #ifdef CONFIG_XMIT_ACK_POLLING
4457 s32 c2h_evt_hdl(_adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter);
4458
4459 /**
4460  * rtw_ack_tx_polling -
4461  * @pxmitpriv: xmit_priv to address ack_tx_ops
4462  * @timeout_ms: timeout msec
4463  *
4464  * Init ack_tx_ops and then do c2h_evt_hdl() and polling ack_tx_ops repeatedly
4465  * till tx report or timeout
4466  * Returns: _SUCCESS if TX report ok, _FAIL for others
4467  */
4468 int rtw_ack_tx_polling(struct xmit_priv *pxmitpriv, u32 timeout_ms)
4469 {
4470         int ret = _FAIL;
4471         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
4472         _adapter *adapter = container_of(pxmitpriv, _adapter, xmitpriv);
4473
4474         pack_tx_ops->submit_time = rtw_get_current_time();
4475         pack_tx_ops->timeout_ms = timeout_ms;
4476         pack_tx_ops->status = RTW_SCTX_SUBMITTED;
4477
4478         do {
4479                 c2h_evt_hdl(adapter, NULL, rtw_hal_c2h_id_filter_ccx(adapter));
4480                 if (pack_tx_ops->status != RTW_SCTX_SUBMITTED)
4481                         break;
4482
4483                 if (adapter->bDriverStopped) {
4484                         pack_tx_ops->status = RTW_SCTX_DONE_DRV_STOP;
4485                         break;
4486                 }
4487                 if (adapter->bSurpriseRemoved) {
4488                         pack_tx_ops->status = RTW_SCTX_DONE_DEV_REMOVE;
4489                         break;
4490                 }
4491
4492                 rtw_msleep_os(10);
4493         } while (rtw_get_passing_time_ms(pack_tx_ops->submit_time) < timeout_ms);
4494
4495         if (pack_tx_ops->status == RTW_SCTX_SUBMITTED) {
4496                 pack_tx_ops->status = RTW_SCTX_DONE_TIMEOUT;
4497                 DBG_871X("%s timeout\n", __func__);
4498         }
4499
4500         if (pack_tx_ops->status == RTW_SCTX_DONE_SUCCESS)
4501                 ret = _SUCCESS;
4502
4503         return ret;
4504 }
4505 #endif
4506
4507 #ifdef CONFIG_DETECT_C2H_BY_POLLING
4508 s32 c2h_evt_hdl(_adapter *adapter, struct c2h_evt_hdr *c2h_evt, c2h_id_filter filter);
4509 #endif
4510
4511 int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
4512 {
4513 #ifdef CONFIG_DETECT_C2H_BY_POLLING
4514         _adapter *adapter = container_of(pxmitpriv, _adapter, xmitpriv);
4515         c2h_id_filter ccx_id_filter = rtw_hal_c2h_id_filter_ccx(adapter);
4516         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
4517         u8 check_c2hcmd;
4518         u8 check_ccx;
4519         int ret = _FAIL;
4520
4521         pack_tx_ops->submit_time = rtw_get_current_time();
4522         pack_tx_ops->timeout_ms = timeout_ms;
4523         pack_tx_ops->status = RTW_SCTX_SUBMITTED;
4524
4525         do {
4526                 rtw_msleep_os(10);
4527                 //check_c2hcmd = rtw_read8(adapter, 0x1AF);
4528                 //check_ccx = rtw_read8(adapter, 0x1A0);
4529                 rtw_hal_get_hwreg(adapter, HW_VAR_C2HEVT_CLEAR, (u8 *)(&check_c2hcmd));
4530                 rtw_hal_get_hwreg(adapter, HW_VAR_C2HEVT_MSG_NORMAL, (u8 *)(&check_ccx));
4531
4532
4533                 if (check_c2hcmd != 0)
4534                 {
4535                         if (check_c2hcmd != 0xFF)
4536                         {
4537                                 c2h_evt_clear(adapter);
4538                         }
4539                         else if (ccx_id_filter(check_ccx & 0x0F) == _TRUE)
4540                         {
4541                                 c2h_evt_hdl(adapter, NULL, ccx_id_filter);
4542                                 if (pack_tx_ops->status != RTW_SCTX_SUBMITTED)
4543                                         break;
4544
4545                                 if (adapter->bDriverStopped) {
4546                                         pack_tx_ops->status = RTW_SCTX_DONE_DRV_STOP;
4547                                         break;
4548                                 }
4549                                 if (adapter->bSurpriseRemoved) {
4550                                         pack_tx_ops->status = RTW_SCTX_DONE_DEV_REMOVE;
4551                                         break;
4552                                 }
4553                         }
4554                 }
4555         } while (rtw_get_passing_time_ms(pack_tx_ops->submit_time) < timeout_ms);
4556
4557         if (pack_tx_ops->status == RTW_SCTX_SUBMITTED) {
4558                 pack_tx_ops->status = RTW_SCTX_DONE_TIMEOUT;
4559                 DBG_871X("%s timeout\n", __func__);
4560         }
4561
4562         if (pack_tx_ops->status == RTW_SCTX_DONE_SUCCESS)
4563                 ret = _SUCCESS;
4564
4565         return ret;
4566 #else
4567 #ifdef CONFIG_XMIT_ACK_POLLING
4568         return rtw_ack_tx_polling(pxmitpriv, timeout_ms);
4569 #else
4570         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
4571
4572         pack_tx_ops->submit_time = rtw_get_current_time();
4573         pack_tx_ops->timeout_ms = timeout_ms;
4574         pack_tx_ops->status = RTW_SCTX_SUBMITTED;
4575
4576         return rtw_sctx_wait(pack_tx_ops);
4577 #endif
4578 #endif
4579 }
4580
4581 void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status)
4582 {
4583         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
4584
4585         if (pxmitpriv->ack_tx) {
4586                 rtw_sctx_done_err(&pack_tx_ops, status);
4587         } else {
4588                 DBG_871X("%s ack_tx not set\n", __func__);
4589         }
4590 }
4591 #endif //CONFIG_XMIT_ACK
4592