drivers/staging/rtl8188eu/core/rtw_xmit.c

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