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