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