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