rtl8821ce/core/rtw_xmit.c

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