drivers/net/wireless/rockchip_wlan/rtl8822be/core/rtw_xmit.c

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