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