drivers/staging/r8188eu/core/rtw_xmit.c

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