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