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