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