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