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