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 (pattrib->priority >= 16)
 956                                         printk(KERN_WARNING "%s: Invalid "
 957                                                "pattrib->priority %i\n",
 958                                                __func__, pattrib->priority);
 959                                 if (psta->htpriv.agg_enable_bitmap &
 960                                     BIT(pattrib->priority))
 961                                         pattrib->ampdu_en = true;
 962                         }
 963                         /* re-check if enable ampdu by BA_starting_seqctrl */
 964                         if (pattrib->ampdu_en) {
 965                                 u16 tx_seq;
 966
 967                                 tx_seq = psta->BA_starting_seqctrl[pattrib->priority & 0x0f];
 968
 969                                 /* check BA_starting_seqctrl */
 970                                 if (SN_LESS(pattrib->seqnum, tx_seq)) {
 971                                         /* DBG_8723A("tx ampdu seqnum(%d) < tx_seq(%d)\n", pattrib->seqnum, tx_seq); */
 972                                         pattrib->ampdu_en = false;/* AGG BK */
 973                                 } else if (SN_EQUAL(pattrib->seqnum, tx_seq)) {
 974                                         psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (tx_seq+1)&0xfff;
 975                                         pattrib->ampdu_en = true;/* AGG EN */
 976                                 } else {
 977                                         /* DBG_8723A("tx ampdu over run\n"); */
 978                                         psta->BA_starting_seqctrl[pattrib->priority & 0x0f] = (pattrib->seqnum+1)&0xfff;
 979                                         pattrib->ampdu_en = true;/* AGG EN */
 980                                 }
 981                         }
 982                 }
 983         }
 984 exit:
 985         return res;
 986 }
 987
 988 s32 rtw_txframes_pending23a(struct rtw_adapter *padapter)
 989 {
 990         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 991
 992         return (!list_empty(&pxmitpriv->be_pending.queue)) ||
 993                 (!list_empty(&pxmitpriv->bk_pending.queue)) ||
 994                 (!list_empty(&pxmitpriv->vi_pending.queue)) ||
 995                 (!list_empty(&pxmitpriv->vo_pending.queue));
 996 }
 997
 998 s32 rtw_txframes_sta_ac_pending23a(struct rtw_adapter *padapter,
 999                                 struct pkt_attrib *pattrib)
1000 {
1001         struct sta_info *psta;
1002         struct tx_servq *ptxservq;
1003         int priority = pattrib->priority;
1004
1005         if (pattrib->psta) {
1006                 psta = pattrib->psta;
1007         } else {
1008                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
1009                 psta = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]);
1010         }
1011         if (psta == NULL) {
1012                 DBG_8723A("%s, psta == NUL\n", __func__);
1013                 return 0;
1014         }
1015         if (!(psta->state &_FW_LINKED)) {
1016                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__,
1017                           psta->state);
1018                 return 0;
1019         }
1020         switch (priority) {
1021         case 1:
1022         case 2:
1023                 ptxservq = &psta->sta_xmitpriv.bk_q;
1024                 break;
1025         case 4:
1026         case 5:
1027                 ptxservq = &psta->sta_xmitpriv.vi_q;
1028                 break;
1029         case 6:
1030         case 7:
1031                 ptxservq = &psta->sta_xmitpriv.vo_q;
1032                 break;
1033         case 0:
1034         case 3:
1035         default:
1036                 ptxservq = &psta->sta_xmitpriv.be_q;
1037                 break;
1038         }
1039         return ptxservq->qcnt;
1040 }
1041
1042 /*
1043  * Calculate wlan 802.11 packet MAX size from pkt_attrib
1044  * This function doesn't consider fragment case
1045  */
1046 u32 rtw_calculate_wlan_pkt_size_by_attribue23a(struct pkt_attrib *pattrib)
1047 {
1048         u32     len = 0;
1049
1050         len = pattrib->hdrlen + pattrib->iv_len; /*  WLAN Header and IV */
1051         len += SNAP_SIZE + sizeof(u16); /*  LLC */
1052         len += pattrib->pktlen;
1053         if (pattrib->encrypt == _TKIP_) len += 8; /*  MIC */
1054         len += ((pattrib->bswenc) ? pattrib->icv_len : 0); /*  ICV */
1055
1056         return len;
1057 }
1058
1059 /*
1060
1061 This sub-routine will perform all the following:
1062
1063 1. remove 802.3 header.
1064 2. create wlan_header, based on the info in pxmitframe
1065 3. append sta's iv/ext-iv
1066 4. append LLC
1067 5. move frag chunk from pframe to pxmitframe->mem
1068 6. apply sw-encrypt, if necessary.
1069
1070 */
1071 s32 rtw_xmitframe_coalesce23a(struct rtw_adapter *padapter, struct sk_buff *skb,
1072                               struct xmit_frame *pxmitframe)
1073 {
1074         struct sta_info *psta;
1075         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1076         struct pkt_attrib *pattrib = &pxmitframe->attrib;
1077         s32 frg_inx, frg_len, mpdu_len, llc_sz, mem_sz;
1078         u8 *pframe, *mem_start;
1079         u8 hw_hdr_offset;
1080         u8 *pbuf_start;
1081         u8 *pdata = skb->data;
1082         int data_len = skb->len;
1083         s32 bmcst = is_multicast_ether_addr(pattrib->ra);
1084         s32 res = _SUCCESS;
1085
1086         if (pattrib->psta)
1087                 psta = pattrib->psta;
1088         else {
1089                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
1090                 psta = rtw_get_stainfo23a(&padapter->stapriv, pattrib->ra);
1091         }
1092
1093         if (!psta) {
1094                 DBG_8723A("%s, psta == NUL\n", __func__);
1095                 return _FAIL;
1096         }
1097
1098         if (!(psta->state &_FW_LINKED)) {
1099                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n",
1100                           __func__, psta->state);
1101                 return _FAIL;
1102         }
1103
1104         if (!pxmitframe->buf_addr) {
1105                 DBG_8723A("==> %s buf_addr == NULL\n", __func__);
1106                 return _FAIL;
1107         }
1108
1109         pbuf_start = pxmitframe->buf_addr;
1110
1111         hw_hdr_offset = TXDESC_OFFSET;
1112
1113         mem_start = pbuf_start + hw_hdr_offset;
1114
1115         if (rtw_make_wlanhdr23a(padapter, mem_start, pattrib) == _FAIL) {
1116                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1117                          ("rtw_xmitframe_coalesce23a: rtw_make_wlanhdr23a "
1118                           "fail; drop pkt\n"));
1119                 res = _FAIL;
1120                 goto exit;
1121         }
1122
1123         pdata += pattrib->pkt_hdrlen;
1124         data_len -= pattrib->pkt_hdrlen;
1125
1126         frg_inx = 0;
1127         frg_len = pxmitpriv->frag_len - 4;/* 2346-4 = 2342 */
1128
1129         while (1) {
1130                 llc_sz = 0;
1131
1132                 mpdu_len = frg_len;
1133
1134                 pframe = mem_start;
1135
1136                 SetMFrag(mem_start);
1137
1138                 pframe += pattrib->hdrlen;
1139                 mpdu_len -= pattrib->hdrlen;
1140
1141                 /* adding icv, if necessary... */
1142                 if (pattrib->iv_len) {
1143                         if (psta) {
1144                                 switch (pattrib->encrypt) {
1145                                 case _WEP40_:
1146                                 case _WEP104_:
1147                                         WEP_IV(pattrib->iv, psta->dot11txpn,
1148                                                pattrib->key_idx);
1149                                         break;
1150                                 case _TKIP_:
1151                                         if (bmcst)
1152                                                 TKIP_IV(pattrib->iv,
1153                                                         psta->dot11txpn,
1154                                                         pattrib->key_idx);
1155                                         else
1156                                                 TKIP_IV(pattrib->iv,
1157                                                         psta->dot11txpn, 0);
1158                                         break;
1159                                 case _AES_:
1160                                         if (bmcst)
1161                                                 AES_IV(pattrib->iv,
1162                                                        psta->dot11txpn,
1163                                                        pattrib->key_idx);
1164                                         else
1165                                                 AES_IV(pattrib->iv,
1166                                                        psta->dot11txpn, 0);
1167                                         break;
1168                                 }
1169                         }
1170
1171                         memcpy(pframe, pattrib->iv, pattrib->iv_len);
1172
1173                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_notice_,
1174                                  ("rtw_xmiaframe_coalesce23a: keyid =%d pattrib"
1175                                   "->iv[3]=%.2x pframe =%.2x %.2x %.2x %.2x\n",
1176                                   padapter->securitypriv.dot11PrivacyKeyIndex,
1177                                   pattrib->iv[3], *pframe, *(pframe+1),
1178                                   *(pframe+2), *(pframe+3)));
1179                         pframe += pattrib->iv_len;
1180                         mpdu_len -= pattrib->iv_len;
1181                 }
1182                 if (frg_inx == 0) {
1183                         llc_sz = rtw_put_snap23a(pframe, pattrib->ether_type);
1184                         pframe += llc_sz;
1185                         mpdu_len -= llc_sz;
1186                 }
1187
1188                 if (pattrib->icv_len > 0 && pattrib->bswenc)
1189                         mpdu_len -= pattrib->icv_len;
1190
1191                 if (bmcst)
1192                         /*  don't do fragment to broadcat/multicast packets */
1193                         mem_sz = min_t(s32, data_len, pattrib->pktlen);
1194                 else
1195                         mem_sz = min_t(s32, data_len, mpdu_len);
1196
1197                 memcpy(pframe, pdata, mem_sz);
1198
1199                 pframe += mem_sz;
1200                 pdata += mem_sz;
1201                 data_len -= mem_sz;
1202
1203                 if ((pattrib->icv_len >0) && (pattrib->bswenc)) {
1204                         memcpy(pframe, pattrib->icv, pattrib->icv_len);
1205                         pframe += pattrib->icv_len;
1206                 }
1207
1208                 frg_inx++;
1209
1210                 if (bmcst || data_len <= 0) {
1211                         pattrib->nr_frags = frg_inx;
1212
1213                         pattrib->last_txcmdsz = pattrib->hdrlen +
1214                                                 pattrib->iv_len +
1215                                                 ((pattrib->nr_frags == 1) ?
1216                                                 llc_sz : 0) +
1217                                                 ((pattrib->bswenc) ?
1218                                                 pattrib->icv_len : 0) + mem_sz;
1219
1220                         ClearMFrag(mem_start);
1221
1222                         break;
1223                 } else {
1224                         RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1225                                  ("%s: There're still something in packet!\n",
1226                                   __func__));
1227                 }
1228
1229                 mem_start = PTR_ALIGN(pframe, 4) + hw_hdr_offset;
1230                 memcpy(mem_start, pbuf_start + hw_hdr_offset, pattrib->hdrlen);
1231         }
1232
1233         if (xmitframe_addmic(padapter, pxmitframe) == _FAIL) {
1234                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1235                          ("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n"));
1236                 DBG_8723A("xmitframe_addmic(padapter, pxmitframe) == _FAIL\n");
1237                 res = _FAIL;
1238                 goto exit;
1239         }
1240
1241         xmitframe_swencrypt(padapter, pxmitframe);
1242
1243         if (bmcst == false)
1244                 update_attrib_vcs_info(padapter, pxmitframe);
1245         else
1246                 pattrib->vcs_mode = NONE_VCS;
1247
1248 exit:
1249         return res;
1250 }
1251
1252 /* Logical Link Control(LLC) SubNetwork Attachment Point(SNAP) header
1253  * IEEE LLC/SNAP header contains 8 octets
1254  * First 3 octets comprise the LLC portion
1255  * SNAP portion, 5 octets, is divided into two fields:
1256  *      Organizationally Unique Identifier(OUI), 3 octets,
1257  *      type, defined by that organization, 2 octets.
1258  */
1259 s32 rtw_put_snap23a(u8 *data, u16 h_proto)
1260 {
1261         struct ieee80211_snap_hdr *snap;
1262         u8 *oui;
1263
1264         snap = (struct ieee80211_snap_hdr *)data;
1265         snap->dsap = 0xaa;
1266         snap->ssap = 0xaa;
1267         snap->ctrl = 0x03;
1268
1269         if (h_proto == 0x8137 || h_proto == 0x80f3)
1270                 oui = P802_1H_OUI;
1271         else
1272                 oui = RFC1042_OUI;
1273         snap->oui[0] = oui[0];
1274         snap->oui[1] = oui[1];
1275         snap->oui[2] = oui[2];
1276         *(u16 *)(data + SNAP_SIZE) = htons(h_proto);
1277         return SNAP_SIZE + sizeof(u16);
1278 }
1279
1280 void rtw_update_protection23a(struct rtw_adapter *padapter, u8 *ie, uint ie_len)
1281 {
1282         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1283         struct registry_priv *pregistrypriv = &padapter->registrypriv;
1284         uint protection;
1285         const u8 *p;
1286
1287         switch (pxmitpriv->vcs_setting) {
1288         case DISABLE_VCS:
1289                 pxmitpriv->vcs = NONE_VCS;
1290                 break;
1291         case ENABLE_VCS:
1292                 break;
1293         case AUTO_VCS:
1294         default:
1295                 p = cfg80211_find_ie(WLAN_EID_ERP_INFO, ie, ie_len);
1296                 if (!p)
1297                         pxmitpriv->vcs = NONE_VCS;
1298                 else {
1299                         protection = (*(p + 2)) & BIT(1);
1300                         if (protection) {
1301                                 if (pregistrypriv->vcs_type == RTS_CTS)
1302                                         pxmitpriv->vcs = RTS_CTS;
1303                                 else
1304                                         pxmitpriv->vcs = CTS_TO_SELF;
1305                         } else {
1306                                 pxmitpriv->vcs = NONE_VCS;
1307                         }
1308                 }
1309                 break;
1310         }
1311 }
1312
1313 void rtw_count_tx_stats23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe, int sz)
1314 {
1315         struct sta_info *psta = NULL;
1316         struct stainfo_stats *pstats = NULL;
1317         struct xmit_priv        *pxmitpriv = &padapter->xmitpriv;
1318         struct mlme_priv        *pmlmepriv = &padapter->mlmepriv;
1319
1320         if ((pxmitframe->frame_tag&0x0f) == DATA_FRAMETAG) {
1321                 pxmitpriv->tx_bytes += sz;
1322                 pmlmepriv->LinkDetectInfo.NumTxOkInPeriod++;
1323
1324                 psta = pxmitframe->attrib.psta;
1325                 if (psta) {
1326                         pstats = &psta->sta_stats;
1327                         pstats->tx_pkts++;
1328                         pstats->tx_bytes += sz;
1329                 }
1330         }
1331 }
1332
1333 struct xmit_buf *rtw_alloc_xmitbuf23a_ext(struct xmit_priv *pxmitpriv)
1334 {
1335         unsigned long irqL;
1336         struct xmit_buf *pxmitbuf =  NULL;
1337         struct list_head *phead;
1338         struct rtw_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
1339
1340         spin_lock_irqsave(&pfree_queue->lock, irqL);
1341
1342         phead = get_list_head(pfree_queue);
1343
1344         if (!list_empty(phead)) {
1345                 pxmitbuf = list_first_entry(phead, struct xmit_buf, list);
1346
1347                 list_del_init(&pxmitbuf->list);
1348
1349                 pxmitpriv->free_xmit_extbuf_cnt--;
1350                 pxmitbuf->priv_data = NULL;
1351                 pxmitbuf->ext_tag = true;
1352
1353                 if (pxmitbuf->sctx) {
1354                         DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__);
1355                         rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
1356                 }
1357         }
1358
1359         spin_unlock_irqrestore(&pfree_queue->lock, irqL);
1360
1361         return pxmitbuf;
1362 }
1363
1364 s32 rtw_free_xmitbuf_ext23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
1365 {
1366         unsigned long irqL;
1367         struct rtw_queue *pfree_queue = &pxmitpriv->free_xmit_extbuf_queue;
1368
1369         if (pxmitbuf == NULL)
1370                 return _FAIL;
1371
1372         spin_lock_irqsave(&pfree_queue->lock, irqL);
1373
1374         list_del_init(&pxmitbuf->list);
1375
1376         list_add_tail(&pxmitbuf->list, get_list_head(pfree_queue));
1377         pxmitpriv->free_xmit_extbuf_cnt++;
1378
1379         spin_unlock_irqrestore(&pfree_queue->lock, irqL);
1380
1381         return _SUCCESS;
1382 }
1383
1384 struct xmit_buf *rtw_alloc_xmitbuf23a(struct xmit_priv *pxmitpriv)
1385 {
1386         unsigned long irqL;
1387         struct xmit_buf *pxmitbuf =  NULL;
1388         struct list_head *phead;
1389         struct rtw_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
1390
1391         /* DBG_8723A("+rtw_alloc_xmitbuf23a\n"); */
1392
1393         spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
1394
1395         phead = get_list_head(pfree_xmitbuf_queue);
1396
1397         if (!list_empty(phead)) {
1398                 pxmitbuf = list_first_entry(phead, struct xmit_buf, list);
1399
1400                 list_del_init(&pxmitbuf->list);
1401
1402                 pxmitpriv->free_xmitbuf_cnt--;
1403                 pxmitbuf->priv_data = NULL;
1404                 pxmitbuf->ext_tag = false;
1405                 pxmitbuf->flags = XMIT_VO_QUEUE;
1406
1407                 if (pxmitbuf->sctx) {
1408                         DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__);
1409                         rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_ALLOC);
1410                 }
1411         }
1412
1413         spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
1414
1415         return pxmitbuf;
1416 }
1417
1418 s32 rtw_free_xmitbuf23a(struct xmit_priv *pxmitpriv, struct xmit_buf *pxmitbuf)
1419 {
1420         unsigned long irqL;
1421         struct rtw_queue *pfree_xmitbuf_queue = &pxmitpriv->free_xmitbuf_queue;
1422
1423         /* DBG_8723A("+rtw_free_xmitbuf23a\n"); */
1424
1425         if (pxmitbuf == NULL)
1426                 return _FAIL;
1427
1428         if (pxmitbuf->sctx) {
1429                 DBG_8723A("%s pxmitbuf->sctx is not NULL\n", __func__);
1430                 rtw23a_sctx_done_err(&pxmitbuf->sctx, RTW_SCTX_DONE_BUF_FREE);
1431         }
1432
1433         if (pxmitbuf->ext_tag) {
1434                 rtw_free_xmitbuf_ext23a(pxmitpriv, pxmitbuf);
1435         } else {
1436                 spin_lock_irqsave(&pfree_xmitbuf_queue->lock, irqL);
1437
1438                 list_del_init(&pxmitbuf->list);
1439
1440                 list_add_tail(&pxmitbuf->list,
1441                               get_list_head(pfree_xmitbuf_queue));
1442
1443                 pxmitpriv->free_xmitbuf_cnt++;
1444                 spin_unlock_irqrestore(&pfree_xmitbuf_queue->lock, irqL);
1445         }
1446
1447         return _SUCCESS;
1448 }
1449
1450 static void rtw_init_xmitframe(struct xmit_frame *pxframe)
1451 {
1452         if (pxframe !=  NULL) {
1453                 /* default value setting */
1454                 pxframe->buf_addr = NULL;
1455                 pxframe->pxmitbuf = NULL;
1456
1457                 memset(&pxframe->attrib, 0, sizeof(struct pkt_attrib));
1458                 /* pxframe->attrib.psta = NULL; */
1459
1460                 pxframe->frame_tag = DATA_FRAMETAG;
1461
1462                 pxframe->pkt = NULL;
1463                 pxframe->pkt_offset = 1;/* default use pkt_offset to fill tx desc */
1464
1465                 pxframe->ack_report = 0;
1466         }
1467 }
1468
1469 /*
1470 Calling context:
1471 1. OS_TXENTRY
1472 2. RXENTRY (rx_thread or RX_ISR/RX_CallBack)
1473
1474 If we turn on USE_RXTHREAD, then, no need for critical section.
1475 Otherwise, we must use _enter/_exit critical to protect free_xmit_queue...
1476
1477 Must be very very cautious...
1478
1479 */
1480 static struct xmit_frame *rtw_alloc_xmitframe(struct xmit_priv *pxmitpriv)
1481 {
1482         struct xmit_frame *pxframe = NULL;
1483         struct list_head *plist, *phead;
1484         struct rtw_queue *pfree_xmit_queue = &pxmitpriv->free_xmit_queue;
1485
1486         spin_lock_bh(&pfree_xmit_queue->lock);
1487
1488         if (list_empty(&pfree_xmit_queue->queue)) {
1489                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1490                          ("rtw_alloc_xmitframe:%d\n",
1491                           pxmitpriv->free_xmitframe_cnt));
1492                 pxframe =  NULL;
1493         } else {
1494                 phead = get_list_head(pfree_xmit_queue);
1495
1496                 plist = phead->next;
1497
1498                 pxframe = container_of(plist, struct xmit_frame, list);
1499
1500                 list_del_init(&pxframe->list);
1501                 pxmitpriv->free_xmitframe_cnt--;
1502                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1503                          ("rtw_alloc_xmitframe():free_xmitframe_cnt =%d\n",
1504                           pxmitpriv->free_xmitframe_cnt));
1505         }
1506
1507         spin_unlock_bh(&pfree_xmit_queue->lock);
1508
1509         rtw_init_xmitframe(pxframe);
1510
1511         return pxframe;
1512 }
1513
1514 struct xmit_frame *rtw_alloc_xmitframe23a_ext(struct xmit_priv *pxmitpriv)
1515 {
1516         struct xmit_frame *pxframe = NULL;
1517         struct list_head *plist, *phead;
1518         struct rtw_queue *queue = &pxmitpriv->free_xframe_ext_queue;
1519
1520         spin_lock_bh(&queue->lock);
1521
1522         if (list_empty(&queue->queue)) {
1523                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a_ext:%d\n", pxmitpriv->free_xframe_ext_cnt));
1524                 pxframe =  NULL;
1525         } else {
1526                 phead = get_list_head(queue);
1527                 plist = phead->next;
1528                 pxframe = container_of(plist, struct xmit_frame, list);
1529
1530                 list_del_init(&pxframe->list);
1531                 pxmitpriv->free_xframe_ext_cnt--;
1532                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_alloc_xmitframe23a_ext():free_xmitframe_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
1533         }
1534
1535         spin_unlock_bh(&queue->lock);
1536
1537         rtw_init_xmitframe(pxframe);
1538
1539         return pxframe;
1540 }
1541
1542 s32 rtw_free_xmitframe23a(struct xmit_priv *pxmitpriv, struct xmit_frame *pxmitframe)
1543 {
1544         struct rtw_queue *queue = NULL;
1545         struct rtw_adapter *padapter = pxmitpriv->adapter;
1546         struct sk_buff *pndis_pkt = NULL;
1547
1548         if (pxmitframe == NULL) {
1549                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_, ("====== rtw_free_xmitframe23a():pxmitframe == NULL!!!!!!!!!!\n"));
1550                 goto exit;
1551         }
1552
1553         if (pxmitframe->pkt) {
1554                 pndis_pkt = pxmitframe->pkt;
1555                 pxmitframe->pkt = NULL;
1556         }
1557
1558         if (pxmitframe->ext_tag == 0)
1559                 queue = &pxmitpriv->free_xmit_queue;
1560         else if (pxmitframe->ext_tag == 1)
1561                 queue = &pxmitpriv->free_xframe_ext_queue;
1562
1563         if (!queue)
1564                 goto check_pkt_complete;
1565         spin_lock_bh(&queue->lock);
1566
1567         list_del_init(&pxmitframe->list);
1568         list_add_tail(&pxmitframe->list, get_list_head(queue));
1569         if (pxmitframe->ext_tag == 0) {
1570                 pxmitpriv->free_xmitframe_cnt++;
1571                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe23a():free_xmitframe_cnt =%d\n", pxmitpriv->free_xmitframe_cnt));
1572         } else if (pxmitframe->ext_tag == 1) {
1573                 pxmitpriv->free_xframe_ext_cnt++;
1574                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_debug_, ("rtw_free_xmitframe23a():free_xframe_ext_cnt =%d\n", pxmitpriv->free_xframe_ext_cnt));
1575         }
1576
1577         spin_unlock_bh(&queue->lock);
1578
1579 check_pkt_complete:
1580
1581         if (pndis_pkt)
1582                 rtw_os_pkt_complete23a(padapter, pndis_pkt);
1583
1584 exit:
1585
1586         return _SUCCESS;
1587 }
1588
1589 void rtw_free_xmitframe_queue23a(struct xmit_priv *pxmitpriv,
1590                                  struct rtw_queue *pframequeue)
1591 {
1592         struct list_head *plist, *phead, *ptmp;
1593         struct  xmit_frame *pxmitframe;
1594
1595         spin_lock_bh(&pframequeue->lock);
1596
1597         phead = get_list_head(pframequeue);
1598
1599         list_for_each_safe(plist, ptmp, phead) {
1600                 pxmitframe = container_of(plist, struct xmit_frame, list);
1601
1602                 rtw_free_xmitframe23a(pxmitpriv, pxmitframe);
1603         }
1604         spin_unlock_bh(&pframequeue->lock);
1605
1606 }
1607
1608 s32 rtw_xmitframe_enqueue23a(struct rtw_adapter *padapter,
1609                              struct xmit_frame *pxmitframe)
1610 {
1611         if (rtw_xmit23a_classifier(padapter, pxmitframe) == _FAIL) {
1612                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1613                          ("rtw_xmitframe_enqueue23a: drop xmit pkt for "
1614                           "classifier fail\n"));
1615                 return _FAIL;
1616         }
1617
1618         return _SUCCESS;
1619 }
1620
1621 static struct xmit_frame *
1622 dequeue_one_xmitframe(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit,
1623                       struct tx_servq *ptxservq, struct rtw_queue *pframe_queue)
1624 {
1625         struct list_head *phead;
1626         struct xmit_frame *pxmitframe = NULL;
1627
1628         phead = get_list_head(pframe_queue);
1629
1630         if (!list_empty(phead)) {
1631                 pxmitframe = list_first_entry(phead, struct xmit_frame, list);
1632                 list_del_init(&pxmitframe->list);
1633                 ptxservq->qcnt--;
1634         }
1635         return pxmitframe;
1636 }
1637
1638 struct xmit_frame *
1639 rtw_dequeue_xframe23a(struct xmit_priv *pxmitpriv, struct hw_xmit *phwxmit_i,
1640                    int entry)
1641 {
1642         struct list_head *sta_plist, *sta_phead, *ptmp;
1643         struct hw_xmit *phwxmit;
1644         struct tx_servq *ptxservq = NULL;
1645         struct rtw_queue *pframe_queue = NULL;
1646         struct xmit_frame *pxmitframe = NULL;
1647         struct rtw_adapter *padapter = pxmitpriv->adapter;
1648         struct registry_priv    *pregpriv = &padapter->registrypriv;
1649         int i, inx[4];
1650
1651         inx[0] = 0;
1652         inx[1] = 1;
1653         inx[2] = 2;
1654         inx[3] = 3;
1655         if (pregpriv->wifi_spec == 1) {
1656                 int j;
1657
1658                 for (j = 0; j < 4; j++)
1659                         inx[j] = pxmitpriv->wmm_para_seq[j];
1660         }
1661
1662         spin_lock_bh(&pxmitpriv->lock);
1663
1664         for (i = 0; i < entry; i++) {
1665                 phwxmit = phwxmit_i + inx[i];
1666
1667                 sta_phead = get_list_head(phwxmit->sta_queue);
1668
1669                 list_for_each_safe(sta_plist, ptmp, sta_phead) {
1670                         ptxservq = container_of(sta_plist, struct tx_servq,
1671                                                 tx_pending);
1672
1673                         pframe_queue = &ptxservq->sta_pending;
1674
1675                         pxmitframe = dequeue_one_xmitframe(pxmitpriv, phwxmit, ptxservq, pframe_queue);
1676
1677                         if (pxmitframe) {
1678                                 phwxmit->accnt--;
1679
1680                                 /* Remove sta node when there is no pending packets. */
1681                                 /* must be done after get_next and
1682                                    before break */
1683                                 if (list_empty(&pframe_queue->queue))
1684                                         list_del_init(&ptxservq->tx_pending);
1685                                 goto exit;
1686                         }
1687                 }
1688         }
1689 exit:
1690         spin_unlock_bh(&pxmitpriv->lock);
1691         return pxmitframe;
1692 }
1693
1694 struct tx_servq *rtw_get_sta_pending23a(struct rtw_adapter *padapter, struct sta_info *psta, int up, u8 *ac)
1695 {
1696         struct tx_servq *ptxservq = NULL;
1697
1698         switch (up) {
1699         case 1:
1700         case 2:
1701                 ptxservq = &psta->sta_xmitpriv.bk_q;
1702                 *(ac) = 3;
1703                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending23a : BK\n"));
1704                 break;
1705         case 4:
1706         case 5:
1707                 ptxservq = &psta->sta_xmitpriv.vi_q;
1708                 *(ac) = 1;
1709                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending23a : VI\n"));
1710                 break;
1711         case 6:
1712         case 7:
1713                 ptxservq = &psta->sta_xmitpriv.vo_q;
1714                 *(ac) = 0;
1715                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending23a : VO\n"));
1716                 break;
1717         case 0:
1718         case 3:
1719         default:
1720                 ptxservq = &psta->sta_xmitpriv.be_q;
1721                 *(ac) = 2;
1722                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("rtw_get_sta_pending23a : BE\n"));
1723                 break;
1724         }
1725         return ptxservq;
1726 }
1727
1728 /*
1729  * Will enqueue pxmitframe to the proper queue,
1730  * and indicate it to xx_pending list.....
1731  */
1732 s32 rtw_xmit23a_classifier(struct rtw_adapter *padapter,
1733                         struct xmit_frame *pxmitframe)
1734 {
1735         struct sta_info *psta;
1736         struct tx_servq *ptxservq;
1737         struct pkt_attrib       *pattrib = &pxmitframe->attrib;
1738         struct sta_priv *pstapriv = &padapter->stapriv;
1739         struct hw_xmit  *phwxmits =  padapter->xmitpriv.hwxmits;
1740         u8      ac_index;
1741         int res = _SUCCESS;
1742
1743         if (pattrib->psta) {
1744                 psta = pattrib->psta;
1745         } else {
1746                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
1747                 psta = rtw_get_stainfo23a(pstapriv, pattrib->ra);
1748         }
1749         if (psta == NULL) {
1750                 res = _FAIL;
1751                 DBG_8723A("rtw_xmit23a_classifier: psta == NULL\n");
1752                 RT_TRACE(_module_rtl871x_xmit_c_, _drv_err_,
1753                          ("rtw_xmit23a_classifier: psta == NULL\n"));
1754                 goto exit;
1755         }
1756         if (!(psta->state & _FW_LINKED)) {
1757                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__,
1758                           psta->state);
1759                 return _FAIL;
1760         }
1761         ptxservq = rtw_get_sta_pending23a(padapter, psta, pattrib->priority,
1762                                        (u8 *)(&ac_index));
1763
1764         if (list_empty(&ptxservq->tx_pending)) {
1765                 list_add_tail(&ptxservq->tx_pending,
1766                               get_list_head(phwxmits[ac_index].sta_queue));
1767         }
1768
1769         list_add_tail(&pxmitframe->list, get_list_head(&ptxservq->sta_pending));
1770         ptxservq->qcnt++;
1771         phwxmits[ac_index].accnt++;
1772 exit:
1773         return res;
1774 }
1775
1776 void rtw_alloc_hwxmits23a(struct rtw_adapter *padapter)
1777 {
1778         struct hw_xmit *hwxmits;
1779         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1780         int size;
1781
1782         pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
1783
1784         size = sizeof(struct hw_xmit) * (pxmitpriv->hwxmit_entry + 1);
1785         pxmitpriv->hwxmits = kzalloc(size, GFP_KERNEL);
1786
1787         hwxmits = pxmitpriv->hwxmits;
1788
1789         if (pxmitpriv->hwxmit_entry == 5) {
1790                 /* pxmitpriv->bmc_txqueue.head = 0; */
1791                 /* hwxmits[0] .phwtxqueue = &pxmitpriv->bmc_txqueue; */
1792                 hwxmits[0] .sta_queue = &pxmitpriv->bm_pending;
1793
1794                 /* pxmitpriv->vo_txqueue.head = 0; */
1795                 /* hwxmits[1] .phwtxqueue = &pxmitpriv->vo_txqueue; */
1796                 hwxmits[1] .sta_queue = &pxmitpriv->vo_pending;
1797
1798                 /* pxmitpriv->vi_txqueue.head = 0; */
1799                 /* hwxmits[2] .phwtxqueue = &pxmitpriv->vi_txqueue; */
1800                 hwxmits[2] .sta_queue = &pxmitpriv->vi_pending;
1801
1802                 /* pxmitpriv->bk_txqueue.head = 0; */
1803                 /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */
1804                 hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
1805
1806                 /* pxmitpriv->be_txqueue.head = 0; */
1807                 /* hwxmits[4] .phwtxqueue = &pxmitpriv->be_txqueue; */
1808                 hwxmits[4] .sta_queue = &pxmitpriv->be_pending;
1809
1810         } else if (pxmitpriv->hwxmit_entry == 4) {
1811
1812                 /* pxmitpriv->vo_txqueue.head = 0; */
1813                 /* hwxmits[0] .phwtxqueue = &pxmitpriv->vo_txqueue; */
1814                 hwxmits[0] .sta_queue = &pxmitpriv->vo_pending;
1815
1816                 /* pxmitpriv->vi_txqueue.head = 0; */
1817                 /* hwxmits[1] .phwtxqueue = &pxmitpriv->vi_txqueue; */
1818                 hwxmits[1] .sta_queue = &pxmitpriv->vi_pending;
1819
1820                 /* pxmitpriv->be_txqueue.head = 0; */
1821                 /* hwxmits[2] .phwtxqueue = &pxmitpriv->be_txqueue; */
1822                 hwxmits[2] .sta_queue = &pxmitpriv->be_pending;
1823
1824                 /* pxmitpriv->bk_txqueue.head = 0; */
1825                 /* hwxmits[3] .phwtxqueue = &pxmitpriv->bk_txqueue; */
1826                 hwxmits[3] .sta_queue = &pxmitpriv->bk_pending;
1827         } else {
1828
1829         }
1830 }
1831
1832 void rtw_free_hwxmits23a(struct rtw_adapter *padapter)
1833 {
1834         struct hw_xmit *hwxmits;
1835         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1836
1837         hwxmits = pxmitpriv->hwxmits;
1838         kfree(hwxmits);
1839 }
1840
1841 void rtw_init_hwxmits23a(struct hw_xmit *phwxmit, int entry)
1842 {
1843         int i;
1844
1845         for (i = 0; i < entry; i++, phwxmit++)
1846                 phwxmit->accnt = 0;
1847 }
1848
1849 u32 rtw_get_ff_hwaddr23a(struct xmit_frame *pxmitframe)
1850 {
1851         u32 addr;
1852         struct pkt_attrib *pattrib = &pxmitframe->attrib;
1853
1854         switch (pattrib->qsel) {
1855         case 0:
1856         case 3:
1857                 addr = BE_QUEUE_INX;
1858                 break;
1859         case 1:
1860         case 2:
1861                 addr = BK_QUEUE_INX;
1862                 break;
1863         case 4:
1864         case 5:
1865                 addr = VI_QUEUE_INX;
1866                 break;
1867         case 6:
1868         case 7:
1869                 addr = VO_QUEUE_INX;
1870                 break;
1871         case 0x10:
1872                 addr = BCN_QUEUE_INX;
1873                 break;
1874         case 0x11:/* BC/MC in PS (HIQ) */
1875                 addr = HIGH_QUEUE_INX;
1876                 break;
1877         case 0x12:
1878         default:
1879                 addr = MGT_QUEUE_INX;
1880                 break;
1881         }
1882
1883         return addr;
1884 }
1885
1886 static void do_queue_select(struct rtw_adapter  *padapter, struct pkt_attrib *pattrib)
1887 {
1888         u8 qsel;
1889
1890         qsel = pattrib->priority;
1891         RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_,
1892                  ("### do_queue_select priority =%d , qsel = %d\n",
1893                   pattrib->priority, qsel));
1894
1895         pattrib->qsel = qsel;
1896 }
1897
1898 /*
1899  * The main transmit(tx) entry
1900  *
1901  * Return
1902  *      1       enqueue
1903  *      0       success, hardware will handle this xmit frame(packet)
1904  *      <0      fail
1905  */
1906 int rtw_xmit23a(struct rtw_adapter *padapter, struct sk_buff *skb)
1907 {
1908         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
1909         struct xmit_frame *pxmitframe = NULL;
1910         s32 res;
1911
1912         pxmitframe = rtw_alloc_xmitframe(pxmitpriv);
1913
1914         if (pxmitframe == NULL) {
1915                 RT_TRACE(_module_xmit_osdep_c_, _drv_err_,
1916                          ("rtw_xmit23a: no more pxmitframe\n"));
1917                 return -1;
1918         }
1919
1920         res = update_attrib(padapter, skb, &pxmitframe->attrib);
1921
1922         if (res == _FAIL) {
1923                 RT_TRACE(_module_xmit_osdep_c_, _drv_err_, ("rtw_xmit23a: update attrib fail\n"));
1924                 rtw_free_xmitframe23a(pxmitpriv, pxmitframe);
1925                 return -1;
1926         }
1927         pxmitframe->pkt = skb;
1928
1929         rtw_led_control(padapter, LED_CTL_TX);
1930
1931         do_queue_select(padapter, &pxmitframe->attrib);
1932
1933 #ifdef CONFIG_8723AU_AP_MODE
1934         spin_lock_bh(&pxmitpriv->lock);
1935         if (xmitframe_enqueue_for_sleeping_sta23a(padapter, pxmitframe)) {
1936                 spin_unlock_bh(&pxmitpriv->lock);
1937                 return 1;
1938         }
1939         spin_unlock_bh(&pxmitpriv->lock);
1940 #endif
1941
1942         if (rtw_hal_xmit23a(padapter, pxmitframe) == false)
1943                 return 1;
1944
1945         return 0;
1946 }
1947
1948 #if defined(CONFIG_8723AU_AP_MODE)
1949
1950 int xmitframe_enqueue_for_sleeping_sta23a(struct rtw_adapter *padapter, struct xmit_frame *pxmitframe)
1951 {
1952         int ret = false;
1953         struct sta_info *psta = NULL;
1954         struct sta_priv *pstapriv = &padapter->stapriv;
1955         struct pkt_attrib *pattrib = &pxmitframe->attrib;
1956         struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
1957         int bmcst = is_multicast_ether_addr(pattrib->ra);
1958
1959         if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == false)
1960             return ret;
1961
1962         if (pattrib->psta) {
1963                 psta = pattrib->psta;
1964         } else {
1965                 DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
1966                 psta = rtw_get_stainfo23a(pstapriv, pattrib->ra);
1967         }
1968
1969         if (psta == NULL) {
1970                 DBG_8723A("%s, psta == NUL\n", __func__);
1971                 return false;
1972         }
1973
1974         if (!(psta->state & _FW_LINKED)) {
1975                 DBG_8723A("%s, psta->state(0x%x) != _FW_LINKED\n", __func__,
1976                           psta->state);
1977                 return false;
1978         }
1979
1980         if (pattrib->triggered == 1) {
1981                 if (bmcst)
1982                         pattrib->qsel = 0x11;/* HIQ */
1983                 return ret;
1984         }
1985
1986         if (bmcst) {
1987                 spin_lock_bh(&psta->sleep_q.lock);
1988
1989                 if (pstapriv->sta_dz_bitmap) {
1990                         /* if anyone sta is in ps mode */
1991                         list_del_init(&pxmitframe->list);
1992
1993                         /* spin_lock_bh(&psta->sleep_q.lock); */
1994
1995                         list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
1996
1997                         psta->sleepq_len++;
1998
1999                         pstapriv->tim_bitmap |= BIT(0);/*  */
2000                         pstapriv->sta_dz_bitmap |= BIT(0);
2001
2002                         /* DBG_8723A("enqueue, sq_len =%d, tim =%x\n", psta->sleepq_len, pstapriv->tim_bitmap); */
2003
2004                         /* tx bc/mc packets after upate bcn */
2005                         update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
2006
2007                         /* spin_unlock_bh(&psta->sleep_q.lock); */
2008
2009                         ret = true;
2010
2011                 }
2012
2013                 spin_unlock_bh(&psta->sleep_q.lock);
2014
2015                 return ret;
2016
2017         }
2018
2019         spin_lock_bh(&psta->sleep_q.lock);
2020
2021         if (psta->state&WIFI_SLEEP_STATE) {
2022                 u8 wmmps_ac = 0;
2023
2024                 if (pstapriv->sta_dz_bitmap & CHKBIT(psta->aid)) {
2025                         list_del_init(&pxmitframe->list);
2026
2027                         /* spin_lock_bh(&psta->sleep_q.lock); */
2028
2029                         list_add_tail(&pxmitframe->list, get_list_head(&psta->sleep_q));
2030
2031                         psta->sleepq_len++;
2032
2033                         switch (pattrib->priority) {
2034                         case 1:
2035                         case 2:
2036                                 wmmps_ac = psta->uapsd_bk & BIT(0);
2037                                 break;
2038                         case 4:
2039                         case 5:
2040                                 wmmps_ac = psta->uapsd_vi & BIT(0);
2041                                 break;
2042                         case 6:
2043                         case 7:
2044                                 wmmps_ac = psta->uapsd_vo & BIT(0);
2045                                 break;
2046                         case 0:
2047                         case 3:
2048                         default:
2049                                 wmmps_ac = psta->uapsd_be & BIT(0);
2050                                 break;
2051                         }
2052
2053                         if (wmmps_ac)
2054                                 psta->sleepq_ac_len++;
2055
2056                         if (((psta->has_legacy_ac) && (!wmmps_ac)) ||
2057                            ((!psta->has_legacy_ac) && (wmmps_ac))) {
2058                                 pstapriv->tim_bitmap |= CHKBIT(psta->aid);
2059
2060                                 if (psta->sleepq_len == 1) {
2061                                         /* upate BCN for TIM IE */
2062                                         update_beacon23a(padapter, WLAN_EID_TIM,
2063                                                          NULL, false);
2064                                 }
2065                         }
2066
2067                         /* spin_unlock_bh(&psta->sleep_q.lock); */
2068
2069                         /* if (psta->sleepq_len > (NR_XMITFRAME>>3)) */
2070                         /*  */
2071                         /*      wakeup_sta_to_xmit23a(padapter, psta); */
2072                         /*  */
2073
2074                         ret = true;
2075
2076                 }
2077
2078         }
2079
2080         spin_unlock_bh(&psta->sleep_q.lock);
2081
2082         return ret;
2083 }
2084
2085 static void
2086 dequeue_xmitframes_to_sleeping_queue(struct rtw_adapter *padapter,
2087                                      struct sta_info *psta,
2088                                      struct rtw_queue *pframequeue)
2089 {
2090         int ret;
2091         struct list_head *plist, *phead, *ptmp;
2092         u8      ac_index;
2093         struct tx_servq *ptxservq;
2094         struct pkt_attrib       *pattrib;
2095         struct xmit_frame       *pxmitframe;
2096         struct hw_xmit *phwxmits =  padapter->xmitpriv.hwxmits;
2097
2098         phead = get_list_head(pframequeue);
2099
2100         list_for_each_safe(plist, ptmp, phead) {
2101                 pxmitframe = container_of(plist, struct xmit_frame, list);
2102
2103                 ret = xmitframe_enqueue_for_sleeping_sta23a(padapter, pxmitframe);
2104
2105                 if (ret == true) {
2106                         pattrib = &pxmitframe->attrib;
2107
2108                         ptxservq = rtw_get_sta_pending23a(padapter, psta, pattrib->priority, (u8 *)(&ac_index));
2109
2110                         ptxservq->qcnt--;
2111                         phwxmits[ac_index].accnt--;
2112                 } else {
2113                         /* DBG_8723A("xmitframe_enqueue_for_sleeping_sta23a return false\n"); */
2114                 }
2115         }
2116 }
2117
2118 void stop_sta_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
2119 {
2120         struct sta_info *psta_bmc;
2121         struct sta_xmit_priv *pstaxmitpriv;
2122         struct sta_priv *pstapriv = &padapter->stapriv;
2123         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2124
2125         pstaxmitpriv = &psta->sta_xmitpriv;
2126
2127         /* for BC/MC Frames */
2128         psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
2129
2130         spin_lock_bh(&pxmitpriv->lock);
2131
2132         psta->state |= WIFI_SLEEP_STATE;
2133
2134         pstapriv->sta_dz_bitmap |= CHKBIT(psta->aid);
2135
2136         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vo_q.sta_pending);
2137         list_del_init(&pstaxmitpriv->vo_q.tx_pending);
2138
2139         dequeue_xmitframes_to_sleeping_queue(padapter, psta, &pstaxmitpriv->vi_q.sta_pending);
2140         list_del_init(&pstaxmitpriv->vi_q.tx_pending);
2141
2142         dequeue_xmitframes_to_sleeping_queue(padapter, psta,
2143                                              &pstaxmitpriv->be_q.sta_pending);
2144         list_del_init(&pstaxmitpriv->be_q.tx_pending);
2145
2146         dequeue_xmitframes_to_sleeping_queue(padapter, psta,
2147                                              &pstaxmitpriv->bk_q.sta_pending);
2148         list_del_init(&pstaxmitpriv->bk_q.tx_pending);
2149
2150         /* for BC/MC Frames */
2151         pstaxmitpriv = &psta_bmc->sta_xmitpriv;
2152         dequeue_xmitframes_to_sleeping_queue(padapter, psta_bmc,
2153                                              &pstaxmitpriv->be_q.sta_pending);
2154         list_del_init(&pstaxmitpriv->be_q.tx_pending);
2155
2156         spin_unlock_bh(&pxmitpriv->lock);
2157 }
2158
2159 void wakeup_sta_to_xmit23a(struct rtw_adapter *padapter, struct sta_info *psta)
2160 {
2161         u8 update_mask = 0, wmmps_ac = 0;
2162         struct sta_info *psta_bmc;
2163         struct list_head *plist, *phead, *ptmp;
2164         struct xmit_frame *pxmitframe = NULL;
2165         struct sta_priv *pstapriv = &padapter->stapriv;
2166         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2167
2168         spin_lock_bh(&pxmitpriv->lock);
2169
2170         phead = get_list_head(&psta->sleep_q);
2171
2172         list_for_each_safe(plist, ptmp, phead) {
2173                 pxmitframe = container_of(plist, struct xmit_frame, list);
2174                 list_del_init(&pxmitframe->list);
2175
2176                 switch (pxmitframe->attrib.priority) {
2177                 case 1:
2178                 case 2:
2179                         wmmps_ac = psta->uapsd_bk & BIT(1);
2180                         break;
2181                 case 4:
2182                 case 5:
2183                         wmmps_ac = psta->uapsd_vi & BIT(1);
2184                         break;
2185                 case 6:
2186                 case 7:
2187                         wmmps_ac = psta->uapsd_vo & BIT(1);
2188                         break;
2189                 case 0:
2190                 case 3:
2191                 default:
2192                         wmmps_ac = psta->uapsd_be & BIT(1);
2193                         break;
2194                 }
2195
2196                 psta->sleepq_len--;
2197                 if (psta->sleepq_len > 0)
2198                         pxmitframe->attrib.mdata = 1;
2199                 else
2200                         pxmitframe->attrib.mdata = 0;
2201
2202                 if (wmmps_ac) {
2203                         psta->sleepq_ac_len--;
2204                         if (psta->sleepq_ac_len > 0) {
2205                                 pxmitframe->attrib.mdata = 1;
2206                                 pxmitframe->attrib.eosp = 0;
2207                         } else {
2208                                 pxmitframe->attrib.mdata = 0;
2209                                 pxmitframe->attrib.eosp = 1;
2210                         }
2211                 }
2212
2213                 pxmitframe->attrib.triggered = 1;
2214                 rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
2215         }
2216
2217         if (psta->sleepq_len == 0) {
2218                 pstapriv->tim_bitmap &= ~CHKBIT(psta->aid);
2219
2220                 /* upate BCN for TIM IE */
2221                 update_mask = BIT(0);
2222
2223                 if (psta->state&WIFI_SLEEP_STATE)
2224                         psta->state ^= WIFI_SLEEP_STATE;
2225
2226                 if (psta->state & WIFI_STA_ALIVE_CHK_STATE) {
2227                         psta->expire_to = pstapriv->expire_to;
2228                         psta->state ^= WIFI_STA_ALIVE_CHK_STATE;
2229                 }
2230
2231                 pstapriv->sta_dz_bitmap &= ~CHKBIT(psta->aid);
2232         }
2233
2234         /* spin_unlock_bh(&psta->sleep_q.lock); */
2235         spin_unlock_bh(&pxmitpriv->lock);
2236
2237         /* for BC/MC Frames */
2238         psta_bmc = rtw_get_bcmc_stainfo23a(padapter);
2239         if (!psta_bmc)
2240                 return;
2241
2242         if ((pstapriv->sta_dz_bitmap&0xfffe) == 0x0) {
2243                 /* no any sta in ps mode */
2244                 spin_lock_bh(&pxmitpriv->lock);
2245
2246                 phead = get_list_head(&psta_bmc->sleep_q);
2247
2248                 list_for_each_safe(plist, ptmp, phead) {
2249                         pxmitframe = container_of(plist, struct xmit_frame,
2250                                                   list);
2251
2252                         list_del_init(&pxmitframe->list);
2253
2254                         psta_bmc->sleepq_len--;
2255                         if (psta_bmc->sleepq_len > 0)
2256                                 pxmitframe->attrib.mdata = 1;
2257                         else
2258                                 pxmitframe->attrib.mdata = 0;
2259
2260                         pxmitframe->attrib.triggered = 1;
2261                         rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
2262                 }
2263                 if (psta_bmc->sleepq_len == 0) {
2264                         pstapriv->tim_bitmap &= ~BIT(0);
2265                         pstapriv->sta_dz_bitmap &= ~BIT(0);
2266
2267                         /* upate BCN for TIM IE */
2268                         /* update_BCNTIM(padapter); */
2269                         update_mask |= BIT(1);
2270                 }
2271
2272                 /* spin_unlock_bh(&psta_bmc->sleep_q.lock); */
2273                 spin_unlock_bh(&pxmitpriv->lock);
2274         }
2275
2276         if (update_mask)
2277                 update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
2278 }
2279
2280 void xmit_delivery_enabled_frames23a(struct rtw_adapter *padapter,
2281                                   struct sta_info *psta)
2282 {
2283         u8 wmmps_ac = 0;
2284         struct list_head *plist, *phead, *ptmp;
2285         struct xmit_frame *pxmitframe;
2286         struct sta_priv *pstapriv = &padapter->stapriv;
2287         struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
2288
2289         /* spin_lock_bh(&psta->sleep_q.lock); */
2290         spin_lock_bh(&pxmitpriv->lock);
2291
2292         phead = get_list_head(&psta->sleep_q);
2293
2294         list_for_each_safe(plist, ptmp, phead) {
2295                 pxmitframe = container_of(plist, struct xmit_frame, list);
2296
2297                 switch (pxmitframe->attrib.priority) {
2298                 case 1:
2299                 case 2:
2300                         wmmps_ac = psta->uapsd_bk & BIT(1);
2301                         break;
2302                 case 4:
2303                 case 5:
2304                         wmmps_ac = psta->uapsd_vi & BIT(1);
2305                         break;
2306                 case 6:
2307                 case 7:
2308                         wmmps_ac = psta->uapsd_vo & BIT(1);
2309                         break;
2310                 case 0:
2311                 case 3:
2312                 default:
2313                         wmmps_ac = psta->uapsd_be & BIT(1);
2314                         break;
2315                 }
2316
2317                 if (!wmmps_ac)
2318                         continue;
2319
2320                 list_del_init(&pxmitframe->list);
2321
2322                 psta->sleepq_len--;
2323                 psta->sleepq_ac_len--;
2324
2325                 if (psta->sleepq_ac_len > 0) {
2326                         pxmitframe->attrib.mdata = 1;
2327                         pxmitframe->attrib.eosp = 0;
2328                 } else {
2329                         pxmitframe->attrib.mdata = 0;
2330                         pxmitframe->attrib.eosp = 1;
2331                 }
2332
2333                 pxmitframe->attrib.triggered = 1;
2334
2335                 rtw_hal_xmit23aframe_enqueue(padapter, pxmitframe);
2336
2337                 if ((psta->sleepq_ac_len == 0) && (!psta->has_legacy_ac) &&
2338                     (wmmps_ac)) {
2339                         pstapriv->tim_bitmap &= ~CHKBIT(psta->aid);
2340
2341                         /* upate BCN for TIM IE */
2342                         update_beacon23a(padapter, WLAN_EID_TIM, NULL, false);
2343                 }
2344         }
2345         spin_unlock_bh(&pxmitpriv->lock);
2346 }
2347
2348 #endif
2349
2350 void rtw_sctx_init23a(struct submit_ctx *sctx, int timeout_ms)
2351 {
2352         sctx->timeout_ms = timeout_ms;
2353         init_completion(&sctx->done);
2354         sctx->status = RTW_SCTX_SUBMITTED;
2355 }
2356
2357 int rtw_sctx_wait23a(struct submit_ctx *sctx)
2358 {
2359         int ret = _FAIL;
2360         unsigned long expire;
2361         int status = 0;
2362
2363         expire = sctx->timeout_ms ? msecs_to_jiffies(sctx->timeout_ms) :
2364                  MAX_SCHEDULE_TIMEOUT;
2365         if (!wait_for_completion_timeout(&sctx->done, expire)) {
2366                 /* timeout, do something?? */
2367                 status = RTW_SCTX_DONE_TIMEOUT;
2368                 DBG_8723A("%s timeout\n", __func__);
2369         } else {
2370                 status = sctx->status;
2371         }
2372
2373         if (status == RTW_SCTX_DONE_SUCCESS)
2374                 ret = _SUCCESS;
2375
2376         return ret;
2377 }
2378
2379 static bool rtw_sctx_chk_waring_status(int status)
2380 {
2381         switch (status) {
2382         case RTW_SCTX_DONE_UNKNOWN:
2383         case RTW_SCTX_DONE_BUF_ALLOC:
2384         case RTW_SCTX_DONE_BUF_FREE:
2385         case RTW_SCTX_DONE_DRV_STOP:
2386         case RTW_SCTX_DONE_DEV_REMOVE:
2387                 return true;
2388         default:
2389                 return false;
2390         }
2391 }
2392
2393 void rtw23a_sctx_done_err(struct submit_ctx **sctx, int status)
2394 {
2395         if (*sctx) {
2396                 if (rtw_sctx_chk_waring_status(status))
2397                         DBG_8723A("%s status:%d\n", __func__, status);
2398                 (*sctx)->status = status;
2399                 complete(&(*sctx)->done);
2400                 *sctx = NULL;
2401         }
2402 }
2403
2404 void rtw_sctx_done23a(struct submit_ctx **sctx)
2405 {
2406         rtw23a_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
2407 }
2408
2409 int rtw_ack_tx_wait23a(struct xmit_priv *pxmitpriv, u32 timeout_ms)
2410 {
2411         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
2412
2413         pack_tx_ops->timeout_ms = timeout_ms;
2414         pack_tx_ops->status = RTW_SCTX_SUBMITTED;
2415
2416         return rtw_sctx_wait23a(pack_tx_ops);
2417 }
2418
2419 void rtw_ack_tx_done23a(struct xmit_priv *pxmitpriv, int status)
2420 {
2421         struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
2422
2423         if (pxmitpriv->ack_tx)
2424                 rtw23a_sctx_done_err(&pack_tx_ops, status);
2425         else
2426                 DBG_8723A("%s ack_tx not set\n", __func__);
2427 }