drivers/staging/rtl8723bs/core/rtw_xmit.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /******************************************************************************
   3  *
   4  * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
   5  *
   6  ******************************************************************************/
   7 #define _RTW_XMIT_C_
   8
   9 #include <drv_types.h>
  10 #include <rtw_debug.h>
  11
  12 static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
  13 static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
  14
  15 static void _init_txservq(struct tx_servq *ptxservq)
  16 {
  17         INIT_LIST_HEAD(&ptxservq->tx_pending);
  18         _rtw_init_queue(&ptxservq->sta_pending);
  19         ptxservq->qcnt = 0;
  20 }
  21
  22 void _rtw_init_sta_xmit_priv(struct sta_xmit_priv *psta_xmitpriv)
  23 {
  24         memset((unsigned char *)psta_xmitpriv, 0, sizeof(struct sta_xmit_priv));
  25
  26         spin_lock_init(&psta_xmitpriv->lock);
  27
  28         /* for (i = 0 ; i < MAX_NUMBLKS; i++) */
  29         /*      _init_txservq(&(psta_xmitpriv->blk_q[i])); */
  30
  31         _init_txservq(&psta_xmitpriv->be_q);
  32         _init_txservq(&psta_xmitpriv->bk_q);
  33         _init_txservq(&psta_xmitpriv->vi_q);
  34         _init_txservq(&psta_xmitpriv->vo_q);
  35         INIT_LIST_HEAD(&psta_xmitpriv->legacy_dz);
  36         INIT_LIST_HEAD(&psta_xmitpriv->apsd);
  37 }
  38
  39 s32 _rtw_init_xmit_priv(struct xmit_priv *pxmitpriv, struct adapter *padapter)
  40 {
  41         int i;
  42         struct xmit_buf *pxmitbuf;
  43         struct xmit_frame *pxframe;
  44         sint    res = _SUCCESS;
  45
  46         spin_lock_init(&pxmitpriv->lock);
  47         spin_lock_init(&pxmitpriv->lock_sctx);
  48         init_completion(&pxmitpriv->xmit_comp);
  49         init_completion(&pxmitpriv->terminate_xmitthread_comp);
  50
  51         /*
  52         Please insert all the queue initializaiton using _rtw_init_queue below
  53         */
  54
  55         pxmitpriv->adapter = padapter;
  56
  57         /* for (i = 0 ; i < MAX_NUMBLKS; i++) */
  58         /*      _rtw_init_queue(&pxmitpriv->blk_strms[i]); */
  59
  60         _rtw_init_queue(&pxmitpriv->be_pending);
  61         _rtw_init_queue(&pxmitpriv->bk_pending);
  62         _rtw_init_queue(&pxmitpriv->vi_pending);
  63         _rtw_init_queue(&pxmitpriv->vo_pending);
  64         _rtw_init_queue(&pxmitpriv->bm_pending);
  65
  66         /* _rtw_init_queue(&pxmitpriv->legacy_dz_queue); */
  67         /* _rtw_init_queue(&pxmitpriv->apsd_queue); */
  68
  69         _rtw_init_queue(&pxmitpriv->free_xmit_queue);
  70
  71         /*
  72         Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
  73         and initialize free_xmit_frame below.
  74         Please also apply  free_txobj to link_up all the xmit_frames...
  75         */
  76
  77         pxmitpriv->pallocated_frame_buf = vzalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4);
  78
  79         if (!pxmitpriv->pallocated_frame_buf) {
  80                 pxmitpriv->pxmit_frame_buf = NULL;
  81                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_frame fail!\n"));
  82                 res = _FAIL;
  83                 goto exit;
  84         }
  85         pxmitpriv->pxmit_frame_buf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_frame_buf), 4);
  86         /* pxmitpriv->pxmit_frame_buf = pxmitpriv->pallocated_frame_buf + 4 - */
  87         /*                                              ((SIZE_PTR) (pxmitpriv->pallocated_frame_buf) &3); */
  88
  89         pxframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf;
  90
  91         for (i = 0; i < NR_XMITFRAME; i++) {
  92                 INIT_LIST_HEAD(&(pxframe->list));
  93
  94                 pxframe->padapter = padapter;
  95                 pxframe->frame_tag = NULL_FRAMETAG;
  96
  97                 pxframe->pkt = NULL;
  98
  99                 pxframe->buf_addr = NULL;
 100                 pxframe->pxmitbuf = NULL;
 101
 102                 list_add_tail(&(pxframe->list), &(pxmitpriv->free_xmit_queue.queue));
 103
 104                 pxframe++;
 105         }
 106
 107         pxmitpriv->free_xmitframe_cnt = NR_XMITFRAME;
 108
 109         pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
 110
 111
 112         /* init xmit_buf */
 113         _rtw_init_queue(&pxmitpriv->free_xmitbuf_queue);
 114         _rtw_init_queue(&pxmitpriv->pending_xmitbuf_queue);
 115
 116         pxmitpriv->pallocated_xmitbuf = vzalloc(NR_XMITBUFF * sizeof(struct xmit_buf) + 4);
 117
 118         if (!pxmitpriv->pallocated_xmitbuf) {
 119                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_buf fail!\n"));
 120                 res = _FAIL;
 121                 goto exit;
 122         }
 123
 124         pxmitpriv->pxmitbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmitbuf), 4);
 125         /* pxmitpriv->pxmitbuf = pxmitpriv->pallocated_xmitbuf + 4 - */
 126         /*                                              ((SIZE_PTR) (pxmitpriv->pallocated_xmitbuf) &3); */
 127
 128         pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
 129
 130         for (i = 0; i < NR_XMITBUFF; i++) {
 131                 INIT_LIST_HEAD(&pxmitbuf->list);
 132
 133                 pxmitbuf->priv_data = NULL;
 134                 pxmitbuf->padapter = padapter;
 135                 pxmitbuf->buf_tag = XMITBUF_DATA;
 136
 137                 /* Tx buf allocation may fail sometimes, so sleep and retry. */
 138                 res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
 139                 if (res == _FAIL) {
 140                         msleep(10);
 141                         res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
 142                         if (res == _FAIL)
 143                                 goto exit;
 144                 }
 145
 146                 pxmitbuf->phead = pxmitbuf->pbuf;
 147                 pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMITBUF_SZ;
 148                 pxmitbuf->len = 0;
 149                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
 150
 151                 pxmitbuf->flags = XMIT_VO_QUEUE;
 152
 153                 list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmitbuf_queue.queue));
 154                 #ifdef DBG_XMIT_BUF
 155                 pxmitbuf->no = i;
 156                 #endif
 157
 158                 pxmitbuf++;
 159
 160         }
 161
 162         pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
 163
 164         /* init xframe_ext queue,  the same count as extbuf  */
 165         _rtw_init_queue(&pxmitpriv->free_xframe_ext_queue);
 166
 167         pxmitpriv->xframe_ext_alloc_addr = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_frame) + 4);
 168
 169         if (!pxmitpriv->xframe_ext_alloc_addr) {
 170                 pxmitpriv->xframe_ext = NULL;
 171                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xframe_ext fail!\n"));
 172                 res = _FAIL;
 173                 goto exit;
 174         }
 175         pxmitpriv->xframe_ext = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->xframe_ext_alloc_addr), 4);
 176         pxframe = (struct xmit_frame *)pxmitpriv->xframe_ext;
 177
 178         for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
 179                 INIT_LIST_HEAD(&(pxframe->list));
 180
 181                 pxframe->padapter = padapter;
 182                 pxframe->frame_tag = NULL_FRAMETAG;
 183
 184                 pxframe->pkt = NULL;
 185
 186                 pxframe->buf_addr = NULL;
 187                 pxframe->pxmitbuf = NULL;
 188
 189                 pxframe->ext_tag = 1;
 190
 191                 list_add_tail(&(pxframe->list), &(pxmitpriv->free_xframe_ext_queue.queue));
 192
 193                 pxframe++;
 194         }
 195         pxmitpriv->free_xframe_ext_cnt = NR_XMIT_EXTBUFF;
 196
 197         /*  Init xmit extension buff */
 198         _rtw_init_queue(&pxmitpriv->free_xmit_extbuf_queue);
 199
 200         pxmitpriv->pallocated_xmit_extbuf = vzalloc(NR_XMIT_EXTBUFF * sizeof(struct xmit_buf) + 4);
 201
 202         if (!pxmitpriv->pallocated_xmit_extbuf) {
 203                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("alloc xmit_extbuf fail!\n"));
 204                 res = _FAIL;
 205                 goto exit;
 206         }
 207
 208         pxmitpriv->pxmit_extbuf = (u8 *)N_BYTE_ALIGMENT((SIZE_PTR)(pxmitpriv->pallocated_xmit_extbuf), 4);
 209
 210         pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
 211
 212         for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
 213                 INIT_LIST_HEAD(&pxmitbuf->list);
 214
 215                 pxmitbuf->priv_data = NULL;
 216                 pxmitbuf->padapter = padapter;
 217                 pxmitbuf->buf_tag = XMITBUF_MGNT;
 218
 219                 res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ, true);
 220                 if (res == _FAIL) {
 221                         res = _FAIL;
 222                         goto exit;
 223                 }
 224
 225                 pxmitbuf->phead = pxmitbuf->pbuf;
 226                 pxmitbuf->pend = pxmitbuf->pbuf + MAX_XMIT_EXTBUF_SZ;
 227                 pxmitbuf->len = 0;
 228                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
 229
 230                 list_add_tail(&pxmitbuf->list, &(pxmitpriv->free_xmit_extbuf_queue.queue));
 231                 #ifdef DBG_XMIT_BUF_EXT
 232                 pxmitbuf->no = i;
 233                 #endif
 234                 pxmitbuf++;
 235
 236         }
 237
 238         pxmitpriv->free_xmit_extbuf_cnt = NR_XMIT_EXTBUFF;
 239
 240         for (i = 0; i < CMDBUF_MAX; i++) {
 241                 pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i];
 242                 if (pxmitbuf) {
 243                         INIT_LIST_HEAD(&pxmitbuf->list);
 244
 245                         pxmitbuf->priv_data = NULL;
 246                         pxmitbuf->padapter = padapter;
 247                         pxmitbuf->buf_tag = XMITBUF_CMD;
 248
 249                         res = rtw_os_xmit_resource_alloc(padapter, pxmitbuf, MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ, true);
 250                         if (res == _FAIL) {
 251                                 res = _FAIL;
 252                                 goto exit;
 253                         }
 254
 255                         pxmitbuf->phead = pxmitbuf->pbuf;
 256                         pxmitbuf->pend = pxmitbuf->pbuf + MAX_CMDBUF_SZ;
 257                         pxmitbuf->len = 0;
 258                         pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
 259                         pxmitbuf->alloc_sz = MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ;
 260                 }
 261         }
 262
 263         res = rtw_alloc_hwxmits(padapter);
 264         if (res == _FAIL)
 265                 goto exit;
 266         rtw_init_hwxmits(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
 267
 268         for (i = 0; i < 4; i++) {
 269                 pxmitpriv->wmm_para_seq[i] = i;
 270         }
 271
 272         pxmitpriv->ack_tx = false;
 273         mutex_init(&pxmitpriv->ack_tx_mutex);
 274         rtw_sctx_init(&pxmitpriv->ack_tx_ops, 0);
 275
 276         rtw_hal_init_xmit_priv(padapter);
 277
 278 exit:
 279         return res;
 280 }
 281
 282 void _rtw_free_xmit_priv(struct xmit_priv *pxmitpriv)
 283 {
 284         int i;
 285         struct adapter *padapter = pxmitpriv->adapter;
 286         struct xmit_frame       *pxmitframe = (struct xmit_frame *) pxmitpriv->pxmit_frame_buf;
 287         struct xmit_buf *pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
 288
 289         rtw_hal_free_xmit_priv(padapter);
 290
 291         if (!pxmitpriv->pxmit_frame_buf)
 292                 return;
 293
 294         for (i = 0; i < NR_XMITFRAME; i++) {
 295                 rtw_os_xmit_complete(padapter, pxmitframe);
 296
 297                 pxmitframe++;
 298         }
 299
 300         for (i = 0; i < NR_XMITBUFF; i++) {
 301                 rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ), true);
 302
 303                 pxmitbuf++;
 304         }
 305
 306         if (pxmitpriv->pallocated_frame_buf)
 307                 vfree(pxmitpriv->pallocated_frame_buf);
 308
 309
 310         if (pxmitpriv->pallocated_xmitbuf)
 311                 vfree(pxmitpriv->pallocated_xmitbuf);
 312
 313         /* free xframe_ext queue,  the same count as extbuf  */
 314         pxmitframe = (struct xmit_frame *)pxmitpriv->xframe_ext;
 315         if (pxmitframe) {
 316                 for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
 317                         rtw_os_xmit_complete(padapter, pxmitframe);
 318                         pxmitframe++;
 319                 }
 320         }
 321         if (pxmitpriv->xframe_ext_alloc_addr)
 322                 vfree(pxmitpriv->xframe_ext_alloc_addr);
 323
 324         /*  free xmit extension buff */
 325         pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmit_extbuf;
 326         for (i = 0; i < NR_XMIT_EXTBUFF; i++) {
 327                 rtw_os_xmit_resource_free(padapter, pxmitbuf, (MAX_XMIT_EXTBUF_SZ + XMITBUF_ALIGN_SZ), true);
 328
 329                 pxmitbuf++;
 330         }
 331
 332         if (pxmitpriv->pallocated_xmit_extbuf) {
 333                 vfree(pxmitpriv->pallocated_xmit_extbuf);
 334         }
 335
 336         for (i = 0; i < CMDBUF_MAX; i++) {
 337                 pxmitbuf = &pxmitpriv->pcmd_xmitbuf[i];
 338                 if (pxmitbuf)
 339                         rtw_os_xmit_resource_free(padapter, pxmitbuf, MAX_CMDBUF_SZ+XMITBUF_ALIGN_SZ, true);
 340         }
 341
 342         rtw_free_hwxmits(padapter);
 343
 344         mutex_destroy(&pxmitpriv->ack_tx_mutex);
 345 }
 346
 347 u8 query_ra_short_GI(struct sta_info *psta)
 348 {
 349         u8 sgi = false, sgi_20m = false, sgi_40m = false, sgi_80m = false;
 350
 351         sgi_20m = psta->htpriv.sgi_20m;
 352         sgi_40m = psta->htpriv.sgi_40m;
 353
 354         switch (psta->bw_mode) {
 355         case CHANNEL_WIDTH_80:
 356                 sgi = sgi_80m;
 357                 break;
 358         case CHANNEL_WIDTH_40:
 359                 sgi = sgi_40m;
 360                 break;
 361         case CHANNEL_WIDTH_20:
 362         default:
 363                 sgi = sgi_20m;
 364                 break;
 365         }
 366
 367         return sgi;
 368 }
 369
 370 static void update_attrib_vcs_info(struct adapter *padapter, struct xmit_frame *pxmitframe)
 371 {
 372         u32 sz;
 373         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
 374         /* struct sta_info *psta = pattrib->psta; */
 375         struct mlme_ext_priv *pmlmeext = &(padapter->mlmeextpriv);
 376         struct mlme_ext_info *pmlmeinfo = &(pmlmeext->mlmext_info);
 377
 378         if (pattrib->nr_frags != 1)
 379                 sz = padapter->xmitpriv.frag_len;
 380         else /* no frag */
 381                 sz = pattrib->last_txcmdsz;
 382
 383         /*  (1) RTS_Threshold is compared to the MPDU, not MSDU. */
 384         /*  (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame. */
 385         /*              Other fragments are protected by previous fragment. */
 386         /*              So we only need to check the length of first fragment. */
 387         if (pmlmeext->cur_wireless_mode < WIRELESS_11_24N  || padapter->registrypriv.wifi_spec) {
 388                 if (sz > padapter->registrypriv.rts_thresh)
 389                         pattrib->vcs_mode = RTS_CTS;
 390                 else {
 391                         if (pattrib->rtsen)
 392                                 pattrib->vcs_mode = RTS_CTS;
 393                         else if (pattrib->cts2self)
 394                                 pattrib->vcs_mode = CTS_TO_SELF;
 395                         else
 396                                 pattrib->vcs_mode = NONE_VCS;
 397                 }
 398         } else {
 399                 while (true) {
 400                         /* IOT action */
 401                         if ((pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS) && (pattrib->ampdu_en == true) &&
 402                                 (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
 403                                 pattrib->vcs_mode = CTS_TO_SELF;
 404                                 break;
 405                         }
 406
 407
 408                         /* check ERP protection */
 409                         if (pattrib->rtsen || pattrib->cts2self) {
 410                                 if (pattrib->rtsen)
 411                                         pattrib->vcs_mode = RTS_CTS;
 412                                 else if (pattrib->cts2self)
 413                                         pattrib->vcs_mode = CTS_TO_SELF;
 414
 415                                 break;
 416                         }
 417
 418                         /* check HT op mode */
 419                         if (pattrib->ht_en) {
 420                                 u8 HTOpMode = pmlmeinfo->HT_protection;
 421                                 if ((pmlmeext->cur_bwmode && (HTOpMode == 2 || HTOpMode == 3)) ||
 422                                         (!pmlmeext->cur_bwmode && HTOpMode == 3)) {
 423                                         pattrib->vcs_mode = RTS_CTS;
 424                                         break;
 425                                 }
 426                         }
 427
 428                         /* check rts */
 429                         if (sz > padapter->registrypriv.rts_thresh) {
 430                                 pattrib->vcs_mode = RTS_CTS;
 431                                 break;
 432                         }
 433
 434                         /* to do list: check MIMO power save condition. */
 435
 436                         /* check AMPDU aggregation for TXOP */
 437                         if (pattrib->ampdu_en == true) {
 438                                 pattrib->vcs_mode = RTS_CTS;
 439                                 break;
 440                         }
 441
 442                         pattrib->vcs_mode = NONE_VCS;
 443                         break;
 444                 }
 445         }
 446
 447         /* for debug : force driver control vrtl_carrier_sense. */
 448         if (padapter->driver_vcs_en == 1)
 449                 pattrib->vcs_mode = padapter->driver_vcs_type;
 450 }
 451
 452 static void update_attrib_phy_info(struct adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
 453 {
 454         struct mlme_ext_priv *mlmeext = &padapter->mlmeextpriv;
 455
 456         pattrib->rtsen = psta->rtsen;
 457         pattrib->cts2self = psta->cts2self;
 458
 459         pattrib->mdata = 0;
 460         pattrib->eosp = 0;
 461         pattrib->triggered = 0;
 462         pattrib->ampdu_spacing = 0;
 463
 464         /* qos_en, ht_en, init rate, , bw, ch_offset, sgi */
 465         pattrib->qos_en = psta->qos_option;
 466
 467         pattrib->raid = psta->raid;
 468
 469         if (mlmeext->cur_bwmode < psta->bw_mode)
 470                 pattrib->bwmode = mlmeext->cur_bwmode;
 471         else
 472                 pattrib->bwmode = psta->bw_mode;
 473
 474         pattrib->sgi = query_ra_short_GI(psta);
 475
 476         pattrib->ldpc = psta->ldpc;
 477         pattrib->stbc = psta->stbc;
 478
 479         pattrib->ht_en = psta->htpriv.ht_option;
 480         pattrib->ch_offset = psta->htpriv.ch_offset;
 481         pattrib->ampdu_en = false;
 482
 483         if (padapter->driver_ampdu_spacing != 0xFF) /* driver control AMPDU Density for peer sta's rx */
 484                 pattrib->ampdu_spacing = padapter->driver_ampdu_spacing;
 485         else
 486                 pattrib->ampdu_spacing = psta->htpriv.rx_ampdu_min_spacing;
 487
 488         /* if (pattrib->ht_en && psta->htpriv.ampdu_enable) */
 489         /*  */
 490         /*      if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority)) */
 491         /*              pattrib->ampdu_en = true; */
 492         /*  */
 493
 494
 495         pattrib->retry_ctrl = false;
 496
 497 #ifdef CONFIG_AUTO_AP_MODE
 498         if (psta->isrc && psta->pid > 0)
 499                 pattrib->pctrl = true;
 500 #endif
 501
 502 }
 503
 504 static s32 update_attrib_sec_info(struct adapter *padapter, struct pkt_attrib *pattrib, struct sta_info *psta)
 505 {
 506         sint res = _SUCCESS;
 507         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 508         struct security_priv *psecuritypriv = &padapter->securitypriv;
 509         sint bmcast = IS_MCAST(pattrib->ra);
 510
 511         memset(pattrib->dot118021x_UncstKey.skey,  0, 16);
 512         memset(pattrib->dot11tkiptxmickey.skey,  0, 16);
 513         pattrib->mac_id = psta->mac_id;
 514
 515         if (psta->ieee8021x_blocked == true) {
 516                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("\n psta->ieee8021x_blocked == true\n"));
 517
 518                 pattrib->encrypt = 0;
 519
 520                 if ((pattrib->ether_type != 0x888e) && (check_fwstate(pmlmepriv, WIFI_MP_STATE) == false)) {
 521                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("\npsta->ieee8021x_blocked == true,  pattrib->ether_type(%.4x) != 0x888e\n", pattrib->ether_type));
 522                         #ifdef DBG_TX_DROP_FRAME
 523                         DBG_871X("DBG_TX_DROP_FRAME %s psta->ieee8021x_blocked == true,  pattrib->ether_type(%04x) != 0x888e\n", __func__, pattrib->ether_type);
 524                         #endif
 525                         res = _FAIL;
 526                         goto exit;
 527                 }
 528         } else {
 529                 GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
 530
 531                 switch (psecuritypriv->dot11AuthAlgrthm) {
 532                 case dot11AuthAlgrthm_Open:
 533                 case dot11AuthAlgrthm_Shared:
 534                 case dot11AuthAlgrthm_Auto:
 535                         pattrib->key_idx = (u8)psecuritypriv->dot11PrivacyKeyIndex;
 536                         break;
 537                 case dot11AuthAlgrthm_8021X:
 538                         if (bmcast)
 539                                 pattrib->key_idx = (u8)psecuritypriv->dot118021XGrpKeyid;
 540                         else
 541                                 pattrib->key_idx = 0;
 542                         break;
 543                 default:
 544                         pattrib->key_idx = 0;
 545                         break;
 546                 }
 547
 548                 /* For WPS 1.0 WEP, driver should not encrypt EAPOL Packet for WPS handshake. */
 549                 if (((pattrib->encrypt == _WEP40_) || (pattrib->encrypt == _WEP104_)) && (pattrib->ether_type == 0x888e))
 550                         pattrib->encrypt = _NO_PRIVACY_;
 551
 552         }
 553
 554         switch (pattrib->encrypt) {
 555         case _WEP40_:
 556         case _WEP104_:
 557                 pattrib->iv_len = 4;
 558                 pattrib->icv_len = 4;
 559                 WEP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
 560                 break;
 561
 562         case _TKIP_:
 563                 pattrib->iv_len = 8;
 564                 pattrib->icv_len = 4;
 565
 566                 if (psecuritypriv->busetkipkey == _FAIL) {
 567                         #ifdef DBG_TX_DROP_FRAME
 568                         DBG_871X("DBG_TX_DROP_FRAME %s psecuritypriv->busetkipkey(%d) == _FAIL drop packet\n", __func__, psecuritypriv->busetkipkey);
 569                         #endif
 570                         res = _FAIL;
 571                         goto exit;
 572                 }
 573
 574                 if (bmcast)
 575                         TKIP_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
 576                 else
 577                         TKIP_IV(pattrib->iv, psta->dot11txpn, 0);
 578
 579
 580                 memcpy(pattrib->dot11tkiptxmickey.skey, psta->dot11tkiptxmickey.skey, 16);
 581
 582                 break;
 583
 584         case _AES_:
 585
 586                 pattrib->iv_len = 8;
 587                 pattrib->icv_len = 8;
 588
 589                 if (bmcast)
 590                         AES_IV(pattrib->iv, psta->dot11txpn, pattrib->key_idx);
 591                 else
 592                         AES_IV(pattrib->iv, psta->dot11txpn, 0);
 593
 594                 break;
 595
 596         default:
 597                 pattrib->iv_len = 0;
 598                 pattrib->icv_len = 0;
 599                 break;
 600         }
 601
 602         if (pattrib->encrypt > 0)
 603                 memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16);
 604
 605         RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 606                 ("update_attrib: encrypt =%d  securitypriv.sw_encrypt =%d\n",
 607                 pattrib->encrypt, padapter->securitypriv.sw_encrypt));
 608
 609         if (pattrib->encrypt &&
 610                 ((padapter->securitypriv.sw_encrypt == true) || (psecuritypriv->hw_decrypted == false))) {
 611                 pattrib->bswenc = true;
 612                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 613                         ("update_attrib: encrypt =%d securitypriv.hw_decrypted =%d bswenc =true\n",
 614                         pattrib->encrypt, padapter->securitypriv.sw_encrypt));
 615         } else {
 616                 pattrib->bswenc = false;
 617                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("update_attrib: bswenc =false\n"));
 618         }
 619
 620 exit:
 621
 622         return res;
 623
 624 }
 625
 626 u8 qos_acm(u8 acm_mask, u8 priority)
 627 {
 628         switch (priority) {
 629         case 0:
 630         case 3:
 631                 if (acm_mask & BIT(1))
 632                         priority = 1;
 633                 break;
 634         case 1:
 635         case 2:
 636                 break;
 637         case 4:
 638         case 5:
 639                 if (acm_mask & BIT(2))
 640                         priority = 0;
 641                 break;
 642         case 6:
 643         case 7:
 644                 if (acm_mask & BIT(3))
 645                         priority = 5;
 646                 break;
 647         default:
 648                 DBG_871X("qos_acm(): invalid pattrib->priority: %d!!!\n", priority);
 649                 break;
 650         }
 651
 652         return priority;
 653 }
 654
 655 static void set_qos(struct pkt_file *ppktfile, struct pkt_attrib *pattrib)
 656 {
 657         struct ethhdr etherhdr;
 658         struct iphdr ip_hdr;
 659         s32 UserPriority = 0;
 660
 661
 662         _rtw_open_pktfile(ppktfile->pkt, ppktfile);
 663         _rtw_pktfile_read(ppktfile, (unsigned char *)&etherhdr, ETH_HLEN);
 664
 665         /*  get UserPriority from IP hdr */
 666         if (pattrib->ether_type == 0x0800) {
 667                 _rtw_pktfile_read(ppktfile, (u8 *)&ip_hdr, sizeof(ip_hdr));
 668 /*              UserPriority = (ntohs(ip_hdr.tos) >> 5) & 0x3; */
 669                 UserPriority = ip_hdr.tos >> 5;
 670         }
 671         pattrib->priority = UserPriority;
 672         pattrib->hdrlen = WLAN_HDR_A3_QOS_LEN;
 673         pattrib->subtype = WIFI_QOS_DATA_TYPE;
 674 }
 675
 676 static s32 update_attrib(struct adapter *padapter, _pkt *pkt, struct pkt_attrib *pattrib)
 677 {
 678         uint i;
 679         struct pkt_file pktfile;
 680         struct sta_info *psta = NULL;
 681         struct ethhdr etherhdr;
 682
 683         sint bmcast;
 684         struct sta_priv         *pstapriv = &padapter->stapriv;
 685         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 686         struct qos_priv         *pqospriv = &pmlmepriv->qospriv;
 687         sint res = _SUCCESS;
 688
 689         DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib);
 690
 691         _rtw_open_pktfile(pkt, &pktfile);
 692         i = _rtw_pktfile_read(&pktfile, (u8 *)&etherhdr, ETH_HLEN);
 693
 694         pattrib->ether_type = ntohs(etherhdr.h_proto);
 695
 696
 697         memcpy(pattrib->dst, &etherhdr.h_dest, ETH_ALEN);
 698         memcpy(pattrib->src, &etherhdr.h_source, ETH_ALEN);
 699
 700
 701         if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
 702                 (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
 703                 memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
 704                 memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
 705                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_adhoc);
 706         } else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 707                 memcpy(pattrib->ra, get_bssid(pmlmepriv), ETH_ALEN);
 708                 memcpy(pattrib->ta, pattrib->src, ETH_ALEN);
 709                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_sta);
 710         } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 711                 memcpy(pattrib->ra, pattrib->dst, ETH_ALEN);
 712                 memcpy(pattrib->ta, get_bssid(pmlmepriv), ETH_ALEN);
 713                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_ap);
 714         } else
 715                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_unknown);
 716
 717         pattrib->pktlen = pktfile.pkt_len;
 718
 719         if (ETH_P_IP == pattrib->ether_type) {
 720                 /*  The following is for DHCP and ARP packet, we use cck1M to tx these packets and let LPS awake some time */
 721                 /*  to prevent DHCP protocol fail */
 722
 723                 u8 tmp[24];
 724
 725                 _rtw_pktfile_read(&pktfile, &tmp[0], 24);
 726
 727                 pattrib->dhcp_pkt = 0;
 728                 if (pktfile.pkt_len > 282) {/* MINIMUM_DHCP_PACKET_SIZE) { */
 729                         if (ETH_P_IP == pattrib->ether_type) {/*  IP header */
 730                                 if (((tmp[21] == 68) && (tmp[23] == 67)) ||
 731                                         ((tmp[21] == 67) && (tmp[23] == 68))) {
 732                                         /*  68 : UDP BOOTP client */
 733                                         /*  67 : UDP BOOTP server */
 734                                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======================update_attrib: get DHCP Packet\n"));
 735                                         pattrib->dhcp_pkt = 1;
 736                                         DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_dhcp);
 737                                 }
 738                         }
 739                 }
 740
 741                 /* for parsing ICMP pakcets */
 742                 {
 743                         struct iphdr *piphdr = (struct iphdr *)tmp;
 744
 745                         pattrib->icmp_pkt = 0;
 746                         if (piphdr->protocol == 0x1) { /*  protocol type in ip header 0x1 is ICMP */
 747                                 pattrib->icmp_pkt = 1;
 748                                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_icmp);
 749                         }
 750                 }
 751
 752
 753         } else if (0x888e == pattrib->ether_type) {
 754                 DBG_871X_LEVEL(_drv_always_, "send eapol packet\n");
 755         }
 756
 757         if ((pattrib->ether_type == 0x888e) || (pattrib->dhcp_pkt == 1))
 758                 rtw_set_scan_deny(padapter, 3000);
 759
 760         /*  If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
 761         if (pattrib->icmp_pkt == 1)
 762                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_LEAVE, 1);
 763         else if (pattrib->dhcp_pkt == 1) {
 764                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_active);
 765                 rtw_lps_ctrl_wk_cmd(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
 766         }
 767
 768         bmcast = IS_MCAST(pattrib->ra);
 769
 770         /*  get sta_info */
 771         if (bmcast) {
 772                 psta = rtw_get_bcmc_stainfo(padapter);
 773         } else {
 774                 psta = rtw_get_stainfo(pstapriv, pattrib->ra);
 775                 if (!psta)      { /*  if we cannot get psta => drop the pkt */
 776                         DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_sta);
 777                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT"\n", MAC_ARG(pattrib->ra)));
 778                         #ifdef DBG_TX_DROP_FRAME
 779                         DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra));
 780                         #endif
 781                         res = _FAIL;
 782                         goto exit;
 783                 } else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) && (!(psta->state & _FW_LINKED))) {
 784                         DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_ucast_ap_link);
 785                         res = _FAIL;
 786                         goto exit;
 787                 }
 788         }
 789
 790         if (!psta) {
 791                 /*  if we cannot get psta => drop the pkt */
 792                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sta);
 793                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("\nupdate_attrib => get sta_info fail, ra:" MAC_FMT "\n", MAC_ARG(pattrib->ra)));
 794                 #ifdef DBG_TX_DROP_FRAME
 795                 DBG_871X("DBG_TX_DROP_FRAME %s get sta_info fail, ra:" MAC_FMT"\n", __func__, MAC_ARG(pattrib->ra));
 796                 #endif
 797                 res = _FAIL;
 798                 goto exit;
 799         }
 800
 801         if (!(psta->state & _FW_LINKED)) {
 802                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_link);
 803                 DBG_871X("%s, psta("MAC_FMT")->state(0x%x) != _FW_LINKED\n", __func__, MAC_ARG(psta->hwaddr), psta->state);
 804                 return _FAIL;
 805         }
 806
 807
 808
 809         /* TODO:_lock */
 810         if (update_attrib_sec_info(padapter, pattrib, psta) == _FAIL) {
 811                 DBG_COUNTER(padapter->tx_logs.core_tx_upd_attrib_err_sec);
 812                 res = _FAIL;
 813                 goto exit;
 814         }
 815
 816         update_attrib_phy_info(padapter, pattrib, psta);
 817
 818         /* DBG_8192C("%s ==> mac_id(%d)\n", __func__, pattrib->mac_id); */
 819
 820         pattrib->psta = psta;
 821         /* TODO:_unlock */
 822
 823         pattrib->pctrl = 0;
 824
 825         pattrib->ack_policy = 0;
 826         /*  get ether_hdr_len */
 827         pattrib->pkt_hdrlen = ETH_HLEN;/* pattrib->ether_type == 0x8100) ? (14 + 4): 14; vlan tag */
 828
 829         pattrib->hdrlen = WLAN_HDR_A3_LEN;
 830         pattrib->subtype = WIFI_DATA_TYPE;
 831         pattrib->priority = 0;
 832
 833         if (check_fwstate(pmlmepriv, WIFI_AP_STATE|WIFI_ADHOC_STATE|WIFI_ADHOC_MASTER_STATE)) {
 834                 if (pattrib->qos_en)
 835                         set_qos(&pktfile, pattrib);
 836         } else {
 837                 if (pqospriv->qos_option) {
 838                         set_qos(&pktfile, pattrib);
 839
 840                         if (pmlmepriv->acm_mask != 0)
 841                                 pattrib->priority = qos_acm(pmlmepriv->acm_mask, pattrib->priority);
 842
 843                 }
 844         }
 845
 846         /* pattrib->priority = 5; force to used VI queue, for testing */
 847
 848 exit:
 849         return res;
 850 }
 851
 852 static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitframe)
 853 {
 854         sint                    curfragnum, length;
 855         u8 *pframe, *payload, mic[8];
 856         struct  mic_data                micdata;
 857         /* struct       sta_info        *stainfo; */
 858         struct  pkt_attrib       *pattrib = &pxmitframe->attrib;
 859         struct  security_priv *psecuritypriv = &padapter->securitypriv;
 860         struct  xmit_priv       *pxmitpriv = &padapter->xmitpriv;
 861         u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
 862         u8 hw_hdr_offset = 0;
 863         sint bmcst = IS_MCAST(pattrib->ra);
 864
 865 /*
 866         if (pattrib->psta)
 867         {
 868                 stainfo = pattrib->psta;
 869         }
 870         else
 871         {
 872                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
 873                 stainfo =rtw_get_stainfo(&padapter->stapriv ,&pattrib->ra[0]);
 874         }
 875
 876         if (stainfo == NULL)
 877         {
 878                 DBG_871X("%s, psta ==NUL\n", __func__);
 879                 return _FAIL;
 880         }
 881
 882         if (!(stainfo->state &_FW_LINKED))
 883         {
 884                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, stainfo->state);
 885                 return _FAIL;
 886         }
 887 */
 888
 889         hw_hdr_offset = TXDESC_OFFSET;
 890
 891         if (pattrib->encrypt == _TKIP_) { /* if (psecuritypriv->dot11PrivacyAlgrthm == _TKIP_PRIVACY_) */
 892                 /* encode mic code */
 893                 /* if (stainfo!= NULL) */
 894                 {
 895                         u8 null_key[16] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 896
 897                         pframe = pxmitframe->buf_addr + hw_hdr_offset;
 898
 899                         if (bmcst) {
 900                                 if (!memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16)) {
 901                                         /* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey == 0\n"); */
 902                                         /* msleep(10); */
 903                                         return _FAIL;
 904                                 }
 905                                 /* start to calculate the mic code */
 906                                 rtw_secmicsetkey(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey);
 907                         } else {
 908                                 if (!memcmp(&pattrib->dot11tkiptxmickey.skey[0], null_key, 16)) {
 909                                         /* DbgPrint("\nxmitframe_addmic:stainfo->dot11tkiptxmickey == 0\n"); */
 910                                         /* msleep(10); */
 911                                         return _FAIL;
 912                                 }
 913                                 /* start to calculate the mic code */
 914                                 rtw_secmicsetkey(&micdata, &pattrib->dot11tkiptxmickey.skey[0]);
 915                         }
 916
 917                         if (pframe[1]&1) {   /* ToDS == 1 */
 918                                 rtw_secmicappend(&micdata, &pframe[16], 6);  /* DA */
 919                                 if (pframe[1]&2)  /* From Ds == 1 */
 920                                         rtw_secmicappend(&micdata, &pframe[24], 6);
 921                                 else
 922                                 rtw_secmicappend(&micdata, &pframe[10], 6);
 923                         } else {        /* ToDS == 0 */
 924                                 rtw_secmicappend(&micdata, &pframe[4], 6);   /* DA */
 925                                 if (pframe[1]&2)  /* From Ds == 1 */
 926                                         rtw_secmicappend(&micdata, &pframe[16], 6);
 927                                 else
 928                                         rtw_secmicappend(&micdata, &pframe[10], 6);
 929
 930                         }
 931
 932                         /* if (pqospriv->qos_option == 1) */
 933                         if (pattrib->qos_en)
 934                                 priority[0] = (u8)pxmitframe->attrib.priority;
 935
 936
 937                         rtw_secmicappend(&micdata, &priority[0], 4);
 938
 939                         payload = pframe;
 940
 941                         for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
 942                                 payload = (u8 *)RND4((SIZE_PTR)(payload));
 943                                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("===curfragnum =%d, pframe = 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
 944                                         curfragnum, *payload, *(payload+1), *(payload+2), *(payload+3), *(payload+4), *(payload+5), *(payload+6), *(payload+7)));
 945
 946                                 payload = payload+pattrib->hdrlen+pattrib->iv_len;
 947                                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum =%d pattrib->hdrlen =%d pattrib->iv_len =%d", curfragnum, pattrib->hdrlen, pattrib->iv_len));
 948                                 if ((curfragnum+1) == pattrib->nr_frags) {
 949                                         length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0);
 950                                         rtw_secmicappend(&micdata, payload, length);
 951                                         payload = payload+length;
 952                                 } else {
 953                                         length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-((pattrib->bswenc) ? pattrib->icv_len : 0);
 954                                         rtw_secmicappend(&micdata, payload, length);
 955                                         payload = payload+length+pattrib->icv_len;
 956                                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("curfragnum =%d length =%d pattrib->icv_len =%d", curfragnum, length, pattrib->icv_len));
 957                                 }
 958                         }
 959                         rtw_secgetmic(&micdata, &(mic[0]));
 960                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: before add mic code!!!\n"));
 961                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: pattrib->last_txcmdsz =%d!!!\n", pattrib->last_txcmdsz));
 962                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: mic[0]= 0x%.2x , mic[1]= 0x%.2x , mic[2]= 0x%.2x , mic[3]= 0x%.2x\n\
 963   mic[4]= 0x%.2x , mic[5]= 0x%.2x , mic[6]= 0x%.2x , mic[7]= 0x%.2x !!!!\n",
 964                                 mic[0], mic[1], mic[2], mic[3], mic[4], mic[5], mic[6], mic[7]));
 965                         /* add mic code  and add the mic code length in last_txcmdsz */
 966
 967                         memcpy(payload, &(mic[0]), 8);
 968                         pattrib->last_txcmdsz += 8;
 969
 970                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("\n ========last pkt ========\n"));
 971                         payload = payload-pattrib->last_txcmdsz+8;
 972                         for (curfragnum = 0; curfragnum < pattrib->last_txcmdsz; curfragnum = curfragnum+8)
 973                                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, (" %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x ",
 974                                         *(payload+curfragnum), *(payload+curfragnum+1), *(payload+curfragnum+2), *(payload+curfragnum+3),
 975                                         *(payload+curfragnum+4), *(payload+curfragnum+5), *(payload+curfragnum+6), *(payload+curfragnum+7)));
 976                         }
 977 /*
 978                         else {
 979                                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic: rtw_get_stainfo == NULL!!!\n"));
 980                         }
 981 */
 982         }
 983         return _SUCCESS;
 984 }
 985
 986 static s32 xmitframe_swencrypt(struct adapter *padapter, struct xmit_frame *pxmitframe)
 987 {
 988
 989         struct  pkt_attrib       *pattrib = &pxmitframe->attrib;
 990         /* struct       security_priv *psecuritypriv =&padapter->securitypriv; */
 991
 992         /* if ((psecuritypriv->sw_encrypt)||(pattrib->bswenc)) */
 993         if (pattrib->bswenc) {
 994                 /* DBG_871X("start xmitframe_swencrypt\n"); */
 995                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_, ("### xmitframe_swencrypt\n"));
 996                 switch (pattrib->encrypt) {
 997                 case _WEP40_:
 998                 case _WEP104_:
 999                         rtw_wep_encrypt(padapter, (u8 *)pxmitframe);
1000                         break;
1001                 case _TKIP_:
1002                         rtw_tkip_encrypt(padapter, (u8 *)pxmitframe);
1003                         break;
1004                 case _AES_:
1005                         rtw_aes_encrypt(padapter, (u8 *)pxmitframe);
1006                         break;
1007                 default:
1008                                 break;
1009                 }
1010
1011         } else
1012                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_, ("### xmitframe_hwencrypt\n"));
1013
1014         return _SUCCESS;
1015 }
1016
1017 s32 rtw_make_wlanhdr(struct adapter *padapter, u8 *hdr, struct pkt_attrib *pattrib)
1018 {
1019         u16 *qc;
1020
1021         struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr;
1022         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1023         struct qos_priv *pqospriv = &pmlmepriv->qospriv;
1024         u8 qos_option = false;
1025         sint res = _SUCCESS;
1026         __le16 *fctrl = &pwlanhdr->frame_control;
1027
1028         memset(hdr, 0, WLANHDR_OFFSET);
1029
1030         SetFrameSubType(fctrl, pattrib->subtype);
1031
1032         if (pattrib->subtype & WIFI_DATA_TYPE) {
1033                 if ((check_fwstate(pmlmepriv,  WIFI_STATION_STATE) == true)) {
1034                         /* to_ds = 1, fr_ds = 0; */
1035
1036                         {
1037                                 /*  1.Data transfer to AP */
1038                                 /*  2.Arp pkt will relayed by AP */
1039                                 SetToDs(fctrl);
1040                                 memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv), ETH_ALEN);
1041                                 memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
1042                                 memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
1043                         }
1044
1045                         if (pqospriv->qos_option)
1046                                 qos_option = true;
1047
1048                 } else if ((check_fwstate(pmlmepriv,  WIFI_AP_STATE) == true)) {
1049                         /* to_ds = 0, fr_ds = 1; */
1050                         SetFrDs(fctrl);
1051                         memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
1052                         memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv), ETH_ALEN);
1053                         memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
1054
1055                         if (pattrib->qos_en)
1056                                 qos_option = true;
1057                 } else if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) ||
1058                 (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == true)) {
1059                         memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
1060                         memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
1061                         memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv), ETH_ALEN);
1062
1063                         if (pattrib->qos_en)
1064                                 qos_option = true;
1065                 } else {
1066                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("fw_state:%x is not allowed to xmit frame\n", get_fwstate(pmlmepriv)));
1067                         res = _FAIL;
1068                         goto exit;
1069                 }
1070
1071                 if (pattrib->mdata)
1072                         SetMData(fctrl);
1073
1074                 if (pattrib->encrypt)
1075                         SetPrivacy(fctrl);
1076
1077                 if (qos_option) {
1078                         qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
1079
1080                         if (pattrib->priority)
1081                                 SetPriority(qc, pattrib->priority);
1082
1083                         SetEOSP(qc, pattrib->eosp);
1084
1085                         SetAckpolicy(qc, pattrib->ack_policy);
1086                 }
1087
1088                 /* TODO: fill HT Control Field */
1089
1090                 /* Update Seq Num will be handled by f/w */
1091                 {
1092                         struct sta_info *psta;
1093                         psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
1094                         if (pattrib->psta != psta) {
1095                                 DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
1096                                 return _FAIL;
1097                         }
1098
1099                         if (!psta) {
1100                                 DBG_871X("%s, psta ==NUL\n", __func__);
1101                                 return _FAIL;
1102                         }
1103
1104                         if (!(psta->state & _FW_LINKED)) {
1105                                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
1106                                 return _FAIL;
1107                         }
1108
1109
1110                         if (psta) {
1111                                 psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
1112                                 psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
1113                                 pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
1114
1115                                 SetSeqNum(hdr, pattrib->seqnum);
1116
1117                                 /* check if enable ampdu */
1118                                 if (pattrib->ht_en && psta->htpriv.ampdu_enable)
1119                                         if (psta->htpriv.agg_enable_bitmap & BIT(pattrib->priority))
1120                                                 pattrib->ampdu_en = true;
1121
1122
1123                                 /* re-check if enable ampdu by BA_starting_seqctrl */
1124                                 if (pattrib->ampdu_en == true) {
1125                                         u16 tx_seq;
1126
1127                                         tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
1128
1129                                         /* check BA_starting_seqctrl */
1130                                         if (SN_LESS(pattrib->seqnum, tx_seq)) {
1131                                                 /* DBG_871X("tx ampdu seqnum(%d) < tx_seq(%d)\n", pattrib->seqnum, tx_seq); */
1132                                                 pattrib->ampdu_en = false;/* AGG BK */
1133                                         } else if (SN_EQUAL(pattrib->seqnum, tx_seq)) {
1134                                                 psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq+1)&0xfff;
1135
1136                                                 pattrib->ampdu_en = true;/* AGG EN */
1137                                         } else {
1138                                                 /* DBG_871X("tx ampdu over run\n"); */
1139                                                 psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff;
1140                                                 pattrib->ampdu_en = true;/* AGG EN */
1141                                         }
1142
1143                                 }
1144                         }
1145                 }
1146
1147         } else {
1148
1149         }
1150
1151 exit:
1152         return res;
1153 }
1154
1155 s32 rtw_txframes_pending(struct adapter *padapter)
1156 {
1157         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1158
1159         return ((!list_empty(&pxmitpriv->be_pending.queue)) ||
1160                          (!list_empty(&pxmitpriv->bk_pending.queue)) ||
1161                          (!list_empty(&pxmitpriv->vi_pending.queue)) ||
1162                          (!list_empty(&pxmitpriv->vo_pending.queue)));
1163 }
1164
1165 /*
1166  * Calculate wlan 802.11 packet MAX size from pkt_attrib
1167  * This function doesn't consider fragment case
1168  */
1169 u32 rtw_calculate_wlan_pkt_size_by_attribue(struct pkt_attrib *pattrib)
1170 {
1171         u32 len = 0;
1172
1173         len = pattrib->hdrlen + pattrib->iv_len; /*  WLAN Header and IV */
1174         len += SNAP_SIZE + sizeof(u16); /*  LLC */
1175         len += pattrib->pktlen;
1176         if (pattrib->encrypt == _TKIP_)
1177                 len += 8; /*  MIC */
1178         len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /*  ICV */
1179
1180         return len;
1181 }
1182
1183 /*
1184
1185 This sub-routine will perform all the following:
1186
1187 1. remove 802.3 header.
1188 2. create wlan_header, based on the info in pxmitframe
1189 3. append sta's iv/ext-iv
1190 4. append LLC
1191 5. move frag chunk from pframe to pxmitframe->mem
1192 6. apply sw-encrypt, if necessary.
1193
1194 */
1195 s32 rtw_xmitframe_coalesce(struct adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
1196 {
1197         struct pkt_file pktfile;
1198
1199         s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
1200
1201         SIZE_PTR addr;
1202
1203         u8 *pframe, *mem_start;
1204         u8 hw_hdr_offset;
1205
1206         /* struct sta_info      *psta; */
1207         /* struct sta_priv      *pstapriv = &padapter->stapriv; */
1208         /* struct mlme_priv *pmlmepriv = &padapter->mlmepriv; */
1209         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1210
1211         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
1212
1213         u8 *pbuf_start;
1214
1215         s32 bmcst = IS_MCAST(pattrib->ra);
1216         s32 res = _SUCCESS;
1217
1218 /*
1219         if (pattrib->psta)
1220         {
1221                 psta = pattrib->psta;
1222         } else
1223         {
1224                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
1225                 psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
1226         }
1227
1228         if (psta == NULL)
1229   {
1230
1231                 DBG_871X("%s, psta ==NUL\n", __func__);
1232                 return _FAIL;
1233         }
1234
1235
1236         if (!(psta->state &_FW_LINKED))
1237         {
1238                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
1239                 return _FAIL;
1240         }
1241 */
1242         if (!pxmitframe->buf_addr) {
1243                 DBG_8192C("==> %s buf_addr == NULL\n", __func__);
1244                 return _FAIL;
1245         }
1246
1247         pbuf_start = pxmitframe->buf_addr;
1248
1249         hw_hdr_offset = TXDESC_OFFSET;
1250         mem_start = pbuf_start +        hw_hdr_offset;
1251
1252         if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
1253                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n"));
1254                 DBG_8192C("rtw_xmitframe_coalesce: rtw_make_wlanhdr fail; drop pkt\n");
1255                 res = _FAIL;
1256                 goto exit;
1257         }
1258
1259         _rtw_open_pktfile(pkt, &pktfile);
1260         _rtw_pktfile_read(&pktfile, NULL, pattrib->pkt_hdrlen);
1261
1262         frg_inx = 0;
1263         frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
1264
1265         while (1) {
1266                 llc_sz = 0;
1267
1268                 mpdu_len = frg_len;
1269
1270                 pframe = mem_start;
1271
1272                 SetMFrag(mem_start);
1273
1274                 pframe += pattrib->hdrlen;
1275                 mpdu_len -= pattrib->hdrlen;
1276
1277                 /* adding icv, if necessary... */
1278                 if (pattrib->iv_len) {
1279                         memcpy(pframe, pattrib->iv, pattrib->iv_len);
1280
1281                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
1282                                  ("rtw_xmitframe_coalesce: keyid =%d pattrib->iv[3]=%.2x pframe =%.2x %.2x %.2x %.2x\n",
1283                                   padapter->securitypriv.dot11PrivacyKeyIndex, pattrib->iv[3], *pframe, *(pframe+1), *(pframe+2), *(pframe+3)));
1284
1285                         pframe += pattrib->iv_len;
1286
1287                         mpdu_len -= pattrib->iv_len;
1288                 }
1289
1290                 if (frg_inx == 0) {
1291                         llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
1292                         pframe += llc_sz;
1293                         mpdu_len -= llc_sz;
1294                 }
1295
1296                 if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
1297                         mpdu_len -= pattrib->icv_len;
1298                 }
1299
1300
1301                 if (bmcst) {
1302                         /*  don't do fragment to broadcat/multicast packets */
1303                         mem_sz = _rtw_pktfile_read(&pktfile, pframe, pattrib->pktlen);
1304                 } else {
1305                         mem_sz = _rtw_pktfile_read(&pktfile, pframe, mpdu_len);
1306                 }
1307
1308                 pframe += mem_sz;
1309
1310                 if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
1311                         memcpy(pframe, pattrib->icv, pattrib->icv_len);
1312                         pframe += pattrib->icv_len;
1313                 }
1314
1315                 frg_inx++;
1316
1317                 if (bmcst || (rtw_endofpktfile(&pktfile) == true)) {
1318                         pattrib->nr_frags = frg_inx;
1319
1320                         pattrib->last_txcmdsz = pattrib->hdrlen + pattrib->iv_len + ((pattrib->nr_frags == 1) ? llc_sz:0) +
1321                                         ((pattrib->bswenc) ? pattrib->icv_len : 0) + mem_sz;
1322
1323                         ClearMFrag(mem_start);
1324
1325                         break;
1326                 } else
1327                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("%s: There're still something in packet!\n", __func__));
1328
1329                 addr = (SIZE_PTR)(pframe);
1330
1331                 mem_start = (unsigned char *)RND4(addr) + hw_hdr_offset;
1332                 memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
1333
1334         }
1335
1336         if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
1337                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
1338                 DBG_8192C("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
1339                 res = _FAIL;
1340                 goto exit;
1341         }
1342
1343         xmitframe_swencrypt(padapter, pxmitframe);
1344
1345         if (bmcst == false)
1346                 update_attrib_vcs_info(padapter, pxmitframe);
1347         else
1348                 pattrib->vcs_mode = NONE_VCS;
1349
1350 exit:
1351         return res;
1352 }
1353
1354 /* broadcast or multicast management pkt use BIP, unicast management pkt use CCMP encryption */
1355 s32 rtw_mgmt_xmitframe_coalesce(struct adapter *padapter, _pkt *pkt, struct xmit_frame *pxmitframe)
1356 {
1357         u8 *pframe, *mem_start = NULL, *tmp_buf = NULL;
1358         u8 subtype;
1359         struct sta_info         *psta = NULL;
1360         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
1361         s32 bmcst = IS_MCAST(pattrib->ra);
1362         u8 *BIP_AAD = NULL;
1363         u8 *MGMT_body = NULL;
1364
1365         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
1366         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1367         struct ieee80211_hdr    *pwlanhdr;
1368         u8 MME[_MME_IE_LENGTH_];
1369         u32 ori_len;
1370         mem_start = pframe = (u8 *)(pxmitframe->buf_addr) + TXDESC_OFFSET;
1371         pwlanhdr = (struct ieee80211_hdr *)pframe;
1372
1373         ori_len = BIP_AAD_SIZE+pattrib->pktlen;
1374         tmp_buf = BIP_AAD = rtw_zmalloc(ori_len);
1375         subtype = GetFrameSubType(pframe); /* bit(7)~bit(2) */
1376
1377         if (!BIP_AAD)
1378                 return _FAIL;
1379
1380         spin_lock_bh(&padapter->security_key_mutex);
1381
1382         /* only support station mode */
1383         if (!check_fwstate(pmlmepriv, WIFI_STATION_STATE) || !check_fwstate(pmlmepriv, _FW_LINKED))
1384                 goto xmitframe_coalesce_success;
1385
1386         /* IGTK key is not install, it may not support 802.11w */
1387         if (padapter->securitypriv.binstallBIPkey != true) {
1388                 DBG_871X("no instll BIP key\n");
1389                 goto xmitframe_coalesce_success;
1390         }
1391         /* station mode doesn't need TX BIP, just ready the code */
1392         if (bmcst) {
1393                 int frame_body_len;
1394                 u8 mic[16];
1395
1396                 memset(MME, 0, 18);
1397
1398                 /* other types doesn't need the BIP */
1399                 if (GetFrameSubType(pframe) != WIFI_DEAUTH && GetFrameSubType(pframe) != WIFI_DISASSOC)
1400                         goto xmitframe_coalesce_fail;
1401
1402                 MGMT_body = pframe + sizeof(struct ieee80211_hdr_3addr);
1403                 pframe += pattrib->pktlen;
1404
1405                 /* octent 0 and 1 is key index , BIP keyid is 4 or 5, LSB only need octent 0 */
1406                 MME[0] = padapter->securitypriv.dot11wBIPKeyid;
1407                 /* copy packet number */
1408                 memcpy(&MME[2], &pmlmeext->mgnt_80211w_IPN, 6);
1409                 /* increase the packet number */
1410                 pmlmeext->mgnt_80211w_IPN++;
1411
1412                 /* add MME IE with MIC all zero, MME string doesn't include element id and length */
1413                 pframe = rtw_set_ie(pframe, _MME_IE_, 16, MME, &(pattrib->pktlen));
1414                 pattrib->last_txcmdsz = pattrib->pktlen;
1415                 /*  total frame length - header length */
1416                 frame_body_len = pattrib->pktlen - sizeof(struct ieee80211_hdr_3addr);
1417
1418                 /* conscruct AAD, copy frame control field */
1419                 memcpy(BIP_AAD, &pwlanhdr->frame_control, 2);
1420                 ClearRetry(BIP_AAD);
1421                 ClearPwrMgt(BIP_AAD);
1422                 ClearMData(BIP_AAD);
1423                 /* conscruct AAD, copy address 1 to address 3 */
1424                 memcpy(BIP_AAD+2, pwlanhdr->addr1, 18);
1425                 /* copy management fram body */
1426                 memcpy(BIP_AAD+BIP_AAD_SIZE, MGMT_body, frame_body_len);
1427                 /* calculate mic */
1428                 if (omac1_aes_128(padapter->securitypriv.dot11wBIPKey[padapter->securitypriv.dot11wBIPKeyid].skey
1429                         , BIP_AAD, BIP_AAD_SIZE+frame_body_len, mic))
1430                         goto xmitframe_coalesce_fail;
1431
1432                 /* copy right BIP mic value, total is 128bits, we use the 0~63 bits */
1433                 memcpy(pframe-8, mic, 8);
1434         } else { /* unicast mgmt frame TX */
1435                 /* start to encrypt mgmt frame */
1436                 if (subtype == WIFI_DEAUTH || subtype == WIFI_DISASSOC ||
1437                         subtype == WIFI_REASSOCREQ || subtype == WIFI_ACTION) {
1438                         if (pattrib->psta)
1439                                 psta = pattrib->psta;
1440                         else
1441                                 psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
1442
1443                         if (!psta) {
1444
1445                                 DBG_871X("%s, psta ==NUL\n", __func__);
1446                                 goto xmitframe_coalesce_fail;
1447                         }
1448
1449                         if (!(psta->state & _FW_LINKED) || !pxmitframe->buf_addr) {
1450                                 DBG_871X("%s, not _FW_LINKED or addr null\n", __func__);
1451                                 goto xmitframe_coalesce_fail;
1452                         }
1453
1454                         /* DBG_871X("%s, action frame category =%d\n", __func__, pframe[WLAN_HDR_A3_LEN]); */
1455                         /* according 802.11-2012 standard, these five types are not robust types */
1456                         if (subtype == WIFI_ACTION &&
1457                         (pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_PUBLIC ||
1458                         pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_HT ||
1459                         pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_UNPROTECTED_WNM ||
1460                         pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_SELF_PROTECTED  ||
1461                         pframe[WLAN_HDR_A3_LEN] == RTW_WLAN_CATEGORY_P2P))
1462                                 goto xmitframe_coalesce_fail;
1463                         /* before encrypt dump the management packet content */
1464                         if (pattrib->encrypt > 0)
1465                                 memcpy(pattrib->dot118021x_UncstKey.skey, psta->dot118021x_UncstKey.skey, 16);
1466                         /* bakeup original management packet */
1467                         memcpy(tmp_buf, pframe, pattrib->pktlen);
1468                         /* move to data portion */
1469                         pframe += pattrib->hdrlen;
1470
1471                         /* 802.11w unicast management packet must be _AES_ */
1472                         pattrib->iv_len = 8;
1473                         /* it's MIC of AES */
1474                         pattrib->icv_len = 8;
1475
1476                         switch (pattrib->encrypt) {
1477                         case _AES_:
1478                                         /* set AES IV header */
1479                                         AES_IV(pattrib->iv, psta->dot11wtxpn, 0);
1480                                 break;
1481                         default:
1482                                 goto xmitframe_coalesce_fail;
1483                         }
1484                         /* insert iv header into management frame */
1485                         memcpy(pframe, pattrib->iv, pattrib->iv_len);
1486                         pframe += pattrib->iv_len;
1487                         /* copy mgmt data portion after CCMP header */
1488                         memcpy(pframe, tmp_buf+pattrib->hdrlen, pattrib->pktlen-pattrib->hdrlen);
1489                         /* move pframe to end of mgmt pkt */
1490                         pframe += pattrib->pktlen-pattrib->hdrlen;
1491                         /* add 8 bytes CCMP IV header to length */
1492                         pattrib->pktlen += pattrib->iv_len;
1493                         if ((pattrib->icv_len > 0) && (pattrib->bswenc)) {
1494                                 memcpy(pframe, pattrib->icv, pattrib->icv_len);
1495                                 pframe += pattrib->icv_len;
1496                         }
1497                         /* add 8 bytes MIC */
1498                         pattrib->pktlen += pattrib->icv_len;
1499                         /* set final tx command size */
1500                         pattrib->last_txcmdsz = pattrib->pktlen;
1501
1502                         /* set protected bit must be beofre SW encrypt */
1503                         SetPrivacy(mem_start);
1504                         /* software encrypt */
1505                         xmitframe_swencrypt(padapter, pxmitframe);
1506                 }
1507         }
1508
1509 xmitframe_coalesce_success:
1510         spin_unlock_bh(&padapter->security_key_mutex);
1511         kfree(BIP_AAD);
1512         return _SUCCESS;
1513
1514 xmitframe_coalesce_fail:
1515         spin_unlock_bh(&padapter->security_key_mutex);
1516         kfree(BIP_AAD);
1517         return _FAIL;
1518 }
1519
1520 /* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
1521  * IEEE LLC/SNAP header contains 8 octets
1522  * First 3 octets comprise the LLC portion
1523  * SNAP portion, 5 octets, is divided into two fields:
1524  *Organizationally Unique Identifier(OUI), 3 octets,
1525  *type, defined by that organization, 2 octets.
1526  */
1527 s32 rtw_put_snap(u8 *data, u16 h_proto)
1528 {
1529         struct ieee80211_snap_hdr *snap;
1530         u8 *oui;
1531
1532         snap = (struct ieee80211_snap_hdr *)data;
1533         snap->dsap = 0xaa;
1534         snap->ssap = 0xaa;
1535         snap->ctrl = 0x03;
1536
1537         if (h_proto == 0x8137 || h_proto == 0x80f3)
1538                 oui = P802_1H_OUI;
1539         else
1540                 oui = RFC1042_OUI;
1541
1542         snap->oui[0] = oui[0];
1543         snap->oui[1] = oui[1];
1544         snap->oui[2] = oui[2];
1545
1546         *(__be16 *)(data + SNAP_SIZE) = htons(h_proto);
1547
1548         return SNAP_SIZE + sizeof(u16);
1549 }
1550
1551 void rtw_update_protection(struct adapter *padapter, u8 *ie, uint ie_len)
1552 {
1553
1554         uint    protection;
1555         u8 *perp;
1556         sint     erp_len;
1557         struct  xmit_priv *pxmitpriv = &padapter->xmitpriv;
1558         struct  registry_priv *pregistrypriv = &padapter->registrypriv;
1559
1560         switch (pxmitpriv->vcs_setting) {
1561         case DISABLE_VCS:
1562                 pxmitpriv->vcs = NONE_VCS;
1563                 break;
1564
1565         case ENABLE_VCS:
1566                 break;
1567
1568         case AUTO_VCS:
1569         default:
1570                 perp = rtw_get_ie(ie, _ERPINFO_IE_, &erp_len, ie_len);
1571                 if (!perp)
1572                         pxmitpriv->vcs = NONE_VCS;
1573                 else {
1574                         protection = (*(perp + 2)) & BIT(1);
1575                         if (protection) {
1576                                 if (pregistrypriv->vcs_type == RTS_CTS)
1577                                         pxmitpriv->vcs = RTS_CTS;
1578                                 else
1579                                         pxmitpriv->vcs = CTS_TO_SELF;
1580                         } else
1581                                 pxmitpriv->vcs = NONE_VCS;
1582                 }
1583
1584                 break;
1585
1586         }
1587 }
1588
1589 void rtw_count_tx_stats(struct adapter *padapter, struct xmit_frame *pxmitframe, int sz)
1590 {
1591         struct sta_info *psta = NULL;
1592         struct stainfo_stats *pstats = NULL;
1593         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1594         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1595         u8 pkt_num = 1;
1596
1597         if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG) {
1598                 pkt_num = pxmitframe->agg_num;
1599
1600                 pmlmepriv->LinkDetectInfo.NumTxOkInPeriod += pkt_num;
1601
1602                 pxmitpriv->tx_pkts += pkt_num;
1603
1604                 pxmitpriv->tx_bytes += sz;
1605
1606                 psta = pxmitframe->attrib.psta;
1607                 if (psta) {
1608                         pstats = &psta->sta_stats;
1609
1610                         pstats->tx_pkts += pkt_num;
1611
1612                         pstats->tx_bytes += sz;
1613                 }
1614         }
1615 }
1616
1617 static struct xmit_buf *__rtw_alloc_cmd_xmitbuf(struct xmit_priv *pxmitpriv,
1618                 enum cmdbuf_type buf_type)
1619 {
1620         struct xmit_buf *pxmitbuf =  NULL;
1621
1622         pxmitbuf = &pxmitpriv->pcmd_xmitbuf[buf_type];
1623         if (pxmitbuf) {
1624                 pxmitbuf->priv_data = NULL;
1625
1626                 pxmitbuf->len = 0;
1627                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
1628                 pxmitbuf->agg_num = 0;
1629                 pxmitbuf->pg_num = 0;
1630
1631                 if (pxmitbuf->sctx) {
1632                         DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
1633                         rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
1634                 }
1635         } else
1636                 DBG_871X("%s fail, no xmitbuf available !!!\n", __func__);
1637
1638         return pxmitbuf;
1639 }
1640
1641 struct xmit_frame *__rtw_alloc_cmdxmitframe(struct xmit_priv *pxmitpriv,
1642                 enum cmdbuf_type buf_type)
1643 {
1644         struct xmit_frame               *pcmdframe;
1645         struct xmit_buf         *pxmitbuf;
1646
1647         pcmdframe = rtw_alloc_xmitframe(pxmitpriv);
1648         if (!pcmdframe) {
1649                 DBG_871X("%s, alloc xmitframe fail\n", __func__);
1650                 return NULL;
1651         }
1652
1653         pxmitbuf = __rtw_alloc_cmd_xmitbuf(pxmitpriv, buf_type);
1654         if (!pxmitbuf) {
1655                 DBG_871X("%s, alloc xmitbuf fail\n", __func__);
1656                 rtw_free_xmitframe(pxmitpriv, pcmdframe);
1657                 return NULL;
1658         }
1659
1660         pcmdframe->frame_tag = MGNT_FRAMETAG;
1661
1662         pcmdframe->pxmitbuf = pxmitbuf;
1663
1664         pcmdframe->buf_addr = pxmitbuf->pbuf;
1665
1666         pxmitbuf->priv_data = pcmdframe;
1667
1668         return pcmdframe;
1669
1670 }
1671
1672 struct xmit_buf *rtw_alloc_xmitbuf_ext(struct xmit_priv *pxmitpriv)
1673 {
1674         _irqL irqL;
1675         struct xmit_buf *pxmitbuf =  NULL;
1676         struct list_head *plist, *phead;
1677         struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
1678
1679         spin_lock_irqsave(&pfree_queue->lock, irqL);
1680
1681         if (list_empty(&pfree_queue->queue)) {
1682                 pxmitbuf = NULL;
1683         } else {
1684
1685                 phead = get_list_head(pfree_queue);
1686
1687                 plist = get_next(phead);
1688
1689                 pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
1690
1691                 list_del_init(&(pxmitbuf->list));
1692         }
1693
1694         if (pxmitbuf) {
1695                 pxmitpriv->free_xmit_extbuf_cnt--;
1696                 #ifdef DBG_XMIT_BUF_EXT
1697                 DBG_871X("DBG_XMIT_BUF_EXT ALLOC no =%d,  free_xmit_extbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
1698                 #endif
1699
1700
1701                 pxmitbuf->priv_data = NULL;
1702
1703                 pxmitbuf->len = 0;
1704                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
1705                 pxmitbuf->agg_num = 1;
1706
1707                 if (pxmitbuf->sctx) {
1708                         DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
1709                         rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
1710                 }
1711
1712         }
1713
1714         spin_unlock_irqrestore(&pfree_queue->lock, irqL);
1715
1716         return pxmitbuf;
1717 }
1718
1719 s32 rtw_free_xmitbuf_ext(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
1720 {
1721         _irqL irqL;
1722         struct __queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
1723
1724         if (!pxmitbuf)
1725                 return _FAIL;
1726
1727         spin_lock_irqsave(&pfree_queue->lock, irqL);
1728
1729         list_del_init(&pxmitbuf->list);
1730
1731         list_add_tail(&(pxmitbuf->list), get_list_head(pfree_queue));
1732         pxmitpriv->free_xmit_extbuf_cnt++;
1733         #ifdef DBG_XMIT_BUF_EXT
1734         DBG_871X("DBG_XMIT_BUF_EXT FREE no =%d, free_xmit_extbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmit_extbuf_cnt);
1735         #endif
1736
1737         spin_unlock_irqrestore(&pfree_queue->lock, irqL);
1738
1739         return _SUCCESS;
1740 }
1741
1742 struct xmit_buf *rtw_alloc_xmitbuf(struct xmit_priv *pxmitpriv)
1743 {
1744         _irqL irqL;
1745         struct xmit_buf *pxmitbuf =  NULL;
1746         struct list_head *plist, *phead;
1747         struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
1748
1749         /* DBG_871X("+rtw_alloc_xmitbuf\n"); */
1750
1751         spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
1752
1753         if (list_empty(&pfree_xmitbuf_queue->queue)) {
1754                 pxmitbuf = NULL;
1755         } else {
1756
1757                 phead = get_list_head(pfree_xmitbuf_queue);
1758
1759                 plist = get_next(phead);
1760
1761                 pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
1762
1763                 list_del_init(&(pxmitbuf->list));
1764         }
1765
1766         if (pxmitbuf) {
1767                 pxmitpriv->free_xmitbuf_cnt--;
1768                 #ifdef DBG_XMIT_BUF
1769                 DBG_871X("DBG_XMIT_BUF ALLOC no =%d,  free_xmitbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
1770                 #endif
1771                 /* DBG_871X("alloc, free_xmitbuf_cnt =%d\n", pxmitpriv->free_xmitbuf_cnt); */
1772
1773                 pxmitbuf->priv_data = NULL;
1774
1775                 pxmitbuf->len = 0;
1776                 pxmitbuf->pdata = pxmitbuf->ptail = pxmitbuf->phead;
1777                 pxmitbuf->agg_num = 0;
1778                 pxmitbuf->pg_num = 0;
1779
1780                 if (pxmitbuf->sctx) {
1781                         DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
1782                         rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
1783                 }
1784         }
1785         #ifdef DBG_XMIT_BUF
1786         else
1787                 DBG_871X("DBG_XMIT_BUF rtw_alloc_xmitbuf return NULL\n");
1788         #endif
1789
1790         spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
1791
1792         return pxmitbuf;
1793 }
1794
1795 s32 rtw_free_xmitbuf(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
1796 {
1797         _irqL irqL;
1798         struct __queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
1799
1800         /* DBG_871X("+rtw_free_xmitbuf\n"); */
1801
1802         if (!pxmitbuf)
1803                 return _FAIL;
1804
1805         if (pxmitbuf->sctx) {
1806                 DBG_871X("%s pxmitbuf->sctx is not NULL\n", __func__);
1807                 rtw_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
1808         }
1809
1810         if (pxmitbuf->buf_tag == XMITBUF_CMD) {
1811         } else if (pxmitbuf->buf_tag == XMITBUF_MGNT) {
1812                 rtw_free_xmitbuf_ext(pxmitpriv, pxmitbuf);
1813         } else {
1814                 spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
1815
1816                 list_del_init(&pxmitbuf->list);
1817
1818                 list_add_tail(&(pxmitbuf->list), get_list_head(pfree_xmitbuf_queue));
1819
1820                 pxmitpriv->free_xmitbuf_cnt++;
1821                 /* DBG_871X("FREE, free_xmitbuf_cnt =%d\n", pxmitpriv->free_xmitbuf_cnt); */
1822                 #ifdef DBG_XMIT_BUF
1823                 DBG_871X("DBG_XMIT_BUF FREE no =%d, free_xmitbuf_cnt =%d\n", pxmitbuf->no, pxmitpriv->free_xmitbuf_cnt);
1824                 #endif
1825                 spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
1826         }
1827         return _SUCCESS;
1828 }
1829
1830 static void rtw_init_xmitframe(struct xmit_frame *pxframe)
1831 {
1832         if (pxframe) { /* default value setting */
1833                 pxframe->buf_addr = NULL;
1834                 pxframe->pxmitbuf = NULL;
1835
1836                 memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
1837                 /* pxframe->attrib.psta = NULL; */
1838
1839                 pxframe->frame_tag = DATA_FRAMETAG;
1840
1841                 pxframe->pg_num = 1;
1842                 pxframe->agg_num = 1;
1843                 pxframe->ack_report = 0;
1844         }
1845 }
1846
1847 /*
1848 Calling context:
1849 1. OS_TXENTRY
1850 2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
1851
1852 If we turn on USE_RXTHREAD, then, no need for critical section.
1853 Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
1854
1855 Must be very very cautious...
1856
1857 */
1858 struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)/* _queue *pfree_xmit_queue) */
1859 {
1860         /*
1861                 Please remember to use all the osdep_service api,
1862                 and lock/unlock or _enter/_exit critical to protect
1863                 pfree_xmit_queue
1864         */
1865
1866         struct xmit_frame *pxframe = NULL;
1867         struct list_head *plist, *phead;
1868         struct __queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
1869
1870         spin_lock_bh(&pfree_xmit_queue->lock);
1871
1872         if (list_empty(&pfree_xmit_queue->queue)) {
1873                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe:%d\n", pxmitpriv->free_xmitframe_cnt));
1874                 pxframe =  NULL;
1875         } else {
1876                 phead = get_list_head(pfree_xmit_queue);
1877
1878                 plist = get_next(phead);
1879
1880                 pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
1881
1882                 list_del_init(&(pxframe->list));
1883                 pxmitpriv->free_xmitframe_cnt--;
1884                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
1885         }
1886
1887         spin_unlock_bh(&pfree_xmit_queue->lock);
1888
1889         rtw_init_xmitframe(pxframe);
1890         return pxframe;
1891 }
1892
1893 struct xmit_frame *rtw_alloc_xmitframe_ext(struct xmit_priv *pxmitpriv)
1894 {
1895         struct xmit_frame *pxframe = NULL;
1896         struct list_head *plist, *phead;
1897         struct __queue *queue = &pxmitpriv->free_xframe_ext_queue;
1898
1899         spin_lock_bh(&queue->lock);
1900
1901         if (list_empty(&queue->queue)) {
1902                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
1903                 pxframe =  NULL;
1904         } else {
1905                 phead = get_list_head(queue);
1906                 plist = get_next(phead);
1907                 pxframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
1908
1909                 list_del_init(&(pxframe->list));
1910                 pxmitpriv->free_xframe_ext_cnt--;
1911                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe_ext():free_xmitframe_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
1912         }
1913
1914         spin_unlock_bh(&queue->lock);
1915
1916         rtw_init_xmitframe(pxframe);
1917
1918         return pxframe;
1919 }
1920
1921 struct xmit_frame *rtw_alloc_xmitframe_once(struct xmit_priv *pxmitpriv)
1922 {
1923         struct xmit_frame *pxframe = NULL;
1924         u8 *alloc_addr;
1925
1926         alloc_addr = rtw_zmalloc(sizeof(struct xmit_frame) + 4);
1927
1928         if (!alloc_addr)
1929                 goto exit;
1930
1931         pxframe = (struct xmit_frame *)N_BYTE_ALIGMENT((SIZE_PTR)(alloc_addr), 4);
1932         pxframe->alloc_addr = alloc_addr;
1933
1934         pxframe->padapter = pxmitpriv->adapter;
1935         pxframe->frame_tag = NULL_FRAMETAG;
1936
1937         pxframe->pkt = NULL;
1938
1939         pxframe->buf_addr = NULL;
1940         pxframe->pxmitbuf = NULL;
1941
1942         rtw_init_xmitframe(pxframe);
1943
1944         DBG_871X("################## %s ##################\n", __func__);
1945
1946 exit:
1947         return pxframe;
1948 }
1949
1950 s32 rtw_free_xmitframe(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
1951 {
1952         struct __queue *queue = NULL;
1953         struct adapter *padapter = pxmitpriv->adapter;
1954         _pkt *pndis_pkt = NULL;
1955
1956         if (!pxmitframe) {
1957                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("======rtw_free_xmitframe():pxmitframe == NULL!!!!!!!!!!\n"));
1958                 goto exit;
1959         }
1960
1961         if (pxmitframe->pkt) {
1962                 pndis_pkt = pxmitframe->pkt;
1963                 pxmitframe->pkt = NULL;
1964         }
1965
1966         if (pxmitframe->alloc_addr) {
1967                 DBG_871X("################## %s with alloc_addr ##################\n", __func__);
1968                 kfree(pxmitframe->alloc_addr);
1969                 goto check_pkt_complete;
1970         }
1971
1972         if (pxmitframe->ext_tag == 0)
1973                 queue = &pxmitpriv->free_xmit_queue;
1974         else if (pxmitframe->ext_tag == 1)
1975                 queue = &pxmitpriv->free_xframe_ext_queue;
1976         else {
1977
1978         }
1979
1980         spin_lock_bh(&queue->lock);
1981
1982         list_del_init(&pxmitframe->list);
1983         list_add_tail(&pxmitframe->list, get_list_head(queue));
1984         if (pxmitframe->ext_tag == 0) {
1985                 pxmitpriv->free_xmitframe_cnt++;
1986                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
1987         } else if (pxmitframe->ext_tag == 1) {
1988                 pxmitpriv->free_xframe_ext_cnt++;
1989                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe():free_xframe_ext_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
1990         } else {
1991         }
1992
1993         spin_unlock_bh(&queue->lock);
1994
1995 check_pkt_complete:
1996
1997         if (pndis_pkt)
1998                 rtw_os_pkt_complete(padapter, pndis_pkt);
1999
2000 exit:
2001         return _SUCCESS;
2002 }
2003
2004 void rtw_free_xmitframe_queue(struct xmit_priv *pxmitpriv, struct __queue *pframequeue)
2005 {
2006         struct list_head        *plist, *phead;
2007         struct  xmit_frame      *pxmitframe;
2008
2009         spin_lock_bh(&(pframequeue->lock));
2010
2011         phead = get_list_head(pframequeue);
2012         plist = get_next(phead);
2013
2014         while (phead != plist) {
2015
2016                 pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
2017
2018                 plist = get_next(plist);
2019
2020                 rtw_free_xmitframe(pxmitpriv, pxmitframe);
2021
2022         }
2023         spin_unlock_bh(&(pframequeue->lock));
2024 }
2025
2026 s32 rtw_xmitframe_enqueue(struct adapter *padapter, struct xmit_frame *pxmitframe)
2027 {
2028         DBG_COUNTER(padapter->tx_logs.core_tx_enqueue);
2029         if (rtw_xmit_classifier(padapter, pxmitframe) == _FAIL) {
2030                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
2031                          ("rtw_xmitframe_enqueue: drop xmit pkt for classifier fail\n"));
2032 /*              pxmitframe->pkt = NULL; */
2033                 return _FAIL;
2034         }
2035
2036         return _SUCCESS;
2037 }
2038
2039 struct tx_servq *rtw_get_sta_pending(struct adapter *padapter, struct sta_info *psta, sint up, u8 *ac)
2040 {
2041         struct tx_servq *ptxservq = NULL;
2042
2043         switch (up) {
2044         case 1:
2045         case 2:
2046                 ptxservq = &(psta->sta_xmitpriv.bk_q);
2047                 *(ac) = 3;
2048                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BK\n"));
2049                 break;
2050
2051         case 4:
2052         case 5:
2053                 ptxservq = &(psta->sta_xmitpriv.vi_q);
2054                 *(ac) = 1;
2055                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VI\n"));
2056                 break;
2057
2058         case 6:
2059         case 7:
2060                 ptxservq = &(psta->sta_xmitpriv.vo_q);
2061                 *(ac) = 0;
2062                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : VO\n"));
2063                 break;
2064
2065         case 0:
2066         case 3:
2067         default:
2068                 ptxservq = &(psta->sta_xmitpriv.be_q);
2069                 *(ac) = 2;
2070                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending : BE\n"));
2071         break;
2072
2073         }
2074
2075         return ptxservq;
2076 }
2077
2078 /*
2079  * Will enqueue pxmitframe to the proper queue,
2080  * and indicate it to xx_pending list.....
2081  */
2082 s32 rtw_xmit_classifier(struct adapter *padapter, struct xmit_frame *pxmitframe)
2083 {
2084         /* _irqL irqL0; */
2085         u8 ac_index;
2086         struct sta_info *psta;
2087         struct tx_servq *ptxservq;
2088         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
2089         struct hw_xmit  *phwxmits =  padapter->xmitpriv.hwxmits;
2090         sint res = _SUCCESS;
2091
2092         DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class);
2093
2094 /*
2095         if (pattrib->psta) {
2096                 psta = pattrib->psta;
2097         } else {
2098                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
2099                 psta = rtw_get_stainfo(pstapriv, pattrib->ra);
2100         }
2101 */
2102
2103         psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
2104         if (pattrib->psta != psta) {
2105                 DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_sta);
2106                 DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
2107                 return _FAIL;
2108         }
2109
2110         if (!psta) {
2111                 DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_nosta);
2112                 res = _FAIL;
2113                 DBG_8192C("rtw_xmit_classifier: psta == NULL\n");
2114                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("rtw_xmit_classifier: psta == NULL\n"));
2115                 goto exit;
2116         }
2117
2118         if (!(psta->state & _FW_LINKED)) {
2119                 DBG_COUNTER(padapter->tx_logs.core_tx_enqueue_class_err_fwlink);
2120                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
2121                 return _FAIL;
2122         }
2123
2124         ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
2125
2126         /* spin_lock_irqsave(&pstapending->lock, irqL0); */
2127
2128         if (list_empty(&ptxservq->tx_pending)) {
2129                 list_add_tail(&ptxservq->tx_pending, get_list_head(phwxmits[ac_index].sta_queue));
2130         }
2131
2132         /* spin_lock_irqsave(&ptxservq->sta_pending.lock, irqL1); */
2133
2134         list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
2135         ptxservq->qcnt++;
2136         phwxmits[ac_index].accnt++;
2137
2138         /* spin_unlock_irqrestore(&ptxservq->sta_pending.lock, irqL1); */
2139
2140         /* spin_unlock_irqrestore(&pstapending->lock, irqL0); */
2141
2142 exit:
2143
2144         return res;
2145 }
2146
2147 s32 rtw_alloc_hwxmits(struct adapter *padapter)
2148 {
2149         struct hw_xmit *hwxmits;
2150         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2151
2152         pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
2153
2154         pxmitpriv->hwxmits = NULL;
2155
2156         pxmitpriv->hwxmits = rtw_zmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry);
2157
2158         if (!pxmitpriv->hwxmits)
2159                 return _FAIL;
2160
2161         hwxmits = pxmitpriv->hwxmits;
2162
2163         if (pxmitpriv->hwxmit_entry == 5) {
2164                 /* pxmitpriv->bmc_txqueue.head = 0; */
2165                 /* hwxmits[0] .phwtxqueue = &pxmitpriv->bmc_txqueue; */
2166                 hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
2167
2168                 /* pxmitpriv->vo_txqueue.head = 0; */
2169                 /* hwxmits[1] .phwtxqueue = &pxmitpriv->vo_txqueue; */
2170                 hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
2171
2172                 /* pxmitpriv->vi_txqueue.head = 0; */
2173                 /* hwxmits[2] .phwtxqueue = &pxmitpriv->vi_txqueue; */
2174                 hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
2175
2176                 /* pxmitpriv->bk_txqueue.head = 0; */
2177                 /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */
2178                 hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
2179
2180                 /* pxmitpriv->be_txqueue.head = 0; */
2181                 /* hwxmits[4] .phwtxqueue = &pxmitpriv->be_txqueue; */
2182                 hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
2183
2184         } else if (pxmitpriv->hwxmit_entry == 4) {
2185
2186                 /* pxmitpriv->vo_txqueue.head = 0; */
2187                 /* hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue; */
2188                 hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
2189
2190                 /* pxmitpriv->vi_txqueue.head = 0; */
2191                 /* hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue; */
2192                 hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
2193
2194                 /* pxmitpriv->be_txqueue.head = 0; */
2195                 /* hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue; */
2196                 hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
2197
2198                 /* pxmitpriv->bk_txqueue.head = 0; */
2199                 /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */
2200                 hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
2201         } else {
2202
2203         }
2204
2205         return _SUCCESS;
2206 }
2207
2208 void rtw_free_hwxmits(struct adapter *padapter)
2209 {
2210         struct hw_xmit *hwxmits;
2211         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2212
2213         hwxmits = pxmitpriv->hwxmits;
2214         if (hwxmits)
2215                 kfree((u8 *)hwxmits);
2216 }
2217
2218 void rtw_init_hwxmits(struct hw_xmit *phwxmit, sint entry)
2219 {
2220         sint i;
2221
2222         for (i = 0; i < entry; i++, phwxmit++) {
2223                 /* spin_lock_init(&phwxmit->xmit_lock); */
2224                 /* INIT_LIST_HEAD(&phwxmit->pending); */
2225                 /* phwxmit->txcmdcnt = 0; */
2226                 phwxmit->accnt = 0;
2227         }
2228 }
2229
2230 u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
2231 {
2232         u32 addr;
2233         struct pkt_attrib *pattrib = &pxmitframe->attrib;
2234
2235         switch (pattrib->qsel) {
2236         case 0:
2237         case 3:
2238                 addr = BE_QUEUE_INX;
2239                 break;
2240         case 1:
2241         case 2:
2242                 addr = BK_QUEUE_INX;
2243                 break;
2244         case 4:
2245         case 5:
2246                 addr = VI_QUEUE_INX;
2247                 break;
2248         case 6:
2249         case 7:
2250                 addr = VO_QUEUE_INX;
2251                 break;
2252         case 0x10:
2253                 addr = BCN_QUEUE_INX;
2254                 break;
2255         case 0x11:/* BC/MC in PS (HIQ) */
2256                 addr = HIGH_QUEUE_INX;
2257                 break;
2258         case 0x12:
2259         default:
2260                 addr = MGT_QUEUE_INX;
2261                 break;
2262
2263         }
2264
2265         return addr;
2266
2267 }
2268
2269 static void do_queue_select(struct adapter      *padapter, struct pkt_attrib *pattrib)
2270 {
2271         u8 qsel;
2272
2273         qsel = pattrib->priority;
2274         RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("### do_queue_select priority =%d , qsel = %d\n", pattrib->priority, qsel));
2275
2276         pattrib->qsel = qsel;
2277 }
2278
2279 /*
2280  * The main transmit(tx) entry
2281  *
2282  * Return
2283  *1     enqueue
2284  *0     success, hardware will handle this xmit frame(packet)
2285  *<0    fail
2286  */
2287 s32 rtw_xmit(struct adapter *padapter, _pkt **ppkt)
2288 {
2289         static unsigned long start;
2290         static u32 drop_cnt;
2291
2292         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2293         struct xmit_frame *pxmitframe = NULL;
2294
2295         s32 res;
2296
2297         DBG_COUNTER(padapter->tx_logs.core_tx);
2298
2299         if (start == 0)
2300                 start = jiffies;
2301
2302         pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
2303
2304         if (jiffies_to_msecs(jiffies - start) > 2000) {
2305                 if (drop_cnt)
2306                         DBG_871X("DBG_TX_DROP_FRAME %s no more pxmitframe, drop_cnt:%u\n", __func__, drop_cnt);
2307                 start = jiffies;
2308                 drop_cnt = 0;
2309         }
2310
2311         if (!pxmitframe) {
2312                 drop_cnt++;
2313                 RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: no more pxmitframe\n"));
2314                 DBG_COUNTER(padapter->tx_logs.core_tx_err_pxmitframe);
2315                 return -1;
2316         }
2317
2318         res = update_attrib(padapter, *ppkt, &pxmitframe->attrib);
2319
2320         if (res == _FAIL) {
2321                 RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit: update attrib fail\n"));
2322                 #ifdef DBG_TX_DROP_FRAME
2323                 DBG_871X("DBG_TX_DROP_FRAME %s update attrib fail\n", __func__);
2324                 #endif
2325                 rtw_free_xmitframe(pxmitpriv, pxmitframe);
2326                 return -1;
2327         }
2328         pxmitframe->pkt = *ppkt;
2329
2330         do_queue_select(padapter, &pxmitframe->attrib);
2331
2332         spin_lock_bh(&pxmitpriv->lock);
2333         if (xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe) == true) {
2334                 spin_unlock_bh(&pxmitpriv->lock);
2335                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue);
2336                 return 1;
2337         }
2338         spin_unlock_bh(&pxmitpriv->lock);
2339
2340         /* pre_xmitframe */
2341         if (rtw_hal_xmit(padapter, pxmitframe) == false)
2342                 return 1;
2343
2344         return 0;
2345 }
2346
2347 #define RTW_HIQ_FILTER_ALLOW_ALL 0
2348 #define RTW_HIQ_FILTER_ALLOW_SPECIAL 1
2349 #define RTW_HIQ_FILTER_DENY_ALL 2
2350
2351 inline bool xmitframe_hiq_filter(struct xmit_frame *xmitframe)
2352 {
2353         bool allow = false;
2354         struct adapter *adapter = xmitframe->padapter;
2355         struct registry_priv *registry = &adapter->registrypriv;
2356
2357         if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_SPECIAL) {
2358
2359                 struct pkt_attrib *attrib = &xmitframe->attrib;
2360
2361                 if (attrib->ether_type == 0x0806
2362                         || attrib->ether_type == 0x888e
2363                         || attrib->dhcp_pkt
2364                 ) {
2365                         DBG_871X(FUNC_ADPT_FMT" ether_type:0x%04x%s\n", FUNC_ADPT_ARG(xmitframe->padapter)
2366                                 , attrib->ether_type, attrib->dhcp_pkt?" DHCP":"");
2367                         allow = true;
2368                 }
2369         } else if (registry->hiq_filter == RTW_HIQ_FILTER_ALLOW_ALL)
2370                 allow = true;
2371         else if (registry->hiq_filter == RTW_HIQ_FILTER_DENY_ALL) {
2372         } else
2373                 rtw_warn_on(1);
2374
2375         return allow;
2376 }
2377
2378 sint xmitframe_enqueue_for_sleeping_sta(struct adapter *padapter, struct xmit_frame *pxmitframe)
2379 {
2380         sint ret = false;
2381         struct sta_info *psta = NULL;
2382         struct sta_priv *pstapriv = &padapter->stapriv;
2383         struct pkt_attrib *pattrib = &pxmitframe->attrib;
2384         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
2385         sint bmcst = IS_MCAST(pattrib->ra);
2386         bool update_tim = false;
2387
2388         if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false) {
2389                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_fwstate);
2390                 return ret;
2391         }
2392 /*
2393         if (pattrib->psta)
2394         {
2395                 psta = pattrib->psta;
2396         }
2397         else
2398         {
2399                 DBG_871X("%s, call rtw_get_stainfo()\n", __func__);
2400                 psta =rtw_get_stainfo(pstapriv, pattrib->ra);
2401         }
2402 */
2403         psta = rtw_get_stainfo(&padapter->stapriv, pattrib->ra);
2404         if (pattrib->psta != psta) {
2405                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_sta);
2406                 DBG_871X("%s, pattrib->psta(%p) != psta(%p)\n", __func__, pattrib->psta, psta);
2407                 return false;
2408         }
2409
2410         if (!psta) {
2411                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_nosta);
2412                 DBG_871X("%s, psta ==NUL\n", __func__);
2413                 return false;
2414         }
2415
2416         if (!(psta->state & _FW_LINKED)) {
2417                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_link);
2418                 DBG_871X("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
2419                 return false;
2420         }
2421
2422         if (pattrib->triggered == 1) {
2423                 DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_warn_trigger);
2424                 /* DBG_871X("directly xmit pspoll_triggered packet\n"); */
2425
2426                 /* pattrib->triggered = 0; */
2427                 if (bmcst && xmitframe_hiq_filter(pxmitframe))
2428                         pattrib->qsel = 0x11;/* HIQ */
2429
2430                 return ret;
2431         }
2432
2433
2434         if (bmcst) {
2435                 spin_lock_bh(&psta->sleep_q.lock);
2436
2437                 if (pstapriv->sta_dz_bitmap) { /* if anyone sta is in ps mode */
2438                         /* pattrib->qsel = 0x11;HIQ */
2439
2440                         list_del_init(&pxmitframe->list);
2441
2442                         /* spin_lock_bh(&psta->sleep_q.lock); */
2443
2444                         list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
2445
2446                         psta->sleepq_len++;
2447
2448                         if (!(pstapriv->tim_bitmap & BIT(0)))
2449                                 update_tim = true;
2450
2451                         pstapriv->tim_bitmap |= BIT(0);/*  */
2452                         pstapriv->sta_dz_bitmap |= BIT(0);
2453
2454                         /* DBG_871X("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
2455
2456                         if (update_tim) {
2457                                 update_beacon(padapter, _TIM_IE_, NULL, true);
2458                         } else {
2459                                 chk_bmc_sleepq_cmd(padapter);
2460                         }
2461
2462                         /* spin_unlock_bh(&psta->sleep_q.lock); */
2463
2464                         ret = true;
2465
2466                         DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_mcast);
2467
2468                 }
2469
2470                 spin_unlock_bh(&psta->sleep_q.lock);
2471
2472                 return ret;
2473
2474         }
2475
2476
2477         spin_lock_bh(&psta->sleep_q.lock);
2478
2479         if (psta->state&WIFI_SLEEP_STATE) {
2480                 u8 wmmps_ac = 0;
2481
2482                 if (pstapriv->sta_dz_bitmap & BIT(psta->aid)) {
2483                         list_del_init(&pxmitframe->list);
2484
2485                         /* spin_lock_bh(&psta->sleep_q.lock); */
2486
2487                         list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
2488
2489                         psta->sleepq_len++;
2490
2491                         switch (pattrib->priority) {
2492                         case 1:
2493                         case 2:
2494                                 wmmps_ac = psta->uapsd_bk&BIT(0);
2495                                 break;
2496                         case 4:
2497                         case 5:
2498                                 wmmps_ac = psta->uapsd_vi&BIT(0);
2499                                 break;
2500                         case 6:
2501                         case 7:
2502                                 wmmps_ac = psta->uapsd_vo&BIT(0);
2503                                 break;
2504                         case 0:
2505                         case 3:
2506                         default:
2507                                 wmmps_ac = psta->uapsd_be&BIT(0);
2508                                 break;
2509                         }
2510
2511                         if (wmmps_ac)
2512                                 psta->sleepq_ac_len++;
2513
2514                         if (((psta->has_legacy_ac) && (!wmmps_ac)) || ((!psta->has_legacy_ac) && (wmmps_ac))) {
2515                                 if (!(pstapriv->tim_bitmap & BIT(psta->aid)))
2516                                         update_tim = true;
2517
2518                                 pstapriv->tim_bitmap |= BIT(psta->aid);
2519
2520                                 /* DBG_871X("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
2521
2522                                 if (update_tim)
2523                                         /* DBG_871X("sleepq_len == 1, update BCNTIM\n"); */
2524                                         /* upate BCN for TIM IE */
2525                                         update_beacon(padapter, _TIM_IE_, NULL, true);
2526                         }
2527
2528                         /* spin_unlock_bh(&psta->sleep_q.lock); */
2529
2530                         /* if (psta->sleepq_len > (NR_XMITFRAME>>3)) */
2531                         /*  */
2532                         /*      wakeup_sta_to_xmit(padapter, psta); */
2533                         /*  */
2534
2535                         ret = true;
2536
2537                         DBG_COUNTER(padapter->tx_logs.core_tx_ap_enqueue_ucast);
2538                 }
2539
2540         }
2541
2542         spin_unlock_bh(&psta->sleep_q.lock);
2543
2544         return ret;
2545
2546 }
2547
2548 static void dequeue_xmitframes_to_sleeping_queue(struct adapter *padapter, struct sta_info *psta, struct __queue *pframequeue)
2549 {
2550         sint ret;
2551         struct list_head        *plist, *phead;
2552         u8 ac_index;
2553         struct tx_servq *ptxservq;
2554         struct pkt_attrib       *pattrib;
2555         struct xmit_frame       *pxmitframe;
2556         struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
2557
2558         phead = get_list_head(pframequeue);
2559         plist = get_next(phead);
2560
2561         while (phead != plist) {
2562                 pxmitframe = LIST_CONTAINOR(plist, struct xmit_frame, list);
2563
2564                 plist = get_next(plist);
2565
2566                 pattrib = &pxmitframe->attrib;
2567
2568                 pattrib->triggered = 0;
2569
2570                 ret = xmitframe_enqueue_for_sleeping_sta(padapter, pxmitframe);
2571
2572                 if (true == ret) {
2573                         ptxservq = rtw_get_sta_pending(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
2574
2575                         ptxservq->qcnt--;
2576                         phwxmits[ac_index].accnt--;
2577                 } else {
2578                         /* DBG_871X("xmitframe_enqueue_for_sleeping_sta return false\n"); */
2579                 }
2580
2581         }
2582
2583 }
2584
2585 void stop_sta_xmit(struct adapter *padapter, struct sta_info *psta)
2586 {
2587         struct sta_info *psta_bmc;
2588         struct sta_xmit_priv *pstaxmitpriv;
2589         struct sta_priv *pstapriv = &padapter->stapriv;
2590         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2591
2592         pstaxmitpriv = &psta->sta_xmitpriv;
2593
2594         /* for BC/MC Frames */
2595         psta_bmc = rtw_get_bcmc_stainfo(padapter);
2596
2597
2598         spin_lock_bh(&pxmitpriv->lock);
2599
2600         psta->state |= WIFI_SLEEP_STATE;
2601
2602         pstapriv->sta_dz_bitmap |= BIT(psta->aid);
2603
2604
2605
2606         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
2607         list_del_init(&(pstaxmitpriv->vo_q.tx_pending));
2608
2609
2610         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
2611         list_del_init(&(pstaxmitpriv->vi_q.tx_pending));
2612
2613
2614         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->be_q.sta_pending);
2615         list_del_init(&(pstaxmitpriv->be_q.tx_pending));
2616
2617
2618         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->bk_q.sta_pending);
2619         list_del_init(&(pstaxmitpriv->bk_q.tx_pending));
2620
2621         /* for BC/MC Frames */
2622         pstaxmitpriv = &psta_bmc->sta_xmitpriv;
2623         dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc, &pstaxmitpriv->be_q.sta_pending);
2624         list_del_init(&(pstaxmitpriv->be_q.tx_pending));
2625
2626         spin_unlock_bh(&pxmitpriv->lock);
2627 }
2628
2629 void wakeup_sta_to_xmit(struct adapter *padapter, struct sta_info *psta)
2630 {
2631         u8 update_mask = 0, wmmps_ac = 0;
2632         struct sta_info *psta_bmc;
2633         struct list_head        *xmitframe_plist, *xmitframe_phead;
2634         struct xmit_frame *pxmitframe = NULL;
2635         struct sta_priv *pstapriv = &padapter->stapriv;
2636         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2637
2638         psta_bmc = rtw_get_bcmc_stainfo(padapter);
2639
2640
2641         /* spin_lock_bh(&psta->sleep_q.lock); */
2642         spin_lock_bh(&pxmitpriv->lock);
2643
2644         xmitframe_phead = get_list_head(&psta->sleep_q);
2645         xmitframe_plist = get_next(xmitframe_phead);
2646
2647         while (xmitframe_phead != xmitframe_plist) {
2648                 pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
2649
2650                 xmitframe_plist = get_next(xmitframe_plist);
2651
2652                 list_del_init(&pxmitframe->list);
2653
2654                 switch (pxmitframe->attrib.priority) {
2655                 case 1:
2656                 case 2:
2657                         wmmps_ac = psta->uapsd_bk&BIT(1);
2658                         break;
2659                 case 4:
2660                 case 5:
2661                         wmmps_ac = psta->uapsd_vi&BIT(1);
2662                         break;
2663                 case 6:
2664                 case 7:
2665                         wmmps_ac = psta->uapsd_vo&BIT(1);
2666                         break;
2667                 case 0:
2668                 case 3:
2669                 default:
2670                         wmmps_ac = psta->uapsd_be&BIT(1);
2671                         break;
2672                 }
2673
2674                 psta->sleepq_len--;
2675                 if (psta->sleepq_len > 0)
2676                         pxmitframe->attrib.mdata = 1;
2677                 else
2678                         pxmitframe->attrib.mdata = 0;
2679
2680                 if (wmmps_ac) {
2681                         psta->sleepq_ac_len--;
2682                         if (psta->sleepq_ac_len > 0) {
2683                                 pxmitframe->attrib.mdata = 1;
2684                                 pxmitframe->attrib.eosp = 0;
2685                         } else {
2686                                 pxmitframe->attrib.mdata = 0;
2687                                 pxmitframe->attrib.eosp = 1;
2688                         }
2689                 }
2690
2691                 pxmitframe->attrib.triggered = 1;
2692
2693 /*
2694                 spin_unlock_bh(&psta->sleep_q.lock);
2695                 if (rtw_hal_xmit(padapter, pxmitframe) == true)
2696                 {
2697                         rtw_os_xmit_complete(padapter, pxmitframe);
2698                 }
2699                 spin_lock_bh(&psta->sleep_q.lock);
2700 */
2701                 rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
2702
2703
2704         }
2705
2706         if (psta->sleepq_len == 0) {
2707                 if (pstapriv->tim_bitmap & BIT(psta->aid)) {
2708                         /* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
2709                         /* upate BCN for TIM IE */
2710                         /* update_BCNTIM(padapter); */
2711                         update_mask = BIT(0);
2712                 }
2713
2714                 pstapriv->tim_bitmap &= ~BIT(psta->aid);
2715
2716                 if (psta->state&WIFI_SLEEP_STATE)
2717                         psta->state ^= WIFI_SLEEP_STATE;
2718
2719                 if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
2720                         DBG_871X("%s alive check\n", __func__);
2721                         psta->expire_to = pstapriv->expire_to;
2722                         psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
2723                 }
2724
2725                 pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
2726         }
2727
2728         /* for BC/MC Frames */
2729         if (!psta_bmc)
2730                 goto _exit;
2731
2732         if ((pstapriv->sta_dz_bitmap&0xfffe) == 0x0) { /* no any sta in ps mode */
2733                 xmitframe_phead = get_list_head(&psta_bmc->sleep_q);
2734                 xmitframe_plist = get_next(xmitframe_phead);
2735
2736                 while (xmitframe_phead != xmitframe_plist) {
2737                         pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
2738
2739                         xmitframe_plist = get_next(xmitframe_plist);
2740
2741                         list_del_init(&pxmitframe->list);
2742
2743                         psta_bmc->sleepq_len--;
2744                         if (psta_bmc->sleepq_len > 0)
2745                                 pxmitframe->attrib.mdata = 1;
2746                         else
2747                                 pxmitframe->attrib.mdata = 0;
2748
2749
2750                         pxmitframe->attrib.triggered = 1;
2751 /*
2752                         spin_unlock_bh(&psta_bmc->sleep_q.lock);
2753                         if (rtw_hal_xmit(padapter, pxmitframe) == true)
2754                         {
2755                                 rtw_os_xmit_complete(padapter, pxmitframe);
2756                         }
2757                         spin_lock_bh(&psta_bmc->sleep_q.lock);
2758
2759 */
2760                         rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
2761
2762                 }
2763
2764                 if (psta_bmc->sleepq_len == 0) {
2765                         if (pstapriv->tim_bitmap & BIT(0)) {
2766                                 /* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
2767                                 /* upate BCN for TIM IE */
2768                                 /* update_BCNTIM(padapter); */
2769                                 update_mask |= BIT(1);
2770                         }
2771                         pstapriv->tim_bitmap &= ~BIT(0);
2772                         pstapriv->sta_dz_bitmap &= ~BIT(0);
2773                 }
2774
2775         }
2776
2777 _exit:
2778
2779         /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
2780         spin_unlock_bh(&pxmitpriv->lock);
2781
2782         if (update_mask)
2783                 /* update_BCNTIM(padapter); */
2784                 /* printk("%s => call update_beacon\n", __func__); */
2785                 update_beacon(padapter, _TIM_IE_, NULL, true);
2786
2787 }
2788
2789 void xmit_delivery_enabled_frames(struct adapter *padapter, struct sta_info *psta)
2790 {
2791         u8 wmmps_ac = 0;
2792         struct list_head        *xmitframe_plist, *xmitframe_phead;
2793         struct xmit_frame *pxmitframe = NULL;
2794         struct sta_priv *pstapriv = &padapter->stapriv;
2795         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2796
2797
2798         /* spin_lock_bh(&psta->sleep_q.lock); */
2799         spin_lock_bh(&pxmitpriv->lock);
2800
2801         xmitframe_phead = get_list_head(&psta->sleep_q);
2802         xmitframe_plist = get_next(xmitframe_phead);
2803
2804         while (xmitframe_phead != xmitframe_plist) {
2805                 pxmitframe = LIST_CONTAINOR(xmitframe_plist, struct xmit_frame, list);
2806
2807                 xmitframe_plist = get_next(xmitframe_plist);
2808
2809                 switch (pxmitframe->attrib.priority) {
2810                 case 1:
2811                 case 2:
2812                         wmmps_ac = psta->uapsd_bk&BIT(1);
2813                         break;
2814                 case 4:
2815                 case 5:
2816                         wmmps_ac = psta->uapsd_vi&BIT(1);
2817                         break;
2818                 case 6:
2819                 case 7:
2820                         wmmps_ac = psta->uapsd_vo&BIT(1);
2821                         break;
2822                 case 0:
2823                 case 3:
2824                 default:
2825                         wmmps_ac = psta->uapsd_be&BIT(1);
2826                         break;
2827                 }
2828
2829                 if (!wmmps_ac)
2830                         continue;
2831
2832                 list_del_init(&pxmitframe->list);
2833
2834                 psta->sleepq_len--;
2835                 psta->sleepq_ac_len--;
2836
2837                 if (psta->sleepq_ac_len > 0) {
2838                         pxmitframe->attrib.mdata = 1;
2839                         pxmitframe->attrib.eosp = 0;
2840                 } else {
2841                         pxmitframe->attrib.mdata = 0;
2842                         pxmitframe->attrib.eosp = 1;
2843                 }
2844
2845                 pxmitframe->attrib.triggered = 1;
2846                 rtw_hal_xmitframe_enqueue(padapter, pxmitframe);
2847
2848                 if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) && (wmmps_ac)) {
2849                         pstapriv->tim_bitmap &= ~BIT(psta->aid);
2850
2851                         /* DBG_871X("wakeup to xmit, qlen == 0, update_BCNTIM, tim =%x\n", pstapriv->tim_bitmap); */
2852                         /* upate BCN for TIM IE */
2853                         /* update_BCNTIM(padapter); */
2854                         update_beacon(padapter, _TIM_IE_, NULL, true);
2855                         /* update_mask = BIT(0); */
2856                 }
2857
2858         }
2859
2860         /* spin_unlock_bh(&psta->sleep_q.lock); */
2861         spin_unlock_bh(&pxmitpriv->lock);
2862 }
2863
2864 void enqueue_pending_xmitbuf(
2865         struct xmit_priv *pxmitpriv,
2866         struct xmit_buf *pxmitbuf)
2867 {
2868         struct __queue *pqueue;
2869         struct adapter *pri_adapter = pxmitpriv->adapter;
2870
2871         pqueue = &pxmitpriv->pending_xmitbuf_queue;
2872
2873         spin_lock_bh(&pqueue->lock);
2874         list_del_init(&pxmitbuf->list);
2875         list_add_tail(&pxmitbuf->list, get_list_head(pqueue));
2876         spin_unlock_bh(&pqueue->lock);
2877
2878         complete(&(pri_adapter->xmitpriv.xmit_comp));
2879 }
2880
2881 void enqueue_pending_xmitbuf_to_head(
2882         struct xmit_priv *pxmitpriv,
2883         struct xmit_buf *pxmitbuf)
2884 {
2885         struct __queue *pqueue;
2886
2887         pqueue = &pxmitpriv->pending_xmitbuf_queue;
2888
2889         spin_lock_bh(&pqueue->lock);
2890         list_del_init(&pxmitbuf->list);
2891         list_add(&pxmitbuf->list, get_list_head(pqueue));
2892         spin_unlock_bh(&pqueue->lock);
2893 }
2894
2895 struct xmit_buf *dequeue_pending_xmitbuf(
2896         struct xmit_priv *pxmitpriv)
2897 {
2898         struct xmit_buf *pxmitbuf;
2899         struct __queue *pqueue;
2900
2901
2902         pxmitbuf = NULL;
2903         pqueue = &pxmitpriv->pending_xmitbuf_queue;
2904
2905         spin_lock_bh(&pqueue->lock);
2906
2907         if (!list_empty(&pqueue->queue)) {
2908                 struct list_head *plist, *phead;
2909
2910                 phead = get_list_head(pqueue);
2911                 plist = get_next(phead);
2912                 pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
2913                 list_del_init(&pxmitbuf->list);
2914         }
2915
2916         spin_unlock_bh(&pqueue->lock);
2917
2918         return pxmitbuf;
2919 }
2920
2921 struct xmit_buf *dequeue_pending_xmitbuf_under_survey(
2922         struct xmit_priv *pxmitpriv)
2923 {
2924         struct xmit_buf *pxmitbuf;
2925         struct __queue *pqueue;
2926
2927
2928         pxmitbuf = NULL;
2929         pqueue = &pxmitpriv->pending_xmitbuf_queue;
2930
2931         spin_lock_bh(&pqueue->lock);
2932
2933         if (!list_empty(&pqueue->queue)) {
2934                 struct list_head *plist, *phead;
2935                 u8 type;
2936
2937                 phead = get_list_head(pqueue);
2938                 plist = phead;
2939                 do {
2940                         plist = get_next(plist);
2941                         if (plist == phead)
2942                                 break;
2943
2944                         pxmitbuf = LIST_CONTAINOR(plist, struct xmit_buf, list);
2945
2946                         type = GetFrameSubType(pxmitbuf->pbuf + TXDESC_OFFSET);
2947
2948                         if ((type == WIFI_PROBEREQ) ||
2949                                 (type == WIFI_DATA_NULL) ||
2950                                 (type == WIFI_QOS_DATA_NULL)) {
2951                                 list_del_init(&pxmitbuf->list);
2952                                 break;
2953                         }
2954                         pxmitbuf = NULL;
2955                 } while (1);
2956         }
2957
2958         spin_unlock_bh(&pqueue->lock);
2959
2960         return pxmitbuf;
2961 }
2962
2963 sint check_pending_xmitbuf(
2964         struct xmit_priv *pxmitpriv)
2965 {
2966         struct __queue *pqueue;
2967         sint    ret = false;
2968
2969         pqueue = &pxmitpriv->pending_xmitbuf_queue;
2970
2971         spin_lock_bh(&pqueue->lock);
2972
2973         if (!list_empty(&pqueue->queue))
2974                 ret = true;
2975
2976         spin_unlock_bh(&pqueue->lock);
2977
2978         return ret;
2979 }
2980
2981 int rtw_xmit_thread(void *context)
2982 {
2983         s32 err;
2984         struct adapter *padapter;
2985
2986
2987         err = _SUCCESS;
2988         padapter = context;
2989
2990         thread_enter("RTW_XMIT_THREAD");
2991
2992         do {
2993                 err = rtw_hal_xmit_thread_handler(padapter);
2994                 flush_signals_thread();
2995         } while (_SUCCESS == err);
2996
2997         complete(&padapter->xmitpriv.terminate_xmitthread_comp);
2998
2999         thread_exit();
3000 }
3001
3002 void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms)
3003 {
3004         sctx->timeout_ms = timeout_ms;
3005         sctx->submit_time = jiffies;
3006         init_completion(&sctx->done);
3007         sctx->status = RTW_SCTX_SUBMITTED;
3008 }
3009
3010 int rtw_sctx_wait(struct submit_ctx *sctx, const char *msg)
3011 {
3012         int ret = _FAIL;
3013         unsigned long expire;
3014         int status = 0;
3015
3016         expire = sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) : MAX_SCHEDULE_TIMEOUT;
3017         if (!wait_for_completion_timeout(&sctx->done, expire)) {
3018                 /* timeout, do something?? */
3019                 status = RTW_SCTX_DONE_TIMEOUT;
3020                 DBG_871X("%s timeout: %s\n", __func__, msg);
3021         } else {
3022                 status = sctx->status;
3023         }
3024
3025         if (status == RTW_SCTX_DONE_SUCCESS) {
3026                 ret = _SUCCESS;
3027         }
3028
3029         return ret;
3030 }
3031
3032 static bool rtw_sctx_chk_warning_status(int status)
3033 {
3034         switch (status) {
3035         case RTW_SCTX_DONE_UNKNOWN:
3036         case RTW_SCTX_DONE_BUF_ALLOC:
3037         case RTW_SCTX_DONE_BUF_FREE:
3038
3039         case RTW_SCTX_DONE_DRV_STOP:
3040         case RTW_SCTX_DONE_DEV_REMOVE:
3041                 return true;
3042         default:
3043                 return false;
3044         }
3045 }
3046
3047 void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
3048 {
3049         if (*sctx) {
3050                 if (rtw_sctx_chk_warning_status(status))
3051                         DBG_871X("%s status:%d\n", __func__, status);
3052                 (*sctx)->status = status;
3053                 complete(&((*sctx)->done));
3054                 *sctx = NULL;
3055         }
3056 }
3057
3058 void rtw_sctx_done(struct submit_ctx **sctx)
3059 {
3060         rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
3061 }
3062
3063 int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
3064 {
3065         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
3066
3067         pack_tx_ops->submit_time = jiffies;
3068         pack_tx_ops->timeout_ms = timeout_ms;
3069         pack_tx_ops->status = RTW_SCTX_SUBMITTED;
3070
3071         return rtw_sctx_wait(pack_tx_ops, __func__);
3072 }
3073
3074 void rtw_ack_tx_done(struct xmit_priv *pxmitpriv, int status)
3075 {
3076         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
3077
3078         if (pxmitpriv->ack_tx) {
3079                 rtw_sctx_done_err(&pack_tx_ops, status);
3080         } else {
3081                 DBG_871X("%s ack_tx not set\n", __func__);
3082         }
3083 }