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