drivers/staging/rtl8723au/core/rtw_xmit.c

   1 /******************************************************************************
   2  *
   3  * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of version 2 of the GNU General Public License as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  12  * more details.
  13  *
  14  ******************************************************************************/
  15 #define _RTW_XMIT_C_
  16
  17 #include <osdep_service.h>
  18 #include <drv_types.h>
  19 #include <wifi.h>
  20 #include <osdep_intf.h>
  21 #include <linux/ip.h>
  22 #include <usb_ops.h>
  23 #include <rtl8723a_xmit.h>
  24
  25 static void _init_txservq(struct tx_servq *ptxservq)
  26 {
  27
  28         INIT_LIST_HEAD(&ptxservq->tx_pending);
  29         _rtw_init_queue23a(&ptxservq->sta_pending);
  30         ptxservq->qcnt = 0;
  31
  32 }
  33
  34 void    _rtw_init_sta_xmit_priv23a(struct sta_xmit_priv *psta_xmitpriv)
  35 {
  36
  37         spin_lock_init(&psta_xmitpriv->lock);
  38
  39         /* for (i = 0 ; i < MAX_NUMBLKS; i++) */
  40         /*      _init_txservq(&psta_xmitpriv->blk_q[i]); */
  41
  42         _init_txservq(&psta_xmitpriv->be_q);
  43         _init_txservq(&psta_xmitpriv->bk_q);
  44         _init_txservq(&psta_xmitpriv->vi_q);
  45         _init_txservq(&psta_xmitpriv->vo_q);
  46         INIT_LIST_HEAD(&psta_xmitpriv->legacy_dz);
  47         INIT_LIST_HEAD(&psta_xmitpriv->apsd);
  48
  49 }
  50
  51 int _rtw_init_xmit_priv23a(struct xmit_priv *pxmitpriv,
  52                            struct rtw_adapter *padapter)
  53 {
  54         int i;
  55         struct xmit_buf *pxmitbuf;
  56         struct xmit_frame *pxframe;
  57         int res = _SUCCESS;
  58         u32 max_xmit_extbuf_size = MAX_XMIT_EXTBUF_SZ;
  59         u32 num_xmit_extbuf = NR_XMIT_EXTBUFF;
  60
  61         spin_lock_init(&pxmitpriv->lock);
  62         spin_lock_init(&pxmitpriv->lock_sctx);
  63         sema_init(&pxmitpriv->xmit_sema, 0);
  64         sema_init(&pxmitpriv->terminate_xmitthread_sema, 0);
  65
  66         pxmitpriv->adapter = padapter;
  67
  68         _rtw_init_queue23a(&pxmitpriv->be_pending);
  69         _rtw_init_queue23a(&pxmitpriv->bk_pending);
  70         _rtw_init_queue23a(&pxmitpriv->vi_pending);
  71         _rtw_init_queue23a(&pxmitpriv->vo_pending);
  72         _rtw_init_queue23a(&pxmitpriv->bm_pending);
  73
  74         _rtw_init_queue23a(&pxmitpriv->free_xmit_queue);
  75
  76         for (i = 0; i < NR_XMITFRAME; i++) {
  77                 pxframe = kzalloc(sizeof(struct xmit_frame), GFP_KERNEL);
  78                 if (!pxframe)
  79                         break;
  80                 INIT_LIST_HEAD(&pxframe->list);
  81
  82                 pxframe->padapter = padapter;
  83                 pxframe->frame_tag = NULL_FRAMETAG;
  84
  85                 list_add_tail(&pxframe->list,
  86                               &pxmitpriv->free_xmit_queue.queue);
  87         }
  88
  89         pxmitpriv->free_xmitframe_cnt = i;
  90
  91         pxmitpriv->frag_len = MAX_FRAG_THRESHOLD;
  92
  93         /* init xmit_buf */
  94         _rtw_init_queue23a(&pxmitpriv->free_xmitbuf_queue);
  95         INIT_LIST_HEAD(&pxmitpriv->xmitbuf_list);
  96         _rtw_init_queue23a(&pxmitpriv->pending_xmitbuf_queue);
  97
  98         for (i = 0; i < NR_XMITBUFF; i++) {
  99                 pxmitbuf = kzalloc(sizeof(struct xmit_buf), GFP_KERNEL);
 100                 if (!pxmitbuf)
 101                         goto fail;
 102                 INIT_LIST_HEAD(&pxmitbuf->list);
 103                 INIT_LIST_HEAD(&pxmitbuf->list2);
 104
 105                 pxmitbuf->padapter = padapter;
 106
 107                 /* Tx buf allocation may fail sometimes, so sleep and retry. */
 108                 res = rtw_os_xmit_resource_alloc23a(padapter, pxmitbuf,
 109                                                  (MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ));
 110                 if (res == _FAIL) {
 111                         goto fail;
 112                 }
 113
 114                 list_add_tail(&pxmitbuf->list,
 115                               &pxmitpriv->free_xmitbuf_queue.queue);
 116                 list_add_tail(&pxmitbuf->list2,
 117                               &pxmitpriv->xmitbuf_list);
 118         }
 119
 120         pxmitpriv->free_xmitbuf_cnt = NR_XMITBUFF;
 121
 122         /* init xframe_ext queue,  the same count as extbuf  */
 123         _rtw_init_queue23a(&pxmitpriv->free_xframe_ext_queue);
 124
 125         for (i = 0; i < num_xmit_extbuf; i++) {
 126                 pxframe = kzalloc(sizeof(struct xmit_frame), GFP_KERNEL);
 127                 if (!pxframe)
 128                         break;
 129                 INIT_LIST_HEAD(&pxframe->list);
 130
 131                 pxframe->padapter = padapter;
 132                 pxframe->frame_tag = NULL_FRAMETAG;
 133
 134                 pxframe->pkt = NULL;
 135
 136                 pxframe->buf_addr = NULL;
 137                 pxframe->pxmitbuf = NULL;
 138
 139                 pxframe->ext_tag = 1;
 140
 141                 list_add_tail(&pxframe->list,
 142                               &pxmitpriv->free_xframe_ext_queue.queue);
 143         }
 144         pxmitpriv->free_xframe_ext_cnt = i;
 145
 146         /*  Init xmit extension buff */
 147         _rtw_init_queue23a(&pxmitpriv->free_xmit_extbuf_queue);
 148         INIT_LIST_HEAD(&pxmitpriv->xmitextbuf_list);
 149
 150         for (i = 0; i < num_xmit_extbuf; i++) {
 151                 pxmitbuf = kzalloc(sizeof(struct xmit_buf), GFP_KERNEL);
 152                 if (!pxmitbuf)
 153                         goto fail;
 154                 INIT_LIST_HEAD(&pxmitbuf->list);
 155                 INIT_LIST_HEAD(&pxmitbuf->list2);
 156
 157                 pxmitbuf->padapter = padapter;
 158
 159                 /* Tx buf allocation may fail sometimes, so sleep and retry. */
 160                 res = rtw_os_xmit_resource_alloc23a(padapter, pxmitbuf,
 161                                                  max_xmit_extbuf_size + XMITBUF_ALIGN_SZ);
 162                 if (res == _FAIL) {
 163                         goto exit;
 164                 }
 165
 166                 list_add_tail(&pxmitbuf->list,
 167                               &pxmitpriv->free_xmit_extbuf_queue.queue);
 168                 list_add_tail(&pxmitbuf->list2,
 169                               &pxmitpriv->xmitextbuf_list);
 170         }
 171
 172         pxmitpriv->free_xmit_extbuf_cnt = num_xmit_extbuf;
 173
 174         rtw_alloc_hwxmits23a(padapter);
 175         rtw_init_hwxmits23a(pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
 176
 177         for (i = 0; i < 4; i ++)
 178                 pxmitpriv->wmm_para_seq[i] = i;
 179
 180         sema_init(&pxmitpriv->tx_retevt, 0);
 181
 182         pxmitpriv->ack_tx = false;
 183         mutex_init(&pxmitpriv->ack_tx_mutex);
 184         rtw_sctx_init23a(&pxmitpriv->ack_tx_ops, 0);
 185         tasklet_init(&padapter->xmitpriv.xmit_tasklet,
 186                      (void(*)(unsigned long))rtl8723au_xmit_tasklet,
 187                      (unsigned long)padapter);
 188
 189 exit:
 190
 191         return res;
 192 fail:
 193         goto exit;
 194 }
 195
 196 void _rtw_free_xmit_priv23a(struct xmit_priv *pxmitpriv)
 197 {
 198         struct rtw_adapter *padapter = pxmitpriv->adapter;
 199         struct xmit_frame *pxframe, *ptmp;
 200         struct xmit_buf *pxmitbuf, *ptmp2;
 201
 202         list_for_each_entry_safe(pxframe, ptmp,
 203                                  &pxmitpriv->free_xmit_queue.queue, list) {
 204                 list_del_init(&pxframe->list);
 205                 rtw_os_xmit_complete23a(padapter, pxframe);
 206                 kfree(pxframe);
 207         }
 208
 209         list_for_each_entry_safe(pxmitbuf, ptmp2,
 210                                  &pxmitpriv->xmitbuf_list, list2) {
 211                 list_del_init(&pxmitbuf->list2);
 212                 rtw_os_xmit_resource_free23a(padapter, pxmitbuf);
 213                 kfree(pxmitbuf);
 214         }
 215
 216         /* free xframe_ext queue,  the same count as extbuf  */
 217         list_for_each_entry_safe(pxframe, ptmp,
 218                                  &pxmitpriv->free_xframe_ext_queue.queue,
 219                                  list) {
 220                 list_del_init(&pxframe->list);
 221                 rtw_os_xmit_complete23a(padapter, pxframe);
 222                 kfree(pxframe);
 223         }
 224
 225         /*  free xmit extension buff */
 226         list_for_each_entry_safe(pxmitbuf, ptmp2,
 227                                  &pxmitpriv->xmitextbuf_list, list2) {
 228                 list_del_init(&pxmitbuf->list2);
 229                 rtw_os_xmit_resource_free23a(padapter, pxmitbuf);
 230                 kfree(pxmitbuf);
 231         }
 232
 233         rtw_free_hwxmits23a(padapter);
 234         mutex_destroy(&pxmitpriv->ack_tx_mutex);
 235 }
 236
 237 static void update_attrib_vcs_info(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
 238 {
 239         u32     sz;
 240         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
 241         struct sta_info *psta = pattrib->psta;
 242         struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 243         struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 244
 245         if (pattrib->psta) {
 246                 psta = pattrib->psta;
 247         } else {
 248                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 249                 psta = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]);
 250         }
 251
 252         if (psta == NULL) {
 253                 DBG_8723A("%s, psta == NUL\n", __func__);
 254                 return;
 255         }
 256
 257         if (!(psta->state &_FW_LINKED)) {
 258                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
 259                 return;
 260         }
 261
 262         if (pattrib->nr_frags != 1)
 263                 sz = padapter->xmitpriv.frag_len;
 264         else /* no frag */
 265                 sz = pattrib->last_txcmdsz;
 266
 267         /*  (1) RTS_Threshold is compared to the MPDU, not MSDU. */
 268         /*  (2) If there are more than one frag in  this MSDU, only the first frag uses protection frame. */
 269         /*              Other fragments are protected by previous fragment. */
 270         /*              So we only need to check the length of first fragment. */
 271         if (pmlmeext->cur_wireless_mode < WIRELESS_11_24N  || padapter->registrypriv.wifi_spec) {
 272                 if (sz > padapter->registrypriv.rts_thresh) {
 273                         pattrib->vcs_mode = RTS_CTS;
 274                 } else {
 275                         if (psta->rtsen)
 276                                 pattrib->vcs_mode = RTS_CTS;
 277                         else if (psta->cts2self)
 278                                 pattrib->vcs_mode = CTS_TO_SELF;
 279                         else
 280                                 pattrib->vcs_mode = NONE_VCS;
 281                 }
 282         } else {
 283                 while (true) {
 284                         /* IOT action */
 285                         if (pmlmeinfo->assoc_AP_vendor == HT_IOT_PEER_ATHEROS &&
 286                             pattrib->ampdu_en &&
 287                             padapter->securitypriv.dot11PrivacyAlgrthm ==
 288                             WLAN_CIPHER_SUITE_CCMP) {
 289                                 pattrib->vcs_mode = CTS_TO_SELF;
 290                                 break;
 291                         }
 292
 293                         /* check ERP protection */
 294                         if (psta->rtsen || psta->cts2self) {
 295                                 if (psta->rtsen)
 296                                         pattrib->vcs_mode = RTS_CTS;
 297                                 else if (psta->cts2self)
 298                                         pattrib->vcs_mode = CTS_TO_SELF;
 299
 300                                 break;
 301                         }
 302
 303                         /* check HT op mode */
 304                         if (pattrib->ht_en) {
 305                                 u8 HTOpMode = pmlmeinfo->HT_protection;
 306
 307                                 if ((pmlmeext->cur_bwmode && (HTOpMode == 2 || HTOpMode == 3)) ||
 308                                     (!pmlmeext->cur_bwmode && HTOpMode == 3)) {
 309                                         pattrib->vcs_mode = RTS_CTS;
 310                                         break;
 311                                 }
 312                         }
 313
 314                         /* check rts */
 315                         if (sz > padapter->registrypriv.rts_thresh) {
 316                                 pattrib->vcs_mode = RTS_CTS;
 317                                 break;
 318                         }
 319
 320                         /* to do list: check MIMO power save condition. */
 321
 322                         /* check AMPDU aggregation for TXOP */
 323                         if (pattrib->ampdu_en) {
 324                                 pattrib->vcs_mode = RTS_CTS;
 325                                 break;
 326                         }
 327
 328                         pattrib->vcs_mode = NONE_VCS;
 329                         break;
 330                 }
 331         }
 332 }
 333
 334 static void update_attrib_phy_info(struct pkt_attrib *pattrib, struct sta_info *psta)
 335 {
 336         /*if (psta->rtsen)
 337                 pattrib->vcs_mode = RTS_CTS;
 338         else if (psta->cts2self)
 339                 pattrib->vcs_mode = CTS_TO_SELF;
 340         else
 341                 pattrib->vcs_mode = NONE_VCS;*/
 342
 343         pattrib->mdata = 0;
 344         pattrib->eosp = 0;
 345         pattrib->triggered = 0;
 346
 347         /* qos_en, ht_en, init rate, , bw, ch_offset, sgi */
 348         pattrib->qos_en = psta->qos_option;
 349
 350         pattrib->raid = psta->raid;
 351         pattrib->ht_en = psta->htpriv.ht_option;
 352         pattrib->bwmode = psta->htpriv.bwmode;
 353         pattrib->ch_offset = psta->htpriv.ch_offset;
 354         pattrib->sgi = psta->htpriv.sgi;
 355         pattrib->ampdu_en = false;
 356
 357         pattrib->retry_ctrl = false;
 358 }
 359
 360 u8 qos_acm23a(u8 acm_mask, u8 priority)
 361 {
 362         u8 change_priority = priority;
 363
 364         switch (priority) {
 365         case 0:
 366         case 3:
 367                 if (acm_mask & BIT(1))
 368                         change_priority = 1;
 369                 break;
 370         case 1:
 371         case 2:
 372                 break;
 373         case 4:
 374         case 5:
 375                 if (acm_mask & BIT(2))
 376                         change_priority = 0;
 377                 break;
 378         case 6:
 379         case 7:
 380                 if (acm_mask & BIT(3))
 381                         change_priority = 5;
 382                 break;
 383         default:
 384                 DBG_8723A("qos_acm23a(): invalid pattrib->priority: %d!!!\n",
 385                           priority);
 386                 change_priority = 0;
 387                 break;
 388         }
 389
 390         return change_priority;
 391 }
 392
 393 static void set_qos(struct sk_buff *skb, struct pkt_attrib *pattrib)
 394 {
 395         u8 *pframe = skb->data;
 396         struct iphdr *ip_hdr;
 397         u8 UserPriority = 0;
 398
 399         /*  get UserPriority from IP hdr */
 400         if (pattrib->ether_type == ETH_P_IP) {
 401                 ip_hdr = (struct iphdr *)(pframe + ETH_HLEN);
 402                 UserPriority = ip_hdr->tos >> 5;
 403         } else if (pattrib->ether_type == ETH_P_PAE) {
 404                 /*  "When priority processing of data frames is supported, */
 405                 /*  a STA's SME should send EAPOL-Key frames at the highest
 406                     priority." */
 407                 UserPriority = 7;
 408         }
 409
 410         pattrib->priority = UserPriority;
 411         pattrib->hdrlen = sizeof(struct ieee80211_qos_hdr);
 412         pattrib->type = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA;
 413 }
 414
 415 static int update_attrib(struct rtw_adapter *padapter,
 416                          struct sk_buff *skb, struct pkt_attrib *pattrib)
 417 {
 418         struct sta_info *psta = NULL;
 419         int bmcast;
 420         struct sta_priv *pstapriv = &padapter->stapriv;
 421         struct security_priv *psecuritypriv = &padapter->securitypriv;
 422         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 423         int res = _SUCCESS;
 424         struct ethhdr *ehdr = (struct ethhdr *) skb->data;
 425
 426         pattrib->ether_type = ntohs(ehdr->h_proto);
 427
 428         ether_addr_copy(pattrib->dst, ehdr->h_dest);
 429         ether_addr_copy(pattrib->src, ehdr->h_source);
 430
 431         pattrib->pctrl = 0;
 432
 433         if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
 434             check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
 435                 ether_addr_copy(pattrib->ra, pattrib->dst);
 436                 ether_addr_copy(pattrib->ta, pattrib->src);
 437         } else if (check_fwstate(pmlmepriv, WIFI_STATION_STATE)) {
 438                 ether_addr_copy(pattrib->ra, get_bssid(pmlmepriv));
 439                 ether_addr_copy(pattrib->ta, pattrib->src);
 440         } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 441                 ether_addr_copy(pattrib->ra, pattrib->dst);
 442                 ether_addr_copy(pattrib->ta, get_bssid(pmlmepriv));
 443         }
 444
 445         pattrib->pktlen = skb->len - ETH_HLEN;
 446
 447         if (pattrib->ether_type == ETH_P_IP) {
 448                 /*  The following is for DHCP and ARP packet, we use cck1M
 449                     to tx these packets and let LPS awake some time */
 450                 /*  to prevent DHCP protocol fail */
 451                 pattrib->dhcp_pkt = 0;
 452                 /* MINIMUM_DHCP_PACKET_SIZE) { */
 453                 if (pattrib->pktlen > 282 + 24) {
 454                         if (pattrib->ether_type == ETH_P_IP) {/*  IP header */
 455                                 u8 *pframe = skb->data;
 456
 457                                 pframe += ETH_HLEN;
 458
 459                                 if ((pframe[21] == 68 && pframe[23] == 67) ||
 460                                     (pframe[21] == 67 && pframe[23] == 68)) {
 461                                         /*  68 : UDP BOOTP client */
 462                                         /*  67 : UDP BOOTP server */
 463                                         RT_TRACE(_module_rtl871x_xmit_c_,
 464                                                  _drv_err_,
 465                                                  "======================update_attrib: get DHCP Packet\n");
 466                                         pattrib->dhcp_pkt = 1;
 467                                 }
 468                         }
 469                 }
 470         } else if (pattrib->ether_type == ETH_P_PAE) {
 471                 DBG_8723A_LEVEL(_drv_always_, "send eapol packet\n");
 472         }
 473
 474         if ((pattrib->ether_type == ETH_P_PAE) || (pattrib->dhcp_pkt == 1)) {
 475                 rtw_set_scan_deny(padapter, 3000);
 476         }
 477
 478         /*  If EAPOL , ARP , OR DHCP packet, driver must be in active mode. */
 479         if ((pattrib->ether_type == ETH_P_ARP) ||
 480             (pattrib->ether_type == ETH_P_PAE) || (pattrib->dhcp_pkt == 1)) {
 481                 rtw_lps_ctrl_wk_cmd23a(padapter, LPS_CTRL_SPECIAL_PACKET, 1);
 482         }
 483
 484         bmcast = is_multicast_ether_addr(pattrib->ra);
 485
 486         /*  get sta_info */
 487         if (bmcast) {
 488                 psta = rtw_get_bcmc_stainfo23a(padapter);
 489         } else {
 490                 psta = rtw_get_stainfo23a(pstapriv, pattrib->ra);
 491                 if (psta == NULL) { /*  if we cannot get psta => drrp the pkt */
 492                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_,
 493                                  "update_attrib => get sta_info fail, ra:%pM\n",
 494                                  pattrib->ra);
 495                         res = _FAIL;
 496                         goto exit;
 497                 } else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) &&
 498                            (!(psta->state & _FW_LINKED))) {
 499                         res = _FAIL;
 500                         goto exit;
 501                 }
 502         }
 503
 504         if (psta) {
 505                 pattrib->mac_id = psta->mac_id;
 506                 /* DBG_8723A("%s ==> mac_id(%d)\n", __func__, pattrib->mac_id); */
 507                 pattrib->psta = psta;
 508         } else {
 509                 /*  if we cannot get psta => drop the pkt */
 510                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_,
 511                          "update_attrib => get sta_info fail, ra:%pM\n",
 512                          pattrib->ra);
 513                 res = _FAIL;
 514                 goto exit;
 515         }
 516
 517         pattrib->ack_policy = 0;
 518         /*  get ether_hdr_len */
 519
 520         /* pattrib->ether_type == 0x8100) ? (14 + 4): 14; vlan tag */
 521         pattrib->pkt_hdrlen = ETH_HLEN;
 522
 523         pattrib->hdrlen = sizeof(struct ieee80211_hdr_3addr);
 524         pattrib->type = IEEE80211_FTYPE_DATA;
 525         pattrib->priority = 0;
 526
 527         if (check_fwstate(pmlmepriv, WIFI_AP_STATE | WIFI_ADHOC_STATE |
 528                           WIFI_ADHOC_MASTER_STATE)) {
 529                 if (psta->qos_option)
 530                         set_qos(skb, pattrib);
 531         } else {
 532                 if (pmlmepriv->qos_option) {
 533                         set_qos(skb, pattrib);
 534
 535                         if (pmlmepriv->acm_mask != 0) {
 536                                 pattrib->priority = qos_acm23a(pmlmepriv->acm_mask,
 537                                                             pattrib->priority);
 538                         }
 539                 }
 540         }
 541
 542         if (psta->ieee8021x_blocked == true) {
 543                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 544                          "psta->ieee8021x_blocked == true\n");
 545
 546                 pattrib->encrypt = 0;
 547
 548                 if ((pattrib->ether_type != ETH_P_PAE) &&
 549                     !check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
 550                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 551                                  "psta->ieee8021x_blocked == true,  pattrib->ether_type(%.4x) != 0x888e\n",
 552                                  pattrib->ether_type);
 553                         res = _FAIL;
 554                         goto exit;
 555                 }
 556         } else {
 557                 GET_ENCRY_ALGO(psecuritypriv, psta, pattrib->encrypt, bmcast);
 558
 559                 switch (psecuritypriv->dot11AuthAlgrthm) {
 560                 case dot11AuthAlgrthm_Open:
 561                 case dot11AuthAlgrthm_Shared:
 562                 case dot11AuthAlgrthm_Auto:
 563                         pattrib->key_idx =
 564                                 (u8)psecuritypriv->dot11PrivacyKeyIndex;
 565                         break;
 566                 case dot11AuthAlgrthm_8021X:
 567                         if (bmcast)
 568                                 pattrib->key_idx =
 569                                         (u8)psecuritypriv->dot118021XGrpKeyid;
 570                         else
 571                                 pattrib->key_idx = 0;
 572                         break;
 573                 default:
 574                         pattrib->key_idx = 0;
 575                         break;
 576                 }
 577
 578         }
 579
 580         switch (pattrib->encrypt) {
 581         case WLAN_CIPHER_SUITE_WEP40:
 582         case WLAN_CIPHER_SUITE_WEP104:
 583                 pattrib->iv_len = IEEE80211_WEP_IV_LEN;
 584                 pattrib->icv_len = IEEE80211_WEP_ICV_LEN;
 585                 break;
 586
 587         case WLAN_CIPHER_SUITE_TKIP:
 588                 pattrib->iv_len = IEEE80211_TKIP_IV_LEN;
 589                 pattrib->icv_len = IEEE80211_TKIP_ICV_LEN;
 590
 591                 if (!padapter->securitypriv.busetkipkey) {
 592                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 593                                  "padapter->securitypriv.busetkipkey(%d) == false drop packet\n",
 594                                  padapter->securitypriv.busetkipkey);
 595                         res = _FAIL;
 596                         goto exit;
 597                 }
 598
 599                 break;
 600         case WLAN_CIPHER_SUITE_CCMP:
 601                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 602                          "pattrib->encrypt =%d (WLAN_CIPHER_SUITE_CCMP)\n",
 603                          pattrib->encrypt);
 604                 pattrib->iv_len = IEEE80211_CCMP_HDR_LEN;
 605                 pattrib->icv_len = IEEE80211_CCMP_MIC_LEN;
 606                 break;
 607
 608         default:
 609                 pattrib->iv_len = 0;
 610                 pattrib->icv_len = 0;
 611                 break;
 612         }
 613
 614         RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 615                  "update_attrib: encrypt =%d\n", pattrib->encrypt);
 616
 617         if (pattrib->encrypt && !psecuritypriv->hw_decrypted) {
 618                 pattrib->bswenc = true;
 619                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 620                          "update_attrib: encrypt =%d bswenc = true\n",
 621                          pattrib->encrypt);
 622         } else {
 623                 pattrib->bswenc = false;
 624                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 625                          "update_attrib: bswenc = false\n");
 626         }
 627         update_attrib_phy_info(pattrib, psta);
 628
 629 exit:
 630
 631         return res;
 632 }
 633
 634 static int xmitframe_addmic(struct rtw_adapter *padapter,
 635                             struct xmit_frame *pxmitframe) {
 636         struct mic_data micdata;
 637         struct sta_info *stainfo;
 638         struct pkt_attrib *pattrib = &pxmitframe->attrib;
 639         struct security_priv *psecuritypriv = &padapter->securitypriv;
 640         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 641         int curfragnum, length;
 642         u8 *pframe, *payload, mic[8];
 643         u8 priority[4]= {0x0, 0x0, 0x0, 0x0};
 644         u8 hw_hdr_offset = 0;
 645         int bmcst = is_multicast_ether_addr(pattrib->ra);
 646
 647         if (pattrib->psta) {
 648                 stainfo = pattrib->psta;
 649         } else {
 650                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 651                 stainfo = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]);
 652         }
 653
 654         if (!stainfo) {
 655                 DBG_8723A("%s, psta == NUL\n", __func__);
 656                 return _FAIL;
 657         }
 658
 659         if (!(stainfo->state &_FW_LINKED)) {
 660                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
 661                           __func__, stainfo->state);
 662                 return _FAIL;
 663         }
 664
 665         hw_hdr_offset = TXDESC_OFFSET;
 666
 667         if (pattrib->encrypt == WLAN_CIPHER_SUITE_TKIP) {
 668                 /* encode mic code */
 669                 if (stainfo) {
 670                         u8 null_key[16]={0x0, 0x0, 0x0, 0x0,
 671                                          0x0, 0x0, 0x0, 0x0,
 672                                          0x0, 0x0, 0x0, 0x0,
 673                                          0x0, 0x0, 0x0, 0x0};
 674
 675                         pframe = pxmitframe->buf_addr + hw_hdr_offset;
 676
 677                         if (bmcst) {
 678                                 if (!memcmp(psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey, null_key, 16)) {
 679                                         return _FAIL;
 680                                 }
 681                                 /* start to calculate the mic code */
 682                                 rtw_secmicsetkey23a(&micdata, psecuritypriv->dot118021XGrptxmickey[psecuritypriv->dot118021XGrpKeyid].skey);
 683                         } else {
 684                                 if (!memcmp(&stainfo->dot11tkiptxmickey.skey[0],
 685                                             null_key, 16)) {
 686                                         return _FAIL;
 687                                 }
 688                                 /* start to calculate the mic code */
 689                                 rtw_secmicsetkey23a(&micdata, &stainfo->dot11tkiptxmickey.skey[0]);
 690                         }
 691
 692                         if (pframe[1] & 1) {   /* ToDS == 1 */
 693                                 /* DA */
 694                                 rtw_secmicappend23a(&micdata, &pframe[16], 6);
 695                                 if (pframe[1] & 2)  /* From Ds == 1 */
 696                                         rtw_secmicappend23a(&micdata,
 697                                                          &pframe[24], 6);
 698                                 else
 699                                         rtw_secmicappend23a(&micdata,
 700                                                          &pframe[10], 6);
 701                         } else {        /* ToDS == 0 */
 702                                 /* DA */
 703                                 rtw_secmicappend23a(&micdata, &pframe[4], 6);
 704                                 if (pframe[1] & 2)  /* From Ds == 1 */
 705                                         rtw_secmicappend23a(&micdata,
 706                                                          &pframe[16], 6);
 707                                 else
 708                                         rtw_secmicappend23a(&micdata,
 709                                                          &pframe[10], 6);
 710                         }
 711
 712                         /* if (pmlmepriv->qos_option == 1) */
 713                         if (pattrib->qos_en)
 714                                 priority[0] = (u8)pxmitframe->attrib.priority;
 715
 716                         rtw_secmicappend23a(&micdata, &priority[0], 4);
 717
 718                         payload = pframe;
 719
 720                         for (curfragnum = 0; curfragnum < pattrib->nr_frags;
 721                              curfragnum++) {
 722                                 payload = PTR_ALIGN(payload, 4);
 723                                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 724                                          "=== curfragnum =%d, pframe = 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x, 0x%.2x,!!!\n",
 725                                          curfragnum, *payload, *(payload + 1),
 726                                          *(payload + 2), *(payload + 3),
 727                                          *(payload + 4), *(payload + 5),
 728                                          *(payload + 6), *(payload + 7));
 729
 730                                 payload = payload + pattrib->hdrlen +
 731                                         pattrib->iv_len;
 732                                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 733                                          "curfragnum =%d pattrib->hdrlen =%d pattrib->iv_len =%d\n",
 734                                          curfragnum,
 735                                          pattrib->hdrlen, pattrib->iv_len);
 736                                 if ((curfragnum + 1) == pattrib->nr_frags) {
 737                                         length = pattrib->last_txcmdsz -
 738                                                 pattrib->hdrlen -
 739                                                 pattrib->iv_len -
 740                                                 ((pattrib->bswenc) ?
 741                                                  pattrib->icv_len : 0);
 742                                         rtw_secmicappend23a(&micdata, payload,
 743                                                          length);
 744                                         payload = payload + length;
 745                                 } else {
 746                                         length = pxmitpriv->frag_len -
 747                                                 pattrib->hdrlen -
 748                                                 pattrib->iv_len -
 749                                                 ((pattrib->bswenc) ?
 750                                                  pattrib->icv_len : 0);
 751                                         rtw_secmicappend23a(&micdata, payload,
 752                                                          length);
 753                                         payload = payload + length +
 754                                                 pattrib->icv_len;
 755                                         RT_TRACE(_module_rtl871x_xmit_c_,
 756                                                  _drv_err_,
 757                                                  "curfragnum =%d length =%d pattrib->icv_len =%d\n",
 758                                                  curfragnum, length,
 759                                                  pattrib->icv_len);
 760                                 }
 761                         }
 762                         rtw_secgetmic23a(&micdata, &mic[0]);
 763                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 764                                  "xmitframe_addmic: before add mic code!!\n");
 765                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 766                                  "xmitframe_addmic: pattrib->last_txcmdsz =%d!!!\n",
 767                                  pattrib->last_txcmdsz);
 768                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 769                                  "xmitframe_addmic: mic[0]= 0x%.2x , mic[1]=0x%.2x , mic[2]= 0x%.2x , mic[3]= 0x%.2x\nmic[4]= 0x%.2x , mic[5]= 0x%.2x , mic[6]= 0x%.2x , mic[7]= 0x%.2x !!!!\n",
 770                                  mic[0], mic[1], mic[2], mic[3],
 771                                  mic[4], mic[5], mic[6], mic[7]);
 772                         /* add mic code  and add the mic code length
 773                            in last_txcmdsz */
 774
 775                         memcpy(payload, &mic[0], 8);
 776                         pattrib->last_txcmdsz += 8;
 777
 778                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 779                                  "======== last pkt ========\n");
 780                         payload = payload - pattrib->last_txcmdsz + 8;
 781                         for (curfragnum = 0; curfragnum < pattrib->last_txcmdsz;
 782                              curfragnum = curfragnum + 8) {
 783                                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
 784                                          "%.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x,  %.2x\n",
 785                                          *(payload + curfragnum),
 786                                          *(payload + curfragnum + 1),
 787                                          *(payload + curfragnum + 2),
 788                                          *(payload + curfragnum + 3),
 789                                          *(payload + curfragnum + 4),
 790                                          *(payload + curfragnum + 5),
 791                                          *(payload + curfragnum + 6),
 792                                          *(payload + curfragnum + 7));
 793                         }
 794                 } else {
 795                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 796                                  "xmitframe_addmic: rtw_get_stainfo23a ==NULL!!!\n");
 797                 }
 798         }
 799
 800         return _SUCCESS;
 801 }
 802
 803 static int xmitframe_swencrypt(struct rtw_adapter *padapter,
 804                                struct xmit_frame *pxmitframe)
 805 {
 806         struct pkt_attrib *pattrib = &pxmitframe->attrib;
 807
 808         /* if ((psecuritypriv->sw_encrypt)||(pattrib->bswenc)) */
 809         if (pattrib->bswenc) {
 810                 /* DBG_8723A("start xmitframe_swencrypt\n"); */
 811                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_alert_,
 812                          "### xmitframe_swencrypt\n");
 813                 switch (pattrib->encrypt) {
 814                 case WLAN_CIPHER_SUITE_WEP40:
 815                 case WLAN_CIPHER_SUITE_WEP104:
 816                         rtw_wep_encrypt23a(padapter, pxmitframe);
 817                         break;
 818                 case WLAN_CIPHER_SUITE_TKIP:
 819                         rtw_tkip_encrypt23a(padapter, pxmitframe);
 820                         break;
 821                 case WLAN_CIPHER_SUITE_CCMP:
 822                         rtw_aes_encrypt23a(padapter, pxmitframe);
 823                         break;
 824                 default:
 825                                 break;
 826                 }
 827
 828         } else {
 829                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
 830                          "### xmitframe_hwencrypt\n");
 831         }
 832
 833         return _SUCCESS;
 834 }
 835
 836 static int rtw_make_wlanhdr(struct rtw_adapter *padapter, u8 *hdr,
 837                             struct pkt_attrib *pattrib)
 838 {
 839         struct ieee80211_hdr *pwlanhdr = (struct ieee80211_hdr *)hdr;
 840         struct ieee80211_qos_hdr *qoshdr;
 841         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 842         u8 qos_option = false;
 843         int res = _SUCCESS;
 844
 845         struct sta_info *psta;
 846
 847         int bmcst = is_multicast_ether_addr(pattrib->ra);
 848
 849         if (pattrib->psta) {
 850                 psta = pattrib->psta;
 851         } else {
 852                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 853                 if (bmcst) {
 854                         psta = rtw_get_bcmc_stainfo23a(padapter);
 855                 } else {
 856                         psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra);
 857                 }
 858         }
 859
 860         if (psta == NULL) {
 861                 DBG_8723A("%s, psta == NUL\n", __func__);
 862                 return _FAIL;
 863         }
 864
 865         if (!(psta->state &_FW_LINKED)) {
 866                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__, psta->state);
 867                 return _FAIL;
 868         }
 869
 870         memset(hdr, 0, WLANHDR_OFFSET);
 871
 872         pwlanhdr->frame_control = cpu_to_le16(pattrib->type);
 873
 874         if (pattrib->type & IEEE80211_FTYPE_DATA) {
 875                 if (check_fwstate(pmlmepriv,  WIFI_STATION_STATE)) {
 876                         /* to_ds = 1, fr_ds = 0; */
 877                         /* Data transfer to AP */
 878                         pwlanhdr->frame_control |=
 879                                 cpu_to_le16(IEEE80211_FCTL_TODS);
 880                         ether_addr_copy(pwlanhdr->addr1, get_bssid(pmlmepriv));
 881                         ether_addr_copy(pwlanhdr->addr2, pattrib->src);
 882                         ether_addr_copy(pwlanhdr->addr3, pattrib->dst);
 883
 884                         if (pmlmepriv->qos_option)
 885                                 qos_option = true;
 886
 887                 } else if (check_fwstate(pmlmepriv,  WIFI_AP_STATE)) {
 888                         /* to_ds = 0, fr_ds = 1; */
 889                         pwlanhdr->frame_control |=
 890                                 cpu_to_le16(IEEE80211_FCTL_FROMDS);
 891                         ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
 892                         ether_addr_copy(pwlanhdr->addr2, get_bssid(pmlmepriv));
 893                         ether_addr_copy(pwlanhdr->addr3, pattrib->src);
 894
 895                         if (psta->qos_option)
 896                                 qos_option = true;
 897                 } else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
 898                            check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
 899                         ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
 900                         ether_addr_copy(pwlanhdr->addr2, pattrib->src);
 901                         ether_addr_copy(pwlanhdr->addr3, get_bssid(pmlmepriv));
 902
 903                         if (psta->qos_option)
 904                                 qos_option = true;
 905                 }
 906                 else {
 907                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
 908                                  "fw_state:%x is not allowed to xmit frame\n",
 909                                  get_fwstate(pmlmepriv));
 910                         res = _FAIL;
 911                         goto exit;
 912                 }
 913                 if (pattrib->mdata)
 914                         pwlanhdr->frame_control |=
 915                                 cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 916                 if (pattrib->encrypt)
 917                         pwlanhdr->frame_control |=
 918                                 cpu_to_le16(IEEE80211_FCTL_PROTECTED);
 919                 if (qos_option) {
 920                         qoshdr = (struct ieee80211_qos_hdr *)hdr;
 921
 922                         qoshdr->qos_ctrl = cpu_to_le16(
 923                                 pattrib->priority & IEEE80211_QOS_CTL_TID_MASK);
 924
 925                         qoshdr->qos_ctrl |= cpu_to_le16(
 926                                 (pattrib->ack_policy << 5) &
 927                                 IEEE80211_QOS_CTL_ACK_POLICY_MASK);
 928
 929                         if (pattrib->eosp)
 930                                 qoshdr->qos_ctrl |=
 931                                         cpu_to_le16(IEEE80211_QOS_CTL_EOSP);
 932                 }
 933                 /* TODO: fill HT Control Field */
 934
 935                 /* Update Seq Num will be handled by f/w */
 936                 if (psta) {
 937                         psta->sta_xmitpriv.txseq_tid[pattrib->priority]++;
 938                         psta->sta_xmitpriv.txseq_tid[pattrib->priority] &= 0xFFF;
 939                         pattrib->seqnum = psta->sta_xmitpriv.txseq_tid[pattrib->priority];
 940                         /* We dont need to worry about frag bits here */
 941                         pwlanhdr->seq_ctrl = cpu_to_le16(IEEE80211_SN_TO_SEQ(
 942                                                               pattrib->seqnum));
 943                         /* check if enable ampdu */
 944                         if (pattrib->ht_en && psta->htpriv.ampdu_enable) {
 945                                 if (pattrib->priority >= 16)
 946                                         printk(KERN_WARNING "%s: Invalid "
 947                                                "pattrib->priority %i\n",
 948                                                __func__, pattrib->priority);
 949                                 if (psta->htpriv.agg_enable_bitmap &
 950                                     BIT(pattrib->priority))
 951                                         pattrib->ampdu_en = true;
 952                         }
 953                         /* re-check if enable ampdu by BA_starting_seqctrl */
 954                         if (pattrib->ampdu_en) {
 955                                 u16 tx_seq;
 956
 957                                 tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
 958
 959                                 /* check BA_starting_seqctrl */
 960                                 if (SN_LESS(pattrib->seqnum, tx_seq)) {
 961                                         /* DBG_8723A("tx ampdu seqnum(%d) < tx_seq(%d)\n", pattrib->seqnum, tx_seq); */
 962                                         pattrib->ampdu_en = false;/* AGG BK */
 963                                 } else if (SN_EQUAL(pattrib->seqnum, tx_seq)) {
 964                                         psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq+1)&0xfff;
 965                                         pattrib->ampdu_en = true;/* AGG EN */
 966                                 } else {
 967                                         /* DBG_8723A("tx ampdu over run\n"); */
 968                                         psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff;
 969                                         pattrib->ampdu_en = true;/* AGG EN */
 970                                 }
 971                         }
 972                 }
 973         }
 974 exit:
 975         return res;
 976 }
 977
 978 s32 rtw_txframes_pending23a(struct rtw_adapter *padapter)
 979 {
 980         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 981
 982         return (!list_empty(&pxmitpriv->be_pending.queue)) ||
 983                 (!list_empty(&pxmitpriv->bk_pending.queue)) ||
 984                 (!list_empty(&pxmitpriv->vi_pending.queue)) ||
 985                 (!list_empty(&pxmitpriv->vo_pending.queue));
 986 }
 987
 988 s32 rtw_txframes_sta_ac_pending23a(struct rtw_adapter *padapter,
 989                                 struct pkt_attrib *pattrib)
 990 {
 991         struct sta_info *psta;
 992         struct tx_servq *ptxservq;
 993         int priority = pattrib->priority;
 994
 995         if (pattrib->psta) {
 996                 psta = pattrib->psta;
 997         } else {
 998                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 999                 psta = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]);
1000         }
1001         if (psta == NULL) {
1002                 DBG_8723A("%s, psta == NUL\n", __func__);
1003                 return 0;
1004         }
1005         if (!(psta->state &_FW_LINKED)) {
1006                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__,
1007                           psta->state);
1008                 return 0;
1009         }
1010         switch (priority) {
1011         case 1:
1012         case 2:
1013                 ptxservq = &psta->sta_xmitpriv.bk_q;
1014                 break;
1015         case 4:
1016         case 5:
1017                 ptxservq = &psta->sta_xmitpriv.vi_q;
1018                 break;
1019         case 6:
1020         case 7:
1021                 ptxservq = &psta->sta_xmitpriv.vo_q;
1022                 break;
1023         case 0:
1024         case 3:
1025         default:
1026                 ptxservq = &psta->sta_xmitpriv.be_q;
1027                 break;
1028         }
1029         return ptxservq->qcnt;
1030 }
1031
1032 /* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
1033  * IEEE LLC/SNAP header contains 8 octets
1034  * First 3 octets comprise the LLC portion
1035  * SNAP portion, 5 octets, is divided into two fields:
1036  *      Organizationally Unique Identifier(OUI), 3 octets,
1037  *      type, defined by that organization, 2 octets.
1038  */
1039 static int rtw_put_snap(u8 *data, u16 h_proto)
1040 {
1041         if (h_proto == ETH_P_IPX || h_proto == ETH_P_AARP)
1042                 ether_addr_copy(data, bridge_tunnel_header);
1043         else
1044                 ether_addr_copy(data, rfc1042_header);
1045
1046         data += ETH_ALEN;
1047         put_unaligned_be16(h_proto, data);
1048         return ETH_ALEN + sizeof(u16);
1049 }
1050
1051 /*
1052
1053 This sub-routine will perform all the following:
1054
1055 1. remove 802.3 header.
1056 2. create wlan_header, based on the info in pxmitframe
1057 3. append sta's iv/ext-iv
1058 4. append LLC
1059 5. move frag chunk from pframe to pxmitframe->mem
1060 6. apply sw-encrypt, if necessary.
1061
1062 */
1063 int rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
1064                               struct xmit_frame *pxmitframe)
1065 {
1066         struct sta_info *psta;
1067         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1068         struct pkt_attrib *pattrib = &pxmitframe->attrib;
1069         struct ieee80211_hdr *hdr;
1070         s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
1071         u8 *pframe, *mem_start;
1072         u8 hw_hdr_offset;
1073         u8 *pbuf_start;
1074         u8 *pdata = skb->data;
1075         int data_len = skb->len;
1076         s32 bmcst = is_multicast_ether_addr(pattrib->ra);
1077         int res = _SUCCESS;
1078
1079         if (pattrib->psta)
1080                 psta = pattrib->psta;
1081         else {
1082                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
1083                 psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra);
1084         }
1085
1086         if (!psta) {
1087                 DBG_8723A("%s, psta == NUL\n", __func__);
1088                 return _FAIL;
1089         }
1090
1091         if (!(psta->state &_FW_LINKED)) {
1092                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
1093                           __func__, psta->state);
1094                 return _FAIL;
1095         }
1096
1097         if (!pxmitframe->buf_addr) {
1098                 DBG_8723A("==> %s buf_addr == NULL\n", __func__);
1099                 return _FAIL;
1100         }
1101
1102         pbuf_start = pxmitframe->buf_addr;
1103
1104         hw_hdr_offset = TXDESC_OFFSET;
1105
1106         mem_start = pbuf_start + hw_hdr_offset;
1107
1108         if (rtw_make_wlanhdr(padapter, mem_start, pattrib) == _FAIL) {
1109                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1110                          "%s: rtw_make_wlanhdr fail; drop pkt\n", __func__);
1111                 res = _FAIL;
1112                 goto exit;
1113         }
1114
1115         pdata += pattrib->pkt_hdrlen;
1116         data_len -= pattrib->pkt_hdrlen;
1117
1118         frg_inx = 0;
1119         frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
1120
1121         while (1) {
1122                 llc_sz = 0;
1123
1124                 mpdu_len = frg_len;
1125
1126                 pframe = mem_start;
1127                 hdr = (struct ieee80211_hdr *)mem_start;
1128
1129                 pframe += pattrib->hdrlen;
1130                 mpdu_len -= pattrib->hdrlen;
1131
1132                 /* adding icv, if necessary... */
1133                 if (pattrib->iv_len) {
1134                         if (psta) {
1135                                 switch (pattrib->encrypt) {
1136                                 case WLAN_CIPHER_SUITE_WEP40:
1137                                 case WLAN_CIPHER_SUITE_WEP104:
1138                                         WEP_IV(pattrib->iv, psta->dot11txpn,
1139                                                pattrib->key_idx);
1140                                         break;
1141                                 case WLAN_CIPHER_SUITE_TKIP:
1142                                         if (bmcst)
1143                                                 TKIP_IV(pattrib->iv,
1144                                                         psta->dot11txpn,
1145                                                         pattrib->key_idx);
1146                                         else
1147                                                 TKIP_IV(pattrib->iv,
1148                                                         psta->dot11txpn, 0);
1149                                         break;
1150                                 case WLAN_CIPHER_SUITE_CCMP:
1151                                         if (bmcst)
1152                                                 AES_IV(pattrib->iv,
1153                                                        psta->dot11txpn,
1154                                                        pattrib->key_idx);
1155                                         else
1156                                                 AES_IV(pattrib->iv,
1157                                                        psta->dot11txpn, 0);
1158                                         break;
1159                                 }
1160                         }
1161
1162                         memcpy(pframe, pattrib->iv, pattrib->iv_len);
1163
1164                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
1165                                  "rtw_xmiaframe_coalesce23a: keyid =%d pattrib->iv[3]=%.2x pframe =%.2x %.2x %.2x %.2x\n",
1166                                  padapter->securitypriv.dot11PrivacyKeyIndex,
1167                                  pattrib->iv[3], *pframe, *(pframe+1),
1168                                  *(pframe+2), *(pframe+3));
1169                         pframe += pattrib->iv_len;
1170                         mpdu_len -= pattrib->iv_len;
1171                 }
1172                 if (frg_inx == 0) {
1173                         llc_sz = rtw_put_snap(pframe, pattrib->ether_type);
1174                         pframe += llc_sz;
1175                         mpdu_len -= llc_sz;
1176                 }
1177
1178                 if (pattrib->icv_len > 0 && pattrib->bswenc)
1179                         mpdu_len -= pattrib->icv_len;
1180
1181                 if (bmcst)
1182                         /*  don't do fragment to broadcast/multicast packets */
1183                         mem_sz = min_t(s32, data_len, pattrib->pktlen);
1184                 else
1185                         mem_sz = min_t(s32, data_len, mpdu_len);
1186
1187                 memcpy(pframe, pdata, mem_sz);
1188
1189                 pframe += mem_sz;
1190                 pdata += mem_sz;
1191                 data_len -= mem_sz;
1192
1193                 if ((pattrib->icv_len >0) && (pattrib->bswenc)) {
1194                         memcpy(pframe, pattrib->icv, pattrib->icv_len);
1195                         pframe += pattrib->icv_len;
1196                 }
1197
1198                 frg_inx++;
1199
1200                 if (bmcst || data_len <= 0) {
1201                         pattrib->nr_frags = frg_inx;
1202
1203                         pattrib->last_txcmdsz = pattrib->hdrlen +
1204                                                 pattrib->iv_len +
1205                                                 ((pattrib->nr_frags == 1) ?
1206                                                 llc_sz : 0) +
1207                                                 ((pattrib->bswenc) ?
1208                                                 pattrib->icv_len : 0) + mem_sz;
1209                         hdr->frame_control &=
1210                                 ~cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
1211
1212                         break;
1213                 } else {
1214                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1215                                  "%s: There're still something in packet!\n",
1216                                  __func__);
1217                 }
1218                 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREFRAGS);
1219
1220                 mem_start = PTR_ALIGN(pframe, 4) + hw_hdr_offset;
1221                 memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
1222         }
1223
1224         if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
1225                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1226                          "xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
1227                 DBG_8723A("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
1228                 res = _FAIL;
1229                 goto exit;
1230         }
1231
1232         xmitframe_swencrypt(padapter, pxmitframe);
1233
1234         if (bmcst == false)
1235                 update_attrib_vcs_info(padapter, pxmitframe);
1236         else
1237                 pattrib->vcs_mode = NONE_VCS;
1238
1239 exit:
1240         return res;
1241 }
1242
1243 void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len)
1244 {
1245         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1246         struct registry_priv *pregistrypriv = &padapter->registrypriv;
1247         uint protection;
1248         const u8 *p;
1249
1250         switch (pregistrypriv->vrtl_carrier_sense) {
1251         case DISABLE_VCS:
1252                 pxmitpriv->vcs = NONE_VCS;
1253                 break;
1254         case ENABLE_VCS:
1255                 break;
1256         case AUTO_VCS:
1257         default:
1258                 p = cfg80211_find_ie(WLAN_EID_ERP_INFO, ie, ie_len);
1259                 if (!p)
1260                         pxmitpriv->vcs = NONE_VCS;
1261                 else {
1262                         protection = (*(p + 2)) & BIT(1);
1263                         if (protection) {
1264                                 if (pregistrypriv->vcs_type == RTS_CTS)
1265                                         pxmitpriv->vcs = RTS_CTS;
1266                                 else
1267                                         pxmitpriv->vcs = CTS_TO_SELF;
1268                         } else {
1269                                 pxmitpriv->vcs = NONE_VCS;
1270                         }
1271                 }
1272                 break;
1273         }
1274 }
1275
1276 void rtw_count_tx_stats23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe, int sz)
1277 {
1278         struct sta_info *psta = NULL;
1279         struct stainfo_stats *pstats = NULL;
1280         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
1281         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
1282
1283         if (pxmitframe->frame_tag == DATA_FRAMETAG) {
1284                 pxmitpriv->tx_bytes += sz;
1285                 pmlmepriv->LinkDetectInfo.NumTxOkInPeriod++;
1286
1287                 psta = pxmitframe->attrib.psta;
1288                 if (psta) {
1289                         pstats = &psta->sta_stats;
1290                         pstats->tx_pkts++;
1291                         pstats->tx_bytes += sz;
1292                 }
1293         }
1294 }
1295
1296 struct xmit_buf *rtw_alloc_xmitbuf23a_ext(struct xmit_priv *pxmitpriv)
1297 {
1298         unsigned long irqL;
1299         struct xmit_buf *pxmitbuf =  NULL;
1300         struct list_head *phead;
1301         struct rtw_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
1302
1303         spin_lock_irqsave(&pfree_queue->lock, irqL);
1304
1305         phead = get_list_head(pfree_queue);
1306
1307         if (!list_empty(phead)) {
1308                 pxmitbuf = list_first_entry(phead, struct xmit_buf, list);
1309
1310                 list_del_init(&pxmitbuf->list);
1311
1312                 pxmitpriv->free_xmit_extbuf_cnt--;
1313                 pxmitbuf->priv_data = NULL;
1314                 pxmitbuf->ext_tag = true;
1315
1316                 if (pxmitbuf->sctx) {
1317                         DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__);
1318                         rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
1319                 }
1320         }
1321
1322         spin_unlock_irqrestore(&pfree_queue->lock, irqL);
1323
1324         return pxmitbuf;
1325 }
1326
1327 int rtw_free_xmitbuf_ext23a(struct xmit_priv *pxmitpriv,
1328                             struct xmit_buf *pxmitbuf)
1329 {
1330         unsigned long irqL;
1331         struct rtw_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
1332
1333         if (pxmitbuf == NULL)
1334                 return _FAIL;
1335
1336         spin_lock_irqsave(&pfree_queue->lock, irqL);
1337
1338         list_del_init(&pxmitbuf->list);
1339
1340         list_add_tail(&pxmitbuf->list, get_list_head(pfree_queue));
1341         pxmitpriv->free_xmit_extbuf_cnt++;
1342
1343         spin_unlock_irqrestore(&pfree_queue->lock, irqL);
1344
1345         return _SUCCESS;
1346 }
1347
1348 struct xmit_buf *rtw_alloc_xmitbuf23a(struct xmit_priv *pxmitpriv)
1349 {
1350         unsigned long irqL;
1351         struct xmit_buf *pxmitbuf =  NULL;
1352         struct list_head *phead;
1353         struct rtw_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
1354
1355         /* DBG_8723A("+rtw_alloc_xmitbuf23a\n"); */
1356
1357         spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
1358
1359         phead = get_list_head(pfree_xmitbuf_queue);
1360
1361         if (!list_empty(phead)) {
1362                 pxmitbuf = list_first_entry(phead, struct xmit_buf, list);
1363
1364                 list_del_init(&pxmitbuf->list);
1365
1366                 pxmitpriv->free_xmitbuf_cnt--;
1367                 pxmitbuf->priv_data = NULL;
1368                 pxmitbuf->ext_tag = false;
1369                 pxmitbuf->flags = XMIT_VO_QUEUE;
1370
1371                 if (pxmitbuf->sctx) {
1372                         DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__);
1373                         rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
1374                 }
1375         }
1376
1377         spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
1378
1379         return pxmitbuf;
1380 }
1381
1382 int rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
1383 {
1384         unsigned long irqL;
1385         struct rtw_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
1386
1387         /* DBG_8723A("+rtw_free_xmitbuf23a\n"); */
1388
1389         if (pxmitbuf == NULL)
1390                 return _FAIL;
1391
1392         if (pxmitbuf->sctx) {
1393                 DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__);
1394                 rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
1395         }
1396
1397         if (pxmitbuf->ext_tag) {
1398                 rtw_free_xmitbuf_ext23a(pxmitpriv, pxmitbuf);
1399         } else {
1400                 spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
1401
1402                 list_del_init(&pxmitbuf->list);
1403
1404                 list_add_tail(&pxmitbuf->list,
1405                               get_list_head(pfree_xmitbuf_queue));
1406
1407                 pxmitpriv->free_xmitbuf_cnt++;
1408                 spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
1409         }
1410
1411         return _SUCCESS;
1412 }
1413
1414 static void rtw_init_xmitframe(struct xmit_frame *pxframe)
1415 {
1416         if (pxframe !=  NULL) {
1417                 /* default value setting */
1418                 pxframe->buf_addr = NULL;
1419                 pxframe->pxmitbuf = NULL;
1420
1421                 memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
1422                 /* pxframe->attrib.psta = NULL; */
1423
1424                 pxframe->frame_tag = DATA_FRAMETAG;
1425
1426                 pxframe->pkt = NULL;
1427                 pxframe->pkt_offset = 1;/* default use pkt_offset to fill tx desc */
1428
1429                 pxframe->ack_report = 0;
1430         }
1431 }
1432
1433 /*
1434 Calling context:
1435 1. OS_TXENTRY
1436 2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
1437
1438 If we turn on USE_RXTHREAD, then, no need for critical section.
1439 Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
1440
1441 Must be very very cautious...
1442
1443 */
1444 static struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)
1445 {
1446         struct xmit_frame *pxframe = NULL;
1447         struct list_head *plist, *phead;
1448         struct rtw_queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
1449
1450         spin_lock_bh(&pfree_xmit_queue->lock);
1451
1452         if (list_empty(&pfree_xmit_queue->queue)) {
1453                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1454                          "rtw_alloc_xmitframe:%d\n",
1455                          pxmitpriv->free_xmitframe_cnt);
1456                 pxframe =  NULL;
1457         } else {
1458                 phead = get_list_head(pfree_xmit_queue);
1459
1460                 plist = phead->next;
1461
1462                 pxframe = container_of(plist, struct xmit_frame, list);
1463
1464                 list_del_init(&pxframe->list);
1465                 pxmitpriv->free_xmitframe_cnt--;
1466                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1467                          "rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n",
1468                          pxmitpriv->free_xmitframe_cnt);
1469         }
1470
1471         spin_unlock_bh(&pfree_xmit_queue->lock);
1472
1473         rtw_init_xmitframe(pxframe);
1474
1475         return pxframe;
1476 }
1477
1478 struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv)
1479 {
1480         struct xmit_frame *pxframe = NULL;
1481         struct list_head *plist, *phead;
1482         struct rtw_queue *queue = &pxmitpriv->free_xframe_ext_queue;
1483
1484         spin_lock_bh(&queue->lock);
1485
1486         if (list_empty(&queue->queue)) {
1487                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1488                          "rtw_alloc_xmitframe23a_ext:%d\n",
1489                          pxmitpriv->free_xframe_ext_cnt);
1490                 pxframe =  NULL;
1491         } else {
1492                 phead = get_list_head(queue);
1493                 plist = phead->next;
1494                 pxframe = container_of(plist, struct xmit_frame, list);
1495
1496                 list_del_init(&pxframe->list);
1497                 pxmitpriv->free_xframe_ext_cnt--;
1498                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1499                          "rtw_alloc_xmitframe23a_ext():free_xmitframe_cnt =%d\n",
1500                          pxmitpriv->free_xframe_ext_cnt);
1501         }
1502
1503         spin_unlock_bh(&queue->lock);
1504
1505         rtw_init_xmitframe(pxframe);
1506
1507         return pxframe;
1508 }
1509
1510 s32 rtw_free_xmitframe23a(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
1511 {
1512         struct rtw_queue *queue = NULL;
1513         struct rtw_adapter *padapter = pxmitpriv->adapter;
1514         struct sk_buff *pndis_pkt = NULL;
1515
1516         if (pxmitframe == NULL) {
1517                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1518                          "====== rtw_free_xmitframe23a():pxmitframe == NULL!!!!!!!!!!\n");
1519                 goto exit;
1520         }
1521
1522         if (pxmitframe->pkt) {
1523                 pndis_pkt = pxmitframe->pkt;
1524                 pxmitframe->pkt = NULL;
1525         }
1526
1527         if (pxmitframe->ext_tag == 0)
1528                 queue = &pxmitpriv->free_xmit_queue;
1529         else if (pxmitframe->ext_tag == 1)
1530                 queue = &pxmitpriv->free_xframe_ext_queue;
1531
1532         if (!queue)
1533                 goto check_pkt_complete;
1534         spin_lock_bh(&queue->lock);
1535
1536         list_del_init(&pxmitframe->list);
1537         list_add_tail(&pxmitframe->list, get_list_head(queue));
1538         if (pxmitframe->ext_tag == 0) {
1539                 pxmitpriv->free_xmitframe_cnt++;
1540                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_,
1541                          "rtw_free_xmitframe23a():free_xmitframe_cnt =%d\n",
1542                          pxmitpriv->free_xmitframe_cnt);
1543         } else if (pxmitframe->ext_tag == 1) {
1544                 pxmitpriv->free_xframe_ext_cnt++;
1545                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_,
1546                          "rtw_free_xmitframe23a():free_xframe_ext_cnt =%d\n",
1547                          pxmitpriv->free_xframe_ext_cnt);
1548         }
1549
1550         spin_unlock_bh(&queue->lock);
1551
1552 check_pkt_complete:
1553
1554         if (pndis_pkt)
1555                 rtw_os_pkt_complete23a(padapter, pndis_pkt);
1556
1557 exit:
1558
1559         return _SUCCESS;
1560 }
1561
1562 void rtw_free_xmitframe_queue23a(struct xmit_priv *pxmitpriv,
1563                                  struct rtw_queue *pframequeue)
1564 {
1565         struct list_head *phead;
1566         struct xmit_frame *pxmitframe, *ptmp;
1567
1568         spin_lock_bh(&pframequeue->lock);
1569         phead = get_list_head(pframequeue);
1570         list_for_each_entry_safe(pxmitframe, ptmp, phead, list)
1571                 rtw_free_xmitframe23a(pxmitpriv, pxmitframe);
1572         spin_unlock_bh(&pframequeue->lock);
1573
1574 }
1575
1576 int rtw_xmitframe_enqueue23a(struct rtw_adapter *padapter,
1577                              struct xmit_frame *pxmitframe)
1578 {
1579         if (rtw_xmit23a_classifier(padapter, pxmitframe) == _FAIL) {
1580                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1581                          "rtw_xmitframe_enqueue23a: drop xmit pkt for classifier fail\n");
1582                 return _FAIL;
1583         }
1584
1585         return _SUCCESS;
1586 }
1587
1588 static struct xmit_frame *
1589 dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit,
1590                       struct tx_servq *ptxservq, struct rtw_queue *pframe_queue)
1591 {
1592         struct list_head *phead;
1593         struct xmit_frame *pxmitframe = NULL;
1594
1595         phead = get_list_head(pframe_queue);
1596
1597         if (!list_empty(phead)) {
1598                 pxmitframe = list_first_entry(phead, struct xmit_frame, list);
1599                 list_del_init(&pxmitframe->list);
1600                 ptxservq->qcnt--;
1601         }
1602         return pxmitframe;
1603 }
1604
1605 struct xmit_frame *
1606 rtw_dequeue_xframe23a(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i,
1607                    int entry)
1608 {
1609         struct list_head *sta_phead;
1610         struct hw_xmit *phwxmit;
1611         struct tx_servq *ptxservq = NULL, *ptmp;
1612         struct rtw_queue *pframe_queue = NULL;
1613         struct xmit_frame *pxmitframe = NULL;
1614         struct rtw_adapter *padapter = pxmitpriv->adapter;
1615         struct registry_priv    *pregpriv = &padapter->registrypriv;
1616         int i, inx[4];
1617
1618         inx[0] = 0;
1619         inx[1] = 1;
1620         inx[2] = 2;
1621         inx[3] = 3;
1622         if (pregpriv->wifi_spec == 1) {
1623                 int j;
1624
1625                 for (j = 0; j < 4; j++)
1626                         inx[j] = pxmitpriv->wmm_para_seq[j];
1627         }
1628
1629         spin_lock_bh(&pxmitpriv->lock);
1630
1631         for (i = 0; i < entry; i++) {
1632                 phwxmit = phwxmit_i + inx[i];
1633
1634                 sta_phead = get_list_head(phwxmit->sta_queue);
1635                 list_for_each_entry_safe(ptxservq, ptmp, sta_phead,
1636                                          tx_pending) {
1637                         pframe_queue = &ptxservq->sta_pending;
1638
1639                         pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue);
1640
1641                         if (pxmitframe) {
1642                                 phwxmit->accnt--;
1643
1644                                 /* Remove sta node when there is no pending packets. */
1645                                 /* must be done after get_next and
1646                                    before break */
1647                                 if (list_empty(&pframe_queue->queue))
1648                                         list_del_init(&ptxservq->tx_pending);
1649                                 goto exit;
1650                         }
1651                 }
1652         }
1653 exit:
1654         spin_unlock_bh(&pxmitpriv->lock);
1655         return pxmitframe;
1656 }
1657
1658 struct tx_servq *rtw_get_sta_pending23a(struct rtw_adapter *padapter, struct sta_info *psta, int up, u8 *ac)
1659 {
1660         struct tx_servq *ptxservq = NULL;
1661
1662         switch (up) {
1663         case 1:
1664         case 2:
1665                 ptxservq = &psta->sta_xmitpriv.bk_q;
1666                 *(ac) = 3;
1667                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1668                          "rtw_get_sta_pending23a : BK\n");
1669                 break;
1670         case 4:
1671         case 5:
1672                 ptxservq = &psta->sta_xmitpriv.vi_q;
1673                 *(ac) = 1;
1674                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1675                          "rtw_get_sta_pending23a : VI\n");
1676                 break;
1677         case 6:
1678         case 7:
1679                 ptxservq = &psta->sta_xmitpriv.vo_q;
1680                 *(ac) = 0;
1681                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1682                          "rtw_get_sta_pending23a : VO\n");
1683                 break;
1684         case 0:
1685         case 3:
1686         default:
1687                 ptxservq = &psta->sta_xmitpriv.be_q;
1688                 *(ac) = 2;
1689                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1690                          "rtw_get_sta_pending23a : BE\n");
1691                 break;
1692         }
1693         return ptxservq;
1694 }
1695
1696 /*
1697  * Will enqueue pxmitframe to the proper queue,
1698  * and indicate it to xx_pending list.....
1699  */
1700 int rtw_xmit23a_classifier(struct rtw_adapter *padapter,
1701                            struct xmit_frame *pxmitframe)
1702 {
1703         struct sta_info *psta;
1704         struct tx_servq *ptxservq;
1705         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
1706         struct sta_priv *pstapriv = &padapter->stapriv;
1707         struct hw_xmit  *phwxmits =  padapter->xmitpriv.hwxmits;
1708         u8      ac_index;
1709         int res = _SUCCESS;
1710
1711         if (pattrib->psta) {
1712                 psta = pattrib->psta;
1713         } else {
1714                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
1715                 psta = rtw_get_stainfo23a(pstapriv, pattrib->ra);
1716         }
1717         if (psta == NULL) {
1718                 res = _FAIL;
1719                 DBG_8723A("rtw_xmit23a_classifier: psta == NULL\n");
1720                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1721                          "rtw_xmit23a_classifier: psta == NULL\n");
1722                 goto exit;
1723         }
1724         if (!(psta->state & _FW_LINKED)) {
1725                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__,
1726                           psta->state);
1727                 return _FAIL;
1728         }
1729         ptxservq = rtw_get_sta_pending23a(padapter, psta, pattrib->priority,
1730                                        (u8 *)(&ac_index));
1731
1732         if (list_empty(&ptxservq->tx_pending)) {
1733                 list_add_tail(&ptxservq->tx_pending,
1734                               get_list_head(phwxmits[ac_index].sta_queue));
1735         }
1736
1737         list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
1738         ptxservq->qcnt++;
1739         phwxmits[ac_index].accnt++;
1740 exit:
1741         return res;
1742 }
1743
1744 void rtw_alloc_hwxmits23a(struct rtw_adapter *padapter)
1745 {
1746         struct hw_xmit *hwxmits;
1747         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1748         int size;
1749
1750         pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
1751
1752         size = sizeof(struct hw_xmit) * (pxmitpriv->hwxmit_entry + 1);
1753         pxmitpriv->hwxmits = kzalloc(size, GFP_KERNEL);
1754
1755         hwxmits = pxmitpriv->hwxmits;
1756
1757         if (pxmitpriv->hwxmit_entry == 5) {
1758                 /* pxmitpriv->bmc_txqueue.head = 0; */
1759                 /* hwxmits[0] .phwtxqueue = &pxmitpriv->bmc_txqueue; */
1760                 hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
1761
1762                 /* pxmitpriv->vo_txqueue.head = 0; */
1763                 /* hwxmits[1] .phwtxqueue = &pxmitpriv->vo_txqueue; */
1764                 hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
1765
1766                 /* pxmitpriv->vi_txqueue.head = 0; */
1767                 /* hwxmits[2] .phwtxqueue = &pxmitpriv->vi_txqueue; */
1768                 hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
1769
1770                 /* pxmitpriv->bk_txqueue.head = 0; */
1771                 /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */
1772                 hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
1773
1774                 /* pxmitpriv->be_txqueue.head = 0; */
1775                 /* hwxmits[4] .phwtxqueue = &pxmitpriv->be_txqueue; */
1776                 hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
1777
1778         } else if (pxmitpriv->hwxmit_entry == 4) {
1779
1780                 /* pxmitpriv->vo_txqueue.head = 0; */
1781                 /* hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue; */
1782                 hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
1783
1784                 /* pxmitpriv->vi_txqueue.head = 0; */
1785                 /* hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue; */
1786                 hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
1787
1788                 /* pxmitpriv->be_txqueue.head = 0; */
1789                 /* hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue; */
1790                 hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
1791
1792                 /* pxmitpriv->bk_txqueue.head = 0; */
1793                 /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */
1794                 hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
1795         } else {
1796
1797         }
1798 }
1799
1800 void rtw_free_hwxmits23a(struct rtw_adapter *padapter)
1801 {
1802         struct hw_xmit *hwxmits;
1803         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1804
1805         hwxmits = pxmitpriv->hwxmits;
1806         kfree(hwxmits);
1807 }
1808
1809 void rtw_init_hwxmits23a(struct hw_xmit *phwxmit, int entry)
1810 {
1811         int i;
1812
1813         for (i = 0; i < entry; i++, phwxmit++)
1814                 phwxmit->accnt = 0;
1815 }
1816
1817 u32 rtw_get_ff_hwaddr23a(struct xmit_frame *pxmitframe)
1818 {
1819         u32 addr;
1820         struct pkt_attrib *pattrib = &pxmitframe->attrib;
1821
1822         switch (pattrib->qsel) {
1823         case 0:
1824         case 3:
1825                 addr = BE_QUEUE_INX;
1826                 break;
1827         case 1:
1828         case 2:
1829                 addr = BK_QUEUE_INX;
1830                 break;
1831         case 4:
1832         case 5:
1833                 addr = VI_QUEUE_INX;
1834                 break;
1835         case 6:
1836         case 7:
1837                 addr = VO_QUEUE_INX;
1838                 break;
1839         case 0x10:
1840                 addr = BCN_QUEUE_INX;
1841                 break;
1842         case 0x11:/* BC/MC in PS (HIQ) */
1843                 addr = HIGH_QUEUE_INX;
1844                 break;
1845         case 0x12:
1846         default:
1847                 addr = MGT_QUEUE_INX;
1848                 break;
1849         }
1850
1851         return addr;
1852 }
1853
1854 /*
1855  * The main transmit(tx) entry
1856  *
1857  * Return
1858  *      1       enqueue
1859  *      0       success, hardware will handle this xmit frame(packet)
1860  *      <0      fail
1861  */
1862 int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb)
1863 {
1864         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1865         struct xmit_frame *pxmitframe = NULL;
1866         int res;
1867
1868         pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
1869
1870         if (pxmitframe == NULL) {
1871                 RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
1872                          "rtw_xmit23a: no more pxmitframe\n");
1873                 return -1;
1874         }
1875
1876         res = update_attrib(padapter, skb, &pxmitframe->attrib);
1877
1878         if (res == _FAIL) {
1879                 RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
1880                          "rtw_xmit23a: update attrib fail\n");
1881                 rtw_free_xmitframe23a(pxmitpriv, pxmitframe);
1882                 return -1;
1883         }
1884         pxmitframe->pkt = skb;
1885
1886         pxmitframe->attrib.qsel = pxmitframe->attrib.priority;
1887
1888 #ifdef CONFIG_8723AU_AP_MODE
1889         spin_lock_bh(&pxmitpriv->lock);
1890         if (xmitframe_enqueue_for_sleeping_sta23a(padapter, pxmitframe)) {
1891                 spin_unlock_bh(&pxmitpriv->lock);
1892                 return 1;
1893         }
1894         spin_unlock_bh(&pxmitpriv->lock);
1895 #endif
1896
1897         if (rtl8723au_hal_xmit(padapter, pxmitframe) == false)
1898                 return 1;
1899
1900         return 0;
1901 }
1902
1903 #if defined(CONFIG_8723AU_AP_MODE)
1904
1905 int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
1906 {
1907         int ret = false;
1908         struct sta_info *psta = NULL;
1909         struct sta_priv *pstapriv = &padapter->stapriv;
1910         struct pkt_attrib *pattrib = &pxmitframe->attrib;
1911         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1912         int bmcst = is_multicast_ether_addr(pattrib->ra);
1913
1914         if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
1915                 return ret;
1916
1917         if (pattrib->psta) {
1918                 psta = pattrib->psta;
1919         } else {
1920                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
1921                 psta = rtw_get_stainfo23a(pstapriv, pattrib->ra);
1922         }
1923
1924         if (psta == NULL) {
1925                 DBG_8723A("%s, psta == NUL\n", __func__);
1926                 return false;
1927         }
1928
1929         if (!(psta->state & _FW_LINKED)) {
1930                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__,
1931                           psta->state);
1932                 return false;
1933         }
1934
1935         if (pattrib->triggered == 1) {
1936                 if (bmcst)
1937                         pattrib->qsel = 0x11;/* HIQ */
1938                 return ret;
1939         }
1940
1941         if (bmcst) {
1942                 spin_lock_bh(&psta->sleep_q.lock);
1943
1944                 if (pstapriv->sta_dz_bitmap) {
1945                         /* if anyone sta is in ps mode */
1946                         list_del_init(&pxmitframe->list);
1947
1948                         /* spin_lock_bh(&psta->sleep_q.lock); */
1949
1950                         list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
1951
1952                         psta->sleepq_len++;
1953
1954                         pstapriv->tim_bitmap |= BIT(0);/*  */
1955                         pstapriv->sta_dz_bitmap |= BIT(0);
1956
1957                         /* DBG_8723A("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
1958
1959                         /* tx bc/mc packets after update bcn */
1960                         update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
1961
1962                         /* spin_unlock_bh(&psta->sleep_q.lock); */
1963
1964                         ret = true;
1965
1966                 }
1967
1968                 spin_unlock_bh(&psta->sleep_q.lock);
1969
1970                 return ret;
1971
1972         }
1973
1974         spin_lock_bh(&psta->sleep_q.lock);
1975
1976         if (psta->state&WIFI_SLEEP_STATE) {
1977                 u8 wmmps_ac = 0;
1978
1979                 if (pstapriv->sta_dz_bitmap & CHKBIT(psta->aid)) {
1980                         list_del_init(&pxmitframe->list);
1981
1982                         /* spin_lock_bh(&psta->sleep_q.lock); */
1983
1984                         list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
1985
1986                         psta->sleepq_len++;
1987
1988                         switch (pattrib->priority) {
1989                         case 1:
1990                         case 2:
1991                                 wmmps_ac = psta->uapsd_bk & BIT(0);
1992                                 break;
1993                         case 4:
1994                         case 5:
1995                                 wmmps_ac = psta->uapsd_vi & BIT(0);
1996                                 break;
1997                         case 6:
1998                         case 7:
1999                                 wmmps_ac = psta->uapsd_vo & BIT(0);
2000                                 break;
2001                         case 0:
2002                         case 3:
2003                         default:
2004                                 wmmps_ac = psta->uapsd_be & BIT(0);
2005                                 break;
2006                         }
2007
2008                         if (wmmps_ac)
2009                                 psta->sleepq_ac_len++;
2010
2011                         if (((psta->has_legacy_ac) && (!wmmps_ac)) ||
2012                            ((!psta->has_legacy_ac) && (wmmps_ac))) {
2013                                 pstapriv->tim_bitmap |= CHKBIT(psta->aid);
2014
2015                                 if (psta->sleepq_len == 1) {
2016                                         /* update BCN for TIM IE */
2017                                         update_beacon23a(padapter, WLAN_EID_TIM,
2018                                                          NULL, false);
2019                                 }
2020                         }
2021
2022                         /* spin_unlock_bh(&psta->sleep_q.lock); */
2023
2024                         /* if (psta->sleepq_len > (NR_XMITFRAME>>3)) */
2025                         /*  */
2026                         /*      wakeup_sta_to_xmit23a(padapter, psta); */
2027                         /*  */
2028
2029                         ret = true;
2030
2031                 }
2032
2033         }
2034
2035         spin_unlock_bh(&psta->sleep_q.lock);
2036
2037         return ret;
2038 }
2039
2040 static void
2041 dequeue_xmitframes_to_sleeping_queue(struct rtw_adapter *padapter,
2042                                      struct sta_info *psta,
2043                                      struct rtw_queue *pframequeue)
2044 {
2045         int ret;
2046         struct list_head *phead;
2047         u8 ac_index;
2048         struct tx_servq *ptxservq;
2049         struct pkt_attrib *pattrib;
2050         struct xmit_frame *pxmitframe, *ptmp;
2051         struct hw_xmit *phwxmits = padapter->xmitpriv.hwxmits;
2052
2053         phead = get_list_head(pframequeue);
2054         list_for_each_entry_safe(pxmitframe, ptmp, phead, list) {
2055                 ret = xmitframe_enqueue_for_sleeping_sta23a(padapter, pxmitframe);
2056
2057                 if (ret == true) {
2058                         pattrib = &pxmitframe->attrib;
2059
2060                         ptxservq = rtw_get_sta_pending23a(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
2061
2062                         ptxservq->qcnt--;
2063                         phwxmits[ac_index].accnt--;
2064                 } else {
2065                         /* DBG_8723A("xmitframe_enqueue_for_sleeping_sta23a return false\n"); */
2066                 }
2067         }
2068 }
2069
2070 void stop_sta_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
2071 {
2072         struct sta_info *psta_bmc;
2073         struct sta_xmit_priv *pstaxmitpriv;
2074         struct sta_priv *pstapriv = &padapter->stapriv;
2075         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2076
2077         pstaxmitpriv = &psta->sta_xmitpriv;
2078
2079         /* for BC/MC Frames */
2080         psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
2081
2082         spin_lock_bh(&pxmitpriv->lock);
2083
2084         psta->state |= WIFI_SLEEP_STATE;
2085
2086         pstapriv->sta_dz_bitmap |= CHKBIT(psta->aid);
2087
2088         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
2089         list_del_init(&pstaxmitpriv->vo_q.tx_pending);
2090
2091         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
2092         list_del_init(&pstaxmitpriv->vi_q.tx_pending);
2093
2094         dequeue_xmitframes_to_sleeping_queue(padapter, psta,
2095                                              &pstaxmitpriv->be_q.sta_pending);
2096         list_del_init(&pstaxmitpriv->be_q.tx_pending);
2097
2098         dequeue_xmitframes_to_sleeping_queue(padapter, psta,
2099                                              &pstaxmitpriv->bk_q.sta_pending);
2100         list_del_init(&pstaxmitpriv->bk_q.tx_pending);
2101
2102         /* for BC/MC Frames */
2103         pstaxmitpriv = &psta_bmc->sta_xmitpriv;
2104         dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc,
2105                                              &pstaxmitpriv->be_q.sta_pending);
2106         list_del_init(&pstaxmitpriv->be_q.tx_pending);
2107
2108         spin_unlock_bh(&pxmitpriv->lock);
2109 }
2110
2111 void wakeup_sta_to_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
2112 {
2113         u8 update_mask = 0, wmmps_ac = 0;
2114         struct sta_info *psta_bmc;
2115         struct list_head *phead;
2116         struct xmit_frame *pxmitframe = NULL, *ptmp;
2117         struct sta_priv *pstapriv = &padapter->stapriv;
2118         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2119
2120         spin_lock_bh(&pxmitpriv->lock);
2121         phead = get_list_head(&psta->sleep_q);
2122         list_for_each_entry_safe(pxmitframe, ptmp, phead, list) {
2123                 list_del_init(&pxmitframe->list);
2124
2125                 switch (pxmitframe->attrib.priority) {
2126                 case 1:
2127                 case 2:
2128                         wmmps_ac = psta->uapsd_bk & BIT(1);
2129                         break;
2130                 case 4:
2131                 case 5:
2132                         wmmps_ac = psta->uapsd_vi & BIT(1);
2133                         break;
2134                 case 6:
2135                 case 7:
2136                         wmmps_ac = psta->uapsd_vo & BIT(1);
2137                         break;
2138                 case 0:
2139                 case 3:
2140                 default:
2141                         wmmps_ac = psta->uapsd_be & BIT(1);
2142                         break;
2143                 }
2144
2145                 psta->sleepq_len--;
2146                 if (psta->sleepq_len > 0)
2147                         pxmitframe->attrib.mdata = 1;
2148                 else
2149                         pxmitframe->attrib.mdata = 0;
2150
2151                 if (wmmps_ac) {
2152                         psta->sleepq_ac_len--;
2153                         if (psta->sleepq_ac_len > 0) {
2154                                 pxmitframe->attrib.mdata = 1;
2155                                 pxmitframe->attrib.eosp = 0;
2156                         } else {
2157                                 pxmitframe->attrib.mdata = 0;
2158                                 pxmitframe->attrib.eosp = 1;
2159                         }
2160                 }
2161
2162                 pxmitframe->attrib.triggered = 1;
2163                 rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
2164         }
2165
2166         if (psta->sleepq_len == 0) {
2167                 pstapriv->tim_bitmap &= ~CHKBIT(psta->aid);
2168
2169                 /* update BCN for TIM IE */
2170                 update_mask = BIT(0);
2171
2172                 if (psta->state&WIFI_SLEEP_STATE)
2173                         psta->state ^= WIFI_SLEEP_STATE;
2174
2175                 if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
2176                         psta->expire_to = pstapriv->expire_to;
2177                         psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
2178                 }
2179
2180                 pstapriv->sta_dz_bitmap &= ~CHKBIT(psta->aid);
2181         }
2182         /* spin_unlock_bh(&psta->sleep_q.lock); */
2183         spin_unlock_bh(&pxmitpriv->lock);
2184
2185         /* for BC/MC Frames */
2186         psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
2187         if (!psta_bmc)
2188                 return;
2189
2190         if ((pstapriv->sta_dz_bitmap&0xfffe) == 0x0) {
2191                 /* no any sta in ps mode */
2192                 spin_lock_bh(&pxmitpriv->lock);
2193                 phead = get_list_head(&psta_bmc->sleep_q);
2194                 list_for_each_entry_safe(pxmitframe, ptmp, phead, list) {
2195                         list_del_init(&pxmitframe->list);
2196
2197                         psta_bmc->sleepq_len--;
2198                         if (psta_bmc->sleepq_len > 0)
2199                                 pxmitframe->attrib.mdata = 1;
2200                         else
2201                                 pxmitframe->attrib.mdata = 0;
2202
2203                         pxmitframe->attrib.triggered = 1;
2204                         rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
2205                 }
2206                 if (psta_bmc->sleepq_len == 0) {
2207                         pstapriv->tim_bitmap &= ~BIT(0);
2208                         pstapriv->sta_dz_bitmap &= ~BIT(0);
2209
2210                         /* update BCN for TIM IE */
2211                         /* update_BCNTIM(padapter); */
2212                         update_mask |= BIT(1);
2213                 }
2214                 /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
2215                 spin_unlock_bh(&pxmitpriv->lock);
2216         }
2217
2218         if (update_mask)
2219                 update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
2220 }
2221
2222 void xmit_delivery_enabled_frames23a(struct rtw_adapter *padapter,
2223                                   struct sta_info *psta)
2224 {
2225         u8 wmmps_ac = 0;
2226         struct list_head *phead;
2227         struct xmit_frame *pxmitframe, *ptmp;
2228         struct sta_priv *pstapriv = &padapter->stapriv;
2229         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2230
2231         /* spin_lock_bh(&psta->sleep_q.lock); */
2232         spin_lock_bh(&pxmitpriv->lock);
2233         phead = get_list_head(&psta->sleep_q);
2234         list_for_each_entry_safe(pxmitframe, ptmp, phead, list) {
2235                 switch (pxmitframe->attrib.priority) {
2236                 case 1:
2237                 case 2:
2238                         wmmps_ac = psta->uapsd_bk & BIT(1);
2239                         break;
2240                 case 4:
2241                 case 5:
2242                         wmmps_ac = psta->uapsd_vi & BIT(1);
2243                         break;
2244                 case 6:
2245                 case 7:
2246                         wmmps_ac = psta->uapsd_vo & BIT(1);
2247                         break;
2248                 case 0:
2249                 case 3:
2250                 default:
2251                         wmmps_ac = psta->uapsd_be & BIT(1);
2252                         break;
2253                 }
2254
2255                 if (!wmmps_ac)
2256                         continue;
2257
2258                 list_del_init(&pxmitframe->list);
2259
2260                 psta->sleepq_len--;
2261                 psta->sleepq_ac_len--;
2262
2263                 if (psta->sleepq_ac_len > 0) {
2264                         pxmitframe->attrib.mdata = 1;
2265                         pxmitframe->attrib.eosp = 0;
2266                 } else {
2267                         pxmitframe->attrib.mdata = 0;
2268                         pxmitframe->attrib.eosp = 1;
2269                 }
2270
2271                 pxmitframe->attrib.triggered = 1;
2272
2273                 rtl8723au_hal_xmitframe_enqueue(padapter, pxmitframe);
2274
2275                 if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) &&
2276                     (wmmps_ac)) {
2277                         pstapriv->tim_bitmap &= ~CHKBIT(psta->aid);
2278
2279                         /* update BCN for TIM IE */
2280                         update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
2281                 }
2282         }
2283         spin_unlock_bh(&pxmitpriv->lock);
2284 }
2285
2286 #endif
2287
2288 void rtw_sctx_init23a(struct submit_ctx *sctx, int timeout_ms)
2289 {
2290         sctx->timeout_ms = timeout_ms;
2291         init_completion(&sctx->done);
2292         sctx->status = RTW_SCTX_SUBMITTED;
2293 }
2294
2295 int rtw_sctx_wait23a(struct submit_ctx *sctx)
2296 {
2297         int ret = _FAIL;
2298         unsigned long expire;
2299         int status = 0;
2300
2301         expire = sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) :
2302                  MAX_SCHEDULE_TIMEOUT;
2303         if (!wait_for_completion_timeout(&sctx->done, expire)) {
2304                 /* timeout, do something?? */
2305                 status = RTW_SCTX_DONE_TIMEOUT;
2306                 DBG_8723A("%s timeout\n", __func__);
2307         } else {
2308                 status = sctx->status;
2309         }
2310
2311         if (status == RTW_SCTX_DONE_SUCCESS)
2312                 ret = _SUCCESS;
2313
2314         return ret;
2315 }
2316
2317 static bool rtw_sctx_chk_waring_status(int status)
2318 {
2319         switch (status) {
2320         case RTW_SCTX_DONE_UNKNOWN:
2321         case RTW_SCTX_DONE_BUF_ALLOC:
2322         case RTW_SCTX_DONE_BUF_FREE:
2323         case RTW_SCTX_DONE_DRV_STOP:
2324         case RTW_SCTX_DONE_DEV_REMOVE:
2325                 return true;
2326         default:
2327                 return false;
2328         }
2329 }
2330
2331 void rtw23a_sctx_done_err(struct submit_ctx **sctx, int status)
2332 {
2333         if (*sctx) {
2334                 if (rtw_sctx_chk_waring_status(status))
2335                         DBG_8723A("%s status:%d\n", __func__, status);
2336                 (*sctx)->status = status;
2337                 complete(&(*sctx)->done);
2338                 *sctx = NULL;
2339         }
2340 }
2341
2342 int rtw_ack_tx_wait23a(struct xmit_priv *pxmitpriv, u32 timeout_ms)
2343 {
2344         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
2345
2346         pack_tx_ops->timeout_ms = timeout_ms;
2347         pack_tx_ops->status = RTW_SCTX_SUBMITTED;
2348
2349         return rtw_sctx_wait23a(pack_tx_ops);
2350 }
2351