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