drivers/staging/r8188eu/core/rtw_xmit.c

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