core/rtw_xmit.c

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