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