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